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
}