From ae7436a41ba53d8bac31670ef352d7c1bd99d9b5 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 12 Aug 2016 18:43:12 -0400 Subject: [PATCH] Fixed bug where very long paths (over maxGraphDepth) were used for scan matching. Added "RGBD/ProximityMaxPaths" parameter (default 3) to limit the number of path comparisons for proximity detection. --- corelib/include/rtabmap/core/Parameters.h | 3 +- corelib/include/rtabmap/core/Rtabmap.h | 3 +- corelib/include/rtabmap/core/Statistics.h | 2 + corelib/src/Graph.cpp | 22 ++-- corelib/src/RegistrationIcp.cpp | 6 +- corelib/src/Rtabmap.cpp | 81 ++++++++---- guilib/src/PreferencesDialog.cpp | 1 + guilib/src/ui/preferencesDialog.ui | 150 ++++++++-------------- 8 files changed, 132 insertions(+), 136 deletions(-) diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index b964e6c0..d8b860af 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -324,7 +324,8 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(RGBD, ProximityByTime, bool, false, "Detection over all locations in STM."); RTABMAP_PARAM(RGBD, ProximityBySpace, bool, true, "Detection over locations (in Working Memory or STM) near in space."); RTABMAP_PARAM(RGBD, ProximityMaxGraphDepth, int, 50, "Maximum depth from the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore."); - RTABMAP_PARAM(RGBD, ProximityPathFilteringRadius, float, 0.5, "Path filtering radius."); + RTABMAP_PARAM(RGBD, ProximityMaxPaths, int, 3, "Maximum paths compared (from the most recent) for proximity detection by space. 0 means no limit."); + RTABMAP_PARAM(RGBD, ProximityPathFilteringRadius, float, 0.5, "Path filtering radius to reduce the number of nodes to compare in a path. A path should also be inside that radius to be considered for proximity detection."); RTABMAP_PARAM(RGBD, ProximityPathRawPosesUsed, bool, true, "When comparing to a local path, merge the scan using the odometry poses (with neighbor link optimizations) instead of the ones in the optimized local graph."); RTABMAP_PARAM(RGBD, ProximityAngle, float, 45, "Maximum angle (degrees) for visual proximity detection."); diff --git a/corelib/include/rtabmap/core/Rtabmap.h b/corelib/include/rtabmap/core/Rtabmap.h index e86500fe..ecf64791 100644 --- a/corelib/include/rtabmap/core/Rtabmap.h +++ b/corelib/include/rtabmap/core/Rtabmap.h @@ -142,7 +142,7 @@ public: const Transform & getPathTransformToGoal() const {return _pathTransformToGoal;} std::map getForwardWMPoses(int fromId, int maxNearestNeighbors, float radius, int maxDiffID) const; - std::list > getPaths(std::map poses) const; + std::map > getPaths(std::map poses, const Transform & target, int maxGraphDepth = 0) const; void adjustLikelihood(std::map & likelihood) const; std::pair selectHypothesis(const std::map & posterior, const std::map & likelihood) const; @@ -195,6 +195,7 @@ private: float _localRadius; float _localImmunizationRatio; int _proximityMaxGraphDepth; + int _proximityMaxPaths; float _proximityFilteringRadius; bool _proximityRawPosesUsed; float _proximityAngle; diff --git a/corelib/include/rtabmap/core/Statistics.h b/corelib/include/rtabmap/core/Statistics.h index e86e8c97..7c0714d4 100644 --- a/corelib/include/rtabmap/core/Statistics.h +++ b/corelib/include/rtabmap/core/Statistics.h @@ -69,6 +69,8 @@ class RTABMAP_EXP Statistics RTABMAP_STATS(Proximity, Time_detections,); RTABMAP_STATS(Proximity, Space_last_detection_id,); RTABMAP_STATS(Proximity, Space_paths,); + RTABMAP_STATS(Proximity, Space_visual_paths_checked,); + RTABMAP_STATS(Proximity, Space_scan_paths_checked,); RTABMAP_STATS(Proximity, Space_detections_added_visually,); RTABMAP_STATS(Proximity, Space_detections_added_icp_only,); diff --git a/corelib/src/Graph.cpp b/corelib/src/Graph.cpp index 1523ecce..610e75a0 100644 --- a/corelib/src/Graph.cpp +++ b/corelib/src/Graph.cpp @@ -1479,20 +1479,16 @@ int findNearestNode( ids[oi++] = iter->first; } - std::map foundNodes; - if(cloud->size()) + pcl::search::KdTree::Ptr kdTree(new pcl::search::KdTree); + kdTree->setInputCloud(cloud); + std::vector ind; + std::vector dist; + pcl::PointXYZ pt(targetPose.x(), targetPose.y(), targetPose.z()); + kdTree->nearestKSearch(pt, 1, ind, dist); + if(ind.size() && dist.size() && ind[0] >= 0) { - pcl::search::KdTree::Ptr kdTree(new pcl::search::KdTree); - kdTree->setInputCloud(cloud); - std::vector ind; - std::vector dist; - pcl::PointXYZ pt(targetPose.x(), targetPose.y(), targetPose.z()); - kdTree->nearestKSearch(pt, 1, ind, dist); - if(ind.size() && dist.size() && ind[0] >= 0) - { - UDEBUG("Nearest node = %d: %f", ids[ind[0]], dist[0]); - id = ids[ind[0]]; - } + UDEBUG("Nearest node = %d: %f", ids[ind[0]], dist[0]); + id = ids[ind[0]]; } } return id; diff --git a/corelib/src/RegistrationIcp.cpp b/corelib/src/RegistrationIcp.cpp index 43158d52..1f1d9076 100644 --- a/corelib/src/RegistrationIcp.cpp +++ b/corelib/src/RegistrationIcp.cpp @@ -348,13 +348,15 @@ Transform RegistrationIcp::computeTransformationImpl( correspondencesRatio = float(correspondences)/float(toScan.cols>fromScan.cols?toScan.cols:fromScan.cols); } - UDEBUG("%d->%d hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)", + UDEBUG("%d->%d hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%), from guess: trans=%f rot=%f", dataTo.id(), dataFrom.id(), hasConverged?"true":"false", variance, correspondences, maxLaserScans>0?maxLaserScans:(int)(toScan.cols>fromScan.cols?toScan.cols:fromScan.cols), - correspondencesRatio*100.0f); + correspondencesRatio*100.0f, + info.icpTranslation, + info.icpRotation); info.variance = variance>0.0f?variance:0.0001; // epsilon if exact transform info.icpInliersRatio = correspondencesRatio; diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 7d58b425..1b31d13e 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -98,6 +98,7 @@ Rtabmap::Rtabmap() : _localRadius(Parameters::defaultRGBDLocalRadius()), _localImmunizationRatio(Parameters::defaultRGBDLocalImmunizationRatio()), _proximityMaxGraphDepth(Parameters::defaultRGBDProximityMaxGraphDepth()), + _proximityMaxPaths(Parameters::defaultRGBDProximityMaxPaths()), _proximityFilteringRadius(Parameters::defaultRGBDProximityPathFilteringRadius()), _proximityRawPosesUsed(Parameters::defaultRGBDProximityPathRawPosesUsed()), _proximityAngle(Parameters::defaultRGBDProximityAngle()*M_PI/180.0f), @@ -409,6 +410,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kRGBDLocalRadius(), _localRadius); Parameters::parse(parameters, Parameters::kRGBDLocalImmunizationRatio(), _localImmunizationRatio); Parameters::parse(parameters, Parameters::kRGBDProximityMaxGraphDepth(), _proximityMaxGraphDepth); + Parameters::parse(parameters, Parameters::kRGBDProximityMaxPaths(), _proximityMaxPaths); Parameters::parse(parameters, Parameters::kRGBDProximityPathFilteringRadius(), _proximityFilteringRadius); Parameters::parse(parameters, Parameters::kRGBDProximityPathRawPosesUsed(), _proximityRawPosesUsed); Parameters::parse(parameters, Parameters::kRGBDProximityAngle(), _proximityAngle); @@ -1766,6 +1768,8 @@ bool Rtabmap::process( int proximityDetectionsAddedByICPOnly = 0; int lastProximitySpaceClosureId = 0; int proximitySpacePaths = 0; + int localVisualPathsChecked = 0; + int localScanPathsChecked = 0; if(_proximityBySpace && _localRadius > 0 && _rgbdSlamMode && @@ -1813,14 +1817,16 @@ bool Rtabmap::process( UDEBUG("nearestPoses=%d", (int)nearestPoses.size()); // segment poses by paths, only one detection per path - std::list > nearestPaths = getPaths(nearestPoses); - UDEBUG("nearestPaths=%d", (int)nearestPaths.size()); + std::map > nearestPaths = getPaths(nearestPoses, _optimizedPoses.at(signature->id()), _proximityMaxGraphDepth); + UDEBUG("nearestPaths=%d proximityMaxPaths=%d", (int)nearestPaths.size(), _proximityMaxPaths); - for(std::list >::const_iterator iter=nearestPaths.begin(); - iter!=nearestPaths.end() && (_memory->isIncremental() || lastProximitySpaceClosureId == 0); + for(std::map >::const_reverse_iterator iter=nearestPaths.rbegin(); + iter!=nearestPaths.rend() && + (_memory->isIncremental() || lastProximitySpaceClosureId == 0) && + (_proximityMaxPaths <= 0 || localVisualPathsChecked < _proximityMaxPaths); ++iter) { - std::map path = *iter; + std::map path = iter->second; UASSERT(path.size()); //find the nearest pose on the path looking in the same direction @@ -1829,11 +1835,12 @@ bool Rtabmap::process( int nearestId = rtabmap::graph::findNearestNode(path, _optimizedPoses.at(signature->id())); if(nearestId > 0) { - // nearest pose must not be linked to current location and enough + // nearest pose must not be linked to current location and enough close if(!signature->hasLink(nearestId) && (_proximityFilteringRadius <= 0.0f || _optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _proximityFilteringRadius*_proximityFilteringRadius)) { + ++localVisualPathsChecked; RegistrationInfo info; Transform guess = _optimizedPoses.at(signature->id()).inverse() * _optimizedPoses.at(nearestId); Transform transform = _memory->computeTransform(signature->id(), nearestId, guess, &info); @@ -1883,18 +1890,19 @@ bool Rtabmap::process( // local visual closure above. proximitySpacePaths = (int)nearestPaths.size(); - - for(std::list >::iterator iter=nearestPaths.begin(); - iter!=nearestPaths.end() && (_memory->isIncremental() || lastProximitySpaceClosureId == 0); + for(std::map >::const_reverse_iterator iter=nearestPaths.rbegin(); + iter!=nearestPaths.rend() && + (_memory->isIncremental() || lastProximitySpaceClosureId == 0) && + (_proximityMaxPaths <= 0 || localScanPathsChecked < _proximityMaxPaths); ++iter) { - std::map & path = *iter; + std::map path = iter->second; UASSERT(path.size()); //find the nearest pose on the path int nearestId = rtabmap::graph::findNearestNode(path, _optimizedPoses.at(signature->id())); UASSERT(nearestId > 0); - UDEBUG("Path %d distance=%fm", nearestId, _optimizedPoses.at(signature->id()).getDistance(_optimizedPoses.at(nearestId))); + UDEBUG("Path %d (size=%d) distance=%fm", nearestId, (int)path.size(), _optimizedPoses.at(signature->id()).getDistance(_optimizedPoses.at(nearestId))); // nearest pose must be close and not linked to current location if(!signature->hasLink(nearestId) && @@ -1930,6 +1938,7 @@ bool Rtabmap::process( //The nearest will be the reference for a loop closure transform if(signature->getLinks().find(nearestId) == signature->getLinks().end()) { + ++localScanPathsChecked; RegistrationInfo info; Transform transform = _memory->computeIcpTransformMulti(signature->id(), nearestId, filteredPath, &info); if(!transform.isNull()) @@ -2238,6 +2247,8 @@ bool Rtabmap::process( 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_visual_paths_checked(), localVisualPathsChecked); + statistics_.addStatistic(Statistics::kProximitySpace_scan_paths_checked(), localScanPathsChecked); statistics_.addStatistic(Statistics::kProximitySpace_last_detection_id(), lastProximitySpaceClosureId); statistics_.setProximityDetectionId(lastProximitySpaceClosureId); if(_loopClosureHypothesis.first || lastProximitySpaceClosureId) @@ -2818,29 +2829,51 @@ std::map Rtabmap::getForwardWMPoses( return poses; } -std::list > Rtabmap::getPaths(std::map poses) const +std::map > Rtabmap::getPaths(std::map poses, const Transform & target, int maxGraphDepth) const { - std::list > paths; - if(_memory && poses.size()) + std::map > paths; + if(_memory && poses.size() && !target.isNull()) { // Segment poses connected only by neighbor links while(poses.size()) { std::map path; - for(std::map::iterator iter=poses.begin(); iter!=poses.end();) + // select nearest pose and iterate neighbors from there + int nearestId = rtabmap::graph::findNearestNode(poses, target); + std::map ids = _memory->getNeighborsId(nearestId, maxGraphDepth, 0, true, true, true); + + for(std::map::iterator iter=ids.begin(); iter!=ids.end(); ++iter) { - if(path.size() == 0 || uContains(_memory->getNeighborLinks(path.rbegin()->first), iter->first)) + std::map::iterator jter = poses.find(iter->first); + if(jter != poses.end()) { - path.insert(*iter); - poses.erase(iter++); - } - else - { - break; + bool valid = path.empty(); + if(!valid) + { + // make sure it has a neighbor added to path + std::map links = _memory->getNeighborLinks(iter->first); + for(std::map::iterator kter=links.begin(); kter!=links.end() && !valid; ++kter) + { + valid = path.find(kter->first) != path.end(); + } + } + + if(valid) + { + UDEBUG("%d <- %d", nearestId, jter->first); + path.insert(*jter); + poses.erase(jter); + } } } - UASSERT(path.size()); - paths.push_back(path); + UASSERT_MSG(path.size(), uFormat("nearestId=%d ids=%d", nearestId, (int)ids.size()).c_str()); + if(maxGraphDepth > 0) + { + UASSERT_MSG((int)path.size() <= maxGraphDepth*2+1, + uFormat("nearestId=%d path=%d ids=%d maxGraphDepth=%d", + nearestId, (int)path.size(), (int)ids.size(), maxGraphDepth).c_str()); + } + paths.insert(std::make_pair(nearestId, path)); } } diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index b8ab738c..047e8b13 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -721,6 +721,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->groupBox_localDetection_space->setObjectName(Parameters::kRGBDProximityBySpace().c_str()); _ui->localDetection_radius->setObjectName(Parameters::kRGBDLocalRadius().c_str()); _ui->localDetection_maxDiffID->setObjectName(Parameters::kRGBDProximityMaxGraphDepth().c_str()); + _ui->localDetection_maxPaths->setObjectName(Parameters::kRGBDProximityMaxPaths().c_str()); _ui->localDetection_pathFilteringRadius->setObjectName(Parameters::kRGBDProximityPathFilteringRadius().c_str()); _ui->localDetection_angle->setObjectName(Parameters::kRGBDProximityAngle().c_str()); _ui->checkBox_localSpacePathOdomPosesUsed->setObjectName(Parameters::kRGBDProximityPathRawPosesUsed().c_str()); diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index b6af4aa9..35d1d1ec 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -64,24 +64,15 @@ 0 0 - 685 - 1896 + 686 + 2023 0 - - 0 - - - 0 - - - 0 - - + 0 @@ -95,7 +86,7 @@ QFrame::Raised - 5 + 13 @@ -4187,16 +4178,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki Directory of images (optional settings) - - 0 - - - 0 - - - 0 - - + 0 @@ -7457,20 +7439,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - When comparing to a local path, merge the laser scans using the odometry poses instead of the ones in the optimized local graph. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - + m @@ -7486,10 +7455,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Path filtering radius to avoid merging laser scans which are close. 0 to ignore. + When comparing to a local path, merge the laser scans using the odometry poses instead of the ones in the optimized local graph. true @@ -7499,7 +7468,20 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + + + + Path filtering radius to avoid merging laser scans which are close. 0 to ignore. A path should also be inside that radius to be considered for proximity detection. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + @@ -7529,7 +7511,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + Save scan matching IDs in link's user data. @@ -7542,14 +7524,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + - + Maximum angle for visual proximity detection. @@ -7562,7 +7544,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + degrees @@ -7578,6 +7560,29 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + Maximum paths compared (from the most recent). 0 means no limit. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 999 + + + 0 + + + @@ -9373,16 +9378,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - 0 - - - 0 - - - 0 - - + 0 @@ -9522,16 +9518,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - - 0 - - - 0 - - - 0 - - + 0 @@ -9689,16 +9676,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare 0 - - 0 - - - 0 - - - 0 - - + 0 @@ -9778,16 +9756,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare 0 - - 0 - - - 0 - - - 0 - - + 0 @@ -9899,16 +9868,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare 0 - - 0 - - - 0 - - - 0 - - + 0