void xRawLogViewerFrame::OnMenuConvertObservationOnly(wxCommandEvent& event) { WX_START_TRY wxMessageBox(_("Select the rawlog file to convert...")); string str; if (!AskForOpenRawlog(str)) return; wxMessageBox(_("Select the target file where to save the new rawlog.")); string filToSave; if (!AskForSaveRawlog(filToSave)) return; wxBusyCursor waitCursor; CFileGZInputStream fil(str); unsigned int filSize = (unsigned int)fil.getTotalBytesCount(); CFileGZOutputStream f_out(filToSave); wxString auxStr; wxProgressDialog progDia( wxT("Progress"), wxT("Parsing file..."), filSize, // range this, // parent wxPD_CAN_ABORT | wxPD_APP_MODAL | wxPD_SMOOTH | wxPD_AUTO_HIDE | wxPD_ELAPSED_TIME | wxPD_ESTIMATED_TIME | wxPD_REMAINING_TIME); wxTheApp->Yield(); // Let the app. process messages unsigned int countLoop = 0; bool keepLoading = true; string errorMsg; CPose2D odometry_accum; // We'll save here all the individual observations ordered in time: TListTimeAndObservations time_ordered_list_observation; mrpt::system::TTimeStamp lastValidObsTime = INVALID_TIMESTAMP; while (keepLoading) { if (countLoop++ % 5 == 0) { auxStr.sprintf(wxT("Parsing file... %u objects"), countLoop); if (!progDia.Update((int)fil.getPosition(), auxStr)) keepLoading = false; wxTheApp->Yield(); // Let the app. process messages } try { CSerializable::Ptr newObj; fil >> newObj; // Check type: if (newObj->GetRuntimeClass() == CLASS_ID(CSensoryFrame)) { CSensoryFrame::Ptr SF( std::dynamic_pointer_cast<CSensoryFrame>(newObj)); for (CSensoryFrame::iterator it = SF->begin(); it != SF->end(); ++it) { time_ordered_list_observation.insert( TTimeObservationPair((*it)->timestamp, (*it))); lastValidObsTime = (*it)->timestamp; } } else if (newObj->GetRuntimeClass() == CLASS_ID(CActionCollection)) { // Replace "odometry action" with "odometry observation": CActionCollection::Ptr acts = std::dynamic_pointer_cast<CActionCollection>(newObj); // Get odometry: CActionRobotMovement2D::Ptr actOdom = acts->getBestMovementEstimation(); if (actOdom) { odometry_accum = odometry_accum + actOdom->poseChange->getMeanVal(); // Generate "odometry obs": CObservationOdometry::Ptr newO = mrpt::make_aligned_shared<CObservationOdometry>(); newO->sensorLabel = "odometry"; newO->timestamp = actOdom->timestamp != INVALID_TIMESTAMP ? actOdom->timestamp : lastValidObsTime; newO->odometry = odometry_accum; time_ordered_list_observation.insert( TTimeObservationPair(newO->timestamp, newO)); } } else if ( newObj->GetRuntimeClass()->derivedFrom(CLASS_ID(CObservation))) { CObservation::Ptr o = std::dynamic_pointer_cast<CObservation>(newObj); time_ordered_list_observation.insert( TTimeObservationPair(o->timestamp, o)); } // Dump to the new file: Only the oldest one: // -------------------------------------------------- if (time_ordered_list_observation.size() > 30) { // Save a few of the oldest and continue: for (unsigned i = 0; i < 15; i++) { f_out << *(time_ordered_list_observation.begin()->second); time_ordered_list_observation.erase( time_ordered_list_observation.begin()); } } // Free memory: newObj.reset(); } catch (exception& e) { errorMsg = e.what(); keepLoading = false; } catch (...) { keepLoading = false; } } // end while keep loading // Save the rest to the out file: while (!time_ordered_list_observation.empty()) { f_out << *(time_ordered_list_observation.begin()->second); time_ordered_list_observation.erase( time_ordered_list_observation.begin()); } progDia.Update(filSize); // Set error msg: wxMessageBox(_("Done")); WX_END_TRY }
void xRawLogViewerFrame::OnMenuLossLessDecimate(wxCommandEvent& event) { WX_START_TRY CRawlog newRawLog; newRawLog.setCommentText(rawlog.getCommentText()); wxString strDecimation = wxGetTextFromUser( _("The number of observations will be decimated (only 1 out of M will " "be kept). Enter the decimation ratio M:"), _("Decimation"), _("1")); long DECIMATE_RATIO; strDecimation.ToLong(&DECIMATE_RATIO); ASSERT_(DECIMATE_RATIO >= 1) wxBusyCursor busyCursor; wxTheApp->Yield(); // Let the app. process messages size_t i, N = rawlog.size(); // ------------------------------------------------------------------------------ // METHOD TO BE MEMORY EFFICIENT: // To free the memory of the current rawlog entries as we create the new // one, // then call "clearWithoutDelete" at the end. // ------------------------------------------------------------------------------ CSensoryFrame::Ptr accum_sf; CActionRobotMovement2D::TMotionModelOptions odometryOptions; bool cummMovementInit = false; long SF_counter = 0; // Reset cummulative pose change: CPose2D accumMovement(0, 0, 0); // For each entry: for (i = 0; i < N; i++) { CSerializable::Ptr obj = rawlog.getAsGeneric(i); if (rawlog.getType(i) == CRawlog::etActionCollection) { // Accumulate Actions // ---------------------- CActionCollection::Ptr curActs = std::dynamic_pointer_cast<CActionCollection>(obj); CActionRobotMovement2D::Ptr mov = curActs->getBestMovementEstimation(); if (mov) { CPose2D incrPose = mov->poseChange->getMeanVal(); // If we have previous observations, shift them according to // this new increment: if (accum_sf) { CPose3D inv_incrPose3D( CPose3D(0, 0, 0) - CPose3D(incrPose)); for (CSensoryFrame::iterator it = accum_sf->begin(); it != accum_sf->end(); ++it) { CPose3D tmpPose; (*it)->getSensorPose(tmpPose); tmpPose = inv_incrPose3D + tmpPose; (*it)->setSensorPose(tmpPose); } } // Accumulate from odometry: accumMovement = accumMovement + incrPose; // Copy the probabilistic options from the first entry we find: if (!cummMovementInit) { odometryOptions = mov->motionModelConfiguration; cummMovementInit = true; } } } else if (rawlog.getType(i) == CRawlog::etSensoryFrame) { // Decimate Observations // --------------------------- // Add observations to the accum. SF: if (!accum_sf) accum_sf = mrpt::make_aligned_shared<CSensoryFrame>(); // Copy pointers to observations only (fast): accum_sf->moveFrom(*std::dynamic_pointer_cast<CSensoryFrame>(obj)); if (++SF_counter >= DECIMATE_RATIO) { SF_counter = 0; // INSERT OBSERVATIONS: newRawLog.addObservationsMemoryReference(accum_sf); accum_sf.reset(); // INSERT ACTIONS: CActionCollection actsCol; if (cummMovementInit) { CActionRobotMovement2D cummMovement; cummMovement.computeFromOdometry( accumMovement, odometryOptions); actsCol.insert(cummMovement); // Reset odometry accumulation: accumMovement = CPose2D(0, 0, 0); } newRawLog.addActions(actsCol); } } else THROW_EXCEPTION( "This operation implemented for SF-based rawlogs only."); // Delete object obj.reset(); } // end for i each entry // Clear the list only (objects already deleted) rawlog.clear(); // Copy as new log: rawlog = newRawLog; rebuildTreeView(); WX_END_TRY }
void xRawLogViewerFrame::OnMenuLossLessDecFILE(wxCommandEvent& event) { WX_START_TRY string filToOpen; if (!AskForOpenRawlog(filToOpen)) return; wxString strDecimation = wxGetTextFromUser( _("The number of observations will be decimated (only 1 out of M will " "be kept). Enter the decimation ratio M:"), _("Decimation"), _("1")); long DECIMATE_RATIO; strDecimation.ToLong(&DECIMATE_RATIO); ASSERT_(DECIMATE_RATIO >= 1) string filToSave; AskForSaveRawlog(filToSave); CFileGZInputStream fil(filToOpen); CFileGZOutputStream f_out(filToSave); wxBusyCursor waitCursor; unsigned int filSize = (unsigned int)fil.getTotalBytesCount(); wxString auxStr; wxProgressDialog progDia( wxT("Progress"), wxT("Parsing file..."), filSize, // range this, // parent wxPD_CAN_ABORT | wxPD_APP_MODAL | wxPD_SMOOTH | wxPD_AUTO_HIDE | wxPD_ELAPSED_TIME | wxPD_ESTIMATED_TIME | wxPD_REMAINING_TIME); wxTheApp->Yield(); // Let the app. process messages unsigned int countLoop = 0; int entryIndex = 0; bool keepLoading = true; string errorMsg; // ------------------------------------------------------------------------------ // METHOD TO BE MEMORY EFFICIENT: // To free the memory of the current rawlog entries as we create the new // one, // then call "clearWithoutDelete" at the end. // ------------------------------------------------------------------------------ CSensoryFrame::Ptr accum_sf; CActionRobotMovement2D::TMotionModelOptions odometryOptions; bool cummMovementInit = false; long SF_counter = 0; // Reset cummulative pose change: CPose2D accumMovement(0, 0, 0); TTimeStamp timestamp_lastAction = INVALID_TIMESTAMP; while (keepLoading) { if (countLoop++ % 100 == 0) { auxStr.sprintf(wxT("Parsing file... %u objects"), entryIndex); if (!progDia.Update((int)fil.getPosition(), auxStr)) keepLoading = false; wxTheApp->Yield(); // Let the app. process messages } CSerializable::Ptr newObj; try { fil >> newObj; entryIndex++; // Check type: if (newObj->GetRuntimeClass() == CLASS_ID(CSensoryFrame)) { // Decimate Observations // --------------------------- // Add observations to the accum. SF: if (!accum_sf) accum_sf = mrpt::make_aligned_shared<CSensoryFrame>(); // Copy pointers to observations only (fast): accum_sf->moveFrom( *std::dynamic_pointer_cast<CSensoryFrame>(newObj)); if (++SF_counter >= DECIMATE_RATIO) { SF_counter = 0; // INSERT OBSERVATIONS: f_out << *accum_sf; accum_sf.reset(); // INSERT ACTIONS: CActionCollection::Ptr actsCol = mrpt::make_aligned_shared<CActionCollection>(); if (cummMovementInit) { CActionRobotMovement2D cummMovement; cummMovement.computeFromOdometry( accumMovement, odometryOptions); cummMovement.timestamp = timestamp_lastAction; actsCol->insert(cummMovement); // Reset odometry accumulation: accumMovement = CPose2D(0, 0, 0); } f_out << *actsCol; actsCol.reset(); } } else if (newObj->GetRuntimeClass() == CLASS_ID(CActionCollection)) { // Accumulate Actions // ---------------------- CActionCollection::Ptr curActs = std::dynamic_pointer_cast<CActionCollection>(newObj); CActionRobotMovement2D::Ptr mov = curActs->getBestMovementEstimation(); if (mov) { timestamp_lastAction = mov->timestamp; CPose2D incrPose = mov->poseChange->getMeanVal(); // If we have previous observations, shift them according to // this new increment: if (accum_sf) { CPose3D inv_incrPose3D( CPose3D(0, 0, 0) - CPose3D(incrPose)); for (CSensoryFrame::iterator it = accum_sf->begin(); it != accum_sf->end(); ++it) { CPose3D tmpPose; (*it)->getSensorPose(tmpPose); tmpPose = inv_incrPose3D + tmpPose; (*it)->setSensorPose(tmpPose); } } // Accumulate from odometry: accumMovement = accumMovement + incrPose; // Copy the probabilistic options from the first entry we // find: if (!cummMovementInit) { odometryOptions = mov->motionModelConfiguration; cummMovementInit = true; } } } else { // Unknown class: THROW_EXCEPTION("Unknown class found in the file!"); } } catch (exception& e) { errorMsg = e.what(); keepLoading = false; } catch (...) { keepLoading = false; } } // end while keep loading progDia.Update(filSize); wxTheApp->Yield(); // Let the app. process messages WX_END_TRY }
// ------------------------------------------------------ // 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 }