Refactoring Odometry

corelib/include/rtabmap/core/Odometry.h

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

corelib/include/rtabmap/core/OdometryLocalMap.h

@@ -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 std::multimap<int, cv::Point3f> & getLocalMap() const {return localMap_;}
-	const Memory * getMemory() const {return _memory;}
+	const Memory * getMemory() const {return memory_;}
 
 private:
 	virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0);
 
 private:
 	//Parameters
-	int _localHistoryMaxSize;
+	int localHistoryMaxSize_;
-	std::string _fixedLocalMapPath;
+	std::string fixedLocalMapPath_;
 
-	Memory * _memory;
+	Memory * memory_;
-	std::map<int, cv::Point3f> localMap_;
+	RegistrationVis * regVis_;
+	std::multimap<int, cv::Point3f> localMap_;
 };
 
 }

corelib/include/rtabmap/core/OdometryMono.h

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

corelib/include/rtabmap/core/Registration.h

@@ -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 isImageRequiredImpl() const {return false;}
-	virtual bool isScanRequiredImpl() const = 0;
+	virtual bool isScanRequiredImpl() const {return false;}
-	virtual bool isUserDataRequiredImpl() const = 0;
+	virtual bool isUserDataRequiredImpl() const {return false;}
+	virtual int getMinVisualCorrespondencesImpl() const {return 0;}
+	virtual float getMinGeometryCorrespondencesRatioImpl() const {return 0.0f;}
 
 private:
 	bool varianceFromInliersCount_;

corelib/include/rtabmap/core/RegistrationIcp.h

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

corelib/include/rtabmap/core/RegistrationInfo.h

@@ -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::vector<int> inliersIDs;
-	std::string rejectedMsg_;
+	int matches;
+	std::vector<int> matchesIDs;
+	std::string rejectedMsg;
 };
 
 }

corelib/include/rtabmap/core/RegistrationVis.h

@@ -45,9 +45,9 @@ public:
 
 	virtual void parseParameters(const ParametersMap & parameters);
 
-	float getBowInlierDistance() const {return _inlierDistance;}
+	float getInlierDistance() const {return _inlierDistance;}
-	int getBowIterations() const {return _iterations;}
+	int getIterations() const {return _iterations;}
-	int getBowMinInliers() const {return _minInliers;}
+	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 int getMinVisualCorrespondencesImpl() const {return _minInliers;}
-	virtual bool isUserDataRequiredImpl() const {return false;}
 
 private:
 	int _minInliers;

corelib/src/Memory.cpp

@@ -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());
 	}

corelib/src/Odometry.cpp

@@ -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::kRegForce3DoF(), _force3DoF);
-	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::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);

corelib/src/OdometryF2F.cpp

@@ -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;
+			int features = 0;
-			if(registrationPipeline_->isImageRequired())
+			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()) &&
+			if((features >= registrationPipeline_->getMinVisualCorrespondences()) &&
-			   (!registrationPipeline_->isScanRequired() || newRefFrame.sensorData().laserScanRaw().cols))
+			   (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)

corelib/src/OdometryLocalMap.cpp

@@ -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()),
+	localHistoryMaxSize_(Parameters::defaultOdomBowLocalHistorySize()),
-	_fixedLocalMapPath(Parameters::defaultOdomBowFixedLocalMapPath()),
+	fixedLocalMapPath_(Parameters::defaultOdomBowFixedLocalMapPath()),
-	_memory(0)
+	memory_(0),
+	regVis_(new RegistrationVis(parameters))
 {
 	UDEBUG("");
-	Parameters::parse(parameters, Parameters::kOdomBowLocalHistorySize(), _localHistoryMaxSize);
+	Parameters::parse(parameters, Parameters::kOdomBowLocalHistorySize(), localHistoryMaxSize_);
-	Parameters::parse(parameters, Parameters::kOdomBowFixedLocalMapPath(), _fixedLocalMapPath);
+	Parameters::parse(parameters, Parameters::kOdomBowFixedLocalMapPath(), fixedLocalMapPath_);
 
 	ParametersMap customParameters;
-	customParameters.insert(ParametersPair(Parameters::kKpMinDepth(), uNumber2Str(this->getMinDepth())));
+	float minDepth = Parameters::defaultVisMinDepth();
-	customParameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(this->getMaxDepth())));
+	float maxDepth = Parameters::defaultVisMaxDepth();
-	customParameters.insert(ParametersPair(Parameters::kKpRoiRatios(), this->getRoiRatios()));
+	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);
+		memory_ = new Memory(customParameters);
-		if(!_memory->init("", false, ParametersMap()))
+		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);
+		memory_ = new Memory(customParameters);
-		if(!_memory->init(_fixedLocalMapPath, false, ParametersMap()))
+		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;
+	RegistrationInfo regInfo;
-	int inliersCount = 0;
-	int correspondences = 0;
 	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
+				Signature tmpLocalMap(-1);
-				{
+				tmpLocalMap.setWords3(localMap_);
-					// 3D to 2D
+				t = regVis_->computeTransformation(tmpLocalMap, *newSignature, this->getPose(), &regInfo);
-					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;
-				}
 
 				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());
+					UWARN("Registration failed: \"%s\"", regInfo.rejectedMsg.c_str());
-				}
-				else if(inliersCount < this->getMinInliers())
-				{
-					UWARN("Not enough inliers (%d < %d)", inliersCount, this->getMinInliers());
 				}
 				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->variance = regInfo.variance;
-		info->inliers = inliersCount;
+		info->inliers = regInfo.inliers;
-		info->matches = correspondences;
+		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,
+			regInfo.inliers,
-			correspondences,
+			regInfo.matches,
-			variance,
+			regInfo.variance,
 			(int)localMap_.size(),
-			(int)_memory->getVWDictionary()->getVisualWords().size(),
+			(int)memory_->getVWDictionary()->getVisualWords().size(),
-			(int)_memory->getStMem().size());
+			(int)memory_->getStMem().size());
 	return output;
 }
 

corelib/src/OdometryMono.cpp

@@ -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())));
+	float minDepth = Parameters::defaultVisMinDepth();
-	customParameters.insert(ParametersPair(Parameters::kKpRoiRatios(), this->getRoiRatios()));
+	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(),
+								iterations_,
-								this->getPnPReprojError(),
+								pnpReprojError_,
 								0, // min inliers
 								inliersV,
-								this->getPnPFlags(),
+								pnpFlags_,
-								this->getPnPRefineIterations());
+								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(),
+										iterations_,
-										this->getPnPReprojError(),
+										pnpReprojError_,
-										this->getPnPFlags(),
+										pnpFlags_,
-										this->getPnPRefineIterations(),
+										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(),
+										iterations_,
-										this->getPnPReprojError(),
+										pnpReprojError_,
 										0, // min inliers
 										inliersPnP,
-										this->getPnPFlags(),
+										pnpFlags_,
-										this->getPnPRefineIterations());
+										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)
 				{

corelib/src/Registration.cpp

@@ -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_)

corelib/src/RegistrationIcp.cpp

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

corelib/src/RegistrationVis.cpp

@@ -192,8 +192,10 @@ Transform RegistrationVis::computeTransformationImpl(
 	////////////////////
 	// Find correspondences
 	////////////////////
-	if(fromSignature.getWords().size() && fromSignature.getWords3().size() &&
+	if((_estimationType<2 || fromSignature.getWords().size()) && // required only for 2D->2D
-	   toSignature.getWords().size() && (_estimationType==1 || toSignature.getWords3().size()))
+	   (_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();
-		}
+			matchesCount = (int)matches[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());
 		}
 	}
 
 	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());

corelib/src/Rtabmap.cpp

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

guilib/src/DatabaseViewer.cpp

@@ -3711,7 +3711,7 @@ void DatabaseViewer::refineConstraintVisually(int from, int to, bool silent, boo
 	{
 		QMessageBox::warning(this,
 				tr("Add link"),
-				tr("Cannot find a transformation between nodes %1 and %2: %3").arg(from).arg(to).arg(info.rejectedMsg_.c_str()));
+				tr("Cannot find a transformation between nodes %1 and %2: %3").arg(from).arg(to).arg(info.rejectedMsg.c_str()));
 	}
 }
 
@@ -3782,7 +3782,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
 		{
 			QMessageBox::warning(this,
 					tr("Add link"),
-					tr("Cannot find a transformation between nodes %1 and %2: %3").arg(from).arg(to).arg(info.rejectedMsg_.c_str()));
+					tr("Cannot find a transformation between nodes %1 and %2: %3").arg(from).arg(to).arg(info.rejectedMsg.c_str()));
 		}
 	}
 	else if(containsLink(linksRemoved_, from, to))

guilib/src/MainWindow.cpp

@@ -3628,7 +3628,7 @@ void MainWindow::postProcessing()
 						}
 						else
 						{
-							QString str = tr("Cannot refine link %1->%2 (%3").arg(from).arg(to).arg(info.rejectedMsg_.c_str());
+							QString str = tr("Cannot refine link %1->%2 (%3").arg(from).arg(to).arg(info.rejectedMsg.c_str());
 							_initProgressDialog->appendText(str, Qt::darkYellow);
 							UWARN("%s", str.toStdString().c_str());
 							warn = true;