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

Refactored motion estimation parameters in the GUI.

Browse Source
This commit is contained in:
matlabbe
2015-06-27 01:43:29 -04:00
parent e5447be23a
commit b7faef35f1
14 changed files with 921 additions and 775 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/Memory.h
+2 -4
View File
@@ -178,7 +178,6 @@ public:
float getBowInlierDistance() const {return _bowInlierDistance;}
int getBowIterations() const {return _bowIterations;}
int getBowMinInliers() const {return _bowMinInliers;}
float getBowMaxDepth() const {return _bowMaxDepth;}
bool getBowForce2D() const {return _bowForce2D;}
Transform computeVisualTransform(int oldId, int newId, std::string * rejectedMsg = 0, int * inliers = 0, double * variance = 0) const;
Transform computeVisualTransform(const Signature & oldS, const Signature & newS, std::string * rejectedMsg = 0, int * inliers = 0, double * variance = 0) const;
@@ -271,11 +270,10 @@ private:
int _bowMinInliers;
float _bowInlierDistance;
int _bowIterations;
float _bowMaxDepth;
int _bowRefineIterations;
bool _bowForce2D;
bool _bowEpipolarGeometry;
float _bowEpipolarGeometryVar;
bool _bowPnPEstimation;
bool _bowEstimationType;
double _bowPnPReprojError;
int _bowPnPFlags;
float _icpMaxTranslation;
corelib/include/rtabmap/core/Odometry.h
+2 -2
View File
@@ -60,7 +60,7 @@ public:
int getRefineIterations() const {return _refineIterations;}
float getMaxDepth() const {return _maxDepth;}
bool isInfoDataFilled() const {return _fillInfoData;}
bool isPnPEstimationUsed() const {return _pnpEstimation;}
bool getEstimationType() const {return _estimationType;}
double getPnPReprojError() const {return _pnpReprojError;}
int getPnPFlags() const {return _pnpFlags;}
const Transform & previousTransform() const {return previousTransform_;}
@@ -85,7 +85,7 @@ private:
float _particleNoiseR;
float _particleLambdaR;
bool _fillInfoData;
bool _pnpEstimation;
int _estimationType;
double _pnpReprojError;
int _pnpFlags;
Transform _pose;
corelib/include/rtabmap/core/Parameters.h
+5 -5
View File
@@ -313,6 +313,7 @@ class RTABMAP_EXP Parameters
// Odometry
RTABMAP_PARAM(Odom, Strategy, int, 0, "0=Bag-of-words 1=Optical Flow");
RTABMAP_PARAM(Odom, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
RTABMAP_PARAM(Odom, EstimationType, int, 0, "Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP)");
RTABMAP_PARAM(Odom, MaxFeatures, int, 400, "0 no limits.");
RTABMAP_PARAM(Odom, InlierDistance, float, 0.02, "Maximum distance for visual word correspondences.");
RTABMAP_PARAM(Odom, MinInliers, int, 20, "Minimum visual word correspondences to compute geometry transform.");
@@ -325,7 +326,6 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Odom, Holonomic, bool, true, "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)).");
RTABMAP_PARAM(Odom, FillInfoData, bool, true, "Fill info with data (inliers/outliers features).");
RTABMAP_PARAM(Odom, ImageBufferSize, unsigned int, 1, "Data buffer size (0 min inf).");
RTABMAP_PARAM(Odom, PnPEstimation, bool, false, "(PnP) Pose estimation from 2D to 3D correspondences instead of 3D to 3D correspondences.");
RTABMAP_PARAM(Odom, PnPReprojError, double, 5.0, "PnP reprojection error.");
RTABMAP_PARAM(Odom, PnPFlags, int, 1, "PnP flags: 0=Iterative, 1=EPNP, 2=P3P");
RTABMAP_PARAM(Odom, ParticleFiltering, bool, false, "Particle filtering to smooth the odometry trajectory.");
@@ -363,14 +363,13 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(LccIcp, MaxTranslation, float, 0.2, "Maximum ICP translation correction accepted (m).");
RTABMAP_PARAM(LccIcp, MaxRotation, float, 0.78, "Maximum ICP rotation correction accepted (rad).");
RTABMAP_PARAM(LccBow, EstimationType, int, 0, "Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP), 2:2D->2D (Epipolar Geometry)");
RTABMAP_PARAM(LccBow, MinInliers, int, 20, "Minimum visual word correspondences to compute geometry transform.");
RTABMAP_PARAM(LccBow, InlierDistance, float, 0.02, "Maximum distance for visual word correspondences.");
RTABMAP_PARAM(LccBow, Iterations, int, 100, "Maximum iterations to compute the transform from visual words.");
RTABMAP_PARAM(LccBow, MaxDepth, float, 4.0, "Max depth of the words (0 means no limit).");
RTABMAP_PARAM(LccBow, Force2D, bool, false, "Force 2D transform (3Dof: x,y and yaw).");
RTABMAP_PARAM(LccBow, EpipolarGeometry, bool, false, "Use epipolar geometry to compute the loop closure transform.");
RTABMAP_PARAM(LccBow, RefineIterations, int, 10, "Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.");
RTABMAP_PARAM(LccBow, Force2D, bool, false, "Force 2D transform (3Dof: x,y and yaw).");
RTABMAP_PARAM(LccBow, EpipolarGeometryVar, float, 0.02, "Epipolar geometry maximum variance to accept the loop closure.");
RTABMAP_PARAM(LccBow, PnPEstimation, bool, false, "(PnP) Pose estimation from 2D to 3D correspondences instead of 3D to 3D correspondences.");
RTABMAP_PARAM(LccBow, PnPReprojError, double, 5.0, "PnP reprojection error.");
RTABMAP_PARAM(LccBow, PnPFlags, int, 1, "PnP flags: 0=Iterative, 1=EPNP, 2=P3P");
RTABMAP_PARAM_COND(LccReextract, Activated, bool, RTABMAP_NONFREE, false, true, "Activate re-extracting features on global loop closure.");
@@ -378,6 +377,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(LccReextract, NNDR, float, 0.8, "NNDR: nearest neighbor distance ratio.");
RTABMAP_PARAM(LccReextract, FeatureType, int, 4, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
RTABMAP_PARAM(LccReextract, MaxWords, int, 600, "0 no limits.");
RTABMAP_PARAM(LccReextract, MaxDepth, float, 0.0, "Max depth of the words (0 means no limit).");
RTABMAP_PARAM(LccIcp3, Decimation, int, 8, "Depth image decimation.");
RTABMAP_PARAM(LccIcp3, MaxDepth, float, 4.0, "Max cloud depth.");
corelib/include/rtabmap/core/Rtabmap.h
+1
View File
@@ -193,6 +193,7 @@ private:
float _reextractNNDR;
int _reextractFeatureType;
int _reextractMaxWords;
float _reextractMaxDepth;
bool _startNewMapOnLoopClosure;
float _goalReachedRadius; // meters
bool _planVirtualLinks;
corelib/src/Memory.cpp
+9 -25
View File
@@ -101,11 +101,10 @@ Memory::Memory(const ParametersMap & parameters) :
_bowMinInliers(Parameters::defaultLccBowMinInliers()),
_bowInlierDistance(Parameters::defaultLccBowInlierDistance()),
_bowIterations(Parameters::defaultLccBowIterations()),
_bowMaxDepth(Parameters::defaultLccBowMaxDepth()),
_bowRefineIterations(Parameters::defaultLccBowRefineIterations()),
_bowForce2D(Parameters::defaultLccBowForce2D()),
_bowEpipolarGeometry(Parameters::defaultLccBowEpipolarGeometry()),
_bowEpipolarGeometryVar(Parameters::defaultLccBowEpipolarGeometryVar()),
_bowPnPEstimation(Parameters::defaultLccBowPnPEstimation()),
_bowEstimationType(Parameters::defaultLccBowEstimationType()),
_bowPnPReprojError(Parameters::defaultLccBowPnPReprojError()),
_bowPnPFlags(Parameters::defaultLccBowPnPFlags()),
@@ -441,11 +440,10 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kLccBowMinInliers(), _bowMinInliers);
Parameters::parse(parameters, Parameters::kLccBowInlierDistance(), _bowInlierDistance);
Parameters::parse(parameters, Parameters::kLccBowIterations(), _bowIterations);
Parameters::parse(parameters, Parameters::kLccBowMaxDepth(), _bowMaxDepth);
Parameters::parse(parameters, Parameters::kLccBowRefineIterations(), _bowRefineIterations);
Parameters::parse(parameters, Parameters::kLccBowForce2D(), _bowForce2D);
Parameters::parse(parameters, Parameters::kLccBowEpipolarGeometry(), _bowEpipolarGeometry);
Parameters::parse(parameters, Parameters::kLccBowEstimationType(), _bowEstimationType);
Parameters::parse(parameters, Parameters::kLccBowEpipolarGeometryVar(), _bowEpipolarGeometryVar);
Parameters::parse(parameters, Parameters::kLccBowPnPEstimation(), _bowPnPEstimation);
Parameters::parse(parameters, Parameters::kLccBowPnPReprojError(), _bowPnPReprojError);
Parameters::parse(parameters, Parameters::kLccBowPnPFlags(), _bowPnPFlags);
Parameters::parse(parameters, Parameters::kLccIcpMaxTranslation(), _icpMaxTranslation);
@@ -474,7 +472,6 @@ void Memory::parseParameters(const ParametersMap & parameters)
UASSERT_MSG(_bowMinInliers >= 1, uFormat("value=%d", _bowMinInliers).c_str());
UASSERT_MSG(_bowInlierDistance > 0.0f, uFormat("value=%f", _bowInlierDistance).c_str());
UASSERT_MSG(_bowIterations > 0, uFormat("value=%d", _bowIterations).c_str());
UASSERT_MSG(_bowMaxDepth >= 0.0f, uFormat("value=%f", _bowMaxDepth).c_str());
UASSERT_MSG(_icpDecimation > 0, uFormat("value=%d", _icpDecimation).c_str());
UASSERT_MSG(_icpMaxDepth >= 0.0f, uFormat("value=%f", _icpMaxDepth).c_str());
UASSERT_MSG(_icpVoxelSize >= 0, uFormat("value=%d", _icpVoxelSize).c_str());
@@ -2020,7 +2017,7 @@ Transform Memory::computeVisualTransform(
int inliersCount= 0;
double variance = 1.0;
if(_bowEpipolarGeometry && !_bowPnPEstimation)
if(_bowEstimationType == 2) // Epipolar Geometry
{
if(!newS.sensorData().stereoCameraModel().isValid() &&
(newS.sensorData().cameraModels().size() != 1 ||
@@ -2088,13 +2085,8 @@ Transform Memory::computeVisualTransform(
UWARN(msg.c_str());
}
}
else if(_bowPnPEstimation)
else if(_bowEstimationType == 1) // PnP
{
if(_bowEpipolarGeometry)
{
UWARN("PnP estimation and Epipolar geometry estimation are set, only PnP is used.");
}
if(!newS.sensorData().stereoCameraModel().isValid() &&
(newS.sensorData().cameraModels().size() != 1 ||
!newS.sensorData().cameraModels()[0].isValid()))
@@ -2158,7 +2150,7 @@ Transform Memory::computeVisualTransform(
_bowMinInliers,
_bowInlierDistance,
_bowIterations,
10,
_bowRefineIterations,
&variance,
0,
&inliersV);
@@ -2919,14 +2911,7 @@ void Memory::dumpSignatures(const char * fileNameSign, bool words3D) const
if(foutSign)
{
if(words3D)
{
fprintf(foutSign, "SignatureID WordsID... (Max features depth=%f)\n", _bowMaxDepth);
}
else
{
fprintf(foutSign, "SignatureID WordsID...\n");
}
fprintf(foutSign, "SignatureID WordsID...\n");
const std::map<int, Signature *> & signatures = this->getSignatures();
for(std::map<int, Signature *>::const_iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
{
@@ -2941,8 +2926,7 @@ void Memory::dumpSignatures(const char * fileNameSign, bool words3D) const
{
//show only valid point according to current parameters
if(pcl::isFinite(jter->second) &&
(jter->second.x != 0 || jter->second.y != 0 || jter->second.z != 0) &&
(_bowMaxDepth <= 0 || jter->second.x <= _bowMaxDepth))
(jter->second.x != 0 || jter->second.y != 0 || jter->second.z != 0))
{
fprintf(foutSign, "%d ", (*jter).first);
}
corelib/src/Odometry.cpp
+2 -2
View File
@@ -51,7 +51,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_particleNoiseR(Parameters::defaultOdomParticleNoiseR()),
_particleLambdaR(Parameters::defaultOdomParticleLambdaR()),
_fillInfoData(Parameters::defaultOdomFillInfoData()),
_pnpEstimation(Parameters::defaultOdomPnPEstimation()),
_estimationType(Parameters::defaultOdomEstimationType()),
_pnpReprojError(Parameters::defaultOdomPnPReprojError()),
_pnpFlags(Parameters::defaultOdomPnPFlags()),
_resetCurrentCount(0),
@@ -69,7 +69,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
Parameters::parse(parameters, Parameters::kOdomForce2D(), _force2D);
Parameters::parse(parameters, Parameters::kOdomHolonomic(), _holonomic);
Parameters::parse(parameters, Parameters::kOdomFillInfoData(), _fillInfoData);
Parameters::parse(parameters, Parameters::kOdomPnPEstimation(), _pnpEstimation);
Parameters::parse(parameters, Parameters::kOdomEstimationType(), _estimationType);
Parameters::parse(parameters, Parameters::kOdomPnPReprojError(), _pnpReprojError);
Parameters::parse(parameters, Parameters::kOdomPnPFlags(), _pnpFlags);
UASSERT(_pnpFlags>=0 && _pnpFlags <=2);
corelib/src/OdometryBOW.cpp
+1 -1
View File
@@ -232,7 +232,7 @@ Transform OdometryBOW::computeTransform(
{
std::vector<int> matches, inliers;
Transform t;
if(this->isPnPEstimationUsed())
if(this->getEstimationType() == 1) // PnP
{
// 3D to 2D
if(data.cameraModels().size() > 1)
corelib/src/OdometryOpticalFlow.cpp
+1 -1
View File
@@ -234,7 +234,7 @@ Transform OdometryOpticalFlow::computeTransform(
if(ki && ki >= this->getMinInliers())
{
if(this->isPnPEstimationUsed())
if(this->getEstimationType() == 1) // PnP
{
// find correspondences
if(this->isInfoDataFilled() && info)
corelib/src/Rtabmap.cpp
+4
View File
@@ -105,6 +105,7 @@ Rtabmap::Rtabmap() :
_reextractNNDR(Parameters::defaultLccReextractNNDR()),
_reextractFeatureType(Parameters::defaultLccReextractFeatureType()),
_reextractMaxWords(Parameters::defaultLccReextractMaxWords()),
_reextractMaxDepth(Parameters::defaultLccReextractMaxDepth()),
_startNewMapOnLoopClosure(Parameters::defaultRtabmapStartNewMapOnLoopClosure()),
_goalReachedRadius(Parameters::defaultRGBDGoalReachedRadius()),
_planVirtualLinks(Parameters::defaultRGBDPlanVirtualLinks()),
@@ -402,6 +403,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kLccReextractNNDR(), _reextractNNDR);
Parameters::parse(parameters, Parameters::kLccReextractFeatureType(), _reextractFeatureType);
Parameters::parse(parameters, Parameters::kLccReextractMaxWords(), _reextractMaxWords);
Parameters::parse(parameters, Parameters::kLccReextractMaxDepth(), _reextractMaxDepth);
Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure);
Parameters::parse(parameters, Parameters::kRGBDGoalReachedRadius(), _goalReachedRadius);
Parameters::parse(parameters, Parameters::kRGBDPlanVirtualLinks(), _planVirtualLinks);
@@ -1627,6 +1629,7 @@ bool Rtabmap::process(
uInsert(customParameters, ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(_reextractNNDR)));
uInsert(customParameters, ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(_reextractFeatureType))); // FAST/BRIEF
uInsert(customParameters, ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(_reextractMaxWords)));
uInsert(customParameters, ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(_reextractMaxDepth)));
uInsert(customParameters, ParametersPair(Parameters::kKpBadSignRatio(), "0"));
uInsert(customParameters, ParametersPair(Parameters::kKpRoiRatios(), "0.0 0.0 0.0 0.0"));
uInsert(customParameters, ParametersPair(Parameters::kMemGenerateIds(), "false"));
@@ -1783,6 +1786,7 @@ bool Rtabmap::process(
uInsert(customParameters, ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(_reextractNNDR)));
uInsert(customParameters, ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(_reextractFeatureType))); // FAST/BRIEF
uInsert(customParameters, ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(_reextractMaxWords)));
uInsert(customParameters, ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(_reextractMaxDepth)));
uInsert(customParameters, ParametersPair(Parameters::kKpBadSignRatio(), "0"));
uInsert(customParameters, ParametersPair(Parameters::kKpRoiRatios(), "0.0 0.0 0.0 0.0"));
uInsert(customParameters, ParametersPair(Parameters::kMemGenerateIds(), "false"));