// ------------------------------------------------------
// 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();
}
void xRawLogViewerFrame::OnMenuCompactRawlog(wxCommandEvent& event)
{
WX_START_TRY
bool onlyOnePerLabel = (wxYES==wxMessageBox( _("Keep only one observation of each label within each sensoryframe?"), _("Compact rawlog"),wxYES_NO, this ));
int progress_N = static_cast<int>(rawlog.size());
int progress_i=progress_N;
wxProgressDialog progDia(
wxT("Compacting rawlog"),
wxT("Processing..."),
progress_N, // 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
CActionRobotMovement2DPtr lastAct;
CSensoryFramePtr lastSF; // = NULL;
unsigned counter_loops = 0;
unsigned nActionsDel = 0;
unsigned nEmptySFDel = 0;
CRawlog::iterator it=rawlog.begin();
for (progress_i=0 ;it!=rawlog.end();progress_i--)
{
if (counter_loops++ % 50 == 0)
{
if (!progDia.Update( progress_N-progress_i ))
break;
wxTheApp->Yield(); // Let the app. process messages
}
bool deleteThis = false;
if (it.getType()==CRawlog::etActionCollection)
{
// Is this a part of multiple actions?
if (lastAct)
{
// Remove this one and add it to the first in the series:
CActionRobotMovement2DPtr act = CActionCollectionPtr(*it)->getMovementEstimationByType( CActionRobotMovement2D::emOdometry );
ASSERT_(act);
lastAct->computeFromOdometry( lastAct->rawOdometryIncrementReading + act->rawOdometryIncrementReading, lastAct->motionModelConfiguration);
deleteThis=true;
nActionsDel++;
}
else
{
// This is the first one:
lastAct = CActionCollectionPtr(*it)->getMovementEstimationByType( CActionRobotMovement2D::emOdometry );
ASSERT_(lastAct);
// Before leaving the last SF, leave only one observation for each sensorLabel:
if (onlyOnePerLabel && lastSF)
{
CSensoryFramePtr newSF = CSensoryFrame::Create();
set<string> knownLabels;
for (CSensoryFrame::const_iterator o=lastSF->begin();o!=lastSF->end();++o)
{
if (knownLabels.find((*o)->sensorLabel)==knownLabels.end())
newSF->insert(*o);
knownLabels.insert((*o)->sensorLabel);
}
*lastSF = *newSF;
}
// Ok, done:
lastSF.clear_unique();
}
}
else if (it.getType()==CRawlog::etSensoryFrame)
{
// Is this a part of a series?
if (lastSF)
{
// remove this one and accumulate in the first in the serie:
lastSF->moveFrom( * CSensoryFramePtr(*it) );
deleteThis = true;
nEmptySFDel++;
}
else
{
// This is the first SF:
CSensoryFramePtr sf = CSensoryFramePtr(*it);
// Only take into account if not empty!
if (sf->size())
{
lastSF = sf;
ASSERT_(lastSF);
lastAct.clear_unique();
}
else
{
deleteThis = true;
nEmptySFDel++;
}
}
}
else THROW_EXCEPTION("Unexpected class found!")
if (deleteThis)
{
it = rawlog.erase(it);
progress_i--; // Extra count
}
else it++;
}
progDia.Update( progress_N );
string str= format( "%u actions deleted\n%u sensory frames deleted", nActionsDel, nEmptySFDel );
::wxMessageBox( _U( str.c_str() ) );
rebuildTreeView();
WX_END_TRY
}
// ------------------------------------------------------
// 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();
}
// ------------------------------------------------------
// 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
}