i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

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.

Browse Source
This commit is contained in:
matlabbe
2016-08-12 18:43:12 -04:00
parent a0d76eabe5
commit ae7436a41b
8 changed files with 132 additions and 136 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
corelib/include/rtabmap/core/Parameters.h
View File

@@ -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.");

3
corelib/include/rtabmap/core/Rtabmap.h
View File

@@ -142,7 +142,7 @@ public:
const Transform & getPathTransformToGoal() const {return _pathTransformToGoal;}
std::map<int, Transform> getForwardWMPoses(int fromId, int maxNearestNeighbors, float radius, int maxDiffID) const;
std::list<std::map<int, Transform> > getPaths(std::map<int, Transform> poses) const;
std::map<int, std::map<int, Transform> > getPaths(std::map<int, Transform> poses, const Transform & target, int maxGraphDepth = 0) const;
void adjustLikelihood(std::map<int, float> & likelihood) const;
std::pair<int, float> selectHypothesis(const std::map<int, float> & posterior,
const std::map<int, float> & likelihood) const;
@@ -195,6 +195,7 @@ private:
float _localRadius;
float _localImmunizationRatio;
int _proximityMaxGraphDepth;
int _proximityMaxPaths;
float _proximityFilteringRadius;
bool _proximityRawPosesUsed;
float _proximityAngle;

2
corelib/include/rtabmap/core/Statistics.h
View File

@@ -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,);

22
corelib/src/Graph.cpp
View File

@@ -1479,20 +1479,16 @@ int findNearestNode(
ids[oi++] = iter->first;
}
std::map<int, float> foundNodes;
if(cloud->size())
pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
kdTree->setInputCloud(cloud);
std::vector<int> ind;
std::vector<float> 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<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
kdTree->setInputCloud(cloud);
std::vector<int> ind;
std::vector<float> 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;

6
corelib/src/RegistrationIcp.cpp
View File

@@ -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;

81
corelib/src/Rtabmap.cpp
View File

@@ -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<std::map<int, Transform> > nearestPaths = getPaths(nearestPoses);
UDEBUG("nearestPaths=%d", (int)nearestPaths.size());
std::map<int, std::map<int, Transform> > nearestPaths = getPaths(nearestPoses, _optimizedPoses.at(signature->id()), _proximityMaxGraphDepth);
UDEBUG("nearestPaths=%d proximityMaxPaths=%d", (int)nearestPaths.size(), _proximityMaxPaths);
for(std::list<std::map<int, Transform> >::const_iterator iter=nearestPaths.begin();
iter!=nearestPaths.end() && (_memory->isIncremental() || lastProximitySpaceClosureId == 0);
for(std::map<int, std::map<int, Transform> >::const_reverse_iterator iter=nearestPaths.rbegin();
iter!=nearestPaths.rend() &&
(_memory->isIncremental() || lastProximitySpaceClosureId == 0) &&
(_proximityMaxPaths <= 0 || localVisualPathsChecked < _proximityMaxPaths);
++iter)
{
std::map<int, Transform> path = *iter;
std::map<int, Transform> 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<std::map<int, Transform> >::iterator iter=nearestPaths.begin();
iter!=nearestPaths.end() && (_memory->isIncremental() || lastProximitySpaceClosureId == 0);
for(std::map<int, std::map<int, Transform> >::const_reverse_iterator iter=nearestPaths.rbegin();
iter!=nearestPaths.rend() &&
(_memory->isIncremental() || lastProximitySpaceClosureId == 0) &&
(_proximityMaxPaths <= 0 || localScanPathsChecked < _proximityMaxPaths);
++iter)
{
std::map<int, Transform> & path = *iter;
std::map<int, Transform> 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<int, Transform> Rtabmap::getForwardWMPoses(
return poses;
}
std::list<std::map<int, Transform> > Rtabmap::getPaths(std::map<int, Transform> poses) const
std::map<int, std::map<int, Transform> > Rtabmap::getPaths(std::map<int, Transform> poses, const Transform & target, int maxGraphDepth) const
{
std::list<std::map<int, Transform> > paths;
if(_memory && poses.size())
std::map<int, std::map<int, Transform> > paths;
if(_memory && poses.size() && !target.isNull())
{
// Segment poses connected only by neighbor links
while(poses.size())
{
std::map<int, Transform> path;
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end();)
// select nearest pose and iterate neighbors from there
int nearestId = rtabmap::graph::findNearestNode(poses, target);
std::map<int, int> ids = _memory->getNeighborsId(nearestId, maxGraphDepth, 0, true, true, true);
for(std::map<int, int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
{
if(path.size() == 0 || uContains(_memory->getNeighborLinks(path.rbegin()->first), iter->first))
std::map<int, Transform>::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<int, Link> links = _memory->getNeighborLinks(iter->first);
for(std::map<int, Link>::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));
}
}