// ------------------------------------------------------
// Visual Odometry
// ------------------------------------------------------
void vOdometry_lightweight()
{
// My Local Variables
CVisualOdometryStereo vOdometer;
unsigned int step = 0;
CTicTac tictac;
std::vector<CPose3DQuat> path1;
std::vector<CPose3DQuatPDFGaussian> path2;
size_t rawlogEntry = 0;
CFileGZInputStream rawlogFile( RAWLOG_FILE );
// Initial pose of the path
path1.push_back( CPose3DQuat() );
path2.push_back( CPose3DQuatPDFGaussian() );
// ----------------------------------------------------------
// vOdometry
// ----------------------------------------------------------
CActionCollectionPtr action;
CSensoryFramePtr observations;
CObservationPtr observation;
// Load options (stereo + matching + odometry)
vOdometer.loadOptions( INI_FILENAME );
// Delete previous files and prepare output dir
deleteFiles( format("%s/*.txt", OUT_DIR) );
FILE *f_cov = os::fopen( format( "%s/cov.txt", OUT_DIR ), "wt");
ASSERT_( f_cov != NULL );
// Iteration counter
int counter = 0;
FILE *f_log = os::fopen( format( "%s/q.txt", OUT_DIR ), "wt");
FILE *f_log2 = os::fopen( format( "%s/path.txt", OUT_DIR ), "wt");
unsigned int imDecimation = 5;
// Main Loop
tictac.Tic();
for (;;)
{
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
if ( rawlogEntry >= RAWLOG_OFFSET && 0 == ( step % DECIMATION ) )
{
// Execute:
// ----------------------------------------
// STEREO IMAGES OBSERVATION
CObservationStereoImagesPtr sImgs;
if( observation )
sImgs = CObservationStereoImagesPtr( observation );
else
sImgs = observations->getObservationByClass<CObservationStereoImages>();
poses::CPose3DQuatPDFGaussian outEst;
if( sImgs )
{
if( counter == 0 )
{
// Set initial parameters
vOdometer.stereoParams.baseline = sImgs->rightCameraPose.x();
vOdometer.stereoParams.K = sImgs->leftCamera.intrinsicParams;
}
cout << "Rawlog Entry: " << rawlogEntry << " Iteration: " << counter++ << endl;
if( step % imDecimation )
{
vOdometer.process_light( sImgs, outEst );
TOdometryInfo info = vOdometer.getInfo();
// Save to file both the quaternion and covariance matrix
os::fprintf( f_log,"%f %f %f %f %f %f %f\n",
outEst.mean[0], outEst.mean[1], outEst.mean[2], outEst.mean[3], outEst.mean[4], outEst.mean[5], outEst.mean[6] );
info.m_Prev_cloud.landmarks.saveToTextFile( format( "%s/clouds%04d.txt", OUT_DIR, step ) );
path1.push_back( path1.back() + outEst.mean );
os::fprintf( f_log2,"%f %f %f %f %f %f %f\n",
path1.back()[0], path1.back()[1], path1.back()[2], path1.back()[3], path1.back()[4], path1.back()[5], path1.back()[6] );
path2.push_back( outEst );
}
else
cout << "Skipped step" << endl;
} // end if sImgs != NULL
} // end if 'rawlogEntry >= rawlog_offset'
step++;
// Free memory:
action.clear_unique();
observations.clear_unique();
}; // end while !end
cout << "*************** Tiempo: " << 1000.0f*tictac.Tac() << "************************" << endl;
os::fclose( f_cov );
os::fclose( f_log );
os::fclose( f_log2 );
// SAVE THE RESULTS
/**/
FILE *fPath1 = os::fopen( format("./%s/EstimatedPath.txt", OUT_DIR).c_str(), "wt");
if( fPath1 != NULL )
{
std::vector<CPose3DQuat>::iterator itPath;
for(itPath = path1.begin(); itPath != path1.end(); ++itPath )
os::fprintf( fPath1,"%f %f %f %f %f %f %f\n",
itPath->x(), itPath->y(), itPath->z(),
itPath->quat().r(), itPath->quat().x(), itPath->quat().y(), itPath->quat().z() );
os::fclose( fPath1 );
}
else
std::cout << "WARNING: The estimated path could not be saved" << std::endl;
FILE *fPath2 = os::fopen( format("./%s/EstimatedPathPDF.txt", OUT_DIR).c_str(), "wt");
if( fPath2 != NULL )
{
std::vector<CPose3DQuatPDFGaussian>::iterator itPath;
for(itPath = path2.begin(); itPath != path2.end(); ++itPath )
{
os::fprintf(fPath2,"%.3f %.3f %.3f %.3f %.3f %.3f %.3f\n",
itPath->mean.x(), itPath->mean.y(), itPath->mean.z(),
itPath->mean.quat().r(), itPath->mean.quat().x(), itPath->mean.quat().y(), itPath->mean.quat().z() );
os::fprintf(fPath2,"%.3f %.3f %.3f %.3f %.3f %.3f %.3f\n", itPath->cov.get_unsafe(0,0), itPath->cov.get_unsafe(0,1), itPath->cov.get_unsafe(0,2), itPath->cov.get_unsafe(0,3), itPath->cov.get_unsafe(0,4), itPath->cov.get_unsafe(0,5), itPath->cov.get_unsafe(0,6));
os::fprintf(fPath2,"%.3f %.3f %.3f %.3f %.3f %.3f %.3f\n", itPath->cov.get_unsafe(1,0), itPath->cov.get_unsafe(1,1), itPath->cov.get_unsafe(1,2), itPath->cov.get_unsafe(1,3), itPath->cov.get_unsafe(1,4), itPath->cov.get_unsafe(1,5), itPath->cov.get_unsafe(1,6));
os::fprintf(fPath2,"%.3f %.3f %.3f %.3f %.3f %.3f %.3f\n", itPath->cov.get_unsafe(2,0), itPath->cov.get_unsafe(2,1), itPath->cov.get_unsafe(2,2), itPath->cov.get_unsafe(2,3), itPath->cov.get_unsafe(2,4), itPath->cov.get_unsafe(2,5), itPath->cov.get_unsafe(2,6));
os::fprintf(fPath2,"%.3f %.3f %.3f %.3f %.3f %.3f %.3f\n", itPath->cov.get_unsafe(3,0), itPath->cov.get_unsafe(3,1), itPath->cov.get_unsafe(3,2), itPath->cov.get_unsafe(3,3), itPath->cov.get_unsafe(3,4), itPath->cov.get_unsafe(3,5), itPath->cov.get_unsafe(3,6));
os::fprintf(fPath2,"%.3f %.3f %.3f %.3f %.3f %.3f %.3f\n", itPath->cov.get_unsafe(4,0), itPath->cov.get_unsafe(4,1), itPath->cov.get_unsafe(4,2), itPath->cov.get_unsafe(4,3), itPath->cov.get_unsafe(4,4), itPath->cov.get_unsafe(4,5), itPath->cov.get_unsafe(4,6));
os::fprintf(fPath2,"%.3f %.3f %.3f %.3f %.3f %.3f %.3f\n", itPath->cov.get_unsafe(5,0), itPath->cov.get_unsafe(5,1), itPath->cov.get_unsafe(5,2), itPath->cov.get_unsafe(5,3), itPath->cov.get_unsafe(5,4), itPath->cov.get_unsafe(5,5), itPath->cov.get_unsafe(5,6));
os::fprintf(fPath2,"%.3f %.3f %.3f %.3f %.3f %.3f %.3f\n", itPath->cov.get_unsafe(6,0), itPath->cov.get_unsafe(6,1), itPath->cov.get_unsafe(6,2), itPath->cov.get_unsafe(6,3), itPath->cov.get_unsafe(6,4), itPath->cov.get_unsafe(6,5), itPath->cov.get_unsafe(6,6));
}
os::fclose( fPath2 );
}
else
std::cout << "WARNING: The estimated pdf path could not be saved" << std::endl;
std::cout << "Saved results!" << std::endl;
/**/
mrpt::system::pause();
}
// ------------------------------------------------------
// TestGeometry3D
// ------------------------------------------------------
void TestLaser2Imgs()
{
// Set your rawlog file name
if (!mrpt::system::fileExists(RAWLOG_FILE))
THROW_EXCEPTION_CUSTOM_MSG1("Rawlog file does not exist: %s",RAWLOG_FILE.c_str())
CActionCollectionPtr action;
CSensoryFramePtr observations;
size_t rawlogEntry = 0;
//bool end = false;
CDisplayWindow wind;
// Set relative path for externally-stored images in rawlogs:
string rawlog_images_path = extractFileDirectory( RAWLOG_FILE );
rawlog_images_path+="/Images";
CImage::IMAGES_PATH_BASE = rawlog_images_path; // Set it.
CFileGZInputStream rawlogFile( RAWLOG_FILE );
for (;;)
{
if (os::kbhit())
{
char c = os::getch();
if (c==27)
break;
}
// Load action/observation pair from the rawlog:
// --------------------------------------------------
cout << "Reading act/oct pair from rawlog..." << endl;
if (! CRawlog::readActionObservationPair( rawlogFile, action, observations, rawlogEntry) )
break; // file EOF
// CAMERA............
// Get CObservationStereoImages
CObservationStereoImagesPtr sImgs;
CObservationImagePtr Img;
sImgs = observations->getObservationByClass<CObservationStereoImages>();
if (!sImgs)
{
Img = observations->getObservationByClass<CObservationImage>();
if(!Img)
continue;
}
CPose3D cameraPose; // Get Camera Pose (B) (CPose3D)
CMatrixDouble33 K; // Get Calibration matrix (K)
sImgs ? sImgs->getSensorPose( cameraPose ) : Img->getSensorPose( cameraPose );
K = sImgs ? sImgs->leftCamera.intrinsicParams : Img->cameraParams.intrinsicParams;
// LASER.............
// Get CObservationRange2D
CObservation2DRangeScanPtr laserScan = observations->getObservationByClass<CObservation2DRangeScan>();
if (!laserScan) continue;
// Get Laser Pose (A) (CPose3D)
CPose3D laserPose;
laserScan->getSensorPose( laserPose );
if (abs(laserPose.yaw())>DEG2RAD(90)) continue; // Only front lasers
// Get 3D Point relative to the Laser coordinate Frame (P1) (CPoint3D)
CPoint3D point;
CSimplePointsMap mapa;
mapa.insertionOptions.minDistBetweenLaserPoints = 0;
observations->insertObservationsInto( &mapa ); // <- The map contains the pose of the points (P1)
// Get the points into the map
vector<float> X, Y, Z;
vector<float>::iterator itX, itY, itZ;
mapa.getAllPoints(X,Y,Z);
unsigned int imgW = sImgs? sImgs->imageLeft.getWidth() : Img->image.getWidth();
unsigned int imgH = sImgs? sImgs->imageLeft.getHeight() : Img->image.getHeight();
//unsigned int idx = 0;
vector<float> imgX,imgY;
vector<float>::iterator itImgX,itImgY;
imgX.resize( X.size() );
imgY.resize( Y.size() );
CImage image;
image = sImgs ? sImgs->imageLeft : Img->image;
// Get pixels in the image:
// Pimg = (kx,ky,k)^T = K(I|0)*P2
// Main loop
for( itX = X.begin(), itY = Y.begin(), itZ = Z.begin(), itImgX = imgX.begin(), itImgY = imgY.begin();
itX != X.end();
itX++, itY++, itZ++, itImgX++, itImgY++)
{
// Coordinates Transformation
CPoint3D pLaser(*itX,*itY,*itZ);
CPoint3D pCamera(pLaser-cameraPose);
if( pCamera.z() > 0 )
{
*itImgX = - K(0,0)*((pCamera.x())/(pCamera.z())) + K(0,2);
*itImgY = - K(1,1)*((pCamera.y())/(pCamera.z())) + K(1,2);
if( *itImgX > 0 && *itImgX < imgW && *itImgY > 0 && *itImgY < imgH )
image.filledRectangle(*itImgX-1,*itImgY-1,*itImgX+1,*itImgY+1,TColor(255,0,0));
} // end if
} // end for
action.clear_unique();
observations.clear_unique();
wind.showImage(image);
mrpt::system::sleep(50);
}; // end for
mrpt::system::pause();
}
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.
// ------------------------------------------------------------------------------
CSensoryFramePtr 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
}
CSerializablePtr 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 = CSensoryFrame::Create();
// Copy pointers to observations only (fast):
accum_sf->moveFrom( *CSensoryFramePtr(newObj) );
if ( ++SF_counter >= DECIMATE_RATIO )
{
SF_counter = 0;
// INSERT OBSERVATIONS:
f_out << *accum_sf;
accum_sf.clear_unique();
// INSERT ACTIONS:
CActionCollectionPtr actsCol = CActionCollection::Create();
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.clear_unique();
}
}
else if ( newObj->GetRuntimeClass() == CLASS_ID(CActionCollection) )
{
// Accumulate Actions
// ----------------------
CActionCollectionPtr curActs = CActionCollectionPtr( newObj );
CActionRobotMovement2DPtr 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
// ------------------------------------------------------
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
}
// ------------------------------------------------------
// Benchmark: A whole ICP-SLAM run
// ------------------------------------------------------
double icp_test_1(int a1, int a2)
{
#ifdef MRPT_DATASET_DIR
const string rawlog_file = MRPT_DATASET_DIR "/2006-01ENE-21-SENA_Telecom Faculty_one_loop_only.rawlog";
if (!mrpt::system::fileExists(rawlog_file))
return 1;
CTicTac tictac;
int step = 0;
size_t rawlogEntry = 0;
CFileGZInputStream rawlogFile( rawlog_file );
TSetOfMetricMapInitializers metricMapsOpts;
const bool use_grid = a1!=0;
if (use_grid)
{
COccupancyGridMap2D::TMapDefinition def;
def.resolution = 0.05;
metricMapsOpts.push_back( def );
}
else
{
CSimplePointsMap::TMapDefinition def;
def.insertionOpts.minDistBetweenLaserPoints = 0.03;
metricMapsOpts.push_back( def );
}
double insertionLinDistance = 0.75;
double insertionAngDistance = DEG2RAD(30);
CICP::TConfigParams icpOptions;
icpOptions.maxIterations = 40;
// ---------------------------------
// Constructor
// ---------------------------------
CMetricMapBuilderICP mapBuilder;
mapBuilder.ICP_options.mapInitializers = metricMapsOpts;
mapBuilder.ICP_options.insertionLinDistance = insertionLinDistance;
mapBuilder.ICP_options.insertionAngDistance = insertionAngDistance;
mapBuilder.ICP_options.matchAgainstTheGrid = use_grid ;
mapBuilder.ICP_params = icpOptions;
// Start with an empty map:
mapBuilder.initialize( CSimpleMap() );
// ---------------------------------
// CMetricMapBuilder::TOptions
// ---------------------------------
mapBuilder.options.verbose = false;
mapBuilder.options.enableMapUpdating = true;
// ----------------------------------------------------------
// Map Building
// ----------------------------------------------------------
CActionCollectionPtr action;
CSensoryFramePtr observations;
for (;;)
{
// Load action/observation pair from the rawlog:
// --------------------------------------------------
if (! CRawlog::readActionObservationPair( rawlogFile, action, observations, rawlogEntry) )
break; // file EOF
// Execute:
mapBuilder.processActionObservation( *action, *observations );
step++;
// printf("\n---------------- STEP %u | RAWLOG ENTRY %u ----------------\n",step, (unsigned)rawlogEntry);
// Free memory:
action.clear_unique();
observations.clear_unique();
}
#if 0
mapBuilder.getCurrentlyBuiltMetricMap()->saveMetricMapRepresentationToFile( format("icp_%i_result",a1) );
#endif
if (!step) step++;
return tictac.Tac()/step;
#else
return 1;
#endif
}
