i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-12 20:49:50 +08:00
Code Issues Packages Projects Releases Wiki Activity

Refactoring of some parameters (LocalLoopDetection* to Proximity*) and variables in code

Browse Source
This commit is contained in:
matlabbe
2015-11-25 12:15:41 -05:00
parent 56383d4cfb
commit da1129cf79
14 changed files with 290 additions and 308 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/Memory.h
-1
View File
@@ -239,7 +239,6 @@ private:
bool _badSignaturesIgnored; bool _badSignaturesIgnored;
int _imageDecimation; int _imageDecimation;
float _laserScanDownsampleStepSize; float _laserScanDownsampleStepSize;
bool _localSpaceLinksKeptInWM;
bool _reextractLoopClosureFeatures; bool _reextractLoopClosureFeatures;
float _rehearsalMaxDistance; float _rehearsalMaxDistance;
float _rehearsalMaxAngle; float _rehearsalMaxAngle;
corelib/include/rtabmap/core/Parameters.h
+8 -9
View File
@@ -200,7 +200,6 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Mem, InitWMWithAllNodes, bool, false, "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session."); RTABMAP_PARAM(Mem, InitWMWithAllNodes, bool, false, "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.");
RTABMAP_PARAM(Mem, ImageDecimation, int, 1, "Image decimation (>=1) when creating a signature."); RTABMAP_PARAM(Mem, ImageDecimation, int, 1, "Image decimation (>=1) when creating a signature.");
RTABMAP_PARAM(Mem, LaserScanDownsampleStepSize, int, 1, "If > 1, downsample the laser scans when creating a signature."); RTABMAP_PARAM(Mem, LaserScanDownsampleStepSize, int, 1, "If > 1, downsample the laser scans when creating a signature.");
RTABMAP_PARAM(Mem, LocalSpaceLinksKeptInWM, bool, true, "If local space links are kept in WM.");
// KeypointMemory (Keypoint-based) // KeypointMemory (Keypoint-based)
RTABMAP_PARAM_COND(Kp, NNStrategy, int, RTABMAP_NONFREE, 1, 3, "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4"); RTABMAP_PARAM_COND(Kp, NNStrategy, int, RTABMAP_NONFREE, 1, 3, "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
@@ -300,17 +299,17 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(RGBD, LocalRadius, float, 10, "Local radius (m) for nodes selection in the local map. This parameter is used in some approaches about the local map management."); RTABMAP_PARAM(RGBD, LocalRadius, float, 10, "Local radius (m) for nodes selection in the local map. This parameter is used in some approaches about the local map management.");
RTABMAP_PARAM(RGBD, LocalImmunizationRatio, float, 0.25, "Ratio of working memory for which local nodes are immunized from transfer."); RTABMAP_PARAM(RGBD, LocalImmunizationRatio, float, 0.25, "Ratio of working memory for which local nodes are immunized from transfer.");
RTABMAP_PARAM(RGBD, ScanMatchingIdsSavedInLinks, bool, true, "Save scan matching IDs in link's user data."); RTABMAP_PARAM(RGBD, ScanMatchingIdsSavedInLinks, bool, true, "Save scan matching IDs in link's user data.");
RTABMAP_PARAM(RGBD, IcpOdomRefining, bool, false, "If the newest added node's pose is refined using ICP with the previous node to correct odometry from pose to pose (laser scans required!)."); RTABMAP_PARAM(RGBD, NeighborLinkRefining, bool, false, "When a new node is added to the graph, the transformation of its neighbor link to the previous node is refined using ICP (laser scans required!).");
RTABMAP_PARAM(RGBD, IcpLoopClosureRefining, bool, false, "If the estimated loop closure transformation is refined using ICP (laser scans required!)."); RTABMAP_PARAM(RGBD, LoopClosureLinkRefining, bool, false, "If the estimated loop closure transformation is refined using ICP (laser scans required!).");
RTABMAP_PARAM(RGBD, LoopClosureReextractFeatures, bool, true, "Extract features even if there are some already in the nodes."); RTABMAP_PARAM(RGBD, LoopClosureReextractFeatures, bool, true, "Extract features even if there are some already in the nodes.");
// Local/Proximity loop closure detection // Local/Proximity loop closure detection
RTABMAP_PARAM(RGBD, LocalLoopDetectionTime, bool, false, "Detection over all locations in STM."); RTABMAP_PARAM(RGBD, ProximityByTime, bool, false, "Detection over all locations in STM.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionSpace, bool, true, "Detection over locations (in Working Memory or STM) near in space."); RTABMAP_PARAM(RGBD, ProximityBySpace, bool, true, "Detection over locations (in Working Memory or STM) near in space.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionPathScansMerged, bool, true, "Merge close laser scans on each path. If false, only the nearest laser scan on the path is used for ICP."); RTABMAP_PARAM(RGBD, ProximityPathScansMerged, bool, true, "Merge close laser scans on each path. If false, only the nearest laser scan on the path is used for ICP.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionMaxGraphDepth, int, 50, "Maximum depth from the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore."); 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, LocalLoopDetectionPathFilteringRadius, float, 0.5, "Path filtering radius."); RTABMAP_PARAM(RGBD, ProximityPathFilteringRadius, float, 0.5, "Path filtering radius.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionPathOdomPosesUsed, bool, true, "When comparing to a local path, merge the scan using the odometry poses instead of the ones in the optimized local graph."); 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.");
// Graph optimization // Graph optimization
RTABMAP_PARAM(RGBD, OptimizeStrategy, int, 0, "Graph optimization strategy: 0=TORO and 1=g2o."); RTABMAP_PARAM(RGBD, OptimizeStrategy, int, 0, "Graph optimization strategy: 0=TORO and 1=g2o.");
corelib/include/rtabmap/core/Registration.h
+3 -3
View File
@@ -39,7 +39,7 @@ public:
virtual ~Registration() {} virtual ~Registration() {}
virtual void parseParameters(const ParametersMap & parameters) virtual void parseParameters(const ParametersMap & parameters)
{ {
Parameters::parse(parameters, Parameters::kRegVarianceFromInliersCount(), _bowVarianceFromInliersCount); Parameters::parse(parameters, Parameters::kRegVarianceFromInliersCount(), _varianceFromInliersCount);
} }
virtual Transform computeTransformation( virtual Transform computeTransformation(
const Signature & from, const Signature & from,
@@ -51,13 +51,13 @@ public:
float * inliersRatioOut = 0) = 0; float * inliersRatioOut = 0) = 0;
protected: protected:
Registration(const ParametersMap & parameters = ParametersMap()) : Registration(const ParametersMap & parameters = ParametersMap()) :
_bowVarianceFromInliersCount(Parameters::defaultRegVarianceFromInliersCount()) _varianceFromInliersCount(Parameters::defaultRegVarianceFromInliersCount())
{ {
this->parseParameters(parameters); this->parseParameters(parameters);
} }
protected: protected:
bool _bowVarianceFromInliersCount; bool _varianceFromInliersCount;
}; };
corelib/include/rtabmap/core/RegistrationIcp.h
+9 -9
View File
@@ -61,16 +61,16 @@ public:
float * inliersRatioOut = 0); float * inliersRatioOut = 0);
private: private:
float _icpMaxTranslation; float _maxTranslation;
float _icpMaxRotation; float _maxRotation;
bool _icp2D; bool _icp2D;
float _icpVoxelSize; float _voxelSize;
int _icpDownsamplingStep; int _downsamplingStep;
float _icpMaxCorrespondenceDistance; float _maxCorrespondenceDistance;
int _icpMaxIterations; int _maxIterations;
float _icpCorrespondenceRatio; float _correspondenceRatio;
bool _icpPointToPlane; bool _pointToPlane;
int _icpPointToPlaneNormalNeighbors; int _pointToPlaneNormalNeighbors;
}; };
} }
corelib/include/rtabmap/core/RegistrationVis.h
+20 -20
View File
@@ -51,29 +51,29 @@ public:
float * varianceOut = 0, float * varianceOut = 0,
float * inliersRatioOut = 0); float * inliersRatioOut = 0);
float getBowInlierDistance() const {return _bowInlierDistance;} float getBowInlierDistance() const {return _inlierDistance;}
int getBowIterations() const {return _bowIterations;} int getBowIterations() const {return _iterations;}
int getBowMinInliers() const {return _bowMinInliers;} int getBowMinInliers() const {return _minInliers;}
bool getBowForce2D() const {return _bowForce2D;} bool getBowForce2D() const {return _force2D;}
private: private:
int _bowMinInliers; int _minInliers;
float _bowInlierDistance; float _inlierDistance;
int _bowIterations; int _iterations;
int _bowRefineIterations; int _refineIterations;
bool _bowForce2D; bool _force2D;
float _bowEpipolarGeometryVar; float _epipolarGeometryVar;
int _bowEstimationType; int _estimationType;
double _bowPnPReprojError; double _PnPReprojError;
int _bowPnPFlags; int _PnPFlags;
int _reextractNNType; int _NNType;
float _reextractNNDR; float _NNDR;
int _reextractFeatureType; int _featureType;
int _reextractMaxWords; int _maxWords;
float _reextractMaxDepth; float _maxDepth;
float _reextractMinDepth; float _minDepth;
std::string _reextractRoiRatios; std::string _roiRatios;
int _subPixWinSize; int _subPixWinSize;
int _subPixIterations; int _subPixIterations;
corelib/include/rtabmap/core/Rtabmap.h
+8 -8
View File
@@ -189,17 +189,17 @@ private:
float _rgbdLinearUpdate; float _rgbdLinearUpdate;
float _rgbdAngularUpdate; float _rgbdAngularUpdate;
float _newMapOdomChangeDistance; float _newMapOdomChangeDistance;
bool _loopClosureIcpRefining; bool _loopClosureRefining;
bool _odomIcpRefining; bool _neighborLinkRefining;
bool _localLoopClosureDetectionTime; bool _proximityByTime;
bool _localLoopClosureDetectionSpace; bool _proximityBySpace;
bool _scanMatchingIdsSavedInLinks; bool _scanMatchingIdsSavedInLinks;
float _localRadius; float _localRadius;
float _localImmunizationRatio; float _localImmunizationRatio;
int _localDetectMaxGraphDepth; int _proximityMaxGraphDepth;
float _localPathFilteringRadius; float _proximityFilteringRadius;
bool _localPathOdomPosesUsed; bool _proximityRawPosesUsed;
bool _localPathScansMerged; bool _proximityScansMerged;
std::string _databasePath; std::string _databasePath;
bool _optimizeFromGraphEnd; bool _optimizeFromGraphEnd;
float _optimizationMaxLinearError; float _optimizationMaxLinearError;
corelib/src/Memory.cpp
-23
View File
@@ -82,7 +82,6 @@ Memory::Memory(const ParametersMap & parameters) :
_badSignaturesIgnored(Parameters::defaultMemBadSignaturesIgnored()), _badSignaturesIgnored(Parameters::defaultMemBadSignaturesIgnored()),
_imageDecimation(Parameters::defaultMemImageDecimation()), _imageDecimation(Parameters::defaultMemImageDecimation()),
_laserScanDownsampleStepSize(Parameters::defaultMemLaserScanDownsampleStepSize()), _laserScanDownsampleStepSize(Parameters::defaultMemLaserScanDownsampleStepSize()),
_localSpaceLinksKeptInWM(Parameters::defaultMemLocalSpaceLinksKeptInWM()),
_reextractLoopClosureFeatures(Parameters::defaultRGBDLoopClosureReextractFeatures()), _reextractLoopClosureFeatures(Parameters::defaultRGBDLoopClosureReextractFeatures()),
_rehearsalMaxDistance(Parameters::defaultRGBDLinearUpdate()), _rehearsalMaxDistance(Parameters::defaultRGBDLinearUpdate()),
_rehearsalMaxAngle(Parameters::defaultRGBDAngularUpdate()), _rehearsalMaxAngle(Parameters::defaultRGBDAngularUpdate()),
@@ -399,7 +398,6 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kMemSTMSize(), _maxStMemSize); Parameters::parse(parameters, Parameters::kMemSTMSize(), _maxStMemSize);
Parameters::parse(parameters, Parameters::kMemImageDecimation(), _imageDecimation); Parameters::parse(parameters, Parameters::kMemImageDecimation(), _imageDecimation);
Parameters::parse(parameters, Parameters::kMemLaserScanDownsampleStepSize(), _laserScanDownsampleStepSize); Parameters::parse(parameters, Parameters::kMemLaserScanDownsampleStepSize(), _laserScanDownsampleStepSize);
Parameters::parse(parameters, Parameters::kMemLocalSpaceLinksKeptInWM(), _localSpaceLinksKeptInWM);
Parameters::parse(parameters, Parameters::kRGBDLoopClosureReextractFeatures(), _reextractLoopClosureFeatures); Parameters::parse(parameters, Parameters::kRGBDLoopClosureReextractFeatures(), _reextractLoopClosureFeatures);
Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rehearsalMaxDistance); Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rehearsalMaxDistance);
Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rehearsalMaxAngle); Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rehearsalMaxAngle);
@@ -748,27 +746,6 @@ void Memory::moveSignatureToWMFromSTM(int id, int * reducedTo)
UASSERT(_stMem.find(id) != _stMem.end()); UASSERT(_stMem.find(id) != _stMem.end());
Signature * s = this->_getSignature(id); Signature * s = this->_getSignature(id);
UASSERT(s!=0); UASSERT(s!=0);
if(!_localSpaceLinksKeptInWM)
{
// remove local space links outside STM
std::map<int, Link> links = s->getLinks(); // get a copy because we will remove some links in "s"
for(std::map<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if(iter->second.type() == Link::kLocalSpaceClosure)
{
Signature * sTo = this->_getSignature(iter->first);
if(sTo)
{
sTo->removeLink(s->id());
}
else
{
UERROR("Link %d of %d not in WM/STM?!?", iter->first, s->id());
}
s->removeLink(iter->first);
}
}
}
if(_reduceGraph) if(_reduceGraph)
{ {
corelib/src/Parameters.cpp
+32 -4
View File
@@ -76,9 +76,12 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
if(removedParameters_.empty()) if(removedParameters_.empty())
{ {
// removed parameters // removed parameters
removedParameters_.insert(std::make_pair("Mem/LaserScanVoxelSize", std::make_pair(false, Parameters::kMemLaserScanDownsampleStepSize())));
removedParameters_.insert(std::make_pair("RGBD/PoseScanMatching", std::make_pair(true, Parameters::kRGBDIcpOdomRefining()))); // 0.11.0
removedParameters_.insert(std::make_pair("Mem/LaserScanVoxelSize", std::make_pair(false, Parameters::kMemLaserScanDownsampleStepSize())));
removedParameters_.insert(std::make_pair("Mem/LocalSpaceLinksKeptInWM", std::make_pair(false, "")));
removedParameters_.insert(std::make_pair("RGBD/PoseScanMatching", std::make_pair(true, Parameters::kRGBDNeighborLinkRefining())));
removedParameters_.insert(std::make_pair("Odom/FeatureType", std::make_pair(true, Parameters::kVisFeatureType()))); removedParameters_.insert(std::make_pair("Odom/FeatureType", std::make_pair(true, Parameters::kVisFeatureType())));
removedParameters_.insert(std::make_pair("Odom/EstimationType", std::make_pair(true, Parameters::kVisEstimationType()))); removedParameters_.insert(std::make_pair("Odom/EstimationType", std::make_pair(true, Parameters::kVisEstimationType())));
@@ -101,7 +104,7 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
removedParameters_.insert(std::make_pair("OdomSubPix/Iterations", std::make_pair(true, Parameters::kVisSubPixIterations()))); removedParameters_.insert(std::make_pair("OdomSubPix/Iterations", std::make_pair(true, Parameters::kVisSubPixIterations())));
removedParameters_.insert(std::make_pair("OdomSubPix/Eps", std::make_pair(true, Parameters::kVisSubPixEps()))); removedParameters_.insert(std::make_pair("OdomSubPix/Eps", std::make_pair(true, Parameters::kVisSubPixEps())));
removedParameters_.insert(std::make_pair("LccReextract/Activated", std::make_pair(false, Parameters::kRGBDLoopClosureReextractFeatures()))); removedParameters_.insert(std::make_pair("LccReextract/Activated", std::make_pair(true, Parameters::kRGBDLoopClosureReextractFeatures())));
removedParameters_.insert(std::make_pair("LccReextract/FeatureType", std::make_pair(false, Parameters::kVisFeatureType()))); removedParameters_.insert(std::make_pair("LccReextract/FeatureType", std::make_pair(false, Parameters::kVisFeatureType())));
removedParameters_.insert(std::make_pair("LccReextract/MaxWords", std::make_pair(false, Parameters::kVisMaxFeatures()))); removedParameters_.insert(std::make_pair("LccReextract/MaxWords", std::make_pair(false, Parameters::kVisMaxFeatures())));
removedParameters_.insert(std::make_pair("LccReextract/MaxDepth", std::make_pair(false, Parameters::kVisMaxDepth()))); removedParameters_.insert(std::make_pair("LccReextract/MaxDepth", std::make_pair(false, Parameters::kVisMaxDepth())));
@@ -120,7 +123,7 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
removedParameters_.insert(std::make_pair("LccBow/PnPFlags", std::make_pair(false, Parameters::kVisPnPFlags()))); removedParameters_.insert(std::make_pair("LccBow/PnPFlags", std::make_pair(false, Parameters::kVisPnPFlags())));
removedParameters_.insert(std::make_pair("LccBow/EpipolarGeometryVar", std::make_pair(true, Parameters::kVisEpipolarGeometryVar()))); removedParameters_.insert(std::make_pair("LccBow/EpipolarGeometryVar", std::make_pair(true, Parameters::kVisEpipolarGeometryVar())));
removedParameters_.insert(std::make_pair("LccIcp/Type", std::make_pair(true, Parameters::kRGBDIcpLoopClosureRefining()))); removedParameters_.insert(std::make_pair("LccIcp/Type", std::make_pair(true, Parameters::kRGBDLoopClosureLinkRefining())));
removedParameters_.insert(std::make_pair("LccIcp3/Decimation", std::make_pair(false, ""))); removedParameters_.insert(std::make_pair("LccIcp3/Decimation", std::make_pair(false, "")));
removedParameters_.insert(std::make_pair("LccIcp3/MaxDepth", std::make_pair(false, ""))); removedParameters_.insert(std::make_pair("LccIcp3/MaxDepth", std::make_pair(false, "")));
@@ -136,6 +139,31 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
removedParameters_.insert(std::make_pair("LccIcp2/Iterations", std::make_pair(true, Parameters::kIcpIterations()))); removedParameters_.insert(std::make_pair("LccIcp2/Iterations", std::make_pair(true, Parameters::kIcpIterations())));
removedParameters_.insert(std::make_pair("LccIcp2/CorrespondenceRatio", std::make_pair(true, Parameters::kIcpCorrespondenceRatio()))); removedParameters_.insert(std::make_pair("LccIcp2/CorrespondenceRatio", std::make_pair(true, Parameters::kIcpCorrespondenceRatio())));
removedParameters_.insert(std::make_pair("LccIcp2/VoxelSize", std::make_pair(true, Parameters::kIcpVoxelSize()))); removedParameters_.insert(std::make_pair("LccIcp2/VoxelSize", std::make_pair(true, Parameters::kIcpVoxelSize())));
removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionByTime", std::make_pair(true, Parameters::kRGBDProximityByTime())));
removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionBySpace", std::make_pair(true, Parameters::kRGBDProximityBySpace())));
removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionPathScansMerged", std::make_pair(true, Parameters::kRGBDProximityPathScansMerged())));
removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionMaxGraphDepth", std::make_pair(true, Parameters::kRGBDProximityMaxGraphDepth())));
removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionPathFilteringRadius", std::make_pair(true, Parameters::kRGBDProximityPathFilteringRadius())));
removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionPathRawPosesUsed", std::make_pair(true, Parameters::kRGBDProximityPathRawPosesUsed())));
// before 0.11.0
removedParameters_.insert(std::make_pair("GFTT/MaxCorners", std::make_pair(true, Parameters::kVisMaxFeatures())));
removedParameters_.insert(std::make_pair("LccBow/MaxDepth", std::make_pair(true, Parameters::kVisMaxDepth())));
removedParameters_.insert(std::make_pair("LccReextract/LoopClosureFeatures", std::make_pair(true, Parameters::kRGBDLoopClosureReextractFeatures())));
removedParameters_.insert(std::make_pair("Rtabmap/DetectorStrategy", std::make_pair(true, Parameters::kKpDetectorStrategy())));
removedParameters_.insert(std::make_pair("RGBD/ScanMatchingSize", std::make_pair(true, Parameters::kRGBDNeighborLinkRefining())));
removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionRadius", std::make_pair(true, Parameters::kRGBDLocalRadius())));
removedParameters_.insert(std::make_pair("RGBD/ToroIterations", std::make_pair(true, Parameters::kRGBDOptimizeIterations())));
removedParameters_.insert(std::make_pair("Mem/RehearsedNodesKept", std::make_pair(true, Parameters::kMemNotLinkedNodesKept())));
removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionMaxDiffID", std::make_pair(true, Parameters::kRGBDProximityMaxGraphDepth())));
removedParameters_.insert(std::make_pair("RGBD/PlanVirtualLinksMaxDiffID", std::make_pair(false, "")));
removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionMaxDiffID", std::make_pair(false, "")));
removedParameters_.insert(std::make_pair("Odom/Type", std::make_pair(true, Parameters::kVisFeatureType())));
removedParameters_.insert(std::make_pair("Odom/MaxWords", std::make_pair(true, Parameters::kVisMaxFeatures())));
removedParameters_.insert(std::make_pair("Odom/LocalHistory", std::make_pair(true, Parameters::kOdomBowLocalHistorySize())));
removedParameters_.insert(std::make_pair("Odom/NearestNeighbor", std::make_pair(true, Parameters::kVisNNType())));
removedParameters_.insert(std::make_pair("Odom/NNDR", std::make_pair(true, Parameters::kVisNNDR())));
} }
return removedParameters_; return removedParameters_;
} }
corelib/src/RegistrationIcp.cpp
+65 -65
View File
@@ -40,16 +40,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap { namespace rtabmap {
RegistrationIcp::RegistrationIcp(const ParametersMap & parameters) : RegistrationIcp::RegistrationIcp(const ParametersMap & parameters) :
_icpMaxTranslation(Parameters::defaultIcpMaxTranslation()), _maxTranslation(Parameters::defaultIcpMaxTranslation()),
_icpMaxRotation(Parameters::defaultIcpMaxRotation()), _maxRotation(Parameters::defaultIcpMaxRotation()),
_icp2D(Parameters::defaultIcp2D()), _icp2D(Parameters::defaultIcp2D()),
_icpVoxelSize(Parameters::defaultIcpVoxelSize()), _voxelSize(Parameters::defaultIcpVoxelSize()),
_icpDownsamplingStep(Parameters::defaultIcpDownsamplingStep()), _downsamplingStep(Parameters::defaultIcpDownsamplingStep()),
_icpMaxCorrespondenceDistance(Parameters::defaultIcpMaxCorrespondenceDistance()), _maxCorrespondenceDistance(Parameters::defaultIcpMaxCorrespondenceDistance()),
_icpMaxIterations(Parameters::defaultIcpIterations()), _maxIterations(Parameters::defaultIcpIterations()),
_icpCorrespondenceRatio(Parameters::defaultIcpCorrespondenceRatio()), _correspondenceRatio(Parameters::defaultIcpCorrespondenceRatio()),
_icpPointToPlane(Parameters::defaultIcpPointToPlane()), _pointToPlane(Parameters::defaultIcpPointToPlane()),
_icpPointToPlaneNormalNeighbors(Parameters::defaultIcpPointToPlaneNormalNeighbors()) _pointToPlaneNormalNeighbors(Parameters::defaultIcpPointToPlaneNormalNeighbors())
{ {
this->parseParameters(parameters); this->parseParameters(parameters);
} }
@@ -58,23 +58,23 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
{ {
Registration::parseParameters(parameters); Registration::parseParameters(parameters);
Parameters::parse(parameters, Parameters::kIcpMaxTranslation(), _icpMaxTranslation); Parameters::parse(parameters, Parameters::kIcpMaxTranslation(), _maxTranslation);
Parameters::parse(parameters, Parameters::kIcpMaxRotation(), _icpMaxRotation); Parameters::parse(parameters, Parameters::kIcpMaxRotation(), _maxRotation);
Parameters::parse(parameters, Parameters::kIcp2D(), _icp2D); Parameters::parse(parameters, Parameters::kIcp2D(), _icp2D);
Parameters::parse(parameters, Parameters::kIcpVoxelSize(), _icpVoxelSize); Parameters::parse(parameters, Parameters::kIcpVoxelSize(), _voxelSize);
Parameters::parse(parameters, Parameters::kIcpDownsamplingStep(), _icpDownsamplingStep); Parameters::parse(parameters, Parameters::kIcpDownsamplingStep(), _downsamplingStep);
Parameters::parse(parameters, Parameters::kIcpMaxCorrespondenceDistance(), _icpMaxCorrespondenceDistance); Parameters::parse(parameters, Parameters::kIcpMaxCorrespondenceDistance(), _maxCorrespondenceDistance);
Parameters::parse(parameters, Parameters::kIcpIterations(), _icpMaxIterations); Parameters::parse(parameters, Parameters::kIcpIterations(), _maxIterations);
Parameters::parse(parameters, Parameters::kIcpCorrespondenceRatio(), _icpCorrespondenceRatio); Parameters::parse(parameters, Parameters::kIcpCorrespondenceRatio(), _correspondenceRatio);
Parameters::parse(parameters, Parameters::kIcpPointToPlane(), _icpPointToPlane); Parameters::parse(parameters, Parameters::kIcpPointToPlane(), _pointToPlane);
Parameters::parse(parameters, Parameters::kIcpPointToPlaneNormalNeighbors(), _icpPointToPlaneNormalNeighbors); Parameters::parse(parameters, Parameters::kIcpPointToPlaneNormalNeighbors(), _pointToPlaneNormalNeighbors);
UASSERT_MSG(_icpVoxelSize >= 0, uFormat("value=%d", _icpVoxelSize).c_str()); UASSERT_MSG(_voxelSize >= 0, uFormat("value=%d", _voxelSize).c_str());
UASSERT_MSG(_icpDownsamplingStep >= 0, uFormat("value=%d", _icpDownsamplingStep).c_str()); UASSERT_MSG(_downsamplingStep >= 0, uFormat("value=%d", _downsamplingStep).c_str());
UASSERT_MSG(_icpMaxCorrespondenceDistance > 0.0f, uFormat("value=%f", _icpMaxCorrespondenceDistance).c_str()); UASSERT_MSG(_maxCorrespondenceDistance > 0.0f, uFormat("value=%f", _maxCorrespondenceDistance).c_str());
UASSERT_MSG(_icpMaxIterations > 0, uFormat("value=%d", _icpMaxIterations).c_str()); UASSERT_MSG(_maxIterations > 0, uFormat("value=%d", _maxIterations).c_str());
UASSERT_MSG(_icpCorrespondenceRatio >=0.0f && _icpCorrespondenceRatio <=1.0f, uFormat("value=%f", _icpCorrespondenceRatio).c_str()); UASSERT_MSG(_correspondenceRatio >=0.0f && _correspondenceRatio <=1.0f, uFormat("value=%f", _correspondenceRatio).c_str());
UASSERT_MSG(_icpPointToPlaneNormalNeighbors > 0, uFormat("value=%d", _icpPointToPlaneNormalNeighbors).c_str()); UASSERT_MSG(_pointToPlaneNormalNeighbors > 0, uFormat("value=%d", _pointToPlaneNormalNeighbors).c_str());
} }
Transform RegistrationIcp::computeTransformation( Transform RegistrationIcp::computeTransformation(
@@ -106,17 +106,17 @@ Transform RegistrationIcp::computeTransformation(
float * inliersRatioOut) float * inliersRatioOut)
{ {
UDEBUG("Guess transform = %s", guess.prettyPrint().c_str()); UDEBUG("Guess transform = %s", guess.prettyPrint().c_str());
UDEBUG("Voxel size=%f", _icpVoxelSize); UDEBUG("Voxel size=%f", _voxelSize);
UDEBUG("2D=%d", _icp2D?1:0); UDEBUG("2D=%d", _icp2D?1:0);
UDEBUG("PointToPlane=%d", _icpPointToPlane?1:0); UDEBUG("PointToPlane=%d", _pointToPlane?1:0);
UDEBUG("Normal neighborhood=%d", _icpPointToPlaneNormalNeighbors); UDEBUG("Normal neighborhood=%d", _pointToPlaneNormalNeighbors);
UDEBUG("Max corrrespondence distance=%f", _icpMaxCorrespondenceDistance); UDEBUG("Max corrrespondence distance=%f", _maxCorrespondenceDistance);
UDEBUG("Max Iterations=%d", _icpMaxIterations); UDEBUG("Max Iterations=%d", _maxIterations);
UDEBUG("Variance from inliers count=%d", _bowVarianceFromInliersCount?1:0); UDEBUG("Variance from inliers count=%d", _varianceFromInliersCount?1:0);
UDEBUG("Correspondence Ratio=%f", _icpCorrespondenceRatio); UDEBUG("Correspondence Ratio=%f", _correspondenceRatio);
UDEBUG("Max translation=%f", _icpMaxTranslation); UDEBUG("Max translation=%f", _maxTranslation);
UDEBUG("Max rotation=%f", _icpMaxRotation); UDEBUG("Max rotation=%f", _maxRotation);
UDEBUG("Downsampling step=%d", _icpDownsamplingStep); UDEBUG("Downsampling step=%d", _downsamplingStep);
std::string msg; std::string msg;
Transform transform; Transform transform;
@@ -127,11 +127,11 @@ Transform RegistrationIcp::computeTransformation(
int maxLaserScans = dataTo.laserScanMaxPts(); int maxLaserScans = dataTo.laserScanMaxPts();
cv::Mat fromScan = dataFrom.laserScanRaw(); cv::Mat fromScan = dataFrom.laserScanRaw();
cv::Mat toScan = dataTo.laserScanRaw(); cv::Mat toScan = dataTo.laserScanRaw();
if(_icpDownsamplingStep>1) if(_downsamplingStep>1)
{ {
fromScan = util3d::downsample(fromScan, _icpDownsamplingStep); fromScan = util3d::downsample(fromScan, _downsamplingStep);
toScan = util3d::downsample(toScan, _icpDownsamplingStep); toScan = util3d::downsample(toScan, _downsamplingStep);
maxLaserScans/=_icpDownsamplingStep; maxLaserScans/=_downsamplingStep;
} }
pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloud = util3d::laserScanToPointCloud(fromScan, Transform()); pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloud = util3d::laserScanToPointCloud(fromScan, Transform());
@@ -143,10 +143,10 @@ Transform RegistrationIcp::computeTransformation(
pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudFiltered = fromCloud; pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudFiltered = fromCloud;
pcl::PointCloud<pcl::PointXYZ>::Ptr toCloudFiltered = toCloud; pcl::PointCloud<pcl::PointXYZ>::Ptr toCloudFiltered = toCloud;
bool filtered = false; bool filtered = false;
if(_icpVoxelSize > 0.0f) if(_voxelSize > 0.0f)
{ {
fromCloudFiltered = util3d::voxelize(fromCloudFiltered, _icpVoxelSize); fromCloudFiltered = util3d::voxelize(fromCloudFiltered, _voxelSize);
toCloudFiltered = util3d::voxelize(toCloudFiltered, _icpVoxelSize); toCloudFiltered = util3d::voxelize(toCloudFiltered, _voxelSize);
filtered = true; filtered = true;
} }
@@ -159,10 +159,10 @@ Transform RegistrationIcp::computeTransformation(
pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>()); pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>());
if(!_icp2D) // 3D ICP if(!_icp2D) // 3D ICP
{ {
if(_icpPointToPlane) if(_pointToPlane)
{ {
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::computeNormals(fromCloudFiltered, _icpPointToPlaneNormalNeighbors); pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::computeNormals(fromCloudFiltered, _pointToPlaneNormalNeighbors);
pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::computeNormals(toCloudFiltered, _icpPointToPlaneNormalNeighbors); pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::computeNormals(toCloudFiltered, _pointToPlaneNormalNeighbors);
std::vector<int> indices; std::vector<int> indices;
toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals); toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
@@ -174,8 +174,8 @@ Transform RegistrationIcp::computeTransformation(
icpT = util3d::icpPointToPlane( icpT = util3d::icpPointToPlane(
toCloudNormals, toCloudNormals,
fromCloudNormals, fromCloudNormals,
_icpMaxCorrespondenceDistance, _maxCorrespondenceDistance,
_icpMaxIterations, _maxIterations,
hasConverged, hasConverged,
*newCloudNormalsRegistered); *newCloudNormalsRegistered);
if(!filtered && if(!filtered &&
@@ -185,7 +185,7 @@ Transform RegistrationIcp::computeTransformation(
util3d::computeVarianceAndCorrespondences( util3d::computeVarianceAndCorrespondences(
newCloudNormalsRegistered, newCloudNormalsRegistered,
fromCloudNormals, fromCloudNormals,
_icpMaxCorrespondenceDistance, _maxCorrespondenceDistance,
variance, variance,
correspondences); correspondences);
correspondencesComputed = true; correspondencesComputed = true;
@@ -197,8 +197,8 @@ Transform RegistrationIcp::computeTransformation(
icpT = util3d::icp( icpT = util3d::icp(
toCloudFiltered, toCloudFiltered,
fromCloudFiltered, fromCloudFiltered,
_icpMaxCorrespondenceDistance, _maxCorrespondenceDistance,
_icpMaxIterations, _maxIterations,
hasConverged, hasConverged,
*newCloudRegistered); *newCloudRegistered);
} }
@@ -208,8 +208,8 @@ Transform RegistrationIcp::computeTransformation(
icpT = util3d::icp2D( icpT = util3d::icp2D(
toCloudFiltered, toCloudFiltered,
fromCloudFiltered, fromCloudFiltered,
_icpMaxCorrespondenceDistance, _maxCorrespondenceDistance,
_icpMaxIterations, _maxIterations,
hasConverged, hasConverged,
*newCloudRegistered); *newCloudRegistered);
} }
@@ -229,21 +229,21 @@ Transform RegistrationIcp::computeTransformation(
{ {
float ix,iy,iz, iroll,ipitch,iyaw; float ix,iy,iz, iroll,ipitch,iyaw;
icpT.getTranslationAndEulerAngles(ix,iy,iz,iroll,ipitch,iyaw); icpT.getTranslationAndEulerAngles(ix,iy,iz,iroll,ipitch,iyaw);
if((_icpMaxTranslation>0.0f && if((_maxTranslation>0.0f &&
(fabs(ix) > _icpMaxTranslation || (fabs(ix) > _maxTranslation ||
fabs(iy) > _icpMaxTranslation || fabs(iy) > _maxTranslation ||
fabs(iz) > _icpMaxTranslation)) fabs(iz) > _maxTranslation))
|| ||
(_icpMaxRotation>0.0f && (_maxRotation>0.0f &&
(fabs(iroll) > _icpMaxRotation || (fabs(iroll) > _maxRotation ||
fabs(ipitch) > _icpMaxRotation || fabs(ipitch) > _maxRotation ||
fabs(iyaw) > _icpMaxRotation))) fabs(iyaw) > _maxRotation)))
{ {
msg = uFormat("Cannot compute transform (ICP correction too large -> %f m %f rad, limits=%f m, %f rad)", msg = uFormat("Cannot compute transform (ICP correction too large -> %f m %f rad, limits=%f m, %f rad)",
uMax3(fabs(ix), fabs(iy), fabs(iz)), uMax3(fabs(ix), fabs(iy), fabs(iz)),
uMax3(fabs(iroll), fabs(ipitch), fabs(iyaw)), uMax3(fabs(iroll), fabs(ipitch), fabs(iyaw)),
_icpMaxTranslation, _maxTranslation,
_icpMaxRotation); _maxRotation);
UINFO(msg.c_str()); UINFO(msg.c_str());
} }
else else
@@ -262,7 +262,7 @@ Transform RegistrationIcp::computeTransformation(
util3d::computeVarianceAndCorrespondences( util3d::computeVarianceAndCorrespondences(
toCloud, toCloud,
fromCloud, fromCloud,
_icpMaxCorrespondenceDistance, _maxCorrespondenceDistance,
variance, variance,
correspondences); correspondences);
} }
@@ -287,7 +287,7 @@ Transform RegistrationIcp::computeTransformation(
maxLaserScans>0?maxLaserScans:dataTo.laserScanMaxPts()?dataTo.laserScanMaxPts():(int)(toCloud->size()>fromCloud->size()?toCloud->size():fromCloud->size()), maxLaserScans>0?maxLaserScans:dataTo.laserScanMaxPts()?dataTo.laserScanMaxPts():(int)(toCloud->size()>fromCloud->size()?toCloud->size():fromCloud->size()),
correspondencesRatio*100.0f); correspondencesRatio*100.0f);
if(_bowVarianceFromInliersCount) if(_varianceFromInliersCount)
{ {
variance = correspondencesRatio > 0?1.0/double(correspondencesRatio):1.0; variance = correspondencesRatio > 0?1.0/double(correspondencesRatio):1.0;
} }
@@ -305,10 +305,10 @@ Transform RegistrationIcp::computeTransformation(
*inliersRatioOut = correspondencesRatio; *inliersRatioOut = correspondencesRatio;
} }
if(correspondencesRatio < _icpCorrespondenceRatio) if(correspondencesRatio < _correspondenceRatio)
{ {
msg = uFormat("Cannot compute transform (cor=%d corrRatio=%f/%f)", msg = uFormat("Cannot compute transform (cor=%d corrRatio=%f/%f)",
correspondences, correspondencesRatio, _icpCorrespondenceRatio); correspondences, correspondencesRatio, _correspondenceRatio);
UINFO(msg.c_str()); UINFO(msg.c_str());
} }
else else
corelib/src/RegistrationVis.cpp
+69 -69
View File
@@ -39,22 +39,22 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap { namespace rtabmap {
RegistrationVis::RegistrationVis(const ParametersMap & parameters) : RegistrationVis::RegistrationVis(const ParametersMap & parameters) :
_bowMinInliers(Parameters::defaultVisMinInliers()), _minInliers(Parameters::defaultVisMinInliers()),
_bowInlierDistance(Parameters::defaultVisInlierDistance()), _inlierDistance(Parameters::defaultVisInlierDistance()),
_bowIterations(Parameters::defaultVisIterations()), _iterations(Parameters::defaultVisIterations()),
_bowRefineIterations(Parameters::defaultVisRefineIterations()), _refineIterations(Parameters::defaultVisRefineIterations()),
_bowForce2D(Parameters::defaultVisForce2D()), _force2D(Parameters::defaultVisForce2D()),
_bowEpipolarGeometryVar(Parameters::defaultVisEpipolarGeometryVar()), _epipolarGeometryVar(Parameters::defaultVisEpipolarGeometryVar()),
_bowEstimationType(Parameters::defaultVisEstimationType()), _estimationType(Parameters::defaultVisEstimationType()),
_bowPnPReprojError(Parameters::defaultVisPnPReprojError()), _PnPReprojError(Parameters::defaultVisPnPReprojError()),
_bowPnPFlags(Parameters::defaultVisPnPFlags()), _PnPFlags(Parameters::defaultVisPnPFlags()),
_reextractNNType(Parameters::defaultVisNNType()), _NNType(Parameters::defaultVisNNType()),
_reextractNNDR(Parameters::defaultVisNNDR()), _NNDR(Parameters::defaultVisNNDR()),
_reextractFeatureType(Parameters::defaultVisFeatureType()), _featureType(Parameters::defaultVisFeatureType()),
_reextractMaxWords(Parameters::defaultVisMaxFeatures()), _maxWords(Parameters::defaultVisMaxFeatures()),
_reextractMaxDepth(Parameters::defaultVisMaxDepth()), _maxDepth(Parameters::defaultVisMaxDepth()),
_reextractMinDepth(Parameters::defaultVisMinDepth()), _minDepth(Parameters::defaultVisMinDepth()),
_reextractRoiRatios(Parameters::defaultVisRoiRatios()), _roiRatios(Parameters::defaultVisRoiRatios()),
_subPixWinSize(Parameters::defaultKpSubPixWinSize()), _subPixWinSize(Parameters::defaultKpSubPixWinSize()),
_subPixIterations(Parameters::defaultKpSubPixIterations()), _subPixIterations(Parameters::defaultKpSubPixIterations()),
_subPixEps(Parameters::defaultKpSubPixEps()) _subPixEps(Parameters::defaultKpSubPixEps())
@@ -66,29 +66,29 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
{ {
Registration::parseParameters(parameters); Registration::parseParameters(parameters);
Parameters::parse(parameters, Parameters::kVisMinInliers(), _bowMinInliers); Parameters::parse(parameters, Parameters::kVisMinInliers(), _minInliers);
Parameters::parse(parameters, Parameters::kVisInlierDistance(), _bowInlierDistance); Parameters::parse(parameters, Parameters::kVisInlierDistance(), _inlierDistance);
Parameters::parse(parameters, Parameters::kVisIterations(), _bowIterations); Parameters::parse(parameters, Parameters::kVisIterations(), _iterations);
Parameters::parse(parameters, Parameters::kVisRefineIterations(), _bowRefineIterations); Parameters::parse(parameters, Parameters::kVisRefineIterations(), _refineIterations);
Parameters::parse(parameters, Parameters::kVisForce2D(), _bowForce2D); Parameters::parse(parameters, Parameters::kVisForce2D(), _force2D);
Parameters::parse(parameters, Parameters::kVisEstimationType(), _bowEstimationType); Parameters::parse(parameters, Parameters::kVisEstimationType(), _estimationType);
Parameters::parse(parameters, Parameters::kVisEpipolarGeometryVar(), _bowEpipolarGeometryVar); Parameters::parse(parameters, Parameters::kVisEpipolarGeometryVar(), _epipolarGeometryVar);
Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _bowPnPReprojError); Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _PnPReprojError);
Parameters::parse(parameters, Parameters::kVisPnPFlags(), _bowPnPFlags); Parameters::parse(parameters, Parameters::kVisPnPFlags(), _PnPFlags);
Parameters::parse(parameters, Parameters::kVisNNType(), _reextractNNType); Parameters::parse(parameters, Parameters::kVisNNType(), _NNType);
Parameters::parse(parameters, Parameters::kVisNNDR(), _reextractNNDR); Parameters::parse(parameters, Parameters::kVisNNDR(), _NNDR);
Parameters::parse(parameters, Parameters::kVisFeatureType(), _reextractFeatureType); Parameters::parse(parameters, Parameters::kVisFeatureType(), _featureType);
Parameters::parse(parameters, Parameters::kVisMaxFeatures(), _reextractMaxWords); Parameters::parse(parameters, Parameters::kVisMaxFeatures(), _maxWords);
Parameters::parse(parameters, Parameters::kVisMaxDepth(), _reextractMaxDepth); Parameters::parse(parameters, Parameters::kVisMaxDepth(), _maxDepth);
Parameters::parse(parameters, Parameters::kKpSubPixWinSize(), _subPixWinSize); Parameters::parse(parameters, Parameters::kKpSubPixWinSize(), _subPixWinSize);
Parameters::parse(parameters, Parameters::kKpSubPixIterations(), _subPixIterations); Parameters::parse(parameters, Parameters::kKpSubPixIterations(), _subPixIterations);
Parameters::parse(parameters, Parameters::kKpSubPixEps(), _subPixEps); Parameters::parse(parameters, Parameters::kKpSubPixEps(), _subPixEps);
UASSERT_MSG(_bowMinInliers >= 1, uFormat("value=%d", _bowMinInliers).c_str()); UASSERT_MSG(_minInliers >= 1, uFormat("value=%d", _minInliers).c_str());
UASSERT_MSG(_bowInlierDistance > 0.0f, uFormat("value=%f", _bowInlierDistance).c_str()); UASSERT_MSG(_inlierDistance > 0.0f, uFormat("value=%f", _inlierDistance).c_str());
UASSERT_MSG(_bowIterations > 0, uFormat("value=%d", _bowIterations).c_str()); UASSERT_MSG(_iterations > 0, uFormat("value=%d", _iterations).c_str());
} }
Transform RegistrationVis::computeTransformation( Transform RegistrationVis::computeTransformation(
@@ -125,17 +125,17 @@ Transform RegistrationVis::computeTransformation(
uInsert(customParameters, ParametersPair(Parameters::kMemSTMSize(), "0")); uInsert(customParameters, ParametersPair(Parameters::kMemSTMSize(), "0"));
uInsert(customParameters, ParametersPair(Parameters::kKpIncrementalDictionary(), "true")); // make sure it is incremental uInsert(customParameters, ParametersPair(Parameters::kKpIncrementalDictionary(), "true")); // make sure it is incremental
uInsert(customParameters, ParametersPair(Parameters::kKpNewWordsComparedTogether(), "false")); uInsert(customParameters, ParametersPair(Parameters::kKpNewWordsComparedTogether(), "false"));
uInsert(customParameters, ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(_reextractNNType))); // bruteforce uInsert(customParameters, ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(_NNType))); // bruteforce
uInsert(customParameters, ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(_reextractNNDR))); uInsert(customParameters, ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(_NNDR)));
uInsert(customParameters, ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(_reextractFeatureType))); // FAST/BRIEF uInsert(customParameters, ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(_featureType))); // FAST/BRIEF
uInsert(customParameters, ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(_reextractMaxWords))); uInsert(customParameters, ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(_maxWords)));
uInsert(customParameters, ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(_reextractMaxDepth))); uInsert(customParameters, ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(_maxDepth)));
uInsert(customParameters, ParametersPair(Parameters::kKpMinDepth(), uNumber2Str(_reextractMinDepth))); uInsert(customParameters, ParametersPair(Parameters::kKpMinDepth(), uNumber2Str(_minDepth)));
uInsert(customParameters, ParametersPair(Parameters::kKpSubPixEps(), uNumber2Str(_subPixEps))); uInsert(customParameters, ParametersPair(Parameters::kKpSubPixEps(), uNumber2Str(_subPixEps)));
uInsert(customParameters, ParametersPair(Parameters::kKpSubPixIterations(), uNumber2Str(_subPixIterations))); uInsert(customParameters, ParametersPair(Parameters::kKpSubPixIterations(), uNumber2Str(_subPixIterations)));
uInsert(customParameters, ParametersPair(Parameters::kKpSubPixWinSize(), uNumber2Str(_subPixWinSize))); uInsert(customParameters, ParametersPair(Parameters::kKpSubPixWinSize(), uNumber2Str(_subPixWinSize)));
uInsert(customParameters, ParametersPair(Parameters::kKpBadSignRatio(), "0")); uInsert(customParameters, ParametersPair(Parameters::kKpBadSignRatio(), "0"));
uInsert(customParameters, ParametersPair(Parameters::kKpRoiRatios(), _reextractRoiRatios)); uInsert(customParameters, ParametersPair(Parameters::kKpRoiRatios(), _roiRatios));
uInsert(customParameters, ParametersPair(Parameters::kMemGenerateIds(), "true")); uInsert(customParameters, ParametersPair(Parameters::kMemGenerateIds(), "true"));
Memory memory(customParameters); Memory memory(customParameters);
@@ -186,7 +186,7 @@ Transform RegistrationVis::computeTransformation(
words3To = &toSignature.getWords3(); words3To = &toSignature.getWords3();
} }
if(_bowEstimationType == 2) // Epipolar Geometry if(_estimationType == 2) // Epipolar Geometry
{ {
if(!toSignature.sensorData().stereoCameraModel().isValid() && if(!toSignature.sensorData().stereoCameraModel().isValid() &&
(toSignature.sensorData().cameraModels().size() != 1 || (toSignature.sensorData().cameraModels().size() != 1 ||
@@ -194,8 +194,8 @@ Transform RegistrationVis::computeTransformation(
{ {
UERROR("Calibrated camera required (multi-cameras not supported)."); UERROR("Calibrated camera required (multi-cameras not supported).");
} }
else if((int)wordsFrom->size() >= _bowMinInliers && else if((int)wordsFrom->size() >= _minInliers &&
(int)wordsTo->size() >= _bowMinInliers) (int)wordsTo->size() >= _minInliers)
{ {
UASSERT(fromSignature.sensorData().stereoCameraModel().isValid() || (fromSignature.sensorData().cameraModels().size() == 1 && fromSignature.sensorData().cameraModels()[0].isValid())); UASSERT(fromSignature.sensorData().stereoCameraModel().isValid() || (fromSignature.sensorData().cameraModels().size() == 1 && fromSignature.sensorData().cameraModels()[0].isValid()));
const CameraModel & cameraModel = fromSignature.sensorData().stereoCameraModel().isValid()?fromSignature.sensorData().stereoCameraModel().left():fromSignature.sensorData().cameraModels()[0]; const CameraModel & cameraModel = fromSignature.sensorData().stereoCameraModel().isValid()?fromSignature.sensorData().stereoCameraModel().left():fromSignature.sensorData().cameraModels()[0];
@@ -207,9 +207,9 @@ Transform RegistrationVis::computeTransformation(
*wordsTo, *wordsTo,
cameraModel, cameraModel,
cameraTransform, cameraTransform,
_bowIterations, _iterations,
_bowPnPReprojError, _PnPReprojError,
_bowPnPFlags, // cv::SOLVEPNP_ITERATIVE _PnPFlags, // cv::SOLVEPNP_ITERATIVE
1.0f, 1.0f,
0.99f, 0.99f,
*words3From, // for scale estimation *words3From, // for scale estimation
@@ -219,21 +219,21 @@ Transform RegistrationVis::computeTransformation(
if(!cameraTransform.isNull()) if(!cameraTransform.isNull())
{ {
if((int)inliers3D.size() >= _bowMinInliers) if((int)inliers3D.size() >= _minInliers)
{ {
if(variance <= _bowEpipolarGeometryVar) if(variance <= _epipolarGeometryVar)
{ {
transform = cameraTransform; transform = cameraTransform;
} }
else else
{ {
msg = uFormat("Variance is too high! (max inlier distance=%f, variance=%f)", _bowEpipolarGeometryVar, variance); msg = uFormat("Variance is too high! (max inlier distance=%f, variance=%f)", _epipolarGeometryVar, variance);
UINFO(msg.c_str()); UINFO(msg.c_str());
} }
} }
else else
{ {
msg = uFormat("Not enough inliers %d < %d", (int)inliers3D.size(), _bowMinInliers); msg = uFormat("Not enough inliers %d < %d", (int)inliers3D.size(), _minInliers);
UINFO(msg.c_str()); UINFO(msg.c_str());
} }
} }
@@ -254,7 +254,7 @@ Transform RegistrationVis::computeTransformation(
UWARN(msg.c_str()); UWARN(msg.c_str());
} }
} }
else if(_bowEstimationType == 1) // PnP else if(_estimationType == 1) // PnP
{ {
if(!toSignature.sensorData().stereoCameraModel().isValid() && if(!toSignature.sensorData().stereoCameraModel().isValid() &&
(toSignature.sensorData().cameraModels().size() != 1 || (toSignature.sensorData().cameraModels().size() != 1 ||
@@ -269,8 +269,8 @@ Transform RegistrationVis::computeTransformation(
else else
{ {
// 3D to 2D // 3D to 2D
if((int)words3From->size() >= _bowMinInliers && if((int)words3From->size() >= _minInliers &&
(int)wordsTo->size() >= _bowMinInliers) (int)wordsTo->size() >= _minInliers)
{ {
UASSERT(toSignature.sensorData().stereoCameraModel().isValid() || (toSignature.sensorData().cameraModels().size() == 1 && toSignature.sensorData().cameraModels()[0].isValid())); UASSERT(toSignature.sensorData().stereoCameraModel().isValid() || (toSignature.sensorData().cameraModels().size() == 1 && toSignature.sensorData().cameraModels()[0].isValid()));
const CameraModel & cameraModel = toSignature.sensorData().stereoCameraModel().isValid()?toSignature.sensorData().stereoCameraModel().left():toSignature.sensorData().cameraModels()[0]; const CameraModel & cameraModel = toSignature.sensorData().stereoCameraModel().isValid()?toSignature.sensorData().stereoCameraModel().left():toSignature.sensorData().cameraModels()[0];
@@ -280,10 +280,10 @@ Transform RegistrationVis::computeTransformation(
uMultimapToMap(*words3From), uMultimapToMap(*words3From),
uMultimapToMap(*wordsTo), uMultimapToMap(*wordsTo),
cameraModel, cameraModel,
_bowMinInliers, _minInliers,
_bowIterations, _iterations,
_bowPnPReprojError, _PnPReprojError,
_bowPnPFlags, _PnPFlags,
Transform::getIdentity(), Transform::getIdentity(),
uMultimapToMap(*words3To), uMultimapToMap(*words3To),
&variance, &variance,
@@ -293,14 +293,14 @@ Transform RegistrationVis::computeTransformation(
if(transform.isNull()) if(transform.isNull())
{ {
msg = uFormat("Not enough inliers %d/%d between %d and %d", msg = uFormat("Not enough inliers %d/%d between %d and %d",
inliersCount, _bowMinInliers, fromSignature.id(), toSignature.id()); inliersCount, _minInliers, fromSignature.id(), toSignature.id());
UINFO(msg.c_str()); UINFO(msg.c_str());
} }
} }
else else
{ {
msg = uFormat("Not enough features in images (old=%d, new=%d, min=%d)", msg = uFormat("Not enough features in images (old=%d, new=%d, min=%d)",
(int)words3From->size(), (int)wordsTo->size(), _bowMinInliers); (int)words3From->size(), (int)wordsTo->size(), _minInliers);
UINFO(msg.c_str()); UINFO(msg.c_str());
} }
} }
@@ -309,17 +309,17 @@ Transform RegistrationVis::computeTransformation(
else else
{ {
// 3D -> 3D // 3D -> 3D
if((int)words3From->size() >= _bowMinInliers && if((int)words3From->size() >= _minInliers &&
(int)words3To->size() >= _bowMinInliers) (int)words3To->size() >= _minInliers)
{ {
std::vector<int> inliersV; std::vector<int> inliersV;
transform = util3d::estimateMotion3DTo3D( transform = util3d::estimateMotion3DTo3D(
uMultimapToMap(*words3From), uMultimapToMap(*words3From),
uMultimapToMap(*words3To), uMultimapToMap(*words3To),
_bowMinInliers, _minInliers,
_bowInlierDistance, _inlierDistance,
_bowIterations, _iterations,
_bowRefineIterations, _refineIterations,
&variance, &variance,
0, 0,
&inliersV); &inliersV);
@@ -327,14 +327,14 @@ Transform RegistrationVis::computeTransformation(
if(transform.isNull()) if(transform.isNull())
{ {
msg = uFormat("Not enough inliers %d/%d between %d and %d", msg = uFormat("Not enough inliers %d/%d between %d and %d",
inliersCount, _bowMinInliers, fromSignature.id(), toSignature.id()); inliersCount, _minInliers, fromSignature.id(), toSignature.id());
UINFO(msg.c_str()); UINFO(msg.c_str());
} }
} }
else else
{ {
msg = uFormat("Not enough 3D features in images (old=%d, new=%d, min=%d)", msg = uFormat("Not enough 3D features in images (old=%d, new=%d, min=%d)",
(int)words3From->size(), (int)words3To->size(), _bowMinInliers); (int)words3From->size(), (int)words3To->size(), _minInliers);
UINFO(msg.c_str()); UINFO(msg.c_str());
} }
} }
@@ -353,14 +353,14 @@ Transform RegistrationVis::computeTransformation(
roll, pitch, yaw); roll, pitch, yaw);
UWARN(msg.c_str()); UWARN(msg.c_str());
} }
else if(_bowForce2D) else if(_force2D)
{ {
UDEBUG("Forcing 2D..."); UDEBUG("Forcing 2D...");
transform = Transform(x,y,0, 0, 0, yaw); transform = Transform(x,y,0, 0, 0, yaw);
} }
} }
if(_bowVarianceFromInliersCount) if(_varianceFromInliersCount)
{ {
variance = inliersCount > 0?1.0/double(inliersCount):1.0; variance = inliersCount > 0?1.0/double(inliersCount):1.0;
} }
corelib/src/Rtabmap.cpp
+35 -35
View File
@@ -90,17 +90,17 @@ Rtabmap::Rtabmap() :
_rgbdLinearUpdate(Parameters::defaultRGBDLinearUpdate()), _rgbdLinearUpdate(Parameters::defaultRGBDLinearUpdate()),
_rgbdAngularUpdate(Parameters::defaultRGBDAngularUpdate()), _rgbdAngularUpdate(Parameters::defaultRGBDAngularUpdate()),
_newMapOdomChangeDistance(Parameters::defaultRGBDNewMapOdomChangeDistance()), _newMapOdomChangeDistance(Parameters::defaultRGBDNewMapOdomChangeDistance()),
_loopClosureIcpRefining(Parameters::defaultRGBDIcpLoopClosureRefining()), _loopClosureRefining(Parameters::defaultRGBDLoopClosureLinkRefining()),
_odomIcpRefining(Parameters::defaultRGBDIcpOdomRefining()), _neighborLinkRefining(Parameters::defaultRGBDNeighborLinkRefining()),
_localLoopClosureDetectionTime(Parameters::defaultRGBDLocalLoopDetectionTime()), _proximityByTime(Parameters::defaultRGBDProximityByTime()),
_localLoopClosureDetectionSpace(Parameters::defaultRGBDLocalLoopDetectionSpace()), _proximityBySpace(Parameters::defaultRGBDProximityBySpace()),
_scanMatchingIdsSavedInLinks(Parameters::defaultRGBDScanMatchingIdsSavedInLinks()), _scanMatchingIdsSavedInLinks(Parameters::defaultRGBDScanMatchingIdsSavedInLinks()),
_localRadius(Parameters::defaultRGBDLocalRadius()), _localRadius(Parameters::defaultRGBDLocalRadius()),
_localImmunizationRatio(Parameters::defaultRGBDLocalImmunizationRatio()), _localImmunizationRatio(Parameters::defaultRGBDLocalImmunizationRatio()),
_localDetectMaxGraphDepth(Parameters::defaultRGBDLocalLoopDetectionMaxGraphDepth()), _proximityMaxGraphDepth(Parameters::defaultRGBDProximityMaxGraphDepth()),
_localPathFilteringRadius(Parameters::defaultRGBDLocalLoopDetectionPathFilteringRadius()), _proximityFilteringRadius(Parameters::defaultRGBDProximityPathFilteringRadius()),
_localPathOdomPosesUsed(Parameters::defaultRGBDLocalLoopDetectionPathOdomPosesUsed()), _proximityRawPosesUsed(Parameters::defaultRGBDProximityPathRawPosesUsed()),
_localPathScansMerged(Parameters::defaultRGBDLocalLoopDetectionPathScansMerged()), _proximityScansMerged(Parameters::defaultRGBDProximityPathScansMerged()),
_databasePath(""), _databasePath(""),
_optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()), _optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()),
_optimizationMaxLinearError(Parameters::defaultRGBDOptimizeMaxError()), _optimizationMaxLinearError(Parameters::defaultRGBDOptimizeMaxError()),
@@ -397,16 +397,16 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rgbdLinearUpdate); Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rgbdLinearUpdate);
Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rgbdAngularUpdate); Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rgbdAngularUpdate);
Parameters::parse(parameters, Parameters::kRGBDNewMapOdomChangeDistance(), _newMapOdomChangeDistance); Parameters::parse(parameters, Parameters::kRGBDNewMapOdomChangeDistance(), _newMapOdomChangeDistance);
Parameters::parse(parameters, Parameters::kRGBDIcpOdomRefining(), _odomIcpRefining); Parameters::parse(parameters, Parameters::kRGBDNeighborLinkRefining(), _neighborLinkRefining);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionTime(), _localLoopClosureDetectionTime); Parameters::parse(parameters, Parameters::kRGBDProximityByTime(), _proximityByTime);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionSpace(), _localLoopClosureDetectionSpace); Parameters::parse(parameters, Parameters::kRGBDProximityBySpace(), _proximityBySpace);
Parameters::parse(parameters, Parameters::kRGBDScanMatchingIdsSavedInLinks(), _scanMatchingIdsSavedInLinks); Parameters::parse(parameters, Parameters::kRGBDScanMatchingIdsSavedInLinks(), _scanMatchingIdsSavedInLinks);
Parameters::parse(parameters, Parameters::kRGBDLocalRadius(), _localRadius); Parameters::parse(parameters, Parameters::kRGBDLocalRadius(), _localRadius);
Parameters::parse(parameters, Parameters::kRGBDLocalImmunizationRatio(), _localImmunizationRatio); Parameters::parse(parameters, Parameters::kRGBDLocalImmunizationRatio(), _localImmunizationRatio);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionMaxGraphDepth(), _localDetectMaxGraphDepth); Parameters::parse(parameters, Parameters::kRGBDProximityMaxGraphDepth(), _proximityMaxGraphDepth);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionPathFilteringRadius(), _localPathFilteringRadius); Parameters::parse(parameters, Parameters::kRGBDProximityPathFilteringRadius(), _proximityFilteringRadius);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionPathOdomPosesUsed(), _localPathOdomPosesUsed); Parameters::parse(parameters, Parameters::kRGBDProximityPathRawPosesUsed(), _proximityRawPosesUsed);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionPathScansMerged(), _localPathScansMerged); Parameters::parse(parameters, Parameters::kRGBDProximityPathScansMerged(), _proximityScansMerged);
Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd); Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
Parameters::parse(parameters, Parameters::kRGBDOptimizeMaxError(), _optimizationMaxLinearError); Parameters::parse(parameters, Parameters::kRGBDOptimizeMaxError(), _optimizationMaxLinearError);
Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure); Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure);
@@ -415,7 +415,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRGBDPlanStuckIterations(), _pathStuckIterations); Parameters::parse(parameters, Parameters::kRGBDPlanStuckIterations(), _pathStuckIterations);
Parameters::parse(parameters, Parameters::kRGBDPlanLinearVelocity(), _pathLinearVelocity); Parameters::parse(parameters, Parameters::kRGBDPlanLinearVelocity(), _pathLinearVelocity);
Parameters::parse(parameters, Parameters::kRGBDPlanAngularVelocity(), _pathAngularVelocity); Parameters::parse(parameters, Parameters::kRGBDPlanAngularVelocity(), _pathAngularVelocity);
Parameters::parse(parameters, Parameters::kRGBDIcpLoopClosureRefining(), _loopClosureIcpRefining); Parameters::parse(parameters, Parameters::kRGBDLoopClosureLinkRefining(), _loopClosureRefining);
UASSERT(_rgbdLinearUpdate >= 0.0f); UASSERT(_rgbdLinearUpdate >= 0.0f);
UASSERT(_rgbdAngularUpdate >= 0.0f); UASSERT(_rgbdAngularUpdate >= 0.0f);
@@ -1000,7 +1000,7 @@ bool Rtabmap::process(
//============================================================ //============================================================
// Scan matching // Scan matching
//============================================================ //============================================================
if(_odomIcpRefining && if(_neighborLinkRefining &&
!signature->sensorData().laserScanCompressed().empty() && !signature->sensorData().laserScanCompressed().empty() &&
rehearsedId == 0) // don't do it if rehearsal happened rehearsedId == 0) // don't do it if rehearsal happened
{ {
@@ -1143,7 +1143,7 @@ bool Rtabmap::process(
//============================================================ //============================================================
// Local loop closure in TIME // Local loop closure in TIME
//============================================================ //============================================================
if(_localLoopClosureDetectionTime && if(_proximityByTime &&
rehearsedId == 0 && // don't do it if rehearsal happened rehearsedId == 0 && // don't do it if rehearsal happened
signature->getWords3().size() && signature->getWords3().size() &&
_memory->isIncremental()) // don't do it in localization mode _memory->isIncremental()) // don't do it in localization mode
@@ -1160,7 +1160,7 @@ bool Rtabmap::process(
float variance = 1.0f; float variance = 1.0f;
int inliers = -1; int inliers = -1;
Transform transform = _memory->computeVisualTransform(signature->id(), *iter, &rejectedMsg, &inliers, &variance); Transform transform = _memory->computeVisualTransform(signature->id(), *iter, &rejectedMsg, &inliers, &variance);
if(!transform.isNull() && _loopClosureIcpRefining) if(!transform.isNull() && _loopClosureRefining)
{ {
transform = _memory->computeIcpTransform(signature->id(), *iter, transform, &rejectedMsg, 0, &variance); transform = _memory->computeIcpTransform(signature->id(), *iter, transform, &rejectedMsg, 0, &variance);
} }
@@ -1718,7 +1718,7 @@ bool Rtabmap::process(
transform = _memory->computeVisualTransform(signature->id(), _loopClosureHypothesis.first, &rejectedMsg, &loopClosureVisualInliers, &variance); transform = _memory->computeVisualTransform(signature->id(), _loopClosureHypothesis.first, &rejectedMsg, &loopClosureVisualInliers, &variance);
if(!transform.isNull() && _loopClosureIcpRefining) if(!transform.isNull() && _loopClosureRefining)
{ {
transform = _memory->computeIcpTransform(signature->id(), _loopClosureHypothesis.first, transform, &rejectedMsg, 0, &variance); transform = _memory->computeIcpTransform(signature->id(), _loopClosureHypothesis.first, transform, &rejectedMsg, 0, &variance);
} }
@@ -1753,7 +1753,7 @@ bool Rtabmap::process(
int localSpaceClosuresAddedByICPOnly = 0; int localSpaceClosuresAddedByICPOnly = 0;
int lastLocalSpaceClosureId = 0; int lastLocalSpaceClosureId = 0;
int localSpacePaths = 0; int localSpacePaths = 0;
if(_localLoopClosureDetectionSpace && if(_proximityBySpace &&
_localRadius > 0 && _localRadius > 0 &&
_rgbdSlamMode && _rgbdSlamMode &&
signature->getWeight() >= 0) // not an intermediate node signature->getWeight() >= 0) // not an intermediate node
@@ -1782,7 +1782,7 @@ bool Rtabmap::process(
std::map<int, float> nearestIds; std::map<int, float> nearestIds;
if(_memory->isIncremental()) if(_memory->isIncremental())
{ {
nearestIds = _memory->getNeighborsIdRadius(signature->id(), _localRadius, _optimizedPoses, _localDetectMaxGraphDepth); nearestIds = _memory->getNeighborsIdRadius(signature->id(), _localRadius, _optimizedPoses, _proximityMaxGraphDepth);
} }
else else
{ {
@@ -1815,18 +1815,18 @@ bool Rtabmap::process(
// nearest pose must not be linked to current location and enough // nearest pose must not be linked to current location and enough
if(!signature->hasLink(nearestId) && if(!signature->hasLink(nearestId) &&
(_localPathFilteringRadius <= 0.0f || (_proximityFilteringRadius <= 0.0f ||
_optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _localPathFilteringRadius*_localPathFilteringRadius)) _optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _proximityFilteringRadius*_proximityFilteringRadius))
{ {
float variance = 1.0f; float variance = 1.0f;
Transform transform = _memory->computeVisualTransform(signature->id(), nearestId, 0, 0, &variance); Transform transform = _memory->computeVisualTransform(signature->id(), nearestId, 0, 0, &variance);
if(!transform.isNull() && _loopClosureIcpRefining) if(!transform.isNull() && _loopClosureRefining)
{ {
transform = _memory->computeIcpTransform(signature->id(), nearestId, transform, 0, 0, &variance); transform = _memory->computeIcpTransform(signature->id(), nearestId, transform, 0, 0, &variance);
} }
if(!transform.isNull()) if(!transform.isNull())
{ {
if(_localPathFilteringRadius <= 0 || transform.getNormSquared() <= _localPathFilteringRadius*_localPathFilteringRadius) if(_proximityFilteringRadius <= 0 || transform.getNormSquared() <= _proximityFilteringRadius*_proximityFilteringRadius)
{ {
UINFO("[Visual] Add local loop closure in SPACE (%d->%d) %s", UINFO("[Visual] Add local loop closure in SPACE (%d->%d) %s",
signature->id(), signature->id(),
@@ -1846,7 +1846,7 @@ bool Rtabmap::process(
{ {
UWARN("Ignoring local loop closure with %d because resulting " UWARN("Ignoring local loop closure with %d because resulting "
"transform is to large!? (%fm > %fm)", "transform is to large!? (%fm > %fm)",
nearestId, transform.getNorm(), _localPathFilteringRadius); nearestId, transform.getNorm(), _proximityFilteringRadius);
} }
} }
} }
@@ -1879,10 +1879,10 @@ bool Rtabmap::process(
// nearest pose must be close and not linked to current location // nearest pose must be close and not linked to current location
if(!signature->hasLink(nearestId) && if(!signature->hasLink(nearestId) &&
(_localPathFilteringRadius <= 0.0f || (_proximityFilteringRadius <= 0.0f ||
_optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _localPathFilteringRadius*_localPathFilteringRadius)) _optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _proximityFilteringRadius*_proximityFilteringRadius))
{ {
if(!_localPathScansMerged) if(!_proximityScansMerged)
{ {
//only keep the nearest node //only keep the nearest node
std::map<int, Transform> tmp; std::map<int, Transform> tmp;
@@ -1892,7 +1892,7 @@ bool Rtabmap::process(
else else
{ {
// Assemble scans in the path and do ICP only // Assemble scans in the path and do ICP only
if(_localPathOdomPosesUsed) if(_proximityRawPosesUsed)
{ {
//optimize the path's poses locally //optimize the path's poses locally
path = optimizeGraph(nearestId, uKeysSet(path), std::map<int, Transform>(), false); path = optimizeGraph(nearestId, uKeysSet(path), std::map<int, Transform>(), false);
@@ -1904,10 +1904,10 @@ bool Rtabmap::process(
jter->second = t * jter->second; jter->second = t * jter->second;
} }
} }
if(path.size() > 2 && _localPathFilteringRadius > 0.0f) if(path.size() > 2 && _proximityFilteringRadius > 0.0f)
{ {
// path filtering // path filtering
std::map<int, Transform> filteredPath = graph::radiusPosesFiltering(path, _localPathFilteringRadius, 0, true); std::map<int, Transform> filteredPath = graph::radiusPosesFiltering(path, _proximityFilteringRadius, 0, true);
// make sure the nearest and farthest poses are still here // make sure the nearest and farthest poses are still here
filteredPath.insert(*path.find(nearestId)); filteredPath.insert(*path.find(nearestId));
filteredPath.insert(*path.begin()); filteredPath.insert(*path.begin());
@@ -1927,7 +1927,7 @@ bool Rtabmap::process(
Transform transform = _memory->computeScanMatchingTransform(signature->id(), nearestId, path, 0, 0, &variance); Transform transform = _memory->computeScanMatchingTransform(signature->id(), nearestId, path, 0, 0, &variance);
if(!transform.isNull()) if(!transform.isNull())
{ {
if(_localPathFilteringRadius <= 0 || transform.getNormSquared() <= _localPathFilteringRadius*_localPathFilteringRadius) if(_proximityFilteringRadius <= 0 || transform.getNormSquared() <= _proximityFilteringRadius*_proximityFilteringRadius)
{ {
UINFO("[Scan matching] Add local loop closure in SPACE (%d->%d) %s", UINFO("[Scan matching] Add local loop closure in SPACE (%d->%d) %s",
signature->id(), signature->id(),
@@ -1973,7 +1973,7 @@ bool Rtabmap::process(
{ {
UWARN("Ignoring local loop closure with %d because resulting " UWARN("Ignoring local loop closure with %d because resulting "
"transform is to large!? (%fm > %fm)", "transform is to large!? (%fm > %fm)",
nearestId, transform.getNorm(), _localPathFilteringRadius); nearestId, transform.getNorm(), _proximityFilteringRadius);
} }
} }
} }
corelib/src/util3d_filtering.cpp
+4 -4
View File
@@ -69,7 +69,7 @@ cv::Mat downsample(
int finalSize = cloud.cols/step; int finalSize = cloud.cols/step;
output = cv::Mat(1, finalSize, cloud.type()); output = cv::Mat(1, finalSize, cloud.type());
int oi = 0; int oi = 0;
for(unsigned int i=0; i<cloud.cols-step+1; i+=step) for(int i=0; i<cloud.cols-step+1; i+=step)
{ {
cv::Mat(cloud, cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(output, cv::Range::all(), cv::Range(oi,oi+1))); cv::Mat(cloud, cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(output, cv::Range::all(), cv::Range(oi,oi+1)));
++oi; ++oi;
@@ -96,7 +96,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr downsample(
} }
else else
{ {
if(cloud->size() > step) if((int)cloud->size() > step)
{ {
int finalSize = cloud->size()/step; int finalSize = cloud->size()/step;
output->resize(finalSize); output->resize(finalSize);
@@ -126,12 +126,12 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr downsample(
} }
else else
{ {
if(cloud->size() > step) if((int)cloud->size() > step)
{ {
int finalSize = cloud->size()/step; int finalSize = cloud->size()/step;
output->resize(finalSize); output->resize(finalSize);
int oi = 0; int oi = 0;
for(unsigned int i=0; i<cloud->size()-step+1; i+=step) for(int i=0; i<(int)cloud->size()-step+1; i+=step)
{ {
(*output)[oi++] = cloud->at(i); (*output)[oi++] = cloud->at(i);
} }
guilib/src/PreferencesDialog.cpp
+8 -9
View File
@@ -478,7 +478,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->general_checkBox_generateIds->setObjectName(Parameters::kMemGenerateIds().c_str()); _ui->general_checkBox_generateIds->setObjectName(Parameters::kMemGenerateIds().c_str());
_ui->general_checkBox_badSignaturesIgnored->setObjectName(Parameters::kMemBadSignaturesIgnored().c_str()); _ui->general_checkBox_badSignaturesIgnored->setObjectName(Parameters::kMemBadSignaturesIgnored().c_str());
_ui->general_checkBox_initWMWithAllNodes->setObjectName(Parameters::kMemInitWMWithAllNodes().c_str()); _ui->general_checkBox_initWMWithAllNodes->setObjectName(Parameters::kMemInitWMWithAllNodes().c_str());
_ui->checkBox_localSpaceLinksKeptInWM->setObjectName(Parameters::kMemLocalSpaceLinksKeptInWM().c_str());
_ui->checkBox_localSpaceScanMatchingIDsSaved->setObjectName(Parameters::kRGBDScanMatchingIdsSavedInLinks().c_str()); _ui->checkBox_localSpaceScanMatchingIDsSaved->setObjectName(Parameters::kRGBDScanMatchingIdsSavedInLinks().c_str());
_ui->spinBox_imageDecimation->setObjectName(Parameters::kMemImageDecimation().c_str()); _ui->spinBox_imageDecimation->setObjectName(Parameters::kMemImageDecimation().c_str());
_ui->general_spinBox_laserScanDownsample->setObjectName(Parameters::kMemLaserScanDownsampleStepSize().c_str()); _ui->general_spinBox_laserScanDownsample->setObjectName(Parameters::kMemLaserScanDownsampleStepSize().c_str());
@@ -590,7 +589,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->rgdb_angularUpdate->setObjectName(Parameters::kRGBDAngularUpdate().c_str()); _ui->rgdb_angularUpdate->setObjectName(Parameters::kRGBDAngularUpdate().c_str());
_ui->rgdb_rehearsalWeightIgnoredWhileMoving->setObjectName(Parameters::kMemRehearsalWeightIgnoredWhileMoving().c_str()); _ui->rgdb_rehearsalWeightIgnoredWhileMoving->setObjectName(Parameters::kMemRehearsalWeightIgnoredWhileMoving().c_str());
_ui->rgdb_newMapOdomChange->setObjectName(Parameters::kRGBDNewMapOdomChangeDistance().c_str()); _ui->rgdb_newMapOdomChange->setObjectName(Parameters::kRGBDNewMapOdomChangeDistance().c_str());
_ui->odomScanHistory->setObjectName(Parameters::kRGBDIcpOdomRefining().c_str()); _ui->odomScanHistory->setObjectName(Parameters::kRGBDNeighborLinkRefining().c_str());
_ui->spinBox_maxLocalLocationsRetrieved->setObjectName(Parameters::kRGBDMaxLocalRetrieved().c_str()); _ui->spinBox_maxLocalLocationsRetrieved->setObjectName(Parameters::kRGBDMaxLocalRetrieved().c_str());
_ui->graphOptimization_type->setObjectName(Parameters::kRGBDOptimizeStrategy().c_str()); _ui->graphOptimization_type->setObjectName(Parameters::kRGBDOptimizeStrategy().c_str());
@@ -608,13 +607,13 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->graphPlan_linearVelocity->setObjectName(Parameters::kRGBDPlanLinearVelocity().c_str()); _ui->graphPlan_linearVelocity->setObjectName(Parameters::kRGBDPlanLinearVelocity().c_str());
_ui->graphPlan_angularVelocity->setObjectName(Parameters::kRGBDPlanAngularVelocity().c_str()); _ui->graphPlan_angularVelocity->setObjectName(Parameters::kRGBDPlanAngularVelocity().c_str());
_ui->groupBox_localDetection_time->setObjectName(Parameters::kRGBDLocalLoopDetectionTime().c_str()); _ui->groupBox_localDetection_time->setObjectName(Parameters::kRGBDProximityByTime().c_str());
_ui->groupBox_localDetection_space->setObjectName(Parameters::kRGBDLocalLoopDetectionSpace().c_str()); _ui->groupBox_localDetection_space->setObjectName(Parameters::kRGBDProximityBySpace().c_str());
_ui->localDetection_radius->setObjectName(Parameters::kRGBDLocalRadius().c_str()); _ui->localDetection_radius->setObjectName(Parameters::kRGBDLocalRadius().c_str());
_ui->localDetection_maxDiffID->setObjectName(Parameters::kRGBDLocalLoopDetectionMaxGraphDepth().c_str()); _ui->localDetection_maxDiffID->setObjectName(Parameters::kRGBDProximityMaxGraphDepth().c_str());
_ui->localDetection_pathFilteringRadius->setObjectName(Parameters::kRGBDLocalLoopDetectionPathFilteringRadius().c_str()); _ui->localDetection_pathFilteringRadius->setObjectName(Parameters::kRGBDProximityPathFilteringRadius().c_str());
_ui->checkBox_localSpacePathOdomPosesUsed->setObjectName(Parameters::kRGBDLocalLoopDetectionPathOdomPosesUsed().c_str()); _ui->checkBox_localSpacePathOdomPosesUsed->setObjectName(Parameters::kRGBDProximityPathRawPosesUsed().c_str());
_ui->checkBox_localSpaceAssembleScans->setObjectName(Parameters::kRGBDLocalLoopDetectionPathScansMerged().c_str()); _ui->checkBox_localSpaceAssembleScans->setObjectName(Parameters::kRGBDProximityPathScansMerged().c_str());
_ui->rgdb_localImmunizationRatio->setObjectName(Parameters::kRGBDLocalImmunizationRatio().c_str()); _ui->rgdb_localImmunizationRatio->setObjectName(Parameters::kRGBDLocalImmunizationRatio().c_str());
_ui->loopClosure_bowMinInliers->setObjectName(Parameters::kVisMinInliers().c_str()); _ui->loopClosure_bowMinInliers->setObjectName(Parameters::kVisMinInliers().c_str());
@@ -642,7 +641,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->subpix_iterations->setObjectName(Parameters::kVisSubPixIterations().c_str()); _ui->subpix_iterations->setObjectName(Parameters::kVisSubPixIterations().c_str());
_ui->subpix_eps->setObjectName(Parameters::kVisSubPixEps().c_str()); _ui->subpix_eps->setObjectName(Parameters::kVisSubPixEps().c_str());
_ui->loopClosure_icp->setObjectName(Parameters::kRGBDIcpLoopClosureRefining().c_str()); _ui->loopClosure_icp->setObjectName(Parameters::kRGBDLoopClosureLinkRefining().c_str());
_ui->globalDetection_icpMaxTranslation->setObjectName(Parameters::kIcpMaxTranslation().c_str()); _ui->globalDetection_icpMaxTranslation->setObjectName(Parameters::kIcpMaxTranslation().c_str());
_ui->globalDetection_icpMaxRotation->setObjectName(Parameters::kIcpMaxRotation().c_str()); _ui->globalDetection_icpMaxRotation->setObjectName(Parameters::kIcpMaxRotation().c_str());
_ui->loopClosure_icp2D->setObjectName(Parameters::kIcp2D().c_str()); _ui->loopClosure_icp2D->setObjectName(Parameters::kIcp2D().c_str());
guilib/src/ui/preferencesDialog.ui
+29 -49
View File
@@ -63,9 +63,9 @@
<property name="geometry"> <property name="geometry">
<rect> <rect>
<x>0</x> <x>0</x>
<y>0</y> <y>-1112</y>
<width>755</width> <width>760</width>
<height>1854</height> <height>1802</height>
</rect> </rect>
</property> </property>
<layout class="QVBoxLayout" name="verticalLayout_16"> <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum> <enum>QFrame::Raised</enum>
</property> </property>
<property name="currentIndex"> <property name="currentIndex">
<number>8</number> <number>20</number>
</property> </property>
<widget class="QWidget" name="page_22"> <widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29"> <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -6935,7 +6935,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<item row="3" column="1"> <item row="3" column="1">
<widget class="QLabel" name="label_scanMatching"> <widget class="QLabel" name="label_scanMatching">
<property name="text"> <property name="text">
<string>Pose ICP refining: odometry pose correction using ICP.</string> <string>Neighbor link refining. When a new node is added to the graph, the transformation of its neighbor link with the previous node is refined using ICP (laser scans required!).</string>
</property> </property>
<property name="wordWrap"> <property name="wordWrap">
<bool>true</bool> <bool>true</bool>
@@ -6987,7 +6987,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<item row="4" column="1"> <item row="4" column="1">
<widget class="QLabel" name="label_scanMatching_8"> <widget class="QLabel" name="label_scanMatching_8">
<property name="text"> <property name="text">
<string>ICP refining. When enabled, loop closure constraints are refined with ICP estimation (3D or 2D). See &quot;ICP&quot; panel for parameters.</string> <string>Loop closure link refining. If the estimated loop closure transformation is refined using ICP (laser scans required!).</string>
</property> </property>
<property name="wordWrap"> <property name="wordWrap">
<bool>true</bool> <bool>true</bool>
@@ -7007,7 +7007,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<item row="5" column="1"> <item row="5" column="1">
<widget class="QLabel" name="label_scanMatching_9"> <widget class="QLabel" name="label_scanMatching_9">
<property name="text"> <property name="text">
<string>Re-extract visual features when computing loop closure constraints.</string> <string>Re-extract visual features when computing loop closure transformations.</string>
</property> </property>
<property name="wordWrap"> <property name="wordWrap">
<bool>true</bool> <bool>true</bool>
@@ -7268,7 +7268,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<item> <item>
<widget class="QGroupBox" name="groupBox_localDetection_time"> <widget class="QGroupBox" name="groupBox_localDetection_time">
<property name="title"> <property name="title">
<string>Local loop closure detection in time</string> <string>Proximity detection in time</string>
</property> </property>
<property name="checkable"> <property name="checkable">
<bool>true</bool> <bool>true</bool>
@@ -7277,7 +7277,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<item> <item>
<widget class="QLabel" name="label_54"> <widget class="QLabel" name="label_54">
<property name="text"> <property name="text">
<string>Activate local detection over all locations in STM. The Bayes filter is not used here: If there are enough correspondences between the current image and others in STM, transformations are computed. This generates more constraints in the map's graph, so more time is required to optimize the graph.</string> <string>Activate proximity detection over all locations in STM. The Bayes filter is not used here: If there are enough correspondences between the current image and others in STM, transformations are computed. This generates more constraints in the map's graph, so more time is required to optimize the graph.</string>
</property> </property>
<property name="wordWrap"> <property name="wordWrap">
<bool>true</bool> <bool>true</bool>
@@ -7293,7 +7293,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<item> <item>
<widget class="QGroupBox" name="groupBox_localDetection_space"> <widget class="QGroupBox" name="groupBox_localDetection_space">
<property name="title"> <property name="title">
<string>Local loop closure detection in space</string> <string>Proximity detection in space</string>
</property> </property>
<property name="checkable"> <property name="checkable">
<bool>true</bool> <bool>true</bool>
@@ -7302,7 +7302,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<item> <item>
<widget class="QLabel" name="label_space1_2"> <widget class="QLabel" name="label_space1_2">
<property name="text"> <property name="text">
<string>Activate local detection over locations (in Working Memory) near in space. The locations are compared to those inside the local radius of the current estimated position of the robot (at a maximum distance of the path filtering radius below). If the laser scans are used, local locations are merged together and the current laser scan is compared to them. It is useful when the robot is visiting backward (camera not in the same direction) already visited locations.</string> <string>Activate proximity detection over locations (in Working Memory) near in space. The locations are compared to those inside the local radius of the current estimated position of the robot (at a maximum distance of the path filtering radius below). If the laser scans are used, local locations are merged together and the current laser scan is compared to them. It is useful when the robot is visiting backward (camera not in the same direction) already visited locations.</string>
</property> </property>
<property name="wordWrap"> <property name="wordWrap">
<bool>true</bool> <bool>true</bool>
@@ -7314,6 +7314,22 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</item> </item>
<item> <item>
<layout class="QGridLayout" name="gridLayout_50" columnstretch="0,1"> <layout class="QGridLayout" name="gridLayout_50" columnstretch="0,1">
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="localDetection_pathFilteringRadius">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>1.000000000000000</double>
</property>
</widget>
</item>
<item row="2" column="1"> <item row="2" column="1">
<widget class="QLabel" name="label_space3_3"> <widget class="QLabel" name="label_space3_3">
<property name="text"> <property name="text">
@@ -7347,22 +7363,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property> </property>
</widget> </widget>
</item> </item>
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="localDetection_pathFilteringRadius">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>1.000000000000000</double>
</property>
</widget>
</item>
<item row="0" column="0"> <item row="0" column="0">
<widget class="QSpinBox" name="localDetection_maxDiffID"> <widget class="QSpinBox" name="localDetection_maxDiffID">
<property name="maximum"> <property name="maximum">
@@ -7376,7 +7376,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<item row="0" column="1"> <item row="0" column="1">
<widget class="QLabel" name="label_space3_2"> <widget class="QLabel" name="label_space3_2">
<property name="text"> <property name="text">
<string>Maximum graph depth between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.</string> <string>Maximum graph depth between the current/last loop closure location and the proximity hypotheses. Set 0 to ignore.</string>
</property> </property>
<property name="wordWrap"> <property name="wordWrap">
<bool>true</bool> <bool>true</bool>
@@ -7386,27 +7386,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property> </property>
</widget> </widget>
</item> </item>
<item row="5" column="0">
<widget class="QCheckBox" name="checkBox_localSpaceLinksKeptInWM">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="5" column="1"> <item row="5" column="1">
<widget class="QLabel" name="label_scanMatching_2">
<property name="text">
<string>If local space links are kept in WM.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_scanMatching_6"> <widget class="QLabel" name="label_scanMatching_6">
<property name="text"> <property name="text">
<string>Save scan matching IDs in link's user data.</string> <string>Save scan matching IDs in link's user data.</string>
@@ -7419,7 +7399,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property> </property>
</widget> </widget>
</item> </item>
<item row="6" column="0"> <item row="5" column="0">
<widget class="QCheckBox" name="checkBox_localSpaceScanMatchingIDsSaved"> <widget class="QCheckBox" name="checkBox_localSpaceScanMatchingIDsSaved">
<property name="text"> <property name="text">
<string/> <string/>