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

Refactoring Odometry

Browse Source
This commit is contained in:
matlabbe
2016-01-07 13:59:21 -05:00
parent 923ba78bfa
commit 6d399b6fb9
18 changed files with 255 additions and 268 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
corelib/include/rtabmap/core/Odometry.h
View File

@@ -59,18 +59,7 @@ public:
//getters
const Transform & getPose() const {return _pose;}
const std::string & getRoiRatios() const {return _roiRatios;}
int getMinInliers() const {return _minInliers;}
float getInlierDistance() const {return _inlierDistance;}
int getIterations() const {return _iterations;}
int getRefineIterations() const {return _refineIterations;}
float getMinDepth() const {return _minDepth;}
float getMaxDepth() const {return _maxDepth;}
bool isInfoDataFilled() const {return _fillInfoData;}
int getEstimationType() const {return _estimationType;}
double getPnPReprojError() const {return _pnpReprojError;}
int getPnPFlags() const {return _pnpFlags;}
int getPnPRefineIterations() const {return _pnpRefineIterations;}
const Transform & previousTransform() const {return previousTransform_;}
bool isVarianceFromInliersCount() const {return _varianceFromInliersCount;}
@@ -81,15 +70,8 @@ private:
void updateKalmanFilter(float dt, float & x, float & y, float & z, float & roll, float & pitch, float & yaw);
private:
std::string _roiRatios;
int _minInliers;
float _inlierDistance;
int _iterations;
int _refineIterations;
float _minDepth;
float _maxDepth;
int _resetCountdown;
bool _force2D;
bool _force3DoF;
bool _holonomic;
int _filteringStrategy;
int _particleSize;
@@ -98,10 +80,6 @@ private:
float _particleNoiseR;
float _particleLambdaR;
bool _fillInfoData;
int _estimationType;
double _pnpReprojError;
int _pnpFlags;
int _pnpRefineIterations;
bool _varianceFromInliersCount;
float _kalmanProcessNoise;
float _kalmanMeasurementNoise;

14
corelib/include/rtabmap/core/OdometryLocalMap.h
View File

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
class Memory;
class RegistrationVis;
class RTABMAP_EXP OdometryLocalMap : public Odometry
{
@@ -41,19 +42,20 @@ public:
virtual ~OdometryLocalMap();
virtual void reset(const Transform & initialPose = Transform::getIdentity());
const std::map<int, cv::Point3f> & getLocalMap() const {return localMap_;}
const Memory * getMemory() const {return _memory;}
const std::multimap<int, cv::Point3f> & getLocalMap() const {return localMap_;}
const Memory * getMemory() const {return memory_;}
private:
virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0);
private:
//Parameters
int _localHistoryMaxSize;
std::string _fixedLocalMapPath;
int localHistoryMaxSize_;
std::string fixedLocalMapPath_;
Memory * _memory;
std::map<int, cv::Point3f> localMap_;
Memory * memory_;
RegistrationVis * regVis_;
std::multimap<int, cv::Point3f> localMap_;
};
}

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

@@ -50,6 +50,11 @@ private:
int flowIterations_;
double flowEps_;
int flowMaxLevel_;
int minInliers_;
int iterations_;
double pnpReprojError_;
int pnpFlags_;
int pnpRefineIterations_;
Stereo * stereo_;

11
corelib/include/rtabmap/core/Registration.h
View File

@@ -58,6 +58,9 @@ public:
bool isScanRequired() const;
bool isUserDataRequired() const;
int getMinVisualCorrespondences() const;
float getMinGeometryCorrespondencesRatio() const;
bool varianceFromInliersCount() const {return varianceFromInliersCount_;}
bool force3DoF() const {return force3DoF_;}
@@ -93,9 +96,11 @@ protected:
Transform guess,
RegistrationInfo & info) const = 0;
virtual bool isImageRequiredImpl() const = 0;
virtual bool isScanRequiredImpl() const = 0;
virtual bool isUserDataRequiredImpl() const = 0;
virtual bool isImageRequiredImpl() const {return false;}
virtual bool isScanRequiredImpl() const {return false;}
virtual bool isUserDataRequiredImpl() const {return false;}
virtual int getMinVisualCorrespondencesImpl() const {return 0;}
virtual float getMinGeometryCorrespondencesRatioImpl() const {return 0.0f;}
private:
bool varianceFromInliersCount_;

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

@@ -51,9 +51,8 @@ protected:
Signature & to,
Transform guess,
RegistrationInfo & info) const;
virtual bool isImageRequiredImpl() const {return false;}
virtual bool isScanRequiredImpl() const {return true;}
virtual bool isUserDataRequiredImpl() const {return false;}
virtual float getMinGeometryCorrespondencesRatioImpl() const {return _correspondenceRatio;}
private:
float _maxTranslation;

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

@@ -17,15 +17,18 @@ public:
RegistrationInfo() :
variance(0),
inliers(0),
inliersRatio(0)
inliersRatio(0),
matches(0)
{
}
float variance;
int inliers;
float inliersRatio;
std::vector<int> inliersIndexes_;
std::string rejectedMsg_;
std::vector<int> inliersIDs;
int matches;
std::vector<int> matchesIDs;
std::string rejectedMsg;
};
}

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

@@ -45,9 +45,9 @@ public:
virtual void parseParameters(const ParametersMap & parameters);
float getBowInlierDistance() const {return _inlierDistance;}
int getBowIterations() const {return _iterations;}
int getBowMinInliers() const {return _minInliers;}
float getInlierDistance() const {return _inlierDistance;}
int getIterations() const {return _iterations;}
int getMinInliers() const {return _minInliers;}
protected:
virtual Transform computeTransformationImpl(
@@ -57,8 +57,7 @@ protected:
RegistrationInfo & info) const;
virtual bool isImageRequiredImpl() const {return true;}
virtual bool isScanRequiredImpl() const {return false;}
virtual bool isUserDataRequiredImpl() const {return false;}
virtual int getMinVisualCorrespondencesImpl() const {return _minInliers;}
private:
int _minInliers;

25
corelib/src/Memory.cpp
View File

@@ -2076,6 +2076,27 @@ Transform Memory::computeTransform(
if(!guess.isNull())
{
transform = _registrationPipeline->computeTransformation(tmpFrom, tmpTo, guess, info);
if(!transform.isNull())
{
UDEBUG("");
// verify if it is a 180 degree transform, well verify > 90
float x,y,z, roll,pitch,yaw;
transform.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
if(fabs(roll) > CV_PI/2 ||
fabs(pitch) > CV_PI/2 ||
fabs(yaw) > CV_PI/2)
{
transform.setNull();
std::string msg = uFormat("Too large rotation detected! (roll=%f, pitch=%f, yaw=%f)",
roll, pitch, yaw);
UINFO(msg.c_str());
if(info)
{
info->rejectedMsg = msg;
}
}
}
}
}
}
@@ -2084,7 +2105,7 @@ Transform Memory::computeTransform(
std::string msg = uFormat("Did not find nodes %d and/or %d", fromId, toId);
if(info)
{
info->rejectedMsg_ = msg;
info->rejectedMsg = msg;
}
UWARN(msg.c_str());
}
@@ -2139,7 +2160,7 @@ Transform Memory::computeIcpTransform(
std::string msg = uFormat("Did not find nodes %d and/or %d", fromId, toId);
if(info)
{
info->rejectedMsg_ = msg;
info->rejectedMsg = msg;
}
UWARN(msg.c_str());
}

51
corelib/src/Odometry.cpp
View File

@@ -62,15 +62,8 @@ Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & paramet
}
Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_roiRatios(Parameters::defaultVisRoiRatios()),
_minInliers(Parameters::defaultVisMinInliers()),
_inlierDistance(Parameters::defaultVisInlierDistance()),
_iterations(Parameters::defaultVisIterations()),
_refineIterations(Parameters::defaultVisRefineIterations()),
_minDepth(Parameters::defaultVisMinDepth()),
_maxDepth(Parameters::defaultVisMaxDepth()),
_resetCountdown(Parameters::defaultOdomResetCountdown()),
_force2D(Parameters::defaultRegForce3DoF()),
_force3DoF(Parameters::defaultRegForce3DoF()),
_holonomic(Parameters::defaultOdomHolonomic()),
_filteringStrategy(Parameters::defaultOdomFilteringStrategy()),
_particleSize(Parameters::defaultOdomParticleSize()),
@@ -79,10 +72,6 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_particleNoiseR(Parameters::defaultOdomParticleNoiseR()),
_particleLambdaR(Parameters::defaultOdomParticleLambdaR()),
_fillInfoData(Parameters::defaultOdomFillInfoData()),
_estimationType(Parameters::defaultVisEstimationType()),
_pnpReprojError(Parameters::defaultVisPnPReprojError()),
_pnpFlags(Parameters::defaultVisPnPFlags()),
_pnpRefineIterations(Parameters::defaultVisPnPRefineIterations()),
_varianceFromInliersCount(Parameters::defaultRegVarianceFromInliersCount()),
_kalmanProcessNoise(Parameters::defaultOdomKalmanProcessNoise()),
_kalmanMeasurementNoise(Parameters::defaultOdomKalmanMeasurementNoise()),
@@ -92,22 +81,10 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
distanceTravelled_(0)
{
Parameters::parse(parameters, Parameters::kOdomResetCountdown(), _resetCountdown);
Parameters::parse(parameters, Parameters::kVisMinInliers(), _minInliers);
UASSERT(_minInliers >= 1);
Parameters::parse(parameters, Parameters::kVisInlierDistance(), _inlierDistance);
Parameters::parse(parameters, Parameters::kVisIterations(), _iterations);
Parameters::parse(parameters, Parameters::kVisRefineIterations(), _refineIterations);
Parameters::parse(parameters, Parameters::kVisMinDepth(), _minDepth);
Parameters::parse(parameters, Parameters::kVisMaxDepth(), _maxDepth);
Parameters::parse(parameters, Parameters::kVisRoiRatios(), _roiRatios);
Parameters::parse(parameters, Parameters::kRegForce3DoF(), _force2D);
Parameters::parse(parameters, Parameters::kRegForce3DoF(), _force3DoF);
Parameters::parse(parameters, Parameters::kOdomHolonomic(), _holonomic);
Parameters::parse(parameters, Parameters::kOdomFillInfoData(), _fillInfoData);
Parameters::parse(parameters, Parameters::kVisEstimationType(), _estimationType);
Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _pnpReprojError);
Parameters::parse(parameters, Parameters::kVisPnPFlags(), _pnpFlags);
Parameters::parse(parameters, Parameters::kVisPnPRefineIterations(), _pnpRefineIterations);
UASSERT(_pnpFlags>=0 && _pnpFlags <=2);
Parameters::parse(parameters, Parameters::kRegVarianceFromInliersCount(), _varianceFromInliersCount);
Parameters::parse(parameters, Parameters::kOdomFilteringStrategy(), _filteringStrategy);
Parameters::parse(parameters, Parameters::kOdomParticleSize(), _particleSize);
@@ -159,12 +136,12 @@ void Odometry::reset(const Transform & initialPose)
_resetCurrentCount = 0;
previousStamp_ = 0;
distanceTravelled_ = 0;
if(_force2D || filters_.size())
if(_force3DoF || filters_.size())
{
float x,y,z, roll,pitch,yaw;
initialPose.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
if(_force2D)
if(_force3DoF)
{
if(z != 0.0f || roll != 0.0f || pitch != 0.0f)
{
@@ -194,7 +171,7 @@ void Odometry::reset(const Transform & initialPose)
if(_filteringStrategy == 1)
{
if(_force2D)
if(_force3DoF)
{
kalmanFilter_.statePost.at<float>(0) = x;
kalmanFilter_.statePost.at<float>(1) = y;
@@ -262,7 +239,7 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
{
_resetCurrentCount = _resetCountdown;
if(_force2D || !_holonomic || filters_.size() || _filteringStrategy==1)
if(_force3DoF || !_holonomic || filters_.size() || _filteringStrategy==1)
{
float x,y,z, roll,pitch,yaw;
t.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
@@ -313,7 +290,7 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
}
}
if(!_force2D)
if(!_force3DoF)
{
z = filters_[2]->filter(z);
roll = filters_[3]->filter(roll);
@@ -343,7 +320,7 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
uIsFinite(roll) && uIsFinite(pitch) && uIsFinite(yaw),
uFormat("x=%f y=%f z=%f roll=%f pitch=%f yaw=%f org T=%s",
x, y, z, roll, pitch, yaw, t.prettyPrint().c_str()).c_str());
t = Transform(x,y,_force2D?0:z, _force2D?0:roll,_force2D?0:pitch,yaw);
t = Transform(x,y,_force3DoF?0:z, _force3DoF?0:roll,_force3DoF?0:pitch,yaw);
info->transformFiltered = t;
}
@@ -386,7 +363,7 @@ void Odometry::initKalmanFilter()
// initialize the Kalman filter
int nStates = 18; // the number of states (x,y,z,x',y',z',x'',y'',z'',roll,pitch,yaw,roll',pitch',yaw',roll'',pitch'',yaw'')
int nMeasurements = 6; // the number of measured states (x,y,z,roll,pitch,yaw)
if(_force2D)
if(_force3DoF)
{
nStates = 9; // the number of states (x,y,x',y',x'',y'',yaw,yaw',yaw'')
nMeasurements = 3; // the number of measured states (x,y,z,roll,pitch,yaw)
@@ -398,7 +375,7 @@ void Odometry::initKalmanFilter()
cv::setIdentity(kalmanFilter_.measurementNoiseCov, cv::Scalar::all(_kalmanMeasurementNoise)); // set measurement noise
cv::setIdentity(kalmanFilter_.errorCovPost, cv::Scalar::all(1)); // error covariance
if(_force2D)
if(_force3DoF)
{
/* MEASUREMENT MODEL */
// [1 0 0 0 0 0 0 0 0]
@@ -429,7 +406,7 @@ void Odometry::initKalmanFilter()
void Odometry::updateKalmanFilter(float dt, float & x, float & y, float & z, float & roll, float & pitch, float & yaw)
{
// Set transition matrix with current dt
if(_force2D)
if(_force3DoF)
{
// 2D:
// [1 0 dt 0 dt2 0 0 0 0] x
@@ -496,7 +473,7 @@ void Odometry::updateKalmanFilter(float dt, float & x, float & y, float & z, flo
// Set measurement to predict
cv::Mat measurements;
if(!_force2D)
if(!_force3DoF)
{
measurements = cv::Mat(6,1,CV_32FC1);
measurements.at<float>(0) = x; // x
@@ -521,7 +498,7 @@ void Odometry::updateKalmanFilter(float dt, float & x, float & y, float & z, flo
UDEBUG("Correct");
cv::Mat estimated = kalmanFilter_.correct(measurements);
if(_force2D)
if(_force3DoF)
{
x = estimated.at<float>(0);
y = estimated.at<float>(1);

29
corelib/src/OdometryF2F.cpp
View File

@@ -104,11 +104,11 @@ Transform OdometryF2F::computeTransform(
idToIndex.insert(std::make_pair(iter->first, i));
++i;
}
info->cornerInliers.resize(regInfo.inliersIndexes_.size(), 1);
info->cornerInliers.resize(regInfo.inliersIDs.size(), 1);
i=0;
for(; i<(int)regInfo.inliersIndexes_.size(); ++i)
for(; i<(int)regInfo.inliersIDs.size(); ++i)
{
info->cornerInliers[i] = idToIndex.at(regInfo.inliersIndexes_[i]);
info->cornerInliers[i] = idToIndex.at(regInfo.inliersIDs[i]);
}
}
@@ -130,8 +130,8 @@ Transform OdometryF2F::computeTransform(
UDEBUG("Update key frame");
Signature newRefFrame(data);
int features = -1;
if(registrationPipeline_->isImageRequired())
int features = 0;
if(registrationPipeline_->getMinVisualCorrespondences()>0)
{
// this will generate features only for the first frame
Signature dummy;
@@ -141,8 +141,10 @@ Transform OdometryF2F::computeTransform(
features = (int)newRefFrame.getWords().size();
}
if((features < 0 || features >= this->getMinInliers()) &&
(!registrationPipeline_->isScanRequired() || newRefFrame.sensorData().laserScanRaw().cols))
if((features >= registrationPipeline_->getMinVisualCorrespondences()) &&
(registrationPipeline_->getMinGeometryCorrespondencesRatio()==0.0f ||
(newRefFrame.sensorData().laserScanRaw().cols &&
(newRefFrame.sensorData().laserScanMaxPts() == 0 || float(newRefFrame.sensorData().laserScanRaw().cols)/float(newRefFrame.sensorData().laserScanMaxPts())>=registrationPipeline_->getMinGeometryCorrespondencesRatio()))))
{
refFrame_ = newRefFrame;
@@ -151,20 +153,25 @@ Transform OdometryF2F::computeTransform(
}
else
{
if(features >= 0 && features < this->getMinInliers())
if(features < registrationPipeline_->getMinVisualCorrespondences())
{
UWARN("Too low 2D features (%d), keeping last key frame...", features);
}
if(registrationPipeline_->isScanRequired() && newRefFrame.sensorData().laserScanRaw().cols==0)
if(registrationPipeline_->getMinGeometryCorrespondencesRatio()>0.0f && newRefFrame.sensorData().laserScanRaw().cols==0)
{
UWARN("Too low scan points (%d), keeping last key frame...", newRefFrame.sensorData().laserScanRaw().cols);
}
else if(registrationPipeline_->getMinGeometryCorrespondencesRatio()>0.0f && newRefFrame.sensorData().laserScanMaxPts() != 0 && float(newRefFrame.sensorData().laserScanRaw().cols)/float(newRefFrame.sensorData().laserScanMaxPts())<registrationPipeline_->getMinGeometryCorrespondencesRatio())
{
UWARN("Too low scan points ratio (%d < %d), keeping last key frame...", float(newRefFrame.sensorData().laserScanRaw().cols)/float(newRefFrame.sensorData().laserScanMaxPts()), registrationPipeline_->getMinGeometryCorrespondencesRatio());
}
}
}
}
else if(!regInfo.rejectedMsg_.empty())
else if(!regInfo.rejectedMsg.empty())
{
UWARN("Registration failed: \"%s\"", regInfo.rejectedMsg_.c_str());
UWARN("Registration failed: \"%s\"", regInfo.rejectedMsg.c_str());
}
if(info)

186
corelib/src/OdometryLocalMap.cpp
View File

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/Memory.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/RegistrationVis.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/core/util3d_registration.h"
#include "rtabmap/core/util3d_correspondences.h"
@@ -51,18 +52,25 @@ namespace rtabmap {
OdometryLocalMap::OdometryLocalMap(const ParametersMap & parameters) :
Odometry(parameters),
_localHistoryMaxSize(Parameters::defaultOdomBowLocalHistorySize()),
_fixedLocalMapPath(Parameters::defaultOdomBowFixedLocalMapPath()),
_memory(0)
localHistoryMaxSize_(Parameters::defaultOdomBowLocalHistorySize()),
fixedLocalMapPath_(Parameters::defaultOdomBowFixedLocalMapPath()),
memory_(0),
regVis_(new RegistrationVis(parameters))
{
UDEBUG("");
Parameters::parse(parameters, Parameters::kOdomBowLocalHistorySize(), _localHistoryMaxSize);
Parameters::parse(parameters, Parameters::kOdomBowFixedLocalMapPath(), _fixedLocalMapPath);
Parameters::parse(parameters, Parameters::kOdomBowLocalHistorySize(), localHistoryMaxSize_);
Parameters::parse(parameters, Parameters::kOdomBowFixedLocalMapPath(), fixedLocalMapPath_);
ParametersMap customParameters;
customParameters.insert(ParametersPair(Parameters::kKpMinDepth(), uNumber2Str(this->getMinDepth())));
customParameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(this->getMaxDepth())));
customParameters.insert(ParametersPair(Parameters::kKpRoiRatios(), this->getRoiRatios()));
float minDepth = Parameters::defaultVisMinDepth();
float maxDepth = Parameters::defaultVisMaxDepth();
std::string roi = Parameters::defaultVisRoiRatios();
Parameters::parse(parameters, Parameters::kVisMinDepth(), minDepth);
Parameters::parse(parameters, Parameters::kVisMaxDepth(), maxDepth);
Parameters::parse(parameters, Parameters::kVisRoiRatios(), roi);
customParameters.insert(ParametersPair(Parameters::kKpMinDepth(), uNumber2Str(minDepth)));
customParameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(maxDepth)));
customParameters.insert(ParametersPair(Parameters::kKpRoiRatios(), roi));
customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
customParameters.insert(ParametersPair(Parameters::kMemBinDataKept(), "false"));
customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
@@ -103,32 +111,32 @@ OdometryLocalMap::OdometryLocalMap(const ParametersMap & parameters) :
}
}
if(_fixedLocalMapPath.empty())
if(fixedLocalMapPath_.empty())
{
_memory = new Memory(customParameters);
if(!_memory->init("", false, ParametersMap()))
memory_ = new Memory(customParameters);
if(!memory_->init("", false, ParametersMap()))
{
UERROR("Error initializing the memory for BOW Odometry.");
}
}
else
{
UINFO("Init odometry from a fixed database: \"%s\"", _fixedLocalMapPath.c_str());
UINFO("Init odometry from a fixed database: \"%s\"", fixedLocalMapPath_.c_str());
// init the local map with a all 3D features contained in the database
customParameters.insert(ParametersPair(Parameters::kMemIncrementalMemory(), "false"));
customParameters.insert(ParametersPair(Parameters::kMemInitWMWithAllNodes(), "true"));
_memory = new Memory(customParameters);
if(!_memory->init(_fixedLocalMapPath, false, ParametersMap()))
memory_ = new Memory(customParameters);
if(!memory_->init(fixedLocalMapPath_, false, ParametersMap()))
{
UERROR("Error initializing the memory for BOW Odometry.");
}
else
{
// get the graph
std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastSignatureId(), 0, -1);
std::map<int, int> ids = memory_->getNeighborsId(memory_->getLastSignatureId(), 0, -1);
std::map<int, Transform> poses;
std::multimap<int, Link> links;
_memory->getMetricConstraints(uKeysSet(ids), poses, links, true);
memory_->getMetricConstraints(uKeysSet(ids), poses, links, true);
if(poses.size())
{
@@ -142,7 +150,7 @@ OdometryLocalMap::OdometryLocalMap(const ParametersMap & parameters) :
posesIter!=optimizedPoses.end();
++posesIter)
{
const Signature * s = _memory->getSignature(posesIter->first);
const Signature * s = memory_->getSignature(posesIter->first);
if(s)
{
// Transform 3D points accordingly to pose and add them to local map
@@ -161,30 +169,30 @@ OdometryLocalMap::OdometryLocalMap(const ParametersMap & parameters) :
}
else
{
UERROR("No pose loaded from database \"%s\"", _fixedLocalMapPath.c_str());
UERROR("No pose loaded from database \"%s\"", fixedLocalMapPath_.c_str());
}
}
if((int)localMap_.size() < this->getMinInliers() || localMap_.size() == 0)
if((int)localMap_.size() < regVis_->getMinInliers() || localMap_.size() == 0)
{
UERROR("The loaded fixed map from \"%s\" is too small! Only %d unique features loaded. Odometry won't be computed!",
_fixedLocalMapPath.c_str(), (int)localMap_.size());
fixedLocalMapPath_.c_str(), (int)localMap_.size());
}
}
}
OdometryLocalMap::~OdometryLocalMap()
{
delete _memory;
delete memory_;
UDEBUG("");
}
void OdometryLocalMap::reset(const Transform & initialPose)
{
if(_fixedLocalMapPath.empty())
if(fixedLocalMapPath_.empty())
{
Odometry::reset(initialPose);
_memory->init("", false, ParametersMap());
memory_->init("", false, ParametersMap());
localMap_.clear();
}
else
@@ -206,14 +214,12 @@ Transform OdometryLocalMap::computeTransform(
info->type = 0;
}
double variance = 0;
int inliersCount = 0;
int correspondences = 0;
RegistrationInfo regInfo;
int nFeatures = 0;
if(_memory->update(data))
if(memory_->update(data))
{
const Signature * newSignature = _memory->getLastWorkingSignature();
const Signature * newSignature = memory_->getLastWorkingSignature();
if(newSignature)
{
nFeatures = (int)newSignature->getWords().size();
@@ -226,120 +232,56 @@ Transform OdometryLocalMap::computeTransform(
if(localMap_.size() && newSignature)
{
Transform transform;
if((int)localMap_.size() >= this->getMinInliers())
if((int)localMap_.size() >= regVis_->getMinInliers())
{
std::vector<int> matches, inliers;
Transform t;
if(this->getEstimationType() == 1) // PnP
{
// 3D to 2D
if(data.cameraModels().size() > 1)
{
UERROR("PnP cannot be used on multi-cameras setup.");
}
else if((int)newSignature->getWords().size() >= this->getMinInliers())
{
UASSERT(data.stereoCameraModel().isValid() || (data.cameraModels().size() == 1 && data.cameraModels()[0].isValid()));
const CameraModel & cameraModel = data.stereoCameraModel().isValid()?data.stereoCameraModel().left():data.cameraModels()[0];
UDEBUG("");
t = util3d::estimateMotion3DTo2D(
localMap_,
uMultimapToMap(newSignature->getWords()),
cameraModel,
this->getMinInliers(),
this->getIterations(),
this->getPnPReprojError(),
this->getPnPFlags(),
this->getPnPRefineIterations(),
this->getPose(),
uMultimapToMap(newSignature->getWords3()),
isVarianceFromInliersCount()?0:&variance, // don't compute variance if we use inliers
&matches,
&inliers);
}
else
{
UWARN("Not enough features in the new image (%d < %d)", (int)newSignature->getWords().size(), this->getMinInliers());
}
}
else
{
// 3D to 3D
if((int)newSignature->getWords3().size() >= this->getMinInliers())
{
t = util3d::estimateMotion3DTo3D(
localMap_,
uMultimapToMap(newSignature->getWords3()),
this->getMinInliers(),
this->getInlierDistance(),
this->getIterations(),
this->getRefineIterations(),
&variance,
&matches,
&inliers);
}
else
{
UWARN("Not enough 3D features in the new image (%d < %d)", (int)newSignature->getWords3().size(), this->getMinInliers());
}
}
correspondences = matches.size();
inliersCount = inliers.size();
if(this->isInfoDataFilled() && info)
{
info->wordMatches = matches;
info->wordInliers = inliers;
}
Signature tmpLocalMap(-1);
tmpLocalMap.setWords3(localMap_);
t = regVis_->computeTransformation(tmpLocalMap, *newSignature, this->getPose(), &regInfo);
if(!t.isNull())
{
// make it incremental
transform = this->getPose().inverse() * t;
}
else if(correspondences < this->getMinInliers())
else if(!regInfo.rejectedMsg.empty())
{
UWARN("Not enough correspondences (%d < %d)", correspondences, this->getMinInliers());
}
else if(inliersCount < this->getMinInliers())
{
UWARN("Not enough inliers (%d < %d)", inliersCount, this->getMinInliers());
UWARN("Registration failed: \"%s\"", regInfo.rejectedMsg.c_str());
}
else
{
UWARN("Unknown estimation error");
UWARN("Unknown registration error");
}
}
else
{
UWARN("Local map too small!? (%d < %d)", (int)localMap_.size(), this->getMinInliers());
UWARN("Local map too small!? (%d < %d)", (int)localMap_.size(), regVis_->getMinInliers());
}
if(transform.isNull())
{
_memory->deleteLocation(newSignature->id());
memory_->deleteLocation(newSignature->id());
}
else if(_fixedLocalMapPath.empty())
else if(fixedLocalMapPath_.empty())
{
output = transform;
// remove words if history max size is reached
while(localMap_.size() && (int)localMap_.size() > _localHistoryMaxSize && _memory->getStMem().size()>1)
while(localMap_.size() && (int)localMap_.size() > localHistoryMaxSize_ && memory_->getStMem().size()>1)
{
int nodeId = *_memory->getStMem().begin();
int nodeId = *memory_->getStMem().begin();
std::list<int> removedPts;
_memory->deleteLocation(nodeId, &removedPts);
memory_->deleteLocation(nodeId, &removedPts);
for(std::list<int>::iterator iter = removedPts.begin(); iter!=removedPts.end(); ++iter)
{
localMap_.erase(*iter);
}
}
if(_localHistoryMaxSize == 0 && localMap_.size() > 0 && localMap_.size() > newSignature->getWords3().size())
if(localHistoryMaxSize_ == 0 && localMap_.size() > 0 && localMap_.size() > newSignature->getWords3().size())
{
UERROR("Local map should have only words of the last added signature here! (size=%d, max history size=%d, newWords=%d)",
(int)localMap_.size(), _localHistoryMaxSize, (int)newSignature->getWords3().size());
(int)localMap_.size(), localHistoryMaxSize_, (int)newSignature->getWords3().size());
}
// update local map
@@ -371,14 +313,14 @@ Transform OdometryLocalMap::computeTransform(
{
// fixed local map, just delete the new signature
output = transform;
_memory->deleteLocation(newSignature->id());
memory_->deleteLocation(newSignature->id());
}
}
else if(newSignature)
{
int count = 0;
std::list<int> uniques = uUniqueKeys(newSignature->getWords3());
if(_fixedLocalMapPath.empty() && (int)uniques.size() >= this->getMinInliers())
if(fixedLocalMapPath_.empty() && (int)uniques.size() >= regVis_->getMinInliers())
{
output.setIdentity();
@@ -404,33 +346,39 @@ Transform OdometryLocalMap::computeTransform(
else
{
// not enough features, just delete it
_memory->deleteLocation(newSignature->id());
memory_->deleteLocation(newSignature->id());
}
UDEBUG("uniques=%d, pt not finite = %d", (int)uniques.size(),count);
}
_memory->emptyTrash();
memory_->emptyTrash();
}
if(info)
{
info->variance = variance;
info->inliers = inliersCount;
info->matches = correspondences;
info->variance = regInfo.variance;
info->inliers = regInfo.inliers;
info->matches = regInfo.matches;
info->features = nFeatures;
info->localMapSize = (int)localMap_.size();
if(this->isInfoDataFilled())
{
info->wordMatches = regInfo.matchesIDs;
info->wordInliers = regInfo.inliersIDs;
}
}
UINFO("Odom update time = %fs lost=%s features=%d inliers=%d/%d variance=%f local_map=%d dict=%d nodes=%d",
timer.elapsed(),
output.isNull()?"true":"false",
nFeatures,
inliersCount,
correspondences,
variance,
regInfo.inliers,
regInfo.matches,
regInfo.variance,
(int)localMap_.size(),
(int)_memory->getVWDictionary()->getVisualWords().size(),
(int)_memory->getStMem().size());
(int)memory_->getVWDictionary()->getVisualWords().size(),
(int)memory_->getStMem().size());
return output;
}

68
corelib/src/OdometryMono.cpp
View File

@@ -54,6 +54,11 @@ OdometryMono::OdometryMono(const rtabmap::ParametersMap & parameters) :
flowIterations_(Parameters::defaultVisCorFlowIterations()),
flowEps_(Parameters::defaultVisCorFlowEps()),
flowMaxLevel_(Parameters::defaultVisCorFlowMaxLevel()),
minInliers_(Parameters::defaultVisMinInliers()),
iterations_(Parameters::defaultVisIterations()),
pnpReprojError_(Parameters::defaultVisPnPReprojError()),
pnpFlags_(Parameters::defaultVisPnPFlags()),
pnpRefineIterations_(Parameters::defaultVisPnPRefineIterations()),
localHistoryMaxSize_(Parameters::defaultOdomBowLocalHistorySize()),
initMinFlow_(Parameters::defaultOdomMonoInitMinFlow()),
initMinTranslation_(Parameters::defaultOdomMonoInitMinTranslation()),
@@ -66,6 +71,12 @@ OdometryMono::OdometryMono(const rtabmap::ParametersMap & parameters) :
Parameters::parse(parameters, Parameters::kVisCorFlowIterations(), flowIterations_);
Parameters::parse(parameters, Parameters::kVisCorFlowEps(), flowEps_);
Parameters::parse(parameters, Parameters::kVisCorFlowMaxLevel(), flowMaxLevel_);
Parameters::parse(parameters, Parameters::kVisMinInliers(), minInliers_);
UASSERT(minInliers_ >= 1);
Parameters::parse(parameters, Parameters::kVisIterations(), iterations_);
Parameters::parse(parameters, Parameters::kVisPnPReprojError(), pnpReprojError_);
Parameters::parse(parameters, Parameters::kVisPnPFlags(), pnpFlags_);
Parameters::parse(parameters, Parameters::kVisPnPRefineIterations(), pnpRefineIterations_);
Parameters::parse(parameters, Parameters::kOdomBowLocalHistorySize(), localHistoryMaxSize_);
Parameters::parse(parameters, Parameters::kOdomMonoInitMinFlow(), initMinFlow_);
@@ -78,8 +89,15 @@ OdometryMono::OdometryMono(const rtabmap::ParametersMap & parameters) :
// Setup memory
ParametersMap customParameters;
customParameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(this->getMaxDepth())));
customParameters.insert(ParametersPair(Parameters::kKpRoiRatios(), this->getRoiRatios()));
float minDepth = Parameters::defaultVisMinDepth();
float maxDepth = Parameters::defaultVisMaxDepth();
std::string roi = Parameters::defaultVisRoiRatios();
Parameters::parse(parameters, Parameters::kVisMinDepth(), minDepth);
Parameters::parse(parameters, Parameters::kVisMaxDepth(), maxDepth);
Parameters::parse(parameters, Parameters::kVisRoiRatios(), roi);
customParameters.insert(ParametersPair(Parameters::kKpMinDepth(), uNumber2Str(minDepth)));
customParameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(maxDepth)));
customParameters.insert(ParametersPair(Parameters::kKpRoiRatios(), roi));
customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
customParameters.insert(ParametersPair(Parameters::kMemBinDataKept(), "false"));
customParameters.insert(ParametersPair(Parameters::kMemImageKept(), "true"));
@@ -217,7 +235,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
const Signature * newS = memory_->getLastWorkingSignature();
UDEBUG("newWords=%d", (int)newS->getWords().size());
nFeatures = (int)newS->getWords().size();
if((int)newS->getWords().size() > this->getMinInliers())
if((int)newS->getWords().size() > minInliers_)
{
cv::Mat K = cameraModel.K();
Transform guess = (this->getPose() * cameraModel.localTransform()).inverse();
@@ -347,9 +365,9 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
}
correspondences = (int)matches.size();
if((int)matches.size() < this->getMinInliers())
if((int)matches.size() < minInliers_)
{
UWARN("not enough matches (%d < %d)...", (int)matches.size(), this->getMinInliers());
UWARN("not enough matches (%d < %d)...", (int)matches.size(), minInliers_);
}
else
{
@@ -363,19 +381,19 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
rvec,
tvec,
true,
this->getIterations(),
this->getPnPReprojError(),
iterations_,
pnpReprojError_,
0, // min inliers
inliersV,
this->getPnPFlags(),
this->getPnPRefineIterations());
pnpFlags_,
pnpRefineIterations_);
UDEBUG("inliers=%d/%d", (int)inliersV.size(), (int)objectPoints.size());
inliers = (int)inliersV.size();
if((int)inliersV.size() < this->getMinInliers())
if((int)inliersV.size() < minInliers_)
{
UWARN("PnP not enough inliers (%d < %d), rejecting the transform...", (int)inliersV.size(), this->getMinInliers());
UWARN("PnP not enough inliers (%d < %d), rejecting the transform...", (int)inliersV.size(), minInliers_);
}
else
{
@@ -431,19 +449,19 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
uMultimapToMapUnique(newS->getWords()),
cameraModel,
cameraTransform,
this->getIterations(),
this->getPnPReprojError(),
this->getPnPFlags(),
this->getPnPRefineIterations(),
iterations_,
pnpReprojError_,
pnpFlags_,
pnpRefineIterations_,
fundMatrixReprojError_,
fundMatrixConfidence_,
previousGuess,
&variance);
if((int)inliers3D.size() < this->getMinInliers())
if((int)inliers3D.size() < minInliers_)
{
UWARN("Epipolar geometry not enough inliers (%d < %d), rejecting the transform (%s)...",
(int)inliers3D.size(), this->getMinInliers(), cameraTransform.prettyPrint().c_str());
(int)inliers3D.size(), minInliers_, cameraTransform.prettyPrint().c_str());
}
else if(variance == 0 || variance > maxVariance_)
{
@@ -612,7 +630,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
UDEBUG("Filtering optical flow outliers...done! (inliers=%d/%d)", oi, (int)statusFlowInliers.size());
if(flow > initMinFlow_ && oi > this->getMinInliers())
if(flow > initMinFlow_ && oi > minInliers_)
{
UDEBUG("flow=%f", flow);
// compute fundamental matrix
@@ -665,7 +683,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
}
UDEBUG("Filtering fundamental matrix outliers...done! (inliers=%d/%d)", oi, (int)statusFInliers.size());
if((int)refCorners.size() > this->getMinInliers())
if((int)refCorners.size() > minInliers_)
{
std::vector<cv::Point2f> refCornersRefined;
std::vector<cv::Point2f> newCornersRefined;
@@ -892,12 +910,12 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
rvec,
tvec,
false,
this->getIterations(),
this->getPnPReprojError(),
iterations_,
pnpReprojError_,
0, // min inliers
inliersPnP,
this->getPnPFlags(),
this->getPnPRefineIterations());
pnpFlags_,
pnpRefineIterations_);
UDEBUG("PnP inliers = %d / %d", (int)inliersPnP.size(), (int)objectPoints.size());
@@ -946,7 +964,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
}
else
{
UWARN("Not enough inliers %d/%d", (int)refCorners.size(), this->getMinInliers());
UWARN("Not enough inliers %d/%d", (int)refCorners.size(), minInliers_);
}
}
else
@@ -969,7 +987,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
if(memory_->update(SensorData(newFrame)))
{
const std::multimap<int, cv::KeyPoint> & words = memory_->getLastWorkingSignature()->getWords();
if((int)words.size() > this->getMinInliers())
if((int)words.size() > minInliers_)
{
for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
{

28
corelib/src/Registration.cpp
View File

@@ -115,6 +115,34 @@ bool Registration::isUserDataRequired() const
return val;
}
int Registration::getMinVisualCorrespondences() const
{
int min = this->getMinVisualCorrespondencesImpl();
if(child_)
{
int childMin = child_->getMinVisualCorrespondences();
if(min == 0 || childMin > min)
{
min = childMin;
}
}
return min;
}
float Registration::getMinGeometryCorrespondencesRatio() const
{
float min = this->getMinGeometryCorrespondencesRatioImpl();
if(child_)
{
float childMin = child_->getMinGeometryCorrespondencesRatio();
if(min == 0 || childMin > min)
{
min = childMin;
}
}
return min;
}
void Registration::setChildRegistration(Registration * child)
{
if(child_)

2
corelib/src/RegistrationIcp.cpp
View File

@@ -318,7 +318,7 @@ Transform RegistrationIcp::computeTransformationImpl(
}
info.rejectedMsg_ = msg;
info.rejectedMsg = msg;
UDEBUG("New transform = %s", transform.prettyPrint().c_str());
return transform;

49
corelib/src/RegistrationVis.cpp
View File

@@ -192,8 +192,10 @@ Transform RegistrationVis::computeTransformationImpl(
////////////////////
// Find correspondences
////////////////////
if(fromSignature.getWords().size() && fromSignature.getWords3().size() &&
toSignature.getWords().size() && (_estimationType==1 || toSignature.getWords3().size()))
if((_estimationType<2 || fromSignature.getWords().size()) && // required only for 2D->2D
(_estimationType==0 || toSignature.getWords().size()) && // required only for 3D->2D or 2D->2D
fromSignature.getWords3().size() && // required in all estimation approaches
(_estimationType==1 || toSignature.getWords3().size())) // required only for 3D->3D and 2D->2D
{
// no need to extract new features, we have all the data we need
UDEBUG("");
@@ -499,10 +501,12 @@ Transform RegistrationVis::computeTransformationImpl(
Transform transform;
float variance = 1.0f;
int inliersCount = 0;
int matchesCount = 0;
if(toSignature.getWords().size() || !toSignature.sensorData().imageRaw().empty())
{
Transform transforms[2];
std::vector<int> inliers[2];
std::vector<int> matches[2];
double variances[2] = {1.0f};
for(int dir=0; dir<(!_forwardEstimateOnly?2:1); ++dir)
{
@@ -552,7 +556,6 @@ Transform RegistrationVis::computeTransformationImpl(
inliers[dir] = uKeys(inliers3D);
if(!cameraTransform.isNull())
{
if((int)inliers3D.size() >= _minInliers)
@@ -614,6 +617,7 @@ Transform RegistrationVis::computeTransformationImpl(
const CameraModel & cameraModel = signatureB->sensorData().stereoCameraModel().isValid()?signatureB->sensorData().stereoCameraModel().left():signatureB->sensorData().cameraModels()[0];
std::vector<int> inliersV;
std::vector<int> matchesV;
transforms[dir] = util3d::estimateMotion3DTo2D(
uMultimapToMapUnique(signatureA->getWords3()),
uMultimapToMapUnique(signatureB->getWords()),
@@ -626,9 +630,10 @@ Transform RegistrationVis::computeTransformationImpl(
dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
uMultimapToMapUnique(signatureA->getWords3()),
varianceFromInliersCount()?0:&variances[dir],
0,
&matchesV,
&inliersV);
inliers[dir] = inliersV;
matches[dir] = matchesV;
if(transforms[dir].isNull())
{
msg = uFormat("Not enough inliers %d/%d between %d and %d",
@@ -653,6 +658,7 @@ Transform RegistrationVis::computeTransformationImpl(
(int)signatureB->getWords3().size() >= _minInliers)
{
std::vector<int> inliersV;
std::vector<int> matchesV;
transforms[dir] = util3d::estimateMotion3DTo3D(
uMultimapToMapUnique(signatureA->getWords3()),
uMultimapToMapUnique(signatureB->getWords3()),
@@ -661,9 +667,10 @@ Transform RegistrationVis::computeTransformationImpl(
_iterations,
_refineIterations,
&variances[dir],
0,
&matchesV,
&inliersV);
inliers[dir] = inliersV;
matches[dir] = matchesV;
if(transforms[dir].isNull())
{
msg = uFormat("Not enough inliers %d/%d between %d and %d",
@@ -692,49 +699,39 @@ Transform RegistrationVis::computeTransformationImpl(
if(transforms[0].isNull())
{
transform = transforms[1];
info.inliersIndexes_ = inliers[1];
info.inliersIDs = inliers[1];
info.matchesIDs = matches[1];
variance = variances[1];
inliersCount = (int)inliers[1].size();
matchesCount = (int)matches[1].size();
}
else
{
transform = transforms[0].interpolate(0.5f, transforms[1]);
info.inliersIndexes_ = inliers[0];
info.inliersIDs = inliers[0];
info.matchesIDs = matches[0];
variance = (variances[0]+variances[1])/2.0f;
inliersCount = (int)(inliers[0].size()+inliers[1].size())/2;
matchesCount = (int)(matches[0].size()+matches[1].size())/2;
}
}
else
{
transform = transforms[0];
info.inliersIndexes_ = inliers[0];
info.inliersIDs = inliers[0];
info.matchesIDs = matches[0];
variance = variances[0];
inliersCount = (int)inliers[0].size();
}
}
if(!transform.isNull())
{
UDEBUG("");
// verify if it is a 180 degree transform, well verify > 90
float x,y,z, roll,pitch,yaw;
transform.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
if(fabs(roll) > CV_PI/2 ||
fabs(pitch) > CV_PI/2 ||
fabs(yaw) > CV_PI/2)
{
transform.setNull();
msg = uFormat("Too large rotation detected! (roll=%f, pitch=%f, yaw=%f)",
roll, pitch, yaw);
UWARN(msg.c_str());
matchesCount = (int)matches[0].size();
}
}
info.inliers = inliersCount;
info.rejectedMsg_ = msg;
info.matches = matchesCount;
info.rejectedMsg = msg;
info.variance = variance>0.0f?variance:0.0001f; // epsilon if exact transform
UDEBUG("transform=%s", transform.prettyPrint().c_str());

4
corelib/src/Rtabmap.cpp
View File

@@ -1039,7 +1039,7 @@ bool Rtabmap::process(
}
else
{
UINFO("Scan matching rejected: %s", info.rejectedMsg_.c_str());
UINFO("Scan matching rejected: %s", info.rejectedMsg.c_str());
if(info.variance > 0)
{
double sqrtVar = sqrt(info.variance);
@@ -1717,7 +1717,7 @@ bool Rtabmap::process(
if(rejectedHypothesis)
{
UWARN("Rejected loop closure %d -> %d: %s",
_loopClosureHypothesis.first, signature->id(), info.rejectedMsg_.c_str());
_loopClosureHypothesis.first, signature->id(), info.rejectedMsg.c_str());
}
}
if(!rejectedHypothesis)