/*--------------------------------------------------------------- auxiliarComputeObservationLikelihood ---------------------------------------------------------------*/ double CLSLAM_RBPF_2DLASER::auxiliarComputeObservationLikelihood( const bayes::CParticleFilter::TParticleFilterOptions &PF_options, const CParticleFilterCapable *obj, size_t particleIndexForMap, const CSensoryFrame *observation, const CPose2D *x ) { const CLocalMetricHypothesis *theObj = static_cast<const CLocalMetricHypothesis*>(obj); CMultiMetricMap *map = &theObj->m_particles[particleIndexForMap].d->metricMaps; return map->computeObservationsLikelihood( *observation, *x ); }
CSimplePointsMap::Ptr CMultiMetricMap_getAsSimplePointsMap( CMultiMetricMap& self) { CSimplePointsMap::Ptr points_map(new CSimplePointsMap); CSimplePointsMap* points_map_ptr = self.getAsSimplePointsMap(); *points_map = *points_map_ptr; return points_map; }
// CMultiMetricMap mrpt::opengl::CSetOfObjects::Ptr CMultiMetricMap_getAs3DObject( CMultiMetricMap& self) { mrpt::opengl::CSetOfObjects::Ptr outObj = mrpt::make_aligned_shared<mrpt::opengl::CSetOfObjects>(); self.getAs3DObject(outObj); return outObj; }
// ------------------------------------------------------ // BenchmarkGridmaps // ------------------------------------------------------ void BenchmarkGridmaps() { randomGenerator.randomize(333); CMultiMetricMap metricMap; TSetOfMetricMapInitializers mapInit; // Create gridmap: mapInit.loadFromConfigFile( CConfigFile(iniFile), "METRIC_MAPS" ); metricMap.setListOfMaps(&mapInit); // prepare the laser scan: 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) ); #if 1 CRawlog rawlog; rawlog.loadFromRawLogFile("/Trabajo/Code/MRPT/share/mrpt/datasets/2006-01ENE-21-SENA_Telecom Faculty_one_loop_only.rawlog"); scan1 = *rawlog.getAsObservations(400)->getObservationByClass<CObservation2DRangeScan>(); #endif ASSERT_( metricMap.m_gridMaps.size() ); COccupancyGridMap2DPtr gridMap = metricMap.m_gridMaps[0]; COccupancyGridMap2D gridMapCopy( *gridMap ); int i, N; CTicTac tictac; // test 1: getcell // ---------------------------------------- if (1) { N = 10000000; cout << "Running test #1: getCell... "; cout.flush(); //COccupancyGridMap2D::cellType cell; float p=0; tictac.Tic(); for (i=0;i<N;i++) { p += gridMap->getCell( 0, 0 ); } double T = tictac.Tac(); cout << "-> " << 1e9*T/N << " ns/iter. p=" << p << endl; // the "p" is to avoid optimizing out the entire loop! } // test 2: setcell // ---------------------------------------- if (1) { N = 10000000; cout << "Running test #2: setCell... "; cout.flush(); float p=0.8f; tictac.Tic(); for (i=0;i<N;i++) { gridMap->setCell( 0, 0, p ); } double T = tictac.Tac(); cout << "-> " << 1e9*T/N << " ns/iter." << endl; // the "p" is to avoid optimizing out the entire loop! } // test 3: updateCell // ---------------------------------------- if (1) { N = 1000000; cout << "Running test #3: updateCell... "; cout.flush(); float p=0.57f; tictac.Tic(); for (i=0;i<N;i++) { gridMap->updateCell( 0, 0, p ); } double T = tictac.Tac(); cout << "-> " << 1e9*T/N << " ns/iter." << endl; // the "p" is to avoid optimizing out the entire loop! } // test 4: updateCell_fast // ---------------------------------------- if (1) { N = 10000000; cout << "Running test #4: updateCell_fast... "; cout.flush(); float p=0.57f; COccupancyGridMap2D::cellType logodd_obs = COccupancyGridMap2D::p2l( p ); //float p_1 = 1-p; COccupancyGridMap2D::cellType *theMapArray = gridMap->getRow(0); unsigned theMapSize_x = gridMap->getSizeX(); COccupancyGridMap2D::cellType logodd_thres_occupied = COccupancyGridMap2D::OCCGRID_CELLTYPE_MIN+logodd_obs; tictac.Tic(); for (i=0;i<N;i++) { COccupancyGridMap2D::updateCell_fast_occupied( 2, 2, logodd_obs,logodd_thres_occupied, theMapArray, theMapSize_x); } double T = tictac.Tac(); cout << "-> " << 1e9*T/N << " ns/iter." << endl; // the "p" is to avoid optimizing out the entire loop! } #if 0 for (i=50;i<51;i++) { CPose3D pose3D(0.21,0.34,0,-2); //scan1.validRange.assign(scan1.validRange.size(), false); //scan1.validRange[i]=true; gridMap->clear(); gridMap->resizeGrid(-5,20,-15,15); gridMap->insertObservation( &scan1, &pose3D ); gridMap->saveAsBitmapFile(format("./gridmap_with_widening_%04i.png",i)); } #endif // test 5: Laser insertion // ---------------------------------------- if (1) { gridMap->insertionOptions.wideningBeamsWithDistance = false; N = 3000; cout << "Running test #5: Laser insert. w/o widen... "; cout.flush(); tictac.Tic(); for (i=0;i<N;i++) { #if 1 CPose2D pose( randomGenerator.drawUniform(-1.0,1.0), randomGenerator.drawUniform(-1.0,1.0), randomGenerator.drawUniform(-M_PI,M_PI) ); CPose3D pose3D(pose); #else CPose3D pose3D; #endif gridMap->insertObservation( &scan1, &pose3D ); } double T = tictac.Tac(); cout << "-> " << 1000*T/N << " ms/iter, scans/sec:" << N/T << endl; CPose3D pose3D; gridMap->clear(); gridMap->insertObservation( &scan1, &pose3D ); gridMap->saveAsBitmapFile("./gridmap_without_widening.png"); } // test 6: Laser insertion without widening // -------------------------------------------------- if (1) { gridMap->insertionOptions.wideningBeamsWithDistance = true; N = 3000; cout << "Running test #6: Laser insert. widen... "; cout.flush(); tictac.Tic(); for (i=0;i<N;i++) { #if 1 CPose2D pose( randomGenerator.drawUniform(-1.0,1.0), randomGenerator.drawUniform(-1.0,1.0), randomGenerator.drawUniform(-M_PI,M_PI) ); CPose3D pose3D(pose); #else CPose3D pose3D; #endif gridMap->insertObservation( &scan1, &pose3D ); } double T = tictac.Tac(); cout << "-> " << 1000*T/N << " ms/iter, scans/sec:" << N/T << endl; CPose3D pose3D; gridMap->clear(); gridMap->insertObservation( &scan1, &pose3D ); gridMap->saveAsBitmapFile("./gridmap_with_widening.png"); } // test 7: Grid resize // ---------------------------------------- if (1) { N = 400; cout << "Running test #7: Grid resize... "; cout.flush(); tictac.Tic(); for (i=0;i<N;i++) { *gridMap = gridMapCopy; gridMap->resizeGrid(-30,30,-40,40); } double T = tictac.Tac(); cout << "-> " << 1000*T/N << " ms/iter" << endl; } // test 8: Likelihood computation // ---------------------------------------- if (1) { N = 5000; *gridMap = gridMapCopy; CPose3D pose3D(0,0,0); gridMap->insertObservation( &scan1, &pose3D ); cout << "Running test #8: Likelihood... "; cout.flush(); double R = 0; tictac.Tic(); for (i=0;i<N;i++) { CPose2D pose( randomGenerator.drawUniform(-1.0,1.0), randomGenerator.drawUniform(-1.0,1.0), randomGenerator.drawUniform(-M_PI,M_PI) ); R+=gridMap->computeObservationLikelihood(&scan1,pose); } double T = tictac.Tac(); cout << "-> " << 1000*T/N << " ms/iter" << endl; } }
void CMultiMetricMap_setListOfMaps( CMultiMetricMap& self, TSetOfMetricMapInitializers& initializers) { self.setListOfMaps(initializers); }
// ------------------------------------------------------ // 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 }
// ------------------------------------------------------ // MAIN // ------------------------------------------------------ int main(int argc, char **argv) { try { printf(" observations2map - Part of the MRPT\n"); printf(" MRPT C++ Library: %s - BUILD DATE %s\n", MRPT_getVersion().c_str(), MRPT_getCompilationDate().c_str()); printf("-------------------------------------------------------------------\n"); // Process arguments: if ((argc!=4 && argc!=6) || (argc==6 && 0!=mrpt::system::os::_strcmp(argv[4],"-s") ) ) { cout << "Use: observations2map <config_file.ini> <observations.simplemap> <outputmap_prefix> [-s INI_FILE_SECTION_NAME] " << endl; cout << " Default: INI_FILE_SECTION_NAME = MappingApplication" << endl; cout << "Push any key to exit..." << endl; os::getch(); return -1; } string configFile = std::string( argv[1] ); string inputFile = std::string( argv[2] ); string outprefix = std::string( argv[3] ); if (argc>4) { METRIC_MAP_CONFIG_SECTION = string(argv[5]); } // Load simplemap: cout << "Loading simplemap..."; mrpt::slam::CSimpleMap simplemap; CFileGZInputStream f( inputFile.c_str() ); f >> simplemap; cout <<"done: " << simplemap.size() << " observations." << endl; // Create metric maps: TSetOfMetricMapInitializers mapCfg; mapCfg.loadFromConfigFile( CConfigFile( configFile ), METRIC_MAP_CONFIG_SECTION ); CMultiMetricMap metricMap; metricMap.setListOfMaps( &mapCfg ); // Build metric maps: cout << "Building metric maps..."; metricMap.loadFromProbabilisticPosesAndObservations( simplemap ); cout << "done." << endl; // Save metric maps: // --------------------------- metricMap.saveMetricMapRepresentationToFile( outprefix ); // grid maps: size_t i; for (i=0;i<metricMap.m_gridMaps.size();i++) { string str = format( "%s_gridmap_no%02u.gridmap", outprefix.c_str(), (unsigned)i ); cout << "Saving gridmap #" << i << " to " << str << endl; CFileGZOutputStream f(str); f << *metricMap.m_gridMaps[i]; cout << "done." << endl; } return 0; } catch (std::exception &e) { std::cerr << e.what() << std::endl << "Program finished for an exception!!" << std::endl; mrpt::system::pause(); return -1; } catch (...) { std::cerr << "Untyped exception!!" << std::endl; mrpt::system::pause(); return -1; } }
// ------------------------------------------------------ // 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; MRPT_LOAD_CONFIG_VAR( SHOW_PROGRESS_3D_REAL_TIME, 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.options.verbose = true; 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_(fileExists(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: CDisplayWindow3DPtr win3D; #if MRPT_HAS_WXWIDGETS if (SHOW_PROGRESS_3D_REAL_TIME) { win3D = CDisplayWindow3D::Create("ICP-SLAM @ MRPT C++ Library", 600, 500); win3D->setCameraZoom(20); win3D->setCameraAzimuthDeg(-45); } #endif // ---------------------------------------------------------- // Map Building // ---------------------------------------------------------- CPose2D odoPose(0,0,0); tictacGlobal.Tic(); for (;;) { CActionCollectionPtr action; CSensoryFramePtr observations; CObservationPtr 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.present(); if (rawlogEntry>=rawlog_offset) { // Update odometry: if (isObsBasedRawlog) { static CPose2D lastOdo; static bool firstOdo = true; if (IS_CLASS(observation,CObservationOdometry)) { CObservationOdometryPtr o = CObservationOdometryPtr(observation); if (!firstOdo) odoPose = odoPose + (o->odometry - lastOdo); lastOdo=o->odometry; firstOdo=false; } } else { CActionRobotMovement2DPtr act= action->getBestMovementEstimation(); if (act) odoPose = odoPose + act->poseChange->getMeanVal(); } // 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.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 ); } // 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); }; 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); 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 }