void PFLocalizationCore::observation(CSensoryFramePtr _sf, CObservationOdometryPtr _odometry) {
CActionCollectionPtr action = CActionCollection::Create();
CActionRobotMovement2D odom_move;
odom_move.timestamp = _sf->getObservationByIndex(0)->timestamp;
if(_odometry) {
if(odomLastObservation_.empty()) {
odomLastObservation_ = _odometry->odometry;
}
mrpt::poses::CPose2D incOdoPose = _odometry->odometry - odomLastObservation_;
odomLastObservation_ = _odometry->odometry;
odom_move.computeFromOdometry(incOdoPose, motion_model_options_);
action->insert(odom_move);
updateFilter(action, _sf);
} else {
if(use_motion_model_default_options_){
log_info("No odometry at update %4i -> using dummy", update_counter_);
odom_move.computeFromOdometry(mrpt::poses::CPose2D(0,0,0), motion_model_default_options_);
action->insert(odom_move);
updateFilter(action, _sf);
} else {
log_info("No odometry at update %4i -> skipping observation", update_counter_);
}
}
}
void PFLocalizationCore::updateFilter(CActionCollectionPtr _action, CSensoryFramePtr _sf) {
if(state_ == INIT) initializeFilter();
tictac_.Tic();
pf_.executeOn( pdf_, _action.pointer(), _sf.pointer(), &pf_stats_ );
timeLastUpdate_ = _sf->getObservationByIndex(0)->timestamp;
update_counter_++;
}
// ------------------------------------------------------
// 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();
}
/*---------------------------------------------------------------
CLSLAM_RBPF_2DLASER
Implements a 2D local SLAM method based on a RBPF
over an occupancy grid map. 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).
WE ALREADY HAVE CONTROL OVER THE CRITICAL SECTION OF THE LMHs!
--------------------------------------------------------------- */
void CLSLAM_RBPF_2DLASER::processOneLMH(
CLocalMetricHypothesis *LMH,
const CActionCollectionPtr &actions,
const CSensoryFramePtr &sf )
{
MRPT_START
// Get the current robot pose estimation:
TPoseID currentPoseID = LMH->m_currentRobotPose;
// If this is the first iteration, just create a new robot pose at the origin:
if (currentPoseID == POSEID_INVALID )
{
currentPoseID = CHMTSLAM::generatePoseID();
LMH->m_currentRobotPose = currentPoseID; // Change it in the LMH
// Create a new robot pose:
CPose3D initPose(0,0,0);
ASSERT_( LMH->m_particles.size()>0 );
for (CLocalMetricHypothesis::CParticleList::iterator it=LMH->m_particles.begin();it!=LMH->m_particles.end();++it)
it->d->robotPoses[ currentPoseID ] = initPose;
ASSERT_( m_parent->m_map.nodeCount()==1 );
m_parent->m_map_cs.enter();
CHMHMapNodePtr firstArea = m_parent->m_map.getFirstNode();
ASSERT_(firstArea);
LMH->m_nodeIDmemberships[currentPoseID] = firstArea->getID();
// Set annotation for the reference pose:
firstArea->m_annotations.setElemental( NODE_ANNOTATION_REF_POSEID, currentPoseID , LMH->m_ID);
m_parent->m_map_cs.leave();
}
bool insertNewRobotPose = false;
if (sf)
{
if ( LMH->m_nodeIDmemberships.size()<2 ) // If there is just 1 node (the current robot pose), then there is no observation in the map yet!
{ // Update map if this is the first observation!
insertNewRobotPose = true;
}
else
{
// Check minimum distance from current robot pose to those in the neighborhood:
map< TPoseID, CPose3D > lstRobotPoses;
LMH->getMeans( lstRobotPoses );
CPose3D *currentRobotPose = & lstRobotPoses[currentPoseID];
float minDistLin = 1e6f;
float minDistAng = 1e6f;
//printf("Poses in graph:\n");
for (map< TPoseID, CPose3D >::iterator it = lstRobotPoses.begin();it!=lstRobotPoses.end();++it)
{
if (it->first != currentPoseID )
{
float linDist = it->second.distanceTo( *currentRobotPose );
float angDist = fabs(math::wrapToPi( it->second.yaw() - currentRobotPose->yaw() ));
minDistLin = min( minDistLin, linDist );
if ( linDist < m_parent->m_options.SLAM_MIN_DIST_BETWEEN_OBS )
minDistAng = min(minDistAng, angDist);
}
}
// time to insert a new node??
insertNewRobotPose = (minDistLin>m_parent->m_options.SLAM_MIN_DIST_BETWEEN_OBS) || ( minDistAng > m_parent->m_options.SLAM_MIN_HEADING_BETWEEN_OBS );
}
} // end if there are SF
// Save data in members so PF callback "prediction_and_update_pfXXXX" have access to them:
m_insertNewRobotPose = insertNewRobotPose;
// ------------------------------------------------
// Execute RBPF method:
// 1) PROCESS ACTION
// 2) PROCESS OBSERVATIONS
// ------------------------------------------------
CParticleFilter pf;
pf.m_options = m_parent->m_options.pf_options;
pf.executeOn( *LMH, actions.pointer(), sf.pointer() );
// 3) The appearance observation: update the log likelihood
// ...
// -----------------------------------------------------------
// 4) UPDATE THE MAP
// -----------------------------------------------------------
if (insertNewRobotPose)
{
m_parent->printf_debug("[CLSLAM_RBPF_2DLASER] Adding new pose...\n");
// Leave the up-to-now "current pose" in the map, insert the SF in it, and...
// ----------------------------------------------------------------------------
TPoseID newCurrentPoseID = CHMTSLAM::generatePoseID();
// ...and create a new "current pose" making a copy of the previous one:
// and insert the observations into the metric maps:
// ----------------------------------------------------------------------------
for (CLocalMetricHypothesis::CParticleList::iterator partIt = LMH->m_particles.begin();
partIt!=LMH->m_particles.end();
partIt++)
{
const CPose3D *curRobotPose = &partIt->d->robotPoses[currentPoseID];
partIt->d->robotPoses[newCurrentPoseID]= *curRobotPose;
sf->insertObservationsInto( &partIt->d->metricMaps, curRobotPose );
}
// Add node membership: for now, a copy of the current pose:
LMH->m_nodeIDmemberships[newCurrentPoseID] = LMH->m_nodeIDmemberships[currentPoseID];
// Now, insert the SF in the just added robot pose:
// -----------------------------------------------------
LMH->m_SFs[ currentPoseID ] = *sf;
// Sets the new poseID as current robot pose:
// ----------------------------------------------------
TPoseID newlyAddedPose = currentPoseID;
currentPoseID = LMH->m_currentRobotPose = newCurrentPoseID;
// Mark the "newlyAddedPose" as pending to reconsider in the graph-cut method
// (Done in the main loop @ LSLAM thread)
// --------------------------------------------------------------------------------
LMH->m_posesPendingAddPartitioner.push_back( newlyAddedPose );
m_parent->printf_debug("[CLSLAM_RBPF_2DLASER] Added pose %i.\n", (int)newlyAddedPose);
// Notice LC detectors:
// ------------------------------
{
synch::CCriticalSectionLocker lock( &m_parent->m_topLCdets_cs );
for (std::deque<CTopLCDetectorBase*>::iterator it=m_parent->m_topLCdets.begin();it!=m_parent->m_topLCdets.end();++it)
(*it)->OnNewPose( newlyAddedPose, sf.pointer() );
}
} // end of insertNewRobotPose
MRPT_END
}
// ------------------------------------------------------
// 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();
}
/* ------------------------------------------------------------
reloadFromRawlog
------------------------------------------------------------ */
void CRawlogTreeView::reloadFromRawlog( int hint_rawlog_items )
{
// Recompute the total height of the scroll area:
// We also compute a list for each index with:
// - Pointer to data
// - level in the hierarchy (0,1,2)
// --------------------------------------------------------
if (m_rawlog)
{
if (hint_rawlog_items<0)
m_tree_nodes.reserve( m_rawlog->size()+100 );
else m_tree_nodes.reserve( hint_rawlog_items+100 );
}
// Create a "tree node" for each element in the rawlog:
// ---------------------------------------------------------
m_tree_nodes.clear();
m_rawlog_start = INVALID_TIMESTAMP;
m_rawlog_last = INVALID_TIMESTAMP;
// Root:
m_tree_nodes.push_back( TNodeData() );
TNodeData &d = m_tree_nodes.back();
d.level = 0;
// CVectorDouble tims;
if (m_rawlog)
{
CRawlog::iterator end_it = m_rawlog->end();
size_t rawlog_index = 0;
for (CRawlog::iterator it=m_rawlog->begin();it!=end_it;it++,rawlog_index++)
{
m_tree_nodes.push_back( TNodeData() );
TNodeData &d = m_tree_nodes.back();
d.level = 1;
d.data = (*it);
d.index = rawlog_index;
// For containers, go recursively:
if ( (*it)->GetRuntimeClass()==CLASS_ID(CSensoryFrame))
{
CSensoryFramePtr sf = CSensoryFramePtr( *it );
for (CSensoryFrame::iterator o=sf->begin();o!=sf->end();++o)
{
m_tree_nodes.push_back( TNodeData() );
TNodeData &d = m_tree_nodes.back();
d.level = 2;
d.data = (*o);
if ((*o)->timestamp!=INVALID_TIMESTAMP)
{
m_rawlog_last = (*o)->timestamp;
if (m_rawlog_start == INVALID_TIMESTAMP)
m_rawlog_start = (*o)->timestamp;
}
}
}
else
if ( (*it)->GetRuntimeClass()==CLASS_ID(CActionCollection))
{
CActionCollectionPtr acts = CActionCollectionPtr( *it );
for (CActionCollection::iterator a=acts->begin();a!=acts->end();++a)
{
m_tree_nodes.push_back( TNodeData() );
TNodeData &d = m_tree_nodes.back();
d.level = 2;
d.data = (*a);
if ((*a)->timestamp!=INVALID_TIMESTAMP)
{
m_rawlog_last = (*a)->timestamp;
if (m_rawlog_start == INVALID_TIMESTAMP)
m_rawlog_start = (*a)->timestamp;
}
}
}
else
if ( (*it)->GetRuntimeClass()->derivedFrom( CLASS_ID(CObservation) ))
{
CObservationPtr o = CObservationPtr(*it);
if (o->timestamp!=INVALID_TIMESTAMP)
{
m_rawlog_last = o->timestamp;
if (m_rawlog_start == INVALID_TIMESTAMP)
m_rawlog_start = o->timestamp;
//tims.push_back( mrpt::system::timeDifference(m_rawlog_start, o->timestamp));
}
}
}
}
// mrpt::system::vectorToTextFile(tims,"tims.txt");
// Set new size:
int ly = m_tree_nodes.size();
SetScrollbars(ROW_HEIGHT, ROW_HEIGHT, 50, ly);
}
/** This is the common function for all operations over a rawlog file ("filter" a rawlog file into a new one) or over the loaded rawlog (depending on the user selection in the GUI).
*/
void CFormChangeSensorPositions::executeOperationOnRawlog( TRawlogFilter operation, const char *endMsg )
{
WX_START_TRY
int processMax;
bool isInMemory;
CStream *in_fil=NULL,*out_fil=NULL;
sensorPoseReadOK = false;
camReadIsOk = false;
if (rbLoaded->GetValue())
{
// APPLY TO rawlog in memory:
isInMemory = true;
processMax = (int)rawlog.size();
}
else
{
// APPLY TO rawlog files:
isInMemory = false;
if ( !txtInputFile->GetValue().size() )
THROW_EXCEPTION("An input rawlog file must be selected")
if ( !txtOutputFile->GetValue().size() )
THROW_EXCEPTION("An output rawlog file must be selected")
string fileName_IN( txtInputFile->GetValue().mbc_str() );
if (!mrpt::system::fileExists(fileName_IN) )
THROW_EXCEPTION("Input file does not exist!")
string fileName_OUT( txtOutputFile->GetValue().mbc_str() );
if (!fileName_OUT.compare(fileName_IN))
THROW_EXCEPTION("Input and output files must be different!")
in_fil = new CFileGZInputStream(fileName_IN);
out_fil = new CFileGZOutputStream(fileName_OUT);
processMax = (int)in_fil->getTotalBytesCount();
}
wxProgressDialog progDia(
wxT("Modifying rawlog"),
wxT("Processing..."),
processMax, // 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;
wxString auxStr;
// Apply changes:
int changes = 0;
wxBusyCursor cursor;
while ((( !isInMemory && keepLoading ) ||
( isInMemory && countLoop < rawlog.size() ))&& !sensorPoseReadOK && !camReadIsOk )
{
CSerializablePtr newObj;
try
{
if (isInMemory)
{
newObj = rawlog.getAsGeneric(countLoop);
}
else
{
(*in_fil) >> newObj;
}
// Check type:
if ( newObj->GetRuntimeClass() == CLASS_ID(CSensoryFrame))
{
// A sensory frame:
CSensoryFramePtr sf(newObj);
// Process & save:
operation(NULL,sf.pointer(),changes );
if (!isInMemory) (*out_fil) << *sf.pointer();
}
else if ( newObj->GetRuntimeClass() == CLASS_ID(CActionCollection))
{
// This is an action:
CActionCollectionPtr acts =CActionCollectionPtr( newObj );
// Process & save:
operation( (CActionCollection*)acts.pointer(),NULL,changes);
if (!isInMemory) (*out_fil) << *acts;
}
else if ( newObj->GetRuntimeClass()->derivedFrom(CLASS_ID(CObservation)))
{
// A sensory frame:
CObservationPtr o(newObj);
static CSensoryFrame sf;
sf.clear();
sf.insert(o);
// Process & save:
operation(NULL,&sf,changes );
if (!isInMemory) (*out_fil) << *o;
}
else
{ // Unknown class:
THROW_EXCEPTION(format("Unexpected class found in the file: '%s'",newObj->GetRuntimeClass()->className) );
}
}
catch (exception &e)
{
errorMsg = e.what();
keepLoading = false;
}
catch (...)
{
keepLoading = false;
}
// Step counter & update progress dialog
if (countLoop++ % 300 == 0)
{
auxStr.sprintf(wxT("Processing... (%u objects processed)"),countLoop);
int curProgr = isInMemory ? countLoop : (int)in_fil->getPosition();
if (!progDia.Update( curProgr , auxStr ))
keepLoading = false;
wxTheApp->Yield(); // Let the app. process messages
}
// Delete only if processing file
if (newObj && !isInMemory)
{
newObj.clear();
}
} // end while keep loading
progDia.Update( processMax ); // Close dialog.
if (strlen(endMsg))
{
char tmpStr[1000];
os::sprintf(tmpStr,sizeof(tmpStr),"%s %i\n\nEnd message:\n%s", endMsg, changes, errorMsg.c_str() );
wxMessageBox( _U(tmpStr), _("Result:"), wxOK,this);
}
if (in_fil) delete in_fil;
if (out_fil) delete out_fil;
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
}
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
{
CSerializablePtr newObj;
fil >> newObj;
// Check type:
if ( newObj->GetRuntimeClass() == CLASS_ID(CSensoryFrame) )
{
CSensoryFramePtr SF(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":
CActionCollectionPtr acts = CActionCollectionPtr( newObj );
// Get odometry:
CActionRobotMovement2DPtr actOdom = acts->getBestMovementEstimation();
if (actOdom)
{
odometry_accum = odometry_accum + actOdom->poseChange->getMeanVal();
// Generate "odometry obs":
CObservationOdometryPtr newO = CObservationOdometry::Create();
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) ) )
{
CObservationPtr o = CObservationPtr( 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.clear_unique();
}
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::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
}
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.
// ------------------------------------------------------------------------------
CSensoryFramePtr 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++)
{
CSerializablePtr obj = rawlog.getAsGeneric(i);
if (rawlog.getType(i)==CRawlog::etActionCollection)
{
// Accumulate Actions
// ----------------------
CActionCollectionPtr curActs = CActionCollectionPtr ( obj );
CActionRobotMovement2DPtr 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 = CSensoryFrame::Create();
// Copy pointers to observations only (fast):
accum_sf->moveFrom( *CSensoryFramePtr(obj) );
if ( ++SF_counter >= DECIMATE_RATIO )
{
SF_counter = 0;
// INSERT OBSERVATIONS:
newRawLog.addObservationsMemoryReference( accum_sf );
accum_sf.clear_unique();
// 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.clear_unique();
} // end for i each entry
// Clear the list only (objects already deleted)
rawlog.clearWithoutDelete();
// Copy as new log:
rawlog = newRawLog;
rebuildTreeView();
WX_END_TRY
}
// ------------------------------------------------------
// 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
}
/*---------------------------------------------------------------
process
---------------------------------------------------------------*/
void CDetectorDoorCrossing::process(
CActionRobotMovement2D &in_poseChange,
CSensoryFrame &in_sf,
TDoorCrossingOutParams &out_estimation
)
{
// Variables for generic use:
size_t i;
out_estimation.cumulativeTurning = 0;
MRPT_START
// 1) Add new pair to the list:
// -----------------------------------------
lastObs.addAction( in_poseChange );
lastObs.addObservations( in_sf );
// 2) Remove oldest pair:
// -----------------------------------------
ASSERT_( options.windowSize > 1 );
ASSERT_( (lastObs.size() % 2) == 0 ); // Assure even size
while (lastObs.size()>options.windowSize*2)
{
lastObs.remove(0);
lastObs.remove(0);
}
if ( lastObs.size() < options.windowSize * 2 )
{
// Not enought old data yet:
out_estimation.enoughtInformation = false;
return;
}
// 3) Build an occupancy grid map with observations
// -------------------------------------------------
CPose2D p, pos;
TSetOfMetricMapInitializers mapInitializer;
{
CSimplePointsMap::TMapDefinition def;
mapInitializer.push_back( def );
}
{
COccupancyGridMap2D::TMapDefinition def;
def.resolution = options.gridResolution;
mapInitializer.push_back( def );
}
CMultiMetricMap auxMap( &mapInitializer );
for (i=0;i<options.windowSize;i++)
{
CActionCollectionPtr acts = lastObs.getAsAction( i*2+0 );
CActionPtr act = acts->get(0);
ASSERT_( act->GetRuntimeClass()->derivedFrom( CLASS_ID( CActionRobotMovement2D ) ) )
CActionRobotMovement2DPtr action = CActionRobotMovement2DPtr( act );
action->poseChange->getMean(pos);
out_estimation.cumulativeTurning+= fabs(pos.phi());
// Get the cumulative pose for the current observation:
p = p + pos;
// Add SF to the grid map:
CSensoryFramePtr sf = lastObs.getAsObservations( i*2+1 );
CPose3D pose3D(p);
sf->insertObservationsInto( &auxMap, &pose3D );
}
// 4) Compute the information differece between this
// "map patch" and the previous one:
// -------------------------------------------------------
auxMap.m_gridMaps[0]->computeEntropy( entropy );
if (!lastEntropyValid)
{
out_estimation.enoughtInformation = false;
}
else
{
// 5) Fill output data
// ---------------------------------
out_estimation.enoughtInformation = true;
out_estimation.informationGain = entropy.I - lastEntropy.I;
out_estimation.pointsMap.copyFrom( *auxMap.m_pointsMaps[0] );
}
// For next iterations:
lastEntropy = entropy;
lastEntropyValid = true;
MRPT_END
}
void CRawlog::addAction( CAction &action )
{
CActionCollectionPtr temp = CActionCollection::Create();
temp->insert( action );
m_seqOfActObs.push_back( temp );
}
// ------------------------------------------------------
// 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
}
