mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-03 16:27:46 +08:00
Statistics: Updated "LocalLoop" to "Proximity"
This commit is contained in:
matlabbe
@@ -66,11 +66,11 @@ class RTABMAP_EXP Statistics
|
||||
RTABMAP_STATS(Loop, Optimization_error, );
|
||||
RTABMAP_STATS(Loop, Optimization_iterations, );
|
||||
|
||||
RTABMAP_STATS(LocalLoop, Time_closures,);
|
||||
RTABMAP_STATS(LocalLoop, Space_last_closure_id,);
|
||||
RTABMAP_STATS(LocalLoop, Space_paths,);
|
||||
RTABMAP_STATS(LocalLoop, Space_closures_added_visually,);
|
||||
RTABMAP_STATS(LocalLoop, Space_closures_added_icp_only,);
|
||||
RTABMAP_STATS(Proximity, Time_detections,);
|
||||
RTABMAP_STATS(Proximity, Space_last_detection_id,);
|
||||
RTABMAP_STATS(Proximity, Space_paths,);
|
||||
RTABMAP_STATS(Proximity, Space_detections_added_visually,);
|
||||
RTABMAP_STATS(Proximity, Space_detections_added_icp_only,);
|
||||
|
||||
RTABMAP_STATS(NeighborLinkRefining, Accepted,);
|
||||
RTABMAP_STATS(NeighborLinkRefining, Inliers,);
|
||||
@@ -144,7 +144,7 @@ public:
|
||||
void setExtended(bool extended) {_extended = extended;}
|
||||
void setRefImageId(int refImageId) {_refImageId = refImageId;}
|
||||
void setLoopClosureId(int loopClosureId) {_loopClosureId = loopClosureId;}
|
||||
void setLocalLoopClosureId(int localLoopClosureId) {_localLoopClosureId = localLoopClosureId;}
|
||||
void setProximityDetectionId(int id) {_proximiyDetectionId = id;}
|
||||
|
||||
void setSignatures(const std::map<int, Signature> & signatures) {_signatures = signatures;}
|
||||
|
||||
@@ -164,7 +164,7 @@ public:
|
||||
bool extended() const {return _extended;}
|
||||
int refImageId() const {return _refImageId;}
|
||||
int loopClosureId() const {return _loopClosureId;}
|
||||
int localLoopClosureId() const {return _localLoopClosureId;}
|
||||
int proximityDetectionId() const {return _proximiyDetectionId;}
|
||||
|
||||
const std::map<int, Signature> & getSignatures() const {return _signatures;}
|
||||
|
||||
@@ -187,7 +187,7 @@ private:
|
||||
|
||||
int _refImageId;
|
||||
int _loopClosureId;
|
||||
int _localLoopClosureId;
|
||||
int _proximiyDetectionId;
|
||||
|
||||
std::map<int, Signature> _signatures;
|
||||
|
||||
|
||||
+33
-35
@@ -377,6 +377,10 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
|
||||
|
||||
ULOGGER_DEBUG("");
|
||||
ParametersMap::const_iterator iter;
|
||||
if((iter=parameters.find(Parameters::kRtabmapWorkingDirectory())) != parameters.end())
|
||||
{
|
||||
this->setWorkingDirectory(iter->second.c_str());
|
||||
}
|
||||
|
||||
Parameters::parse(parameters, Parameters::kRtabmapPublishStats(), _publishStats);
|
||||
Parameters::parse(parameters, Parameters::kRtabmapPublishLastSignature(), _publishLastSignatureData);
|
||||
@@ -484,12 +488,6 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
|
||||
{
|
||||
_bayesFilter->parseParameters(parameters);
|
||||
}
|
||||
|
||||
// update working directory at the end
|
||||
if((iter=parameters.find(Parameters::kRtabmapWorkingDirectory())) != parameters.end())
|
||||
{
|
||||
this->setWorkingDirectory(iter->second.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
int Rtabmap::getLastLocationId() const
|
||||
@@ -846,7 +844,7 @@ bool Rtabmap::process(
|
||||
|
||||
std::map<int, int> childCount;
|
||||
std::set<int> signaturesRetrieved;
|
||||
int localLoopClosuresInTimeFound = 0;
|
||||
int proximityDetectionsInTimeFound = 0;
|
||||
|
||||
const Signature * signature = 0;
|
||||
const Signature * sLoop = 0;
|
||||
@@ -1192,7 +1190,7 @@ bool Rtabmap::process(
|
||||
UASSERT(info.variance > 0.0);
|
||||
if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, info.variance, info.variance)))
|
||||
{
|
||||
++localLoopClosuresInTimeFound;
|
||||
++proximityDetectionsInTimeFound;
|
||||
UINFO("Local loop closure found between %d and %d with t=%s",
|
||||
*iter, signature->id(), transform.prettyPrint().c_str());
|
||||
}
|
||||
@@ -1762,10 +1760,10 @@ bool Rtabmap::process(
|
||||
timeAddLoopClosureLink = timer.ticks();
|
||||
ULOGGER_INFO("timeAddLoopClosureLink=%fs", timeAddLoopClosureLink);
|
||||
|
||||
int localSpaceClosuresAddedVisually = 0;
|
||||
int localSpaceClosuresAddedByICPOnly = 0;
|
||||
int lastLocalSpaceClosureId = 0;
|
||||
int localSpacePaths = 0;
|
||||
int proximityDetectionsAddedVisually = 0;
|
||||
int proximityDetectionsAddedByICPOnly = 0;
|
||||
int lastProximitySpaceClosureId = 0;
|
||||
int proximitySpacePaths = 0;
|
||||
if(_proximityBySpace &&
|
||||
_localRadius > 0 &&
|
||||
_rgbdSlamMode &&
|
||||
@@ -1817,7 +1815,7 @@ bool Rtabmap::process(
|
||||
UDEBUG("nearestPaths=%d", (int)nearestPaths.size());
|
||||
|
||||
for(std::list<std::map<int, Transform> >::const_iterator iter=nearestPaths.begin();
|
||||
iter!=nearestPaths.end() && (_memory->isIncremental() || lastLocalSpaceClosureId == 0);
|
||||
iter!=nearestPaths.end() && (_memory->isIncremental() || lastProximitySpaceClosureId == 0);
|
||||
++iter)
|
||||
{
|
||||
std::map<int, Transform> path = *iter;
|
||||
@@ -1855,8 +1853,8 @@ bool Rtabmap::process(
|
||||
|
||||
if(_loopClosureHypothesis.first == 0)
|
||||
{
|
||||
++localSpaceClosuresAddedVisually;
|
||||
lastLocalSpaceClosureId = nearestId;
|
||||
++proximityDetectionsAddedVisually;
|
||||
lastProximitySpaceClosureId = nearestId;
|
||||
}
|
||||
}
|
||||
else
|
||||
@@ -1875,16 +1873,16 @@ bool Rtabmap::process(
|
||||
//
|
||||
UDEBUG("Proximity detection (local loop closure in SPACE with scan matching)");
|
||||
if( !signature->sensorData().laserScanCompressed().empty() &&
|
||||
(_memory->isIncremental() || lastLocalSpaceClosureId == 0))
|
||||
(_memory->isIncremental() || lastProximitySpaceClosureId == 0))
|
||||
{
|
||||
// In localization mode, no need to check local loop
|
||||
// closures if we are already localized by at least one
|
||||
// local visual closure above.
|
||||
|
||||
localSpacePaths = (int)nearestPaths.size();
|
||||
proximitySpacePaths = (int)nearestPaths.size();
|
||||
|
||||
for(std::list<std::map<int, Transform> >::iterator iter=nearestPaths.begin();
|
||||
iter!=nearestPaths.end() && (_memory->isIncremental() || lastLocalSpaceClosureId == 0);
|
||||
iter!=nearestPaths.end() && (_memory->isIncremental() || lastProximitySpaceClosureId == 0);
|
||||
++iter)
|
||||
{
|
||||
std::map<int, Transform> & path = *iter;
|
||||
@@ -1979,12 +1977,12 @@ bool Rtabmap::process(
|
||||
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, sqrtVar, sqrtVar, scanMatchingIds));
|
||||
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
|
||||
|
||||
++localSpaceClosuresAddedByICPOnly;
|
||||
++proximityDetectionsAddedByICPOnly;
|
||||
|
||||
// no local loop closure added visually
|
||||
if(localSpaceClosuresAddedVisually == 0 && _loopClosureHypothesis.first == 0)
|
||||
if(proximityDetectionsAddedVisually == 0 && _loopClosureHypothesis.first == 0)
|
||||
{
|
||||
lastLocalSpaceClosureId = nearestId;
|
||||
lastProximitySpaceClosureId = nearestId;
|
||||
}
|
||||
}
|
||||
else
|
||||
@@ -2043,9 +2041,9 @@ bool Rtabmap::process(
|
||||
int optimizationIterations = 0;
|
||||
if(_rgbdSlamMode &&
|
||||
(_loopClosureHypothesis.first>0 ||
|
||||
lastLocalSpaceClosureId>0 || // can be different map of the current one
|
||||
lastProximitySpaceClosureId>0 || // can be different map of the current one
|
||||
statistics_.reducedIds().size() ||
|
||||
localLoopClosuresInTimeFound>0 ||
|
||||
proximityDetectionsInTimeFound>0 ||
|
||||
((_memory->isIncremental() || signature->getLinks().size()) && // In localization mode, the new node should be linked
|
||||
signaturesRetrieved.size()))) // can be different map of the current one
|
||||
{
|
||||
@@ -2115,7 +2113,7 @@ bool Rtabmap::process(
|
||||
}
|
||||
updateConstraints = false;
|
||||
_loopClosureHypothesis.first = 0;
|
||||
lastLocalSpaceClosureId = 0;
|
||||
lastProximitySpaceClosureId = 0;
|
||||
rejectedHypothesis = true;
|
||||
}
|
||||
else if(_memory->isIncremental() && // FIXME: not tested in localization mode, so do it only in mapping mode
|
||||
@@ -2164,7 +2162,7 @@ bool Rtabmap::process(
|
||||
}
|
||||
updateConstraints = false;
|
||||
_loopClosureHypothesis.first = 0;
|
||||
lastLocalSpaceClosureId = 0;
|
||||
lastProximitySpaceClosureId = 0;
|
||||
rejectedHypothesis = true;
|
||||
}
|
||||
}
|
||||
@@ -2185,7 +2183,7 @@ bool Rtabmap::process(
|
||||
UERROR("Map correction should be identity when optimizing from the last node. T=%s", _mapCorrection.prettyPrint().c_str());
|
||||
}
|
||||
}
|
||||
_lastLocalizationNodeId = _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:lastLocalSpaceClosureId>0?lastLocalSpaceClosureId:_lastLocalizationNodeId;
|
||||
_lastLocalizationNodeId = _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:lastProximitySpaceClosureId>0?lastProximitySpaceClosureId:_lastLocalizationNodeId;
|
||||
|
||||
timeMapOptimization = timer.ticks();
|
||||
ULOGGER_INFO("timeMapOptimization=%fs", timeMapOptimization);
|
||||
@@ -2200,7 +2198,7 @@ bool Rtabmap::process(
|
||||
int lcHypothesisReactivated = 0;
|
||||
float rehearsalValue = uValue(statistics_.data(), Statistics::kMemoryRehearsal_sim(), 0.0f);
|
||||
int rehearsalMaxId = (int)uValue(statistics_.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
|
||||
sLoop = _memory->getSignature(_loopClosureHypothesis.first?_loopClosureHypothesis.first:lastLocalSpaceClosureId?lastLocalSpaceClosureId:_highestHypothesis.first);
|
||||
sLoop = _memory->getSignature(_loopClosureHypothesis.first?_loopClosureHypothesis.first:lastProximitySpaceClosureId?lastProximitySpaceClosureId:_highestHypothesis.first);
|
||||
if(sLoop)
|
||||
{
|
||||
lcHypothesisReactivated = sLoop->isSaved()?1.0f:0.0f;
|
||||
@@ -2240,13 +2238,13 @@ bool Rtabmap::process(
|
||||
statistics_.addStatistic(Statistics::kLoopOptimization_error(), optimizationError);
|
||||
statistics_.addStatistic(Statistics::kLoopOptimization_iterations(), optimizationIterations);
|
||||
|
||||
statistics_.addStatistic(Statistics::kLocalLoopTime_closures(), localLoopClosuresInTimeFound);
|
||||
statistics_.addStatistic(Statistics::kLocalLoopSpace_closures_added_visually(), localSpaceClosuresAddedVisually);
|
||||
statistics_.addStatistic(Statistics::kLocalLoopSpace_closures_added_icp_only(), localSpaceClosuresAddedByICPOnly);
|
||||
statistics_.addStatistic(Statistics::kLocalLoopSpace_paths(), localSpacePaths);
|
||||
statistics_.addStatistic(Statistics::kLocalLoopSpace_last_closure_id(), lastLocalSpaceClosureId);
|
||||
statistics_.setLocalLoopClosureId(lastLocalSpaceClosureId);
|
||||
if(_loopClosureHypothesis.first || lastLocalSpaceClosureId)
|
||||
statistics_.addStatistic(Statistics::kProximityTime_detections(), proximityDetectionsInTimeFound);
|
||||
statistics_.addStatistic(Statistics::kProximitySpace_detections_added_visually(), proximityDetectionsAddedVisually);
|
||||
statistics_.addStatistic(Statistics::kProximitySpace_detections_added_icp_only(), proximityDetectionsAddedByICPOnly);
|
||||
statistics_.addStatistic(Statistics::kProximitySpace_paths(), proximitySpacePaths);
|
||||
statistics_.addStatistic(Statistics::kProximitySpace_last_detection_id(), lastProximitySpaceClosureId);
|
||||
statistics_.setProximityDetectionId(lastProximitySpaceClosureId);
|
||||
if(_loopClosureHypothesis.first || lastProximitySpaceClosureId)
|
||||
{
|
||||
UASSERT(uContains(sLoop->getLinks(), signature->id()));
|
||||
UINFO("Set loop closure transform = %s", sLoop->getLinks().at(signature->id()).transform().prettyPrint().c_str());
|
||||
@@ -2343,7 +2341,7 @@ bool Rtabmap::process(
|
||||
signaturesRemoved.push_back(signature->id());
|
||||
_memory->deleteLocation(signature->id());
|
||||
}
|
||||
else if(smallDisplacement && _loopClosureHypothesis.first == 0 && lastLocalSpaceClosureId == 0)
|
||||
else if(smallDisplacement && _loopClosureHypothesis.first == 0 && lastProximitySpaceClosureId == 0)
|
||||
{
|
||||
// Don't delete the location if a loop closure is detected
|
||||
UINFO("Ignoring location %d because the displacement is too small! (d=%f a=%f)",
|
||||
|
||||
@@ -42,7 +42,7 @@ Statistics::Statistics() :
|
||||
_extended(0),
|
||||
_refImageId(0),
|
||||
_loopClosureId(0),
|
||||
_localLoopClosureId(0),
|
||||
_proximiyDetectionId(0),
|
||||
_currentGoalId(0)
|
||||
{
|
||||
_defaultDataInitialized = true;
|
||||
|
||||
+11
-11
@@ -664,7 +664,7 @@ void MainWindow::handleEvent(UEvent* anEvent)
|
||||
RtabmapEvent * rtabmapEvent = (RtabmapEvent*)anEvent;
|
||||
Statistics stats = rtabmapEvent->getStats();
|
||||
int highestHypothesisId = int(uValue(stats.data(), Statistics::kLoopHighest_hypothesis_id(), 0.0f));
|
||||
int localLoopClosureId = int(uValue(stats.data(), Statistics::kLocalLoopSpace_last_closure_id(), 0.0f));
|
||||
int proximityClosureId = int(uValue(stats.data(), Statistics::kProximitySpace_last_detection_id(), 0.0f));
|
||||
bool rejectedHyp = bool(uValue(stats.data(), Statistics::kLoopRejectedHypothesis(), 0.0f));
|
||||
float highestHypothesisValue = uValue(stats.data(), Statistics::kLoopHighest_hypothesis_value(), 0.0f);
|
||||
if((stats.loopClosureId() > 0 &&
|
||||
@@ -676,7 +676,7 @@ void MainWindow::handleEvent(UEvent* anEvent)
|
||||
_ui->actionPause_when_a_loop_hypothesis_is_rejected->isChecked() &&
|
||||
rejectedHyp)
|
||||
||
|
||||
(localLoopClosureId > 0 &&
|
||||
(proximityClosureId > 0 &&
|
||||
_ui->actionPause_on_local_loop_detection->isChecked()))
|
||||
{
|
||||
if(_state != kPaused && _state != kMonitoringPaused && !_processingDownloadedMap)
|
||||
@@ -1219,7 +1219,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
|
||||
refMapId = stat.getSignatures().at(stat.refImageId()).mapId();
|
||||
}
|
||||
int highestHypothesisId = static_cast<float>(uValue(stat.data(), Statistics::kLoopHighest_hypothesis_id(), 0.0f));
|
||||
int loopId = stat.loopClosureId()>0?stat.loopClosureId():stat.localLoopClosureId()>0?stat.localLoopClosureId():highestHypothesisId;
|
||||
int loopId = stat.loopClosureId()>0?stat.loopClosureId():stat.proximityDetectionId()>0?stat.proximityDetectionId():highestHypothesisId;
|
||||
if(_cachedSignatures.contains(loopId))
|
||||
{
|
||||
loopMapId = _cachedSignatures.value(loopId).mapId();
|
||||
@@ -1261,7 +1261,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
|
||||
|
||||
int rehearsalMerged = (int)uValue(stat.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
|
||||
bool rehearsedSimilarity = (float)uValue(stat.data(), Statistics::kMemoryRehearsal_id(), 0.0f) != 0.0f;
|
||||
int localTimeClosures = (int)uValue(stat.data(), Statistics::kLocalLoopTime_closures(), 0.0f);
|
||||
int proximityTimeDetections = (int)uValue(stat.data(), Statistics::kProximityTime_detections(), 0.0f);
|
||||
bool scanMatchingSuccess = (bool)uValue(stat.data(), Statistics::kNeighborLinkRefiningAccepted(), 0.0f);
|
||||
bool smallMovement = (bool)uValue(stat.data(), Statistics::kMemorySmall_movement(), 0.0f);
|
||||
_ui->label_stats_imageNumber->setText(QString("%1 [%2]").arg(stat.refImageId()).arg(refMapId));
|
||||
@@ -1270,7 +1270,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
|
||||
{
|
||||
_ui->imageView_source->setBackgroundColor(Qt::blue);
|
||||
}
|
||||
else if(localTimeClosures > 0)
|
||||
else if(proximityTimeDetections > 0)
|
||||
{
|
||||
_ui->imageView_source->setBackgroundColor(Qt::darkYellow);
|
||||
}
|
||||
@@ -1312,7 +1312,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
|
||||
int matchId = 0;
|
||||
Signature loopSignature;
|
||||
int shownLoopId = 0;
|
||||
if(highestHypothesisId > 0 || stat.localLoopClosureId()>0)
|
||||
if(highestHypothesisId > 0 || stat.proximityDetectionId()>0)
|
||||
{
|
||||
bool show = true;
|
||||
if(stat.loopClosureId() > 0)
|
||||
@@ -1326,11 +1326,11 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
|
||||
_ui->label_matchId->setText(QString("Match ID = %1 [%2]").arg(stat.loopClosureId()).arg(loopMapId));
|
||||
matchId = stat.loopClosureId();
|
||||
}
|
||||
else if(stat.localLoopClosureId())
|
||||
else if(stat.proximityDetectionId())
|
||||
{
|
||||
_ui->imageView_loopClosure->setBackgroundColor(Qt::yellow);
|
||||
_ui->label_matchId->setText(QString("Local match = %1 [%2]").arg(stat.localLoopClosureId()).arg(loopMapId));
|
||||
matchId = stat.localLoopClosureId();
|
||||
_ui->label_matchId->setText(QString("Local match = %1 [%2]").arg(stat.proximityDetectionId()).arg(loopMapId));
|
||||
matchId = stat.proximityDetectionId();
|
||||
}
|
||||
else if(rejectedHyp && highestHypothesisValue >= _preferencesDialog->getLoopThr())
|
||||
{
|
||||
@@ -1353,7 +1353,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
|
||||
|
||||
if(show)
|
||||
{
|
||||
shownLoopId = stat.loopClosureId()>0?stat.loopClosureId():stat.localLoopClosureId()>0?stat.localLoopClosureId():highestHypothesisId;
|
||||
shownLoopId = stat.loopClosureId()>0?stat.loopClosureId():stat.proximityDetectionId()>0?stat.proximityDetectionId():highestHypothesisId;
|
||||
QMap<int, Signature>::iterator iter = _cachedSignatures.find(shownLoopId);
|
||||
if(iter != _cachedSignatures.end())
|
||||
{
|
||||
@@ -1493,7 +1493,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
|
||||
_ui->statsToolBox->updateStat("GUI/RGB-D cloud/ms", stat.refImageId(), int(timerVis.elapsed()*1000.0f));
|
||||
|
||||
// loop closure view
|
||||
if((stat.loopClosureId() > 0 || stat.localLoopClosureId() > 0) &&
|
||||
if((stat.loopClosureId() > 0 || stat.proximityDetectionId() > 0) &&
|
||||
!stat.loopClosureTransform().isNull() &&
|
||||
!loopSignature.sensorData().imageRaw().empty())
|
||||
{
|
||||
|
||||
Reference in New Issue
Block a user