Skip to content

Commit

Permalink
fix tum dataset extrinsics
Browse files Browse the repository at this point in the history
  • Loading branch information
@jlblancoc
jlblancoc committed Oct 17, 2023
1 parent dd429c4 commit b920d2f
Showing 1 changed file with 103 additions and 50 deletions.
153 changes: 103 additions & 50 deletions samples/maps_voxelmap_from_tum_dataset/test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,11 +11,14 @@
#include <mrpt/io/lazy_load_path.h>
#include <mrpt/maps/CVoxelMap.h>
#include <mrpt/math/CMatrixDynamic.h>
#include <mrpt/obs/CObservation3DRangeScan.h>
#include <mrpt/obs/CRawlog.h>
#include <mrpt/obs/stock_observations.h>
#include <mrpt/opengl/CFrustum.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/COctoMapVoxels.h>
#include <mrpt/opengl/CPointCloudColoured.h>
#include <mrpt/opengl/stock_objects.h>
#include <mrpt/poses/CPose3DInterpolator.h>
#include <mrpt/system/os.h>

#include <chrono>
Expand Down Expand Up @@ -47,42 +50,33 @@ void TestVoxelMapFromTUM(

std::cout << "Done! " << gtData.rows() << " rows." << std::endl;

mrpt::maps::CVoxelMap map(0.1);

// map.insertionOptions.max_range = 5.0; // [m]

// Manually update voxels:
if (false)
// # timestamp tx ty tz qx qy qz qw
mrpt::poses::CPose3DInterpolator gt;
for (int i = 0; i < gtData.rows(); i++)
{
map.updateVoxel(1, 1, 1, true); // integrate 'occupied' measurement

map.updateVoxel(1.5, 1, 1, true); // integrate 'occupied' measurement
map.updateVoxel(1.5, 1, 1, true); // integrate 'occupied' measurement
map.updateVoxel(1.5, 1, 1, true); // integrate 'occupied' measurement

map.updateVoxel(-1, -1, 1, false); // integrate 'free' measurement

double occup;
bool is_mapped;
mrpt::math::TPoint3D pt;
gt.insert(
mrpt::Clock::fromDouble(gtData(i, 0)),
mrpt::poses::CPose3D::FromQuaternionAndTranslation(
mrpt::math::CQuaternionDouble(
gtData(i, 7), gtData(i, 4), gtData(i, 5), gtData(i, 6)),
mrpt::math::TPoint3D(
gtData(i, 1), gtData(i, 2), gtData(i, 3))));
}

pt = mrpt::math::TPoint3D(1, 1, 1);
is_mapped = map.getPointOccupancy(pt.x, pt.y, pt.z, occup);
std::cout << "pt: " << pt
<< " is mapped?: " << (is_mapped ? "YES" : "NO")
<< " occupancy: " << occup << std::endl;
mrpt::maps::CVoxelMap map(0.02);

pt = mrpt::math::TPoint3D(-1, -1, 1);
is_mapped = map.getPointOccupancy(pt.x, pt.y, pt.z, occup);
std::cout << "pt: " << pt
<< " is mapped?: " << (is_mapped ? "YES" : "NO")
<< " occupancy: " << occup << std::endl;
}
// map.insertionOptions.max_range = 5.0; // [m]

mrpt::gui::CDisplayWindow3D win("VoxelMap demo", 640, 480);

auto gl_map = mrpt::opengl::COctoMapVoxels::Create();

auto glCamGroup = mrpt::opengl::CSetOfObjects::Create();
glCamGroup->insert(mrpt::opengl::stock_objects::CornerXYZSimple(0.3));
auto glObsPts = mrpt::opengl::CPointCloudColoured::Create();
glCamGroup->insert(glObsPts);
bool glCamFrustrumDone = false;

{
mrpt::opengl::Scene::Ptr& scene = win.get3DSceneAndLock();

Expand All @@ -94,6 +88,8 @@ void TestVoxelMapFromTUM(
}
scene->insert(mrpt::opengl::stock_objects::CornerXYZSimple());

scene->insert(glCamGroup);

map.getAsOctoMapVoxels(*gl_map);

// View occupied points:
Expand All @@ -113,10 +109,64 @@ void TestVoxelMapFromTUM(

std::cout << "Close the window to exit" << std::endl;

bool update_msg = true;
size_t rawlogIndex = 0;

mrpt::Clock::time_point lastObsTim;

while (win.isOpen())
{
win.get3DSceneAndLock();

// Get and process one observation:
if (rawlogIndex < dataset.size())
{
mrpt::obs::CObservation3DRangeScan::Ptr obs;

if (dataset.getType(rawlogIndex) ==
mrpt::obs::CRawlog::etObservation &&
(obs = std::dynamic_pointer_cast<
mrpt::obs::CObservation3DRangeScan>(
dataset.getAsObservation(rawlogIndex))))
{
bool poseOk = false;
mrpt::poses::CPose3D camPose;
lastObsTim = obs->getTimeStamp();
gt.interpolate(lastObsTim, camPose, poseOk);

if (poseOk)
{
// set viz camera pose:
glCamGroup->setPose(camPose);

using namespace mrpt::literals;
obs->sensorPose = mrpt::poses::CPose3D::FromYawPitchRoll(
0.0_deg, -90.0_deg, 90.0_deg);

// draw observation raw data:
mrpt::obs::T3DPointsProjectionParams pp;
pp.takeIntoAccountSensorPoseOnRobot = true;
obs->unprojectInto(*glObsPts, pp);

if (!glCamFrustrumDone)
{
glCamFrustrumDone = true;
auto glFrustrum = mrpt::opengl::CFrustum::Create(
obs->cameraParamsIntensity,
1e-3 /*focalDistScale*/);
glFrustrum->setPose(obs->sensorPose);
glCamGroup->insert(glFrustrum);
}

// update the voxel map:

// Update the voxel map visualization:
}
}
rawlogIndex++;
}

win.unlockAccess3DScene();

if (win.keyHit())
{
const unsigned int k = win.getPushedKey();
Expand Down Expand Up @@ -146,28 +196,31 @@ void TestVoxelMapFromTUM(
gl_map->enableLights(!gl_map->areLightsEnabled());
break;
};
update_msg = true;
}

if (update_msg)
{
update_msg = false;

win.addTextMessage(
5, 5,
mrpt::format(
"Commands: 'f' (freespace=%s) | 'o' (occupied=%s) | 'l' "
"(lights=%s)",
gl_map->areVoxelsVisible(mrpt::opengl::VOXEL_SET_FREESPACE)
? "YES"
: "NO",
gl_map->areVoxelsVisible(mrpt::opengl::VOXEL_SET_OCCUPIED)
? "YES"
: "NO",
gl_map->areLightsEnabled() ? "YES" : "NO"));

win.repaint();
}
win.addTextMessage(
5, 5,
mrpt::format(
"Commands: 'f' (freespace=%s) | 'o' (occupied=%s) | 'l' "
"(lights=%s)",
gl_map->areVoxelsVisible(mrpt::opengl::VOXEL_SET_FREESPACE)
? "YES"
: "NO",
gl_map->areVoxelsVisible(mrpt::opengl::VOXEL_SET_OCCUPIED)
? "YES"
: "NO",
gl_map->areLightsEnabled() ? "YES" : "NO"),
0 /*id*/);

win.addTextMessage(
5, 20,
mrpt::format(
"Timestamp: %s RawlogIndex: %zu",
mrpt::system::dateTimeLocalToString(lastObsTim).c_str(),
rawlogIndex),
1 /*id*/);

win.repaint();

using namespace std::chrono_literals;
std::this_thread::sleep_for(10ms);
Expand Down

0 comments on commit b920d2f

Please sign in to comment.