void CPoseRandomSampler::setPosePDF( const CPosePDFPtr &pdf ) {
setPosePDF(pdf.pointer());
}
// ------------------------------------------------------
// MapBuilding_ICP
// ------------------------------------------------------
void MapBuilding_ICP()
{
MRPT_TRY_START
CTicTac tictac,tictacGlobal,tictac_JH;
int step = 0;
std::string str;
CSensFrameProbSequence finalMap;
float t_exec;
COccupancyGridMap2D::TEntropyInfo entropy;
size_t rawlogEntry = 0;
CFileGZInputStream rawlogFile( RAWLOG_FILE.c_str() );
CFileGZOutputStream sensor_data;
// ---------------------------------
// Constructor
// ---------------------------------
CMetricMapBuilderICP mapBuilder(
&metricMapsOpts,
insertionLinDistance,
insertionAngDistance,
&icpOptions
);
mapBuilder.ICP_options.matchAgainstTheGrid = matchAgainstTheGrid;
// ---------------------------------
// CMetricMapBuilder::TOptions
// ---------------------------------
mapBuilder.options.verbose = true;
mapBuilder.options.enableMapUpdating = true;
mapBuilder.options.debugForceInsertion = false;
mapBuilder.options.insertImagesAlways = false;
// 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:
CDisplayWindow3DPtr win3D;
#if MRPT_HAS_WXWIDGETS
if (SHOW_PROGRESS_3D_REAL_TIME)
{
win3D = CDisplayWindow3DPtr( new CDisplayWindow3D("ICP-SLAM @ MRPT C++ Library (C) 2004-2008", 600, 500) );
win3D->setCameraZoom(20);
win3D->setCameraAzimuthDeg(-45);
}
#endif
if(OBS_FROM_FILE == 0){
sensor_data.open("sensor_data.rawlog");
printf("Receive From Sensor\n");
initLaser();
printf("OK\n");
}
// ----------------------------------------------------------
// Map Building
// ----------------------------------------------------------
CActionCollectionPtr action;
CSensoryFramePtr observations, temp_obs;
CSensoryFramePtr obs_set;
CPose2D odoPose(0,0,0);
CSimplePointsMap oldMap, newMap;
CICP ICP;
vector_float accum_x, accum_y, accum_z;
// ICP Setting
ICP.options.ICP_algorithm = (TICPAlgorithm)ICP_method;
ICP.options.maxIterations = 40;
ICP.options.thresholdAng = 0.15;
ICP.options.thresholdDist = 0.75f;
ICP.options.ALFA = 0.30f;
ICP.options.smallestThresholdDist = 0.10f;
ICP.options.doRANSAC = false;
ICP.options.dumpToConsole();
//
CObservationPtr obsSick = CObservationPtr(new CObservation2DRangeScan());
CObservationPtr obsHokuyo = CObservationPtr(new CObservation2DRangeScan());
CSimplePointsMap hokuyoMap;
bool isFirstTime = true;
bool loop = true;
/*
cout << index << "frame, Input the any key to continue" << endl;
getc(stdin);
fflush(stdin);
*/
tictacGlobal.Tic();
while(loop)
{
/*
if(BREAK){
cout << index << "frame, Input the any key to continue" << endl;
getc(stdin);
fflush(stdin);
}else{
if(os::kbhit())
loop = true;
}
*/
if(os::kbhit())
loop = true;
if(DELAY) {
sleep(15);
}
// Load action/observation pair from the rawlog:
// --------------------------------------------------
if(OBS_FROM_FILE == 1) {
if (! CRawlog::readActionObservationPair( rawlogFile, action, temp_obs, rawlogEntry) )
break; // file EOF
obsSick = temp_obs->getObservationByIndex(0);
obsHokuyo = temp_obs->getObservationByIndex(1);
observations = CSensoryFramePtr(new CSensoryFrame());
observations->insert((CObservationPtr)obsSick);
hokuyoMap.clear();
hokuyoMap.insertObservation(obsHokuyo.pointer());
}else{
rawlogEntry = rawlogEntry+2;
tictac.Tic();
obsSick = CObservationPtr(new CObservation2DRangeScan());
obsHokuyo = CObservationPtr(new CObservation2DRangeScan());
if(!receiveDataFromSensor((CObservation2DRangeScan*)obsSick.pointer(),SICK)){
cout << " Error in receive sensor data" << endl;
return;
}
if(!receiveDataFromSensor((CObservation2DRangeScan*)obsHokuyo.pointer(),HOKUYO)){
cout << " Error in receive sensor data" << endl;
return;
}
cout << "Time to receive data : " << tictac.Tac()*1000.0f << endl;
obsSick->timestamp = mrpt::system::now();
obsSick->setSensorPose(CPose3D(0,0,0,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0)));
((CObservation2DRangeScan*)obsSick.pointer())->rightToLeft = true;
((CObservation2DRangeScan*)obsSick.pointer())->stdError = 0.003f;
obsHokuyo->timestamp = mrpt::system::now();
obsHokuyo->setSensorPose(CPose3D(0,0,0.4,DEG2RAD(0),DEG2RAD(-90),DEG2RAD(0)));
((CObservation2DRangeScan*)obsHokuyo.pointer())->rightToLeft = true;
((CObservation2DRangeScan*)obsHokuyo.pointer())->stdError = 0.003f;
cout << "rawlogEntry : " << rawlogEntry << endl;
CActionRobotMovement2D myAction;
newMap.clear();
obsSick.pointer()->insertObservationInto(&newMap);
if(!isFirstTime){
static float runningTime;
static CICP::TReturnInfo info;
static CPose2D initial(0,0,0);
CPosePDFPtr ICPPdf = ICP.AlignPDF(&oldMap, &newMap, initial, &runningTime, (void*)&info);
CPose2D estMean;
CMatrixDouble33 estCov;
ICPPdf->getCovarianceAndMean(estCov, estMean);
printf("ICP run in %.02fms, %d iterations (%.02fms/iter), %.01f%% goodness\n -> ", runningTime*1000, info.nIterations, runningTime*1000.0f/info.nIterations, info.goodness*100 );
cout << "ICP Odometry : " << ICPPdf->getMeanVal() << endl;
myAction.estimationMethod = CActionRobotMovement2D::emScan2DMatching;
myAction.poseChange = CPosePDFPtr( new CPosePDFGaussian(estMean, estCov));
}else{
isFirstTime = false;
}
oldMap.clear();
oldMap.copyFrom(newMap);
observations = CSensoryFramePtr(new CSensoryFrame());
action = CActionCollectionPtr(new CActionCollection());
observations->insert((CObservationPtr)obsSick);
obs_set = CSensoryFramePtr(new CSensoryFrame());
obs_set->insert((CObservationPtr)obsSick);
obs_set->insert((CObservationPtr)obsHokuyo);
action->insert(myAction);
sensor_data << action << obs_set;
hokuyoMap.clear();
hokuyoMap.insertObservation(obsHokuyo.pointer());
}
if (rawlogEntry>=rawlog_offset)
{
// Update odometry:
{
CActionRobotMovement2DPtr act= action->getBestMovementEstimation();
if (act)
odoPose = odoPose + act->poseChange->getMeanVal();
}
// Execute:
// ----------------------------------------
tictac.Tic();
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.present()))
{
CPose3D robotPose;
mapBuilder.getCurrentPoseEstimation()->getMean(robotPose);
COpenGLScenePtr scene = COpenGLScene::Create();
COpenGLViewportPtr view=scene->getViewport("main");
ASSERT_(view);
COpenGLViewportPtr 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::CGridPlaneXYPtr 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( CRenderizablePtr( 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::CSetOfObjectsPtr obj = opengl::CSetOfObjects::Create();
mostLikMap->getAs3DObject( obj );
view->insert(obj);
// Only the point map:
opengl::CSetOfObjectsPtr ptsMap = opengl::CSetOfObjects::Create();
if (mostLikMap->m_pointsMaps.size())
{
mostLikMap->m_pointsMaps[0]->getAs3DObject(ptsMap);
view_map->insert( ptsMap );
}
}
// Draw the robot path:
CPose3DPDFPtr posePDF = mapBuilder.getCurrentPoseEstimation();
CPose3D curRobotPose;
posePDF->getMean(curRobotPose);
{
opengl::CSetOfObjectsPtr obj = opengl::stock_objects::RobotPioneer();
obj->setPose( curRobotPose );
view->insert(obj);
}
{
opengl::CSetOfObjectsPtr obj = opengl::stock_objects::RobotPioneer();
obj->setPose( curRobotPose );
view_map->insert( obj );
}
// Draw Hokuyo total data
{
CRenderizablePtr hokuyoRender_t = scene->getByName("hokuyo_total");
if(!hokuyoRender_t){
hokuyoRender_t = CPointCloud::Create();
hokuyoRender_t->setName("hokuyo_total");
hokuyoRender_t->setColor(0,0,1);
hokuyoRender_t->setPose( CPose3D(0,0,0) );
getAs<CPointCloud>(hokuyoRender_t)->setPointSize(3);
scene->insert( hokuyoRender_t);
}
for(size_t i =0 ; i < accum_x.size(); i++){
getAs<CPointCloud>(hokuyoRender_t)->insertPoint(accum_x[i], accum_y[i], accum_z[i]);
}
cout << "accum_x size : " << accum_x.size() << endl;
}
// Draw Hokuyo Current data plate
{
CRenderizablePtr hokuyoRender = scene->getByName("hokuyo_cur");
hokuyoRender = CPointCloud::Create();
hokuyoRender->setName("hokuyo_cur");
hokuyoRender->setColor(0,1,0);
hokuyoRender->setPose( curRobotPose );
getAs<CPointCloud>(hokuyoRender)->setPointSize(0.1);
getAs<CPointCloud>(hokuyoRender)->loadFromPointsMap(&hokuyoMap);
scene->insert( hokuyoRender);
vector_float cur_x = getAs<CPointCloud>(hokuyoRender)->getArrayX();
vector_float cur_y = getAs<CPointCloud>(hokuyoRender)->getArrayY();
vector_float cur_z = getAs<CPointCloud>(hokuyoRender)->getArrayZ();
// cout << "current pose : " << curRobotPose << endl;
for(size_t i =0 ; i < cur_x.size(); i++){
/*
float rotate_x = cur_x[i]+ curRobotPose.y()*sin(curRobotPose.yaw()*3.14/180.0);
float rotate_y = cur_y[i]+ curRobotPose.y()*cos(curRobotPose.yaw()*3.14/180.0);
*/
float rotate_x = curRobotPose.x() + cur_y[i]*sin(-curRobotPose.yaw());
float rotate_y = curRobotPose.y() + cur_y[i]*cos(-curRobotPose.yaw());
// printf("cur_x, cur_y, cur_z : %f %f %f \n", rotate_x,rotate_y, cur_z[i]);
accum_x.push_back(rotate_x);
accum_y.push_back(rotate_y);
accum_z.push_back(cur_z[i]);
}
}
// 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::COpenGLScenePtr &ptrScene = win3D->get3DSceneAndLock();
ptrScene = scene;
win3D->unlockAccess3DScene();
// Move camera:
win3D->setCameraPointingToPoint( curRobotPose.x(),curRobotPose.y(),curRobotPose.z() );
// Update:
win3D->forceRepaint();
sleep( 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);
// Free memory:
action.clear_unique();
observations.clear_unique();
};
printf("\n---------------- END!! (total time: %.03f sec) ----------------\n",tictacGlobal.Tac());
// hokuyo.turnOff();
sick.stop();
// 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);
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_TRY_END
}
// ------------------------------------------------------
// TestICP
// ------------------------------------------------------
void TestICP()
{
CSimplePointsMap m1,m2;
float runningTime;
CICP::TReturnInfo info;
CICP ICP;
// Load scans:
CObservation2DRangeScan scan1;
scan1.aperture = M_PIf;
scan1.rightToLeft = true;
scan1.validRange.resize( SCANS_SIZE );
scan1.scan.resize(SCANS_SIZE);
ASSERT_( sizeof(SCAN_RANGES_1) == sizeof(float)*SCANS_SIZE );
memcpy( &scan1.scan[0], SCAN_RANGES_1, sizeof(SCAN_RANGES_1) );
memcpy( &scan1.validRange[0], SCAN_VALID_1, sizeof(SCAN_VALID_1) );
CObservation2DRangeScan scan2 = scan1;
memcpy( &scan2.scan[0], SCAN_RANGES_2, sizeof(SCAN_RANGES_2) );
memcpy( &scan2.validRange[0], SCAN_VALID_2, sizeof(SCAN_VALID_2) );
// Build the points maps from the scans:
m1.insertObservation( &scan1 );
m2.insertObservation( &scan2 );
#if MRPT_HAS_PCL
cout << "Saving map1.pcd and map2.pcd in PCL format...\n";
m1.savePCDFile("map1.pcd", false);
m2.savePCDFile("map2.pcd", false);
#endif
// -----------------------------------------------------
// ICP.options.ICP_algorithm = icpLevenbergMarquardt;
// ICP.options.ICP_algorithm = icpClassic;
ICP.options.ICP_algorithm = (TICPAlgorithm)ICP_method;
ICP.options.maxIterations = 100;
ICP.options.thresholdAng = DEG2RAD(10.0f);
ICP.options.thresholdDist = 0.75f;
ICP.options.ALFA = 0.5f;
ICP.options.smallestThresholdDist = 0.05f;
ICP.options.doRANSAC = false;
ICP.options.dumpToConsole();
// -----------------------------------------------------
CPose2D initialPose(0.8f,0.0f,(float)DEG2RAD(0.0f));
CPosePDFPtr pdf = ICP.Align(
&m1,
&m2,
initialPose,
&runningTime,
(void*)&info);
printf("ICP run in %.02fms, %d iterations (%.02fms/iter), %.01f%% goodness\n -> ",
runningTime*1000,
info.nIterations,
runningTime*1000.0f/info.nIterations,
info.goodness*100 );
cout << "Mean of estimation: " << pdf->getMeanVal() << endl<< endl;
CPosePDFGaussian gPdf;
gPdf.copyFrom(*pdf);
cout << "Covariance of estimation: " << endl << gPdf.cov << endl;
cout << " std(x): " << sqrt( gPdf.cov(0,0) ) << endl;
cout << " std(y): " << sqrt( gPdf.cov(1,1) ) << endl;
cout << " std(phi): " << RAD2DEG(sqrt( gPdf.cov(2,2) )) << " (deg)" << endl;
//cout << "Covariance of estimation (MATLAB format): " << endl << gPdf.cov.inMatlabFormat() << endl;
cout << "-> Saving reference map as scan1.txt" << endl;
m1.save2D_to_text_file("scan1.txt");
cout << "-> Saving map to align as scan2.txt" << endl;
m2.save2D_to_text_file("scan2.txt");
cout << "-> Saving transformed map to align as scan2_trans.txt" << endl;
CSimplePointsMap m2_trans = m2;
m2_trans.changeCoordinatesReference( gPdf.mean );
m2_trans.save2D_to_text_file("scan2_trans.txt");
cout << "-> Saving MATLAB script for drawing 2D ellipsoid as view_ellip.m" << endl;
CMatrixFloat COV22 = CMatrixFloat( CMatrixDouble( gPdf.cov ));
COV22.setSize(2,2);
CVectorFloat MEAN2D(2);
MEAN2D[0] = gPdf.mean.x();
MEAN2D[1] = gPdf.mean.y();
{
ofstream f("view_ellip.m");
f << math::MATLAB_plotCovariance2D( COV22, MEAN2D, 3.0f);
}
// If we have 2D windows, use'em:
#if MRPT_HAS_WXWIDGETS
if (!skip_window)
{
gui::CDisplayWindowPlots win("ICP results");
// Reference map:
vector<float> map1_xs, map1_ys, map1_zs;
m1.getAllPoints(map1_xs,map1_ys,map1_zs);
win.plot( map1_xs, map1_ys, "b.3", "map1");
// Translated map:
vector<float> map2_xs, map2_ys, map2_zs;
m2_trans.getAllPoints(map2_xs,map2_ys,map2_zs);
win.plot( map2_xs, map2_ys, "r.3", "map2");
// Uncertainty
win.plotEllipse(MEAN2D[0],MEAN2D[1],COV22,3.0,"b2", "cov");
win.axis(-1,10,-6,6);
win.axis_equal();
cout << "Close the window to exit" << endl;
win.waitForKey();
}
#endif
}
