void CPoseRandomSampler::setPosePDF(const CPose3DPDF::Ptr& pdf)
{
setPosePDF(pdf.get());
}
void test_icp3D()
{
// Create the reference objects:
COpenGLScene::Ptr scene1 = mrpt::make_aligned_shared<COpenGLScene>();
COpenGLScene::Ptr scene2 = mrpt::make_aligned_shared<COpenGLScene>();
COpenGLScene::Ptr scene3 = mrpt::make_aligned_shared<COpenGLScene>();
opengl::CGridPlaneXY::Ptr plane1 =
mrpt::make_aligned_shared<CGridPlaneXY>(-20, 20, -20, 20, 0, 1);
plane1->setColor(0.3, 0.3, 0.3);
scene1->insert(plane1);
scene2->insert(plane1);
scene3->insert(plane1);
CSetOfObjects::Ptr world = mrpt::make_aligned_shared<CSetOfObjects>();
generateObjects(world);
scene1->insert(world);
// Perform the 3D scans:
CAngularObservationMesh::Ptr aom1 =
mrpt::make_aligned_shared<CAngularObservationMesh>();
CAngularObservationMesh::Ptr aom2 =
mrpt::make_aligned_shared<CAngularObservationMesh>();
cout << "Performing ray-tracing..." << endl;
CAngularObservationMesh::trace2DSetOfRays(
scene1, viewpoint1, aom1,
CAngularObservationMesh::TDoubleRange::CreateFromAperture(
M_PI, HOW_MANY_PITCHS),
CAngularObservationMesh::TDoubleRange::CreateFromAperture(
M_PI, HOW_MANY_YAWS));
CAngularObservationMesh::trace2DSetOfRays(
scene1, viewpoint2, aom2,
CAngularObservationMesh::TDoubleRange::CreateFromAperture(
M_PI, HOW_MANY_PITCHS),
CAngularObservationMesh::TDoubleRange::CreateFromAperture(
M_PI, HOW_MANY_YAWS));
cout << "Ray-tracing done" << endl;
// Put the viewpoints origins:
{
CSetOfObjects::Ptr origin1 = opengl::stock_objects::CornerXYZ();
origin1->setPose(viewpoint1);
origin1->setScale(0.6);
scene1->insert(origin1);
scene2->insert(origin1);
}
{
CSetOfObjects::Ptr origin2 = opengl::stock_objects::CornerXYZ();
origin2->setPose(viewpoint2);
origin2->setScale(0.6);
scene1->insert(origin2);
scene2->insert(origin2);
}
// Show the scanned points:
CSimplePointsMap M1, M2;
aom1->generatePointCloud(&M1);
aom2->generatePointCloud(&M2);
// Create the wrongly-localized M2:
CSimplePointsMap M2_noisy;
M2_noisy = M2;
M2_noisy.changeCoordinatesReference(SCAN2_POSE_ERROR);
CSetOfObjects::Ptr PTNS1 = mrpt::make_aligned_shared<CSetOfObjects>();
CSetOfObjects::Ptr PTNS2 = mrpt::make_aligned_shared<CSetOfObjects>();
CPointsMap::COLOR_3DSCENE(mrpt::utils::TColorf(1, 0, 0));
M1.getAs3DObject(PTNS1);
CPointsMap::COLOR_3DSCENE(mrpt::utils::TColorf(0, 0, 1));
M2_noisy.getAs3DObject(PTNS2);
scene2->insert(PTNS1);
scene2->insert(PTNS2);
// --------------------------------------
// Do the ICP-3D
// --------------------------------------
float run_time;
CICP icp;
CICP::TReturnInfo icp_info;
icp.options.thresholdDist = 0.40;
icp.options.thresholdAng = 0;
std::vector<double> xs, ys, zs;
M1.getAllPoints(xs, ys, ys);
cout << "Size of xs in M1: " << xs.size() << endl;
M2.getAllPoints(xs, ys, ys);
cout << "Size of xs in M2: " << xs.size() << endl;
CPose3DPDF::Ptr pdf = icp.Align3D(
&M2_noisy, // Map to align
&M1, // Reference map
CPose3D(), // Initial gross estimate
&run_time, &icp_info);
CPose3D mean = pdf->getMeanVal();
cout << "ICP run took " << run_time << " secs." << endl;
cout << "Goodness: " << 100 * icp_info.goodness
<< "% , # of iterations= " << icp_info.nIterations
<< " Quality: " << icp_info.quality << endl;
cout << "ICP output: mean= " << mean << endl;
cout << "Real displacement: " << SCAN2_POSE_ERROR << endl;
// Aligned maps:
CSetOfObjects::Ptr PTNS2_ALIGN = mrpt::make_aligned_shared<CSetOfObjects>();
M2_noisy.changeCoordinatesReference(CPose3D() - mean);
M2_noisy.getAs3DObject(PTNS2_ALIGN);
scene3->insert(PTNS1);
scene3->insert(PTNS2_ALIGN);
// Show in Windows:
CDisplayWindow3D window("ICP-3D demo: scene", 500, 500);
CDisplayWindow3D window2("ICP-3D demo: UNALIGNED scans", 500, 500);
CDisplayWindow3D window3("ICP-3D demo: ICP-ALIGNED scans", 500, 500);
window.setPos(10, 10);
window2.setPos(530, 10);
window3.setPos(10, 520);
window.get3DSceneAndLock() = scene1;
window.unlockAccess3DScene();
window2.get3DSceneAndLock() = scene2;
window2.unlockAccess3DScene();
window3.get3DSceneAndLock() = scene3;
window3.unlockAccess3DScene();
std::this_thread::sleep_for(20ms);
window.forceRepaint();
window2.forceRepaint();
window.setCameraElevationDeg(15);
window.setCameraAzimuthDeg(90);
window.setCameraZoom(15);
window2.setCameraElevationDeg(15);
window2.setCameraAzimuthDeg(90);
window2.setCameraZoom(15);
window3.setCameraElevationDeg(15);
window3.setCameraAzimuthDeg(90);
window3.setCameraZoom(15);
cout << "Press any key to exit..." << endl;
window.waitForKey();
}
void ICPslamWrapper::run3Dwindow()
{
// Create 3D window if requested (the code is copied from ../mrpt/apps/icp-slam/icp-slam_main.cpp):
if (SHOW_PROGRESS_3D_REAL_TIME && win3D_)
{
// get currently builded map
metric_map_ = mapBuilder.getCurrentlyBuiltMetricMap();
lst_current_laser_scans.clear();
CPose3D robotPose;
mapBuilder.getCurrentPoseEstimation()->getMean(robotPose);
COpenGLScene::Ptr scene = COpenGLScene::Create();
COpenGLViewport::Ptr view = scene->getViewport("main");
COpenGLViewport::Ptr view_map = scene->createViewport("mini-map");
view_map->setBorderSize(2);
view_map->setViewportPosition(0.01, 0.01, 0.35, 0.35);
view_map->setTransparent(false);
{
mrpt::opengl::CCamera &cam = view_map->getCamera();
cam.setAzimuthDegrees(-90);
cam.setElevationDegrees(90);
cam.setPointingAt(robotPose);
cam.setZoomDistance(20);
cam.setOrthogonal();
}
// The ground:
mrpt::opengl::CGridPlaneXY::Ptr groundPlane = mrpt::opengl::CGridPlaneXY::Create(-200, 200, -200, 200, 0, 5);
groundPlane->setColor(0.4, 0.4, 0.4);
view->insert(groundPlane);
view_map->insert(CRenderizable::Ptr(groundPlane)); // A copy
// The camera pointing to the current robot pose:
if (CAMERA_3DSCENE_FOLLOWS_ROBOT)
{
scene->enableFollowCamera(true);
mrpt::opengl::CCamera &cam = view_map->getCamera();
cam.setAzimuthDegrees(-45);
cam.setElevationDegrees(45);
cam.setPointingAt(robotPose);
}
// The maps:
{
opengl::CSetOfObjects::Ptr obj = opengl::CSetOfObjects::Create();
metric_map_->getAs3DObject(obj);
view->insert(obj);
// Only the point map:
opengl::CSetOfObjects::Ptr ptsMap = opengl::CSetOfObjects::Create();
if (metric_map_->m_pointsMaps.size())
{
metric_map_->m_pointsMaps[0]->getAs3DObject(ptsMap);
view_map->insert(ptsMap);
}
}
// Draw the robot path:
CPose3DPDF::Ptr posePDF = mapBuilder.getCurrentPoseEstimation();
CPose3D curRobotPose;
posePDF->getMean(curRobotPose);
{
opengl::CSetOfObjects::Ptr obj = opengl::stock_objects::RobotPioneer();
obj->setPose(curRobotPose);
view->insert(obj);
}
{
opengl::CSetOfObjects::Ptr obj = opengl::stock_objects::RobotPioneer();
obj->setPose(curRobotPose);
view_map->insert(obj);
}
opengl::COpenGLScene::Ptr &ptrScene = win3D_->get3DSceneAndLock();
ptrScene = scene;
win3D_->unlockAccess3DScene();
// Move camera:
win3D_->setCameraPointingToPoint(curRobotPose.x(), curRobotPose.y(), curRobotPose.z());
// Update:
win3D_->forceRepaint();
// Build list of scans:
if (SHOW_LASER_SCANS_3D)
{
// Rawlog in "Observation-only" format:
if (isObsBasedRawlog)
{
if (IS_CLASS(observation, CObservation2DRangeScan))
{
lst_current_laser_scans.push_back(mrpt::ptr_cast<CObservation2DRangeScan>::from(observation));
}
}
else
{
// Rawlog in the Actions-SF format:
for (size_t i = 0;; i++)
{
CObservation2DRangeScan::Ptr new_obs = observations->getObservationByClass<CObservation2DRangeScan>(i);
if (!new_obs)
break; // There're no more scans
else
lst_current_laser_scans.push_back(new_obs);
}
}
}
// Draw laser scanners in 3D:
if (SHOW_LASER_SCANS_3D)
{
for (size_t i = 0; i < lst_current_laser_scans.size(); i++)
{
// Create opengl object and load scan data from the scan observation:
opengl::CPlanarLaserScan::Ptr obj = opengl::CPlanarLaserScan::Create();
obj->setScan(*lst_current_laser_scans[i]);
obj->setPose(curRobotPose);
obj->setSurfaceColor(1.0f, 0.0f, 0.0f, 0.5f);
// inser into the scene:
view->insert(obj);
}
}
}
}
// ------------------------------------------------------
// MapBuilding_ICP
// override_rawlog_file: If not empty, use that rawlog
// instead of that in the config file.
// ------------------------------------------------------
void MapBuilding_ICP(
const string& INI_FILENAME, const string& override_rawlog_file)
{
MRPT_START
CConfigFile iniFile(INI_FILENAME);
// ------------------------------------------
// Load config from file:
// ------------------------------------------
const string RAWLOG_FILE = !override_rawlog_file.empty()
? override_rawlog_file
: iniFile.read_string(
"MappingApplication", "rawlog_file",
"", /*Force existence:*/ true);
const unsigned int rawlog_offset = iniFile.read_int(
"MappingApplication", "rawlog_offset", 0, /*Force existence:*/ true);
const string OUT_DIR_STD = iniFile.read_string(
"MappingApplication", "logOutput_dir", "log_out",
/*Force existence:*/ true);
const int LOG_FREQUENCY = iniFile.read_int(
"MappingApplication", "LOG_FREQUENCY", 5, /*Force existence:*/ true);
const bool SAVE_POSE_LOG = iniFile.read_bool(
"MappingApplication", "SAVE_POSE_LOG", false,
/*Force existence:*/ true);
const bool SAVE_3D_SCENE = iniFile.read_bool(
"MappingApplication", "SAVE_3D_SCENE", false,
/*Force existence:*/ true);
const bool CAMERA_3DSCENE_FOLLOWS_ROBOT = iniFile.read_bool(
"MappingApplication", "CAMERA_3DSCENE_FOLLOWS_ROBOT", true,
/*Force existence:*/ true);
bool SHOW_PROGRESS_3D_REAL_TIME = false;
int SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS = 0;
bool SHOW_LASER_SCANS_3D = true;
MRPT_LOAD_CONFIG_VAR(
SHOW_PROGRESS_3D_REAL_TIME, bool, iniFile, "MappingApplication");
MRPT_LOAD_CONFIG_VAR(
SHOW_LASER_SCANS_3D, bool, iniFile, "MappingApplication");
MRPT_LOAD_CONFIG_VAR(
SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS, int, iniFile,
"MappingApplication");
const char* OUT_DIR = OUT_DIR_STD.c_str();
// ------------------------------------
// Constructor of ICP-SLAM object
// ------------------------------------
CMetricMapBuilderICP mapBuilder;
mapBuilder.ICP_options.loadFromConfigFile(iniFile, "MappingApplication");
mapBuilder.ICP_params.loadFromConfigFile(iniFile, "ICP");
// Construct the maps with the loaded configuration.
mapBuilder.initialize();
// ---------------------------------
// CMetricMapBuilder::TOptions
// ---------------------------------
mapBuilder.setVerbosityLevel(LVL_DEBUG);
mapBuilder.options.alwaysInsertByClass.fromString(
iniFile.read_string("MappingApplication", "alwaysInsertByClass", ""));
// Print params:
printf("Running with the following parameters:\n");
printf(" RAWLOG file:'%s'\n", RAWLOG_FILE.c_str());
printf(" Output directory:\t\t\t'%s'\n", OUT_DIR);
printf(
" matchAgainstTheGrid:\t\t\t%c\n",
mapBuilder.ICP_options.matchAgainstTheGrid ? 'Y' : 'N');
printf(" Log record freq:\t\t\t%u\n", LOG_FREQUENCY);
printf(" SAVE_3D_SCENE:\t\t\t%c\n", SAVE_3D_SCENE ? 'Y' : 'N');
printf(" SAVE_POSE_LOG:\t\t\t%c\n", SAVE_POSE_LOG ? 'Y' : 'N');
printf(
" CAMERA_3DSCENE_FOLLOWS_ROBOT:\t%c\n",
CAMERA_3DSCENE_FOLLOWS_ROBOT ? 'Y' : 'N');
printf("\n");
mapBuilder.ICP_params.dumpToConsole();
mapBuilder.ICP_options.dumpToConsole();
// Checks:
ASSERT_(RAWLOG_FILE.size() > 0)
ASSERT_FILE_EXISTS_(RAWLOG_FILE)
CTicTac tictac, tictacGlobal, tictac_JH;
int step = 0;
string str;
CSimpleMap finalMap;
float t_exec;
COccupancyGridMap2D::TEntropyInfo entropy;
size_t rawlogEntry = 0;
CFileGZInputStream rawlogFile(RAWLOG_FILE.c_str());
// Prepare output directory:
// --------------------------------
deleteFilesInDirectory(OUT_DIR);
createDirectory(OUT_DIR);
// Open log files:
// ----------------------------------
CFileOutputStream f_log(format("%s/log_times.txt", OUT_DIR));
CFileOutputStream f_path(format("%s/log_estimated_path.txt", OUT_DIR));
CFileOutputStream f_pathOdo(format("%s/log_odometry_path.txt", OUT_DIR));
// Create 3D window if requested:
CDisplayWindow3D::Ptr win3D;
#if MRPT_HAS_WXWIDGETS
if (SHOW_PROGRESS_3D_REAL_TIME)
{
win3D = mrpt::make_aligned_shared<CDisplayWindow3D>(
"ICP-SLAM @ MRPT C++ Library", 600, 500);
win3D->setCameraZoom(20);
win3D->setCameraAzimuthDeg(-45);
}
#endif
// ----------------------------------------------------------
// Map Building
// ----------------------------------------------------------
CPose2D odoPose(0, 0, 0);
tictacGlobal.Tic();
for (;;)
{
CActionCollection::Ptr action;
CSensoryFrame::Ptr observations;
CObservation::Ptr observation;
if (os::kbhit())
{
char c = os::getch();
if (c == 27) break;
}
// Load action/observation pair from the rawlog:
// --------------------------------------------------
if (!CRawlog::getActionObservationPairOrObservation(
rawlogFile, action, observations, observation, rawlogEntry))
break; // file EOF
const bool isObsBasedRawlog = observation ? true : false;
std::vector<mrpt::obs::CObservation2DRangeScan::Ptr>
lst_current_laser_scans; // Just for drawing in 3D views
if (rawlogEntry >= rawlog_offset)
{
// Update odometry:
if (isObsBasedRawlog)
{
static CPose2D lastOdo;
static bool firstOdo = true;
if (IS_CLASS(observation, CObservationOdometry))
{
CObservationOdometry::Ptr o =
std::dynamic_pointer_cast<CObservationOdometry>(
observation);
if (!firstOdo) odoPose = odoPose + (o->odometry - lastOdo);
lastOdo = o->odometry;
firstOdo = false;
}
}
else
{
CActionRobotMovement2D::Ptr act =
action->getBestMovementEstimation();
if (act) odoPose = odoPose + act->poseChange->getMeanVal();
}
// Build list of scans:
if (SHOW_LASER_SCANS_3D)
{
// Rawlog in "Observation-only" format:
if (isObsBasedRawlog)
{
if (IS_CLASS(observation, CObservation2DRangeScan))
{
lst_current_laser_scans.push_back(
std::dynamic_pointer_cast<CObservation2DRangeScan>(
observation));
}
}
else
{
// Rawlog in the Actions-SF format:
for (size_t i = 0;; i++)
{
CObservation2DRangeScan::Ptr new_obs =
observations->getObservationByClass<
CObservation2DRangeScan>(i);
if (!new_obs)
break; // There're no more scans
else
lst_current_laser_scans.push_back(new_obs);
}
}
}
// Execute:
// ----------------------------------------
tictac.Tic();
if (isObsBasedRawlog)
mapBuilder.processObservation(observation);
else
mapBuilder.processActionObservation(*action, *observations);
t_exec = tictac.Tac();
printf("Map building executed in %.03fms\n", 1000.0f * t_exec);
// Info log:
// -----------
f_log.printf(
"%f %i\n", 1000.0f * t_exec,
mapBuilder.getCurrentlyBuiltMapSize());
const CMultiMetricMap* mostLikMap =
mapBuilder.getCurrentlyBuiltMetricMap();
if (0 == (step % LOG_FREQUENCY))
{
// Pose log:
// -------------
if (SAVE_POSE_LOG)
{
printf("Saving pose log information...");
mapBuilder.getCurrentPoseEstimation()->saveToTextFile(
format("%s/mapbuild_posepdf_%03u.txt", OUT_DIR, step));
printf("Ok\n");
}
}
// Save a 3D scene view of the mapping process:
if (0 == (step % LOG_FREQUENCY) || (SAVE_3D_SCENE || win3D))
{
CPose3D robotPose;
mapBuilder.getCurrentPoseEstimation()->getMean(robotPose);
COpenGLScene::Ptr scene =
mrpt::make_aligned_shared<COpenGLScene>();
COpenGLViewport::Ptr view = scene->getViewport("main");
ASSERT_(view);
COpenGLViewport::Ptr view_map =
scene->createViewport("mini-map");
view_map->setBorderSize(2);
view_map->setViewportPosition(0.01, 0.01, 0.35, 0.35);
view_map->setTransparent(false);
{
mrpt::opengl::CCamera& cam = view_map->getCamera();
cam.setAzimuthDegrees(-90);
cam.setElevationDegrees(90);
cam.setPointingAt(robotPose);
cam.setZoomDistance(20);
cam.setOrthogonal();
}
// The ground:
mrpt::opengl::CGridPlaneXY::Ptr groundPlane =
mrpt::make_aligned_shared<mrpt::opengl::CGridPlaneXY>(
-200, 200, -200, 200, 0, 5);
groundPlane->setColor(0.4, 0.4, 0.4);
view->insert(groundPlane);
view_map->insert(CRenderizable::Ptr(groundPlane)); // A copy
// The camera pointing to the current robot pose:
if (CAMERA_3DSCENE_FOLLOWS_ROBOT)
{
scene->enableFollowCamera(true);
mrpt::opengl::CCamera& cam = view_map->getCamera();
cam.setAzimuthDegrees(-45);
cam.setElevationDegrees(45);
cam.setPointingAt(robotPose);
}
// The maps:
{
opengl::CSetOfObjects::Ptr obj =
mrpt::make_aligned_shared<opengl::CSetOfObjects>();
mostLikMap->getAs3DObject(obj);
view->insert(obj);
// Only the point map:
opengl::CSetOfObjects::Ptr ptsMap =
mrpt::make_aligned_shared<opengl::CSetOfObjects>();
if (mostLikMap->m_pointsMaps.size())
{
mostLikMap->m_pointsMaps[0]->getAs3DObject(ptsMap);
view_map->insert(ptsMap);
}
}
// Draw the robot path:
CPose3DPDF::Ptr posePDF = mapBuilder.getCurrentPoseEstimation();
CPose3D curRobotPose;
posePDF->getMean(curRobotPose);
{
opengl::CSetOfObjects::Ptr obj =
opengl::stock_objects::RobotPioneer();
obj->setPose(curRobotPose);
view->insert(obj);
}
{
opengl::CSetOfObjects::Ptr obj =
opengl::stock_objects::RobotPioneer();
obj->setPose(curRobotPose);
view_map->insert(obj);
}
// Draw laser scanners in 3D:
if (SHOW_LASER_SCANS_3D)
{
for (size_t i = 0; i < lst_current_laser_scans.size(); i++)
{
// Create opengl object and load scan data from the scan
// observation:
opengl::CPlanarLaserScan::Ptr obj =
mrpt::make_aligned_shared<
opengl::CPlanarLaserScan>();
obj->setScan(*lst_current_laser_scans[i]);
obj->setPose(curRobotPose);
obj->setSurfaceColor(1.0f, 0.0f, 0.0f, 0.5f);
// inser into the scene:
view->insert(obj);
}
}
// Save as file:
if (0 == (step % LOG_FREQUENCY) && SAVE_3D_SCENE)
{
CFileGZOutputStream f(
format("%s/buildingmap_%05u.3Dscene", OUT_DIR, step));
f << *scene;
}
// Show 3D?
if (win3D)
{
opengl::COpenGLScene::Ptr& ptrScene =
win3D->get3DSceneAndLock();
ptrScene = scene;
win3D->unlockAccess3DScene();
// Move camera:
win3D->setCameraPointingToPoint(
curRobotPose.x(), curRobotPose.y(), curRobotPose.z());
// Update:
win3D->forceRepaint();
std::this_thread::sleep_for(
std::chrono::milliseconds(
SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS));
}
}
// Save the memory usage:
// ------------------------------------------------------------------
{
printf("Saving memory usage...");
unsigned long memUsage = getMemoryUsage();
FILE* f = os::fopen(
format("%s/log_MemoryUsage.txt", OUT_DIR).c_str(), "at");
if (f)
{
os::fprintf(f, "%u\t%lu\n", step, memUsage);
os::fclose(f);
}
printf("Ok! (%.04fMb)\n", ((float)memUsage) / (1024 * 1024));
}
// Save the robot estimated pose for each step:
f_path.printf(
"%i %f %f %f\n", step,
mapBuilder.getCurrentPoseEstimation()->getMeanVal().x(),
mapBuilder.getCurrentPoseEstimation()->getMeanVal().y(),
mapBuilder.getCurrentPoseEstimation()->getMeanVal().yaw());
f_pathOdo.printf(
"%i %f %f %f\n", step, odoPose.x(), odoPose.y(), odoPose.phi());
} // end of if "rawlog_offset"...
step++;
printf(
"\n---------------- STEP %u | RAWLOG ENTRY %u ----------------\n",
step, (unsigned)rawlogEntry);
};
printf(
"\n---------------- END!! (total time: %.03f sec) ----------------\n",
tictacGlobal.Tac());
// Save map:
mapBuilder.getCurrentlyBuiltMap(finalMap);
str = format("%s/_finalmap_.simplemap", OUT_DIR);
printf("Dumping final map in binary format to: %s\n", str.c_str());
mapBuilder.saveCurrentMapToFile(str);
const CMultiMetricMap* finalPointsMap =
mapBuilder.getCurrentlyBuiltMetricMap();
str = format("%s/_finalmaps_.txt", OUT_DIR);
printf("Dumping final metric maps to %s_XXX\n", str.c_str());
finalPointsMap->saveMetricMapRepresentationToFile(str);
if (win3D) win3D->waitForKey();
MRPT_END
}
