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_++;
}
void CFormPlayVideo::OnprogressBarCmdScrollChanged(wxScrollEvent& event)
{
int idx = progressBar->GetValue();
m_idxInRawlog = idx;
if (idx>0 && idx<(int)rawlog.size())
{
if (rawlog.getType(idx)==CRawlog::etSensoryFrame)
{
size_t dummy = 0;
CSensoryFramePtr sf = rawlog.getAsObservations(idx);
showSensoryFrame(sf.pointer(), dummy);
edIndex->SetValue(idx);
}
else if (rawlog.getType(idx)==CRawlog::etObservation)
{
size_t dummy = 0;
CObservationPtr o = rawlog.getAsObservation(idx);
CSensoryFrame sf;
sf.insert(o);
showSensoryFrame( &sf, dummy);
edIndex->SetValue(idx);
}
}
}
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_);
}
}
}
/*---------------------------------------------------------------
pushObservation
---------------------------------------------------------------*/
void CHMTSLAM::pushObservation( const CObservationPtr &obs )
{
if (m_terminateThreads)
{ // Discard it:
//delete obs;
return;
}
// Add a CSensoryFrame with the obs:
CSensoryFramePtr sf = CSensoryFrame::Create();
sf->insert(obs); // memory will be freed when deleting the SF in other thread
{ // Wait for critical section
CCriticalSectionLocker locker( &m_inputQueue_cs);
m_inputQueue.push( sf );
}
}
// Get the rawlog entry (from cur. loaded rawlog), build and displays its map:
void CScanAnimation::RebuildMaps()
{
WX_START_TRY
int idx = slPos->GetValue();
if ( idx>=0 && idx<(int)rawlog.size() )
{
if (rawlog.getType(idx)==CRawlog::etSensoryFrame)
{
CSensoryFramePtr sf = rawlog.getAsObservations(idx);
BuildMapAndRefresh(sf.pointer());
}
else
if (rawlog.getType(idx)==CRawlog::etObservation)
{
CSensoryFramePtr sf = CSensoryFrame::Create();
sf->insert( rawlog.getAsObservation(idx) );
BuildMapAndRefresh(sf.pointer());
}
}
WX_END_TRY
}
// ------------------------------------------------------
// 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
}
/*---------------------------------------------------------------
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
}
/*---------------------------------------------------------------
addMapFrame
---------------------------------------------------------------*/
unsigned int CIncrementalMapPartitioner::addMapFrame(
const CSensoryFramePtr &frame,
const CPose3DPDFPtr &robotPose )
{
size_t i=0,j=0,n=0;
CPose3D pose_i, pose_j, relPose;
CPose3DPDFPtr posePDF_i, posePDF_j;
CSensoryFramePtr sf_i, sf_j;
CMultiMetricMap *map_i=NULL,*map_j=NULL;
int debug_CheckPoint = 0;
mrpt::utils::TMatchingPairList corrs;
static CPose3D nullPose(0,0,0);
// Set options in graph partition:
CGraphPartitioner::DEBUG_SAVE_EIGENVECTOR_FILES = options.debugSaveAllEigenvectors;
// Create the maps:
TSetOfMetricMapInitializers mapInitializer;
TMetricMapInitializer mapElement;
mapElement.metricMapClassType = CLASS_ID( CSimplePointsMap );
mapInitializer.push_back( mapElement );
// mapElement.metricMapClassType = CLASS_ID( COccupancyGridMap2D );
// mapElement.occupancyGridMap2D_options.resolution = options.gridResolution;
// mapInitializer.push_back( mapElement );
mapElement.metricMapClassType = CLASS_ID( CLandmarksMap );
// mapElement.landmarksMap_options.insertionOpts.
mapInitializer.push_back( mapElement );
// Add new metric map to "m_individualMaps"
// --------------------------------------------
//CMultiMetricMap *newMetricMap = new CMultiMetricMap( &mapInitializer );
m_individualMaps.push_back( CMultiMetricMap() );
CMultiMetricMap &newMetricMap = m_individualMaps.back();
newMetricMap.setListOfMaps( &mapInitializer );
MRPT_START
// Create the metric map:
// -----------------------------------------------------------------
ASSERT_(newMetricMap.m_pointsMaps.size()>0);
newMetricMap.m_pointsMaps[0]->insertionOptions.isPlanarMap = false; // true
newMetricMap.m_pointsMaps[0]->insertionOptions.minDistBetweenLaserPoints = 0.20f;
options.minDistForCorrespondence = max(options.minDistForCorrespondence,1.3f*newMetricMap.m_pointsMaps[0]->insertionOptions.minDistBetweenLaserPoints);
// JLBC,17/AGO/2006: "m_individualMaps" were created from the robot pose, but it is
// more convenient now to save them as the robot being at (0,0,0).
//frame->insertObservationsInto( newMetricMap.m_pointsMaps[0] );
newMetricMap.m_pointsMaps[0]->copyFrom( * frame->buildAuxPointsMap<CPointsMap>(&newMetricMap.m_pointsMaps[0]->insertionOptions)); // Faster :-)
// Insert just the VisualLandmarkObservations:
newMetricMap.m_landmarksMap->insertionOptions.insert_SIFTs_from_monocular_images = false;
newMetricMap.m_landmarksMap->insertionOptions.insert_SIFTs_from_stereo_images = false;
newMetricMap.m_landmarksMap->insertionOptions.insert_Landmarks_from_range_scans = false;
frame->insertObservationsInto( newMetricMap.m_landmarksMap );
debug_CheckPoint=1;
// Add to corresponding vectors:
m_individualFrames.insert(robotPose, frame);
// Already added to "m_individualMaps" above
debug_CheckPoint=2;
// Ampliar la matriz de adyacencias:
// -----------------------------------------------------------------
n = m_A.getColCount();
n++;
m_A.setSize(n,n);
debug_CheckPoint=3;
ASSERT_(m_individualMaps.size() == n);
ASSERT_(m_individualFrames.size() == n);
// Ajustar tamaño del vector de nodos modificados:
// ---------------------------------------------------
// El nuevo evidentemente debe ser tenido en cuenta:
m_modified_nodes.push_back(true);
// Methods to compute adjacency matrix:
// true: matching between maps
// false: matching between observations through "CObservation::likelihoodWith"
// ------------------------------------------------------------------------------
bool useMapOrSF = options.useMapMatching;
debug_CheckPoint=4;
// Calcular los nuevos matchings y meterlos en la matriz:
// ----------------------------------------------------------------
//for (i=n-1;i<n;i++)
i=n-1; // Solo ejecutar para "i=n-1" la ultima fila/columna que esta vacia
{
// Get node "i":
m_individualFrames.get(i, posePDF_i, sf_i);
posePDF_i->getMean(pose_i);
// And its points map:
map_i = &m_individualMaps[i];
debug_CheckPoint=5;
// for (j=0;j<n;j++)
for (j=0;j<n-1;j++)
{
// Get node "j":
m_individualFrames.get(j, posePDF_j, sf_j);
posePDF_j->getMean( pose_j );
debug_CheckPoint=6;
relPose = pose_j - pose_i;
// And its points map:
map_j = &m_individualMaps[j];
debug_CheckPoint=66;
// Compute matching ratio:
if (useMapOrSF)
{
m_A(i,j) = map_i->compute3DMatchingRatio(
map_j,
relPose,
options.minDistForCorrespondence,
options.minMahaDistForCorrespondence );
}
else
{
//m_A(i,j) = sf_i->likelihoodWith(sf_j.pointer());
m_A(i,j) = observationsOverlap(sf_i, sf_j, &relPose );
}
} // for j
} // for i
for (i=0;i<n-1;i++) // Solo ejecutar para "i=n-1" la ultima fila/columna que esta vacia
{
debug_CheckPoint=8;
// Get node "i":
m_individualFrames.get(i, posePDF_i, sf_i);
posePDF_i->getMean(pose_i);
// And its points map:
map_i = &m_individualMaps[i];
debug_CheckPoint=9;
j=n-1; //for (j=n-1;j<n;j++)
{
// Get node "j":
m_individualFrames.get(j, posePDF_j, sf_j);
posePDF_j->getMean(pose_j);
debug_CheckPoint=10;
relPose = pose_j - pose_i;
// And its points map:
map_j = &m_individualMaps[j];
// Compute matching ratio:
if (useMapOrSF)
{
m_A(i,j) = map_i->compute3DMatchingRatio(
map_j,
CPose3D(relPose),
options.minDistForCorrespondence,
options.minMahaDistForCorrespondence );
}
else
{
//m_A(i,j) = sf_i->likelihoodWith(sf_j.pointer());
m_A(i,j) = observationsOverlap(sf_i, sf_j, &relPose );
}
debug_CheckPoint=12;
} // for j
} // for i
debug_CheckPoint=13;
// Self-similatity: Not used
m_A(n-1,n-1) = 0;
// Hacer que la matriz sea simetrica:
// -----------------------------------------------------------------
for (i=0;i<n;i++)
for (j=i+1;j<n;j++)
m_A(i,j) = m_A(j,i) = 0.5f * (m_A(i,j) + m_A(j,i) );
debug_CheckPoint=14;
/* DEBUG: Guardar la matriz: * /
A.saveToTextFile("debug_matriz.txt",1);
/ **/
// Añadir a la lista de nodos modificados los nodos afectados:
// -----------------------------------------------------------------
for (i=0;i<n;i++)
m_modified_nodes[i] = m_A(i,n-1) > 0;
debug_CheckPoint=15;
if (m_last_last_partition_are_new_ones)
{
// Insert into the "new_ones" partition:
m_last_partition[m_last_partition.size()-1].push_back( n-1 );
}
else
{
// Add a new partition:
vector_uint dummyPart;
dummyPart.push_back(n-1);
m_last_partition.push_back( dummyPart );
// The last one is the new_ones partition:
m_last_last_partition_are_new_ones = true;
}
return n-1; // Index of the new node
MRPT_END_WITH_CLEAN_UP( \
cout << "Unexpected runtime error at checkPoint="<< debug_CheckPoint << "\n"; \
cout << "\tn=" << n << "\n"; \
cout << "\ti=" << i << "\n"; \
cout << "\tj=" << j << "\n"; \
cout << "\tmap_i=" << map_i << "\n"; \
cout << "\tmap_j=" << map_j << "\n"; \
cout << "relPose: "<< relPose << endl; \
cout << "map_i.size()=" << map_i->m_pointsMaps[0]->size() << "\n"; \
cout << "map_j.size()=" << map_j->m_pointsMaps[0]->size() << "\n"; \
map_i->m_pointsMaps[0]->save2D_to_text_file(string("debug_DUMP_map_i.txt")); \
map_j->m_pointsMaps[0]->save2D_to_text_file(string("debug_DUMP_map_j.txt")); \
m_A.saveToTextFile("debug_DUMP_exception_A.txt"); \
);
void xRawLogViewerFrame::OnMenuDistanceBtwGPSs(wxCommandEvent& event)
{
WX_START_TRY
wxMessageBox(_("It will be measured the distance between two GPSs, assuming they are fixed on the vehicle,\n and using only RTK fixed observations."));
if (listOfSensorLabels.empty())
{
wxMessageBox(_("No sensors were found with proper sensor labels. Labels are required for this operation."));
return;
}
// List of labels:
wxArrayString lstLabels;
for (std::map<std::string,TInfoPerSensorLabel>::iterator i=listOfSensorLabels.begin();i!=listOfSensorLabels.end();++i)
lstLabels.Add( _U( i->first.c_str() ) );
wxString ret = wxGetSingleChoice(
_("Choose the first GPS:"),
_("Sensor Labels"),
lstLabels,
this );
if (ret.IsEmpty()) return;
string gps1 = string(ret.mb_str());
ret = wxGetSingleChoice(
_("Choose the second GPS:"),
_("Sensor Labels"),
lstLabels,
this );
if (ret.IsEmpty()) return;
string gps2 = string(ret.mb_str());
size_t i, n = rawlog.size();
// Look for the 2 observations:
CObservationGPSPtr last_GPS1, last_GPS2;
vector_double dists;
TGeodeticCoords refCoords(0,0,0);
// Load configuration block:
CConfigFileMemory memFil;
rawlog.getCommentTextAsConfigFile(memFil);
refCoords.lat = memFil.read_double("GPS_ORIGIN","lat_deg",0);
refCoords.lon = memFil.read_double("GPS_ORIGIN","lon_deg",0);
refCoords.height = memFil.read_double("GPS_ORIGIN","height",0);
bool ref_valid = !refCoords.isClear();
for (i=0;i<n;i++)
{
switch ( rawlog.getType(i) )
{
case CRawlog::etSensoryFrame:
{
CSensoryFramePtr sf = rawlog.getAsObservations(i);
if (!ref_valid)
{
CObservationGPSPtr o = sf->getObservationByClass<CObservationGPS>();
if (o && o->has_GGA_datum)
{
refCoords = o->GGA_datum.getAsStruct<TGeodeticCoords>();
ref_valid = true;
}
}
CObservationPtr o1 = sf->getObservationBySensorLabel(gps1);
CObservationPtr o2 = sf->getObservationBySensorLabel(gps2);
if (o1)
{
ASSERT_(o1->GetRuntimeClass()==CLASS_ID(CObservationGPS));
CObservationGPSPtr obs = CObservationGPSPtr(o1);
if (obs->has_GGA_datum && obs->GGA_datum.fix_quality==4)
last_GPS1 = obs;
}
if (o2)
{
ASSERT_(o2->GetRuntimeClass()==CLASS_ID(CObservationGPS));
CObservationGPSPtr obs = CObservationGPSPtr(o2);
if (obs->has_GGA_datum && obs->GGA_datum.fix_quality==4)
last_GPS2 = obs;
}
}
break;
case CRawlog::etObservation:
{
CObservationPtr o = rawlog.getAsObservation(i);
if (!ref_valid && IS_CLASS(o,CObservationGPS))
{
CObservationGPSPtr ob = CObservationGPSPtr(o);
if (ob && ob->has_GGA_datum)
{
refCoords = ob->GGA_datum.getAsStruct<TGeodeticCoords>();
ref_valid = true;
}
}
if (o->sensorLabel == gps1)
{
ASSERT_(IS_CLASS(o,CObservationGPS));
CObservationGPSPtr obs = CObservationGPSPtr(o);
if (obs->has_GGA_datum && obs->GGA_datum.fix_quality==4)
last_GPS1 = obs;
}
if (o->sensorLabel == gps2)
{
ASSERT_(IS_CLASS(o,CObservationGPS));
CObservationGPSPtr obs = CObservationGPSPtr(o);
if (obs->has_GGA_datum && obs->GGA_datum.fix_quality==4)
last_GPS2 = obs;
}
}
break;
default:
break;
} // end switch type
// Now check if we have 2 gps with the same time stamp:
if (last_GPS1 && last_GPS2)
{
if (last_GPS1->GGA_datum.UTCTime == last_GPS2->GGA_datum.UTCTime)
{
// Compute distance:
TPoint3D p1;
mrpt::topography::geodeticToENU_WGS84(
last_GPS1->GGA_datum.getAsStruct<TGeodeticCoords>(),
p1,
refCoords);
TPoint3D p2;
mrpt::topography::geodeticToENU_WGS84(
last_GPS2->GGA_datum.getAsStruct<TGeodeticCoords>(),
p2,
refCoords);
// Fix offset:
p1.x += memFil.read_double( string("OFFSET_")+last_GPS1->sensorLabel, "x", 0 );
p1.y += memFil.read_double( string("OFFSET_")+last_GPS1->sensorLabel, "y", 0 );
p1.z += memFil.read_double( string("OFFSET_")+last_GPS1->sensorLabel, "z", 0 );
p2.x += memFil.read_double( string("OFFSET_")+last_GPS2->sensorLabel, "x", 0 );
p2.y += memFil.read_double( string("OFFSET_")+last_GPS2->sensorLabel, "y", 0 );
p2.z += memFil.read_double( string("OFFSET_")+last_GPS2->sensorLabel, "z", 0 );
double d = mrpt::math::distance(p1,p2);
dists.push_back(d);
last_GPS1.clear_unique();
last_GPS2.clear_unique();
}
}
} // end for
if (dists.empty())
{
wxMessageBox(_("No valid GPS observations were found."),_("Done"),wxOK,this);
}
else
{
double d_mean,d_std;
mrpt::math::meanAndStd(dists,d_mean,d_std);
wxMessageBox(_U(
format("The distance between GPS sensors is %.04fm, with\n a sigma=%.04fm, average from %u entries.",
d_mean,d_std, (unsigned)dists.size()).c_str() ),_("Done"),wxOK,this);
}
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();
}
void xRawLogViewerFrame::OnMenuShiftTimestampsByLabel(wxCommandEvent& event)
{
WX_START_TRY
wxMessageBox(_("The timestamps of all the observations of a given sensor label will be shifted a given number of seconds. Press OK to continue."));
vector_string the_labels = AskForObservationByLabelMultiple("Choose the sensor(s):");
if (the_labels.empty()) return;
wxString s = wxGetTextFromUser(
_("Enter the number of seconds to shift (can have fractions, be positive or negative)"),
_("Timestamp shift"),
_("0.0"), this );
if (s.IsEmpty()) return;
double delta_time_secs;
if (!s.ToDouble(&delta_time_secs))
{
wxMessageBox(_("Invalid number"));
return;
}
size_t i,n = rawlog.size();
unsigned int nChanges = 0;
TTimeStamp DeltaTime = mrpt::system::secondsToTimestamp( delta_time_secs );
for (i=0;i<n;i++)
{
switch ( rawlog.getType(i) )
{
case CRawlog::etSensoryFrame:
{
CSensoryFramePtr sf = rawlog.getAsObservations(i);
CObservationPtr o;
for (size_t k=0;k<the_labels.size();k++)
{
size_t idx = 0;
while ( (o=sf->getObservationBySensorLabel(the_labels[k],idx++)).present() )
{
o->timestamp += DeltaTime;
nChanges++;
}
}
}
break;
case CRawlog::etObservation:
{
CObservationPtr o = rawlog.getAsObservation(i);
for (size_t k=0;k<the_labels.size();k++)
if (o->sensorLabel==the_labels[k])
{
o->timestamp += DeltaTime;
nChanges++;
}
}
break;
default:
break;
} // end switch type
} // end for
if (wxYES==wxMessageBox( wxString::Format(_("%u changes. Do you want to re-order by timestamp?"),nChanges), _("Done"),wxYES_NO, this ))
{
OnMenuResortByTimestamp(event);
}
WX_END_TRY
}
/* ------------------------------------------------------------
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);
}
void xRawLogViewerFrame::OnMenuRegenerateGPSTimestamps(wxCommandEvent& event)
{
WX_START_TRY
wxBusyCursor waitCursor;
vector_string the_labels = AskForObservationByLabelMultiple("Choose the GPS(s) to consider:");
vector_double time_changes; // all the shifts
std::map<std::string,double> DeltaTimes;
// First, build an ordered list of "times"->"indexes":
// ------------------------------------------------------
std::map<TTimeStamp,size_t> ordered_times;
size_t i, n = rawlog.size();
for (i=0;i<n;i++)
{
switch ( rawlog.getType(i) )
{
default:
wxMessageBox(_("Error: this command is for rawlogs without sensory frames."));
return;
break;
case CRawlog::etSensoryFrame:
{
CSensoryFramePtr sf = rawlog.getAsObservations(i);
for (CSensoryFrame::iterator it=sf->begin();it!=sf->end();++it)
{
if ( (*it)->GetRuntimeClass()==CLASS_ID(CObservationGPS) && find_in_vector( (*it)->sensorLabel, the_labels)!=string::npos )
{
CObservationGPSPtr obs = CObservationGPSPtr(*it);
fixGPStimestamp(obs, time_changes, DeltaTimes);
}
}
}
break;
case CRawlog::etObservation:
{
CObservationPtr o = rawlog.getAsObservation(i);
if (IS_CLASS(o,CObservationGPS) && find_in_vector( o->sensorLabel, the_labels)!=string::npos)
{
CObservationGPSPtr obs = CObservationGPSPtr(o);
fixGPStimestamp(obs, time_changes, DeltaTimes);
}
}
break;
} // end switch type
} // end for i
unsigned int nChanges = time_changes.size();
double average_time_change = 0, std_time_change = 0;
mrpt::math::meanAndStd(time_changes,average_time_change, std_time_change );
if (wxYES==wxMessageBox( wxString::Format(_("%u changes, average/std time shift is: %f/%f sec. Do you want to re-order by timestamp?"),nChanges, average_time_change, std_time_change ), _("Done"),wxYES_NO, this ))
{
OnMenuResortByTimestamp(event);
}
WX_END_TRY
}
// Show GPS path in a window:
void xRawLogViewerFrame::OnMenuDrawGPSPath(wxCommandEvent& event)
{
WX_START_TRY
string the_label = AskForObservationByLabel("Choose the GPS to use:");
size_t i, M = 0, n = rawlog.size();
TGeodeticCoords ref;
bool ref_valid = false;
// Ask the user for the reference?
if (wxYES!=wxMessageBox(_("Do you want to take the GPS reference automatically from the first found entry?"),_("GPS path"),wxYES_NO ))
{
wxString s = wxGetTextFromUser(
_("Reference Latitude (degrees):"),
_("GPS reference"),
_("0.0"), this );
if (s.IsEmpty()) return;
if (!s.ToDouble(&ref.lat.decimal_value)) { wxMessageBox(_("Invalid number")); return; }
s = wxGetTextFromUser(
_("Reference Longitude (degrees):"),
_("GPS reference"),
_("0.0"), this );
if (s.IsEmpty()) return;
if (!s.ToDouble(&ref.lon.decimal_value)) { wxMessageBox(_("Invalid number")); return; }
s = wxGetTextFromUser(
_("Reference Height (meters):"),
_("GPS reference"),
_("0.0"), this );
if (s.IsEmpty()) return;
if (!s.ToDouble(&ref.height)) { wxMessageBox(_("Invalid number")); return; }
ref_valid=true;
}
// Only RTK fixed?
bool only_rtk = wxYES==wxMessageBox(_("Take into account 'rtk' (modes 4-5) readings only?"),_("GPS path"),wxYES_NO );
vector<float> xs,ys,zs;
double overall_distance = 0;
for (i=0;i<n;i++)
{
CObservationGPSPtr obs;
switch ( rawlog.getType(i) )
{
case CRawlog::etSensoryFrame:
{
CSensoryFramePtr sf = rawlog.getAsObservations(i);
CObservationPtr o= sf->getObservationBySensorLabel(the_label);
if (o && IS_CLASS(o,CObservationGPS))
{
obs = CObservationGPSPtr(o);
}
}
break;
case CRawlog::etObservation:
{
CObservationPtr o = rawlog.getAsObservation(i);
if ( !os::_strcmpi(o->sensorLabel.c_str(), the_label.c_str()) && IS_CLASS(o,CObservationGPS))
{
obs = CObservationGPSPtr(o);
}
}
break;
default: break;
}
// If we had a GPS obs, process it:
if (obs && obs->has_GGA_datum && (!only_rtk || obs->GGA_datum.fix_quality==4 || obs->GGA_datum.fix_quality==5) )
{
TPoint3D X_ENU; // Transformed coordinates
const TGeodeticCoords obsCoords = obs->GGA_datum.getAsStruct<TGeodeticCoords>();
// The first gps datum?
if (!ref_valid)
{
ref_valid=true;
ref = obsCoords;
}
// Local XYZ coordinates transform:
geodeticToENU_WGS84( obsCoords, X_ENU, ref );
// Geocentric XYZ:
TPoint3D X_geo;
geodeticToGeocentric_WGS84( obsCoords, X_geo);
if (!xs.empty())
overall_distance+=sqrt( square(X_ENU.x-*xs.rbegin())+square(X_ENU.y-*ys.rbegin())+square(X_ENU.z-*zs.rbegin()) );
xs.push_back(X_ENU.x);
ys.push_back(X_ENU.y);
zs.push_back(X_ENU.z);
M++;
}
}
// Window 3d:
winGPSPath = CDisplayWindow3D::Create(format("GPS path, %i points (%s) %.03f meters length", int(M), the_label.c_str(), overall_distance ) );
COpenGLScene scene;
CPointCloudPtr gl_path = CPointCloud::Create();
gl_path->setAllPoints(xs,ys,zs);
gl_path->setColor(0,0,1);
gl_path->setPointSize(3);
scene.insert( gl_path );
scene.insert( CGridPlaneXYPtr( CGridPlaneXY::Create(-300,300,-300,300,0,10)));
scene.insert( CAxisPtr( CAxis::Create(-300,-300,-50, 300,300,50, 1.0, 3, true ) ) );
COpenGLScenePtr the_scene = winGPSPath->get3DSceneAndLock();
*the_scene = scene;
winGPSPath->unlockAccess3DScene();
winGPSPath->repaint();
// 2D wins:
winGPSPath2D_xy = CDisplayWindowPlots::Create( format("GPS path - XY (%s)", the_label.c_str() ) );
winGPSPath2D_xy->plot(xs,ys,"b");
winGPSPath2D_xy->axis_fit(true);
winGPSPath2D_xz = CDisplayWindowPlots::Create( format("GPS path - XZ (%s)", the_label.c_str() ) );
winGPSPath2D_xz->plot(xs,zs,"b");
winGPSPath2D_xz->axis_fit(true);
WX_END_TRY
}
/*---------------------------------------------------------------
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 RBPFSlamCore::updateFilter(CActionCollectionPtr _action, CSensoryFramePtr _sf) {
if(state_ == INIT) initializeFilter();
tictac_.Tic();
timeLastUpdate_ = _sf->getObservationByIndex(0)->timestamp;
update_counter_++;
}
// ------------------------------------------------------
// 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
}
