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

Added parameters Vis/MeanInliersDistance and Vis/MinInliersDistribution

Browse Source
This commit is contained in:
matlabbe
2019-06-16 18:54:39 -04:00
parent 85edc57ba5
commit d205683eb5
13 changed files with 206 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -342,7 +342,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0, "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd, bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest node of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
RTABMAP_PARAM(RGBD, OptimizeMaxError, float, 3.0, uFormat("Reject loop closures if optimization error ratio is greater than this value (0=disabled). Ratio is computed as absolute error over standard deviation of each link. This will help to detect when a wrong loop closure is added to the graph. Not compatible with \"%s\" if enabled.", kOptimizerRobust().c_str()));
RTABMAP_PARAM(RGBD, MaxLocalizationDistance, float, 0.0, "Reject localizations if the distance from the map is over this distance (0=disabled). Only used in localization mode.");
RTABMAP_PARAM(RGBD, MaxLoopClosureDistance, float, 0.0, "Reject loop closures/localizations if the distance from the map is over this distance (0=disabled).");
RTABMAP_PARAM(RGBD, SavedLocalizationIgnored, bool, false, "Ignore last saved localization pose from previous session. If true, RTAB-Map won't assume it is restarting from the same place than where it shut down previously.");
RTABMAP_PARAM(RGBD, GoalReachedRadius, float, 0.5, "Goal reached radius (m).");
RTABMAP_PARAM(RGBD, PlanStuckIterations, int, 0, "Mark the current goal node on the path as unreachable if it is not updated after X iterations (0=disabled). If all upcoming nodes on the path are unreachabled, the plan fails.");
@@ -564,6 +564,9 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Vis, EpipolarGeometryVar, float, 0.02, uFormat("[%s = 2] Epipolar geometry maximum variance to accept the transformation.", kVisEstimationType().c_str()));
RTABMAP_PARAM(Vis, MinInliers, int, 20, "Minimum feature correspondences to compute/accept the transformation.");
RTABMAP_PARAM(Vis, MeanInliersDistance, float, 0.0, "Maximum distance (m) of the mean distance of inliers from the camera to accept the transformation. 0 means disabled.");
RTABMAP_PARAM(Vis, MinInliersDistribution, float, 0.0, "Minimum distribution value of the inliers in the image to accept the transformation. The distribution is the second eigen value of the PCA (Principal Component Analysis) on the keypoints of the normalized image [-0.5, 0.5]. The value would be between 0 and 0.5. 0 means disabled.");
RTABMAP_PARAM(Vis, Iterations, int, 300, "Maximum iterations to compute the transform.");
#ifndef RTABMAP_NONFREE
#ifdef RTABMAP_OPENCV3

6
corelib/include/rtabmap/core/RegistrationInfo.h
View File

@@ -37,6 +37,8 @@ public:
RegistrationInfo() :
totalTime(0.0),
inliers(0),
inliersMeanDistance(0.0f),
inliersDistribution(0.0f),
matches(0),
icpInliersRatio(0),
icpTranslation(0.0f),
@@ -53,6 +55,8 @@ public:
output.covariance = covariance.clone();
output.rejectedMsg = rejectedMsg;
output.inliers = inliers;
output.inliersMeanDistance = inliersMeanDistance;
output.inliersDistribution = inliersDistribution;
output.matches = matches;
output.icpInliersRatio = icpInliersRatio;
output.icpTranslation = icpTranslation;
@@ -67,6 +71,8 @@ public:
// RegistrationVis
int inliers;
float inliersMeanDistance;
float inliersDistribution;
std::vector<int> inliersIDs;
int matches;
std::vector<int> matchesIDs;

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

@@ -85,6 +85,8 @@ private:
bool _guessMatchToProjection;
int _bundleAdjustment;
bool _depthAsMask;
float _minInliersDistributionThr;
float _maxInliersMeanDistance;
ParametersMap _featureParameters;
ParametersMap _bundleParameters;

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

@@ -230,7 +230,7 @@ private:
unsigned int _maxMemoryAllowed; // signatures count in WM
float _loopThr;
float _loopRatio;
float _localizationMaxDistance;
float _maxLoopClosureDistance;
bool _verifyLoopClosureHypothesis;
unsigned int _maxRetrieved;
unsigned int _maxLocalRetrieved;

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

@@ -71,6 +71,12 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(Loop, Angular_variance,);
RTABMAP_STATS(Loop, Landmark_detected,);
RTABMAP_STATS(Loop, Landmark_detected_node_ref,);
RTABMAP_STATS(Loop, Visual_inliers_mean_dist,m);
RTABMAP_STATS(Loop, Visual_inliers_distribution,);
RTABMAP_STATS(Loop, Map_correction_norm, m);
RTABMAP_STATS(Loop, Map_correction_roll, deg);
RTABMAP_STATS(Loop, Map_correction_pitch, deg);
RTABMAP_STATS(Loop, Map_correction_yaw, deg);
RTABMAP_STATS(Proximity, Time_detections,);
RTABMAP_STATS(Proximity, Space_last_detection_id,);

3
corelib/src/Parameters.cpp
View File

@@ -237,6 +237,9 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
{
// removed parameters
// 0.19.4
removedParameters_.insert(std::make_pair("RGBD/MaxLocalizationDistance", std::make_pair(true, Parameters::kRGBDMaxLoopClosureDistance())));
// 0.19.3
removedParameters_.insert(std::make_pair("Aruco/Dictionary", std::make_pair(true, Parameters::kMarkerDictionary())));
removedParameters_.insert(std::make_pair("Aruco/MarkerLength", std::make_pair(true, Parameters::kMarkerLength())));

101
corelib/src/RegistrationVis.cpp
View File

@@ -68,7 +68,9 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
_guessWinSize(Parameters::defaultVisCorGuessWinSize()),
_guessMatchToProjection(Parameters::defaultVisCorGuessMatchToProjection()),
_bundleAdjustment(Parameters::defaultVisBundleAdjustment()),
_depthAsMask(Parameters::defaultVisDepthAsMask())
_depthAsMask(Parameters::defaultVisDepthAsMask()),
_minInliersDistributionThr(Parameters::defaultVisMinInliersDistribution()),
_maxInliersMeanDistance(Parameters::defaultVisMeanInliersDistance())
{
_featureParameters = Parameters::getDefaultParameters();
uInsert(_featureParameters, ParametersPair(Parameters::kKpNNStrategy(), _featureParameters.at(Parameters::kVisCorNNType())));
@@ -112,6 +114,8 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kVisCorGuessMatchToProjection(), _guessMatchToProjection);
Parameters::parse(parameters, Parameters::kVisBundleAdjustment(), _bundleAdjustment);
Parameters::parse(parameters, Parameters::kVisDepthAsMask(), _depthAsMask);
Parameters::parse(parameters, Parameters::kVisMinInliersDistribution(), _minInliersDistributionThr);
Parameters::parse(parameters, Parameters::kVisMeanInliersDistance(), _maxInliersMeanDistance);
uInsert(_bundleParameters, parameters);
UASSERT_MSG(_minInliers >= 1, uFormat("value=%d", _minInliers).c_str());
@@ -555,6 +559,7 @@ Transform RegistrationVis::computeTransformationImpl(
cv::cvtColor(imageFrom, tmp, cv::COLOR_BGR2GRAY);
imageFrom = tmp;
}
UDEBUG("cleared orignalWordsFromIds");
orignalWordsFromIds.clear();
descriptorsFrom = detectorFrom->generateDescriptors(imageFrom, kptsFrom);
}
@@ -729,6 +734,7 @@ Transform RegistrationVis::computeTransformationImpl(
UDEBUG("descriptorsFrom=%d", descriptorsFrom.rows);
UDEBUG("descriptorsTo=%d", descriptorsTo.rows);
UDEBUG("orignalWordsFromIds=%d", (int)orignalWordsFromIds.size());
// We have all data we need here, so match!
if(descriptorsFrom.rows > 0 && descriptorsTo.rows > 0)
@@ -1632,6 +1638,99 @@ Transform RegistrationVis::computeTransformationImpl(
transform = transforms[0];
covariance = covariances[0];
}
if(!transform.isNull() && !allInliers.empty() && (_minInliersDistributionThr>0.0f || _maxInliersMeanDistance>0.0f))
{
cv::Mat pcaData;
float cx=0, cy=0, w=0, h=0;
if(_minInliersDistributionThr > 0)
{
if(toSignature.sensorData().stereoCameraModel().isValidForProjection() ||
(toSignature.sensorData().cameraModels().size() == 1 && toSignature.sensorData().cameraModels()[0].isValidForReprojection()))
{
const CameraModel & cameraModel = toSignature.sensorData().stereoCameraModel().isValidForProjection()?toSignature.sensorData().stereoCameraModel().left():toSignature.sensorData().cameraModels()[0];
cx = cameraModel.cx();
cy = cameraModel.cy();
w = cameraModel.imageWidth();
h = cameraModel.imageHeight();
if(w>0 && h>0)
{
pcaData = cv::Mat(allInliers.size(), 2, CV_32FC1);
}
else
{
UERROR("Invalid calibration image size (%dx%d), cannot compute inliers distribution! (see %s=%f)", w, h, Parameters::kVisMinInliersDistribution().c_str(), _minInliersDistributionThr);
}
}
else if(toSignature.sensorData().cameraModels().size() > 1)
{
UERROR("Multi-camera not supported when computing inliers distribution! (see %s=%f)", Parameters::kVisMinInliersDistribution().c_str(), _minInliersDistributionThr);
}
else
{
UERROR("Calibration not valid, cannot compute inliers distribution! (see %s=%f)", Parameters::kVisMinInliersDistribution().c_str(), _minInliersDistributionThr);
}
}
Transform transformInv = transform.inverse();
std::vector<float> distances;
if(_maxInliersMeanDistance>0.0f)
{
distances.reserve(allInliers.size());
}
for(unsigned int i=0; i<allInliers.size(); ++i)
{
if(_maxInliersMeanDistance>0.0f)
{
std::multimap<int, cv::Point3f>::const_iterator words3Iter = fromSignature.getWords3().find(allInliers[i]);
if(words3Iter != fromSignature.getWords3().end())
{
if(uIsFinite(words3Iter->second.x))
{
cv::Point3f pt = util3d::transformPoint(words3Iter->second, transformInv);
distances.push_back(pt.x);
}
}
}
if(!pcaData.empty())
{
std::multimap<int, cv::KeyPoint>::const_iterator wordsIter = fromSignature.getWords().find(allInliers[i]);
UASSERT(wordsIter != fromSignature.getWords().end());
float * ptr = pcaData.ptr<float>(i, 0);
ptr[0] = (wordsIter->second.pt.x-cx) / w;
ptr[1] = (wordsIter->second.pt.y-cy) / h;
}
}
if(!distances.empty())
{
info.inliersMeanDistance = uMean(distances);
if(info.inliersMeanDistance > _maxInliersMeanDistance)
{
msg = uFormat("The mean distance of the inliers is over %s threshold (%f)",
info.inliersMeanDistance, Parameters::kVisMeanInliersDistance().c_str(), _maxInliersMeanDistance);
transform.setNull();
}
}
if(!transform.isNull() && !pcaData.empty())
{
cv::Mat pcaEigenVectors, pcaEigenValues;
cv::PCA pca_analysis(pcaData, cv::Mat(), CV_PCA_DATA_AS_ROW);
// We take the second eigen value
info.inliersDistribution = pca_analysis.eigenvalues.at<float>(0, 1);
if(info.inliersDistribution < _minInliersDistributionThr)
{
msg = uFormat("The distribution (%f) of inliers is under %s threshold (%f)",
info.inliersDistribution, Parameters::kVisMinInliersDistribution().c_str(), _minInliersDistributionThr);
transform.setNull();
}
}
}
}
else if(toSignature.sensorData().isValid())
{

32
corelib/src/Rtabmap.cpp
View File

@@ -87,7 +87,7 @@ Rtabmap::Rtabmap() :
_maxMemoryAllowed(Parameters::defaultRtabmapMemoryThr()), // 0=inf
_loopThr(Parameters::defaultRtabmapLoopThr()),
_loopRatio(Parameters::defaultRtabmapLoopRatio()),
_localizationMaxDistance(Parameters::defaultRGBDMaxLocalizationDistance()),
_maxLoopClosureDistance(Parameters::defaultRGBDMaxLoopClosureDistance()),
_verifyLoopClosureHypothesis(Parameters::defaultVhEpEnabled()),
_maxRetrieved(Parameters::defaultRtabmapMaxRetrieved()),
_maxLocalRetrieved(Parameters::defaultRGBDMaxLocalRetrieved()),
@@ -442,7 +442,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRtabmapMemoryThr(), _maxMemoryAllowed);
Parameters::parse(parameters, Parameters::kRtabmapLoopThr(), _loopThr);
Parameters::parse(parameters, Parameters::kRtabmapLoopRatio(), _loopRatio);
Parameters::parse(parameters, Parameters::kRGBDMaxLocalizationDistance(), _localizationMaxDistance);
Parameters::parse(parameters, Parameters::kRGBDMaxLoopClosureDistance(), _maxLoopClosureDistance);
Parameters::parse(parameters, Parameters::kVhEpEnabled(), _verifyLoopClosureHypothesis);
Parameters::parse(parameters, Parameters::kRtabmapMaxRetrieved(), _maxRetrieved);
Parameters::parse(parameters, Parameters::kRGBDMaxLocalRetrieved(), _maxLocalRetrieved);
@@ -2128,6 +2128,8 @@ bool Rtabmap::process(
int loopClosureVisualMatches = 0;
float loopClosureLinearVariance = 0.0f;
float loopClosureAngularVariance = 0.0f;
float loopClosureVisualInliersMeanDist = 0;
float loopClosureVisualInliersDistribution = 0;
if(_loopClosureHypothesis.first>0)
{
//Compute transform if metric data are present
@@ -2137,6 +2139,9 @@ bool Rtabmap::process(
if(_rgbdSlamMode)
{
transform = _memory->computeTransform(_loopClosureHypothesis.first, signature->id(), Transform(), &info);
loopClosureVisualInliersMeanDist = info.inliersMeanDistance;
loopClosureVisualInliersDistribution = info.inliersDistribution;
loopClosureVisualInliers = info.inliers;
loopClosureVisualMatches = info.matches;
rejectedHypothesis = transform.isNull();
@@ -2145,11 +2150,11 @@ bool Rtabmap::process(
UWARN("Rejected loop closure %d -> %d: %s",
_loopClosureHypothesis.first, signature->id(), info.rejectedMsg.c_str());
}
else if(!_memory->isIncremental() && _localizationMaxDistance>0.0f && transform.getNorm() > _localizationMaxDistance)
else if(_maxLoopClosureDistance>0.0f && transform.getNorm() > _maxLoopClosureDistance)
{
rejectedHypothesis = true;
UWARN("Rejected localization %d -> %d because distance to map (%fm) is over %s=%fm.",
_loopClosureHypothesis.first, signature->id(), transform.getNorm(), Parameters::kRGBDMaxLocalizationDistance().c_str(), _localizationMaxDistance);
_loopClosureHypothesis.first, signature->id(), transform.getNorm(), Parameters::kRGBDMaxLoopClosureDistance().c_str(), _maxLoopClosureDistance);
}
else
{
@@ -2275,9 +2280,9 @@ bool Rtabmap::process(
UDEBUG("nearestPaths=%d proximityMaxPaths=%d", (int)nearestPaths.size(), _proximityMaxPaths);
float proximityFilteringRadius = _proximityFilteringRadius;
if(!_memory->isIncremental() && _localizationMaxDistance>0.0f && (proximityFilteringRadius <= 0.0f || _localizationMaxDistance<proximityFilteringRadius))
if(_maxLoopClosureDistance>0.0f && (proximityFilteringRadius <= 0.0f || _maxLoopClosureDistance<proximityFilteringRadius))
{
proximityFilteringRadius = _localizationMaxDistance;
proximityFilteringRadius = _maxLoopClosureDistance;
}
for(std::map<NearestPathKey, std::map<int, Transform> >::const_reverse_iterator iter=nearestPaths.rbegin();
iter!=nearestPaths.rend() &&
@@ -2330,6 +2335,12 @@ bool Rtabmap::process(
if(_loopClosureHypothesis.first == 0)
{
if(proximityDetectionsAddedVisually == 0)
{
loopClosureVisualInliersMeanDist = info.inliersMeanDistance;
loopClosureVisualInliersDistribution = info.inliersDistribution;
}
++proximityDetectionsAddedVisually;
lastProximitySpaceClosureId = nearestId;
@@ -3052,6 +3063,8 @@ bool Rtabmap::process(
statistics_.addStatistic(Statistics::kLoopOptimization_iterations(), optimizationIterations);
statistics_.addStatistic(Statistics::kLoopLandmark_detected(), -landmarkDetected);
statistics_.addStatistic(Statistics::kLoopLandmark_detected_node_ref(), landmarkDetectedNodesRef.empty()?0:*landmarkDetectedNodesRef.begin());
statistics_.addStatistic(Statistics::kLoopVisual_inliers_mean_dist(), loopClosureVisualInliersMeanDist);
statistics_.addStatistic(Statistics::kLoopVisual_inliers_distribution(), loopClosureVisualInliersDistribution);
statistics_.addStatistic(Statistics::kProximityTime_detections(), proximityDetectionsInTimeFound);
statistics_.addStatistic(Statistics::kProximitySpace_detections_added_visually(), proximityDetectionsAddedVisually);
@@ -3067,6 +3080,13 @@ bool Rtabmap::process(
UASSERT(uContains(sLoop->getLinks(), signature->id()));
UINFO("Set loop closure transform = %s", sLoop->getLinks().find(signature->id())->second.transform().prettyPrint().c_str());
statistics_.setLoopClosureTransform(sLoop->getLinks().find(signature->id())->second.transform());
statistics_.addStatistic(Statistics::kLoopMap_correction_norm(), _mapCorrection.getNorm());
float roll,pitch,yaw;
_mapCorrection.getEulerAngles(roll, pitch, yaw);
statistics_.addStatistic(Statistics::kLoopMap_correction_roll(), roll*180/M_PI);
statistics_.addStatistic(Statistics::kLoopMap_correction_pitch(), pitch*180/M_PI);
statistics_.addStatistic(Statistics::kLoopMap_correction_yaw(), yaw*180/M_PI);
}
statistics_.setMapCorrection(_mapCorrection);
UINFO("Set map correction = %s", _mapCorrection.prettyPrint().c_str());