void drawCameraStateEllipse(VNL::Vector<double> location, VNL::Matrix<double> Pxx, COpenGLScenePtr & mrpt3dscene){
CEllipsoidPtr objEllip = CEllipsoid::Create();
objEllip->setLocation(location.Get(0), location.Get(1), location.Get(2));
CMatrixDouble33 COV3;
int size=3;
for (int i = 0; i < size; i++) {
for (int j = 0; j < size; j++) {
COV3(i, j) = Pxx.Get(i, j);
}
}
objEllip->setCovMatrix(COV3, COV3(2, 2) == 0 ? 2 : 3);
objEllip->setColor(1, 1, 0);
objEllip->enableDrawSolid3D(true);
mrpt3dscene->insert(objEllip);
}
void drawEllipse(Feature* feature, COpenGLScenePtr & mrpt3dscene) {
VNL::Vector<double> estimateY = feature->get_y();//get_feature_measurement_model()->get_yigraphicsRES();
//VNL::Matrix<double> matrixPyy = feature->get_feature_measurement_model()->get_PyiyigraphicsRES();
VNL::Matrix<double> matrixPyy = feature->get_Pyy();
CEllipsoidPtr objEllip = CEllipsoid::Create();
objEllip->setLocation(estimateY.Get(0), estimateY.Get(1), estimateY.Get(2));
// CMatrixDouble matrix;
// CMatrixDouble66 COV;
CMatrixDouble33 COV3;
int size=3;
for (int i = 0; i < size; i++) {
for (int j = 0; j < size; j++) {
COV3(i, j) = matrixPyy.Get(i, j);
//cout << (COV3) (j, i) << " ";
}
//cout << endl;
}
objEllip->setCovMatrix(COV3, COV3(2, 2) == 0 ? 2 : 3);
//int size = 3;
//cout << "Size: " << b->Rows() << "-" << b->Cols() << endl;
// matrix.resize(size, size);
// for (int i = 0; i < size; i++) {
// for (int j = 0; j < size; j++) {
// (matrix) (j, i) = feature->get_Pxy().Get(j, i);
// cout << (matrix) (j, i) << " ";
// }
// cout << endl;
// }
// cout << endl;
//convertMatrix(&matrix, &feature->get_Pxy());
// objEllip->setCovMatrix(matrix, 3);
//objEllip->setCovMatrix(COV3, COV3(2, 2) == 0 ? 2 : 3);
objEllip->setColor(0, 1, 1);
objEllip->enableDrawSolid3D(true);
mrpt3dscene->insert(objEllip);
}
/*---------------------------------------------------------------
CHMTSLAM_LOG
Implements a 2D local SLAM method based on scan matching
between near observations and an EKF. A part of HMT-SLAM
\param LMH The local metric hypothesis which must be updated by this SLAM algorithm.
\param act The action to process (or NULL).
\param sf The observations to process (or NULL).
--------------------------------------------------------------- */
void CHMTSLAM::generateLogFiles(unsigned int nIteration)
{
MRPT_START
// Speed up the storage of images (in opengl::CTexturedPlane's):
//CImage::DISABLE_ZIP_COMPRESSION = true;
static CTicTac tictac;
tictac.Tic();
THypothesisID bestHypoID;
CLocalMetricHypothesis *bestLMH = NULL;
{
CCriticalSectionLocker locker( &m_LMHs_cs );
printf_debug("[LOG] Number of LMHs: %u\n", (unsigned int)m_LMHs.size() );
// Generate 3D view of local areas:
{
string filLocalAreas = format("%s/LSLAM_3D/mostLikelyLMH_LSLAM_%05u.3Dscene", m_options.LOG_OUTPUT_DIR.c_str(), nIteration );
COpenGLScenePtr sceneLSLAM = COpenGLScene::Create();
// Look for the most likely LMH:
aligned_containers<THypothesisID, CLocalMetricHypothesis>::map_t::iterator it;
for ( it = m_LMHs.begin();it!=m_LMHs.end();it++)
{
if (!bestLMH)
{
bestLMH = & it->second;
}
else if ( it->second.m_log_w > bestLMH->m_log_w)
{
bestLMH = & it->second;
}
}
ASSERT_(bestLMH!=NULL)
bestHypoID = bestLMH->m_ID;
{
CCriticalSectionLocker lockerLMH( &bestLMH->m_lock );
{
// Generate the metric maps 3D view...
opengl::CSetOfObjectsPtr maps3D = opengl::CSetOfObjects::Create();
maps3D->setName("metric-maps");
bestLMH->getMostLikelyParticle()->d->metricMaps.getAs3DObject( maps3D );
sceneLSLAM->insert( maps3D );
// ...and the robot poses, areas, etc:
opengl::CSetOfObjectsPtr LSLAM_3D = opengl::CSetOfObjects::Create();
LSLAM_3D->setName("LSLAM_3D");
bestLMH->getAs3DScene( LSLAM_3D );
sceneLSLAM->insert( LSLAM_3D );
sceneLSLAM->enableFollowCamera(true);
printf_debug("[LOG] Saving %s\n", filLocalAreas.c_str());
CFileGZOutputStream(filLocalAreas) << *sceneLSLAM;
}
// Save the SSO matrix:
#if 0
{
CCriticalSectionLocker locker( &bestLMH->m_robotPosesGraph.lock );
string filSSO = format("%s/ASSO/mostLikelyLMH_ASSO_%05u.3Dscene", m_options.LOG_OUTPUT_DIR.c_str(), nIteration );
COpenGLScene sceneSSO;
opengl::CSetOfObjectsPtr sso3D = opengl::CSetOfObjects::Create();
bestLMH->m_robotPosesGraph.partitioner.getAs3DScene( sso3D, &bestLMH->m_robotPosesGraph.idx2pose );
sceneSSO.insert(sso3D);
CFileGZOutputStream(filSSO) << sceneSSO;
if (1)
{
CMatrix A;
bestLMH->m_robotPosesGraph.partitioner.getAdjacencyMatrix( A );
if (A.getColCount()>0)
{
A.adjustRange();
A.saveToTextFile( format("%s/ASSO/mostLikelyLMH_ASSO_%05u.txt", m_options.LOG_OUTPUT_DIR.c_str(), nIteration ) );
CImage(A,true).saveToFile( format("%s/ASSO/mostLikelyLMH_ASSO_%05u.png", m_options.LOG_OUTPUT_DIR.c_str(), nIteration ) );
}
}
} // end lock partitioner's CS
#endif
} // end LMH's lock
}
} // end of lock on LMHs_cs
#if 1
{
// Save the whole HMT-SLAM state to a dump file
static int CNT = 0;
if ((CNT++ % 20) == 0)
{
string hmtmap_file( format("%s/HMTSLAM_state/state_%05u.hmtslam", m_options.LOG_OUTPUT_DIR.c_str(), nIteration ) );
printf_debug("[LOG] Saving %s\n", hmtmap_file.c_str());
CFileGZOutputStream(hmtmap_file) << *this;
}
}
#endif
#if 1
{
// Update the poses-graph in the HMT-map from the LMH to draw it:
static int CNT = 0;
if ((CNT++ % 5) == 0)
{
CCriticalSectionLocker lockerLMH( &bestLMH->m_lock );
for (TNodeIDSet::const_iterator n = bestLMH->m_neighbors.begin();n!=bestLMH->m_neighbors.end();++n)
bestLMH->updateAreaFromLMH( *n );
// Save global map for most likely hypothesis:
COpenGLScene sceneGlobalHMTMAP;
{
CCriticalSectionLocker locker( &m_map_cs );
printf_debug("[LOG] HMT-map: %u nodes/ %u arcs\n", (unsigned int)m_map.nodeCount(), (unsigned int)m_map.arcCount() );
m_map.getAs3DScene(
sceneGlobalHMTMAP, // Scene
m_map.getFirstNode()->getID(), // Reference node
bestHypoID, // Hypothesis to get
3 // iterations
);
}
string hmtmap_file( format("%s/HMAP_3D/mostLikelyHMT_MAP_%05u.3Dscene", m_options.LOG_OUTPUT_DIR.c_str(), nIteration ) );
printf_debug("[LOG] Saving %s\n", hmtmap_file.c_str());
CFileGZOutputStream(hmtmap_file) << sceneGlobalHMTMAP;
}
}
#endif
// Save the memory usage:
unsigned long memUsage = mrpt::system::getMemoryUsage();
FILE *f=os::fopen( format("%s/log_MemoryUsage.txt",m_options.LOG_OUTPUT_DIR.c_str()).c_str() ,"at");
if (f)
{
os::fprintf(f,"%u\t%f\n",nIteration,memUsage/(1024.0*1024.0));
os::fclose(f);
}
double t_log = tictac.Tac();
printf_debug("[LOG] Time for logging: %f ms\n", 1000*t_log);
MRPT_END
}
// ------------------------------------------------------
// 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
}
// ------------------------------------------------------
// 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
}
