Refactored 3DTo2D and 3DTo3D motion estimations (Memory and OdometryBOW are now using the same methods)

corelib/include/rtabmap/core/util3d_correspondences.h

@@ -55,7 +55,7 @@ void RTABMAP_EXP findCorrespondences(
 		pcl::PointCloud<pcl::PointXYZ> & inliers1,
 		pcl::PointCloud<pcl::PointXYZ> & inliers2,
 		float maxDepth,
-		std::set<int> * uniqueCorrespondences = 0);
+		std::vector<int> * uniqueCorrespondences = 0);
 
 // remove depth by z axis
 void RTABMAP_EXP extractXYZCorrespondences(const std::multimap<int, pcl::PointXYZ> & words1,

corelib/include/rtabmap/core/util3d_motion_estimation.h

@@ -0,0 +1,72 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef UTIL3D_MOTION_ESTIMATION_H_
+#define UTIL3D_MOTION_ESTIMATION_H_
+
+#include <rtabmap/core/RtabmapExp.h>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <rtabmap/core/Transform.h>
+#include <rtabmap/core/CameraModel.h>
+
+namespace rtabmap
+{
+
+namespace util3d
+{
+
+Transform estimateMotion3DTo2D(
+			const std::multimap<int, pcl::PointXYZ> & words3A,
+			const std::multimap<int, cv::KeyPoint> & words2B,
+			const CameraModel & cameraModel,
+			int minInliers = 10,
+			int iterations = 100,
+			double reprojError = 5.,
+			int flagsPnP = 0,
+			const Transform & guess = Transform::getIdentity(),
+			const std::multimap<int, pcl::PointXYZ> & words3B = std::multimap<int, pcl::PointXYZ>(),
+			double * varianceOut = 0,
+			std::vector<int> * matchesOut = 0,
+			std::vector<int> * inliersOut = 0);
+
+Transform estimateMotion3DTo3D(
+			const std::multimap<int, pcl::PointXYZ> & words3A,
+			const std::multimap<int, pcl::PointXYZ> & words3B,
+			int minInliers = 10,
+			double inliersDistance = 0.1,
+			int iterations = 100,
+			int refineIterations = 5,
+			double * varianceOut = 0,
+			std::vector<int> * matchesOut = 0,
+			std::vector<int> * inliersOut = 0);
+
+} // namespace util3d
+} // namespace rtabmap
+
+#endif /* UTIL3D_TRANSFORMS_H_ */

corelib/src/CMakeLists.txt

@@ -37,6 +37,7 @@ SET(SRC_FILES
 	util3d_surface.cpp
 	util3d_features.cpp
 	util3d_correspondences.cpp
+	util3d_motion_estimation.cpp
 	
 	SensorData.cpp
 	Graph.cpp

corelib/src/Memory.cpp

@@ -47,6 +47,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util3d_registration.h"
 #include "rtabmap/core/util3d_surface.h"
 #include "rtabmap/core/util3d_transforms.h"
+#include "rtabmap/core/util3d_motion_estimation.h"
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/util2d.h"
 #include "rtabmap/core/Statistics.h"
@@ -2009,186 +2010,46 @@ Transform Memory::computeVisualTransform(
 		const Signature & oldS,
 		const Signature & newS,
 		std::string * rejectedMsg,
-		int * inliers,
+		int * inliersOut,
 		double * varianceOut) const
 {
 	Transform transform;
 	std::string msg;
 	// Guess transform from visual words
 
-	if(_bowPnPEstimation)
+	int inliersCount= 0;
-	{
+	double variance = 1.0;
-		if(_bowEpipolarGeometry)
-		{
-			UWARN("PnP estimation and Epipolar geometry estimation are set, only PnP is used.");
-		}
 
-		if((!newS.sensorData().rightRaw().empty() ||
+	if(_bowEpipolarGeometry && !_bowPnPEstimation)
-		   !newS.sensorData().stereoCameraModel().isValid()) &&
+	{
-		   (!newS.sensorData().depthRaw().empty() ||
+		if(!newS.sensorData().stereoCameraModel().isValid() &&
-			newS.sensorData().cameraModels().size() != 1 ||
+		   (newS.sensorData().cameraModels().size() != 1 ||
 			!newS.sensorData().cameraModels()[0].isValid()))
 		{
 			UERROR("Calibrated camera required (multi-cameras not supported).");
 		}
-		else
+		else if((int)oldS.getWords().size() >= _bowMinInliers &&
+				(int)newS.getWords().size() >= _bowMinInliers)
 		{
-			cv::Mat K;
+			UASSERT(oldS.sensorData().stereoCameraModel().isValid() || (oldS.sensorData().cameraModels().size() == 1 && oldS.sensorData().cameraModels()[0].isValid()));
-			Transform localTransform;
+			const CameraModel & cameraModel = oldS.sensorData().stereoCameraModel().isValid()?oldS.sensorData().stereoCameraModel().left():oldS.sensorData().cameraModels()[0];
-			if(newS.sensorData().cameraModels().size())
-			{
-				K = newS.sensorData().cameraModels()[0].K();
-				localTransform = newS.sensorData().cameraModels()[0].localTransform();
-			}
-			else
-			{
-				K = newS.sensorData().stereoCameraModel().left().K();
-				localTransform = newS.sensorData().stereoCameraModel().left().localTransform();
-			}
-			UASSERT(!K.empty() && !localTransform.isNull());
-			// 2D -> 3D
-			if(!oldS.getWords3().empty() && !newS.getWords().empty())
-			{
-				// find correspondences
-				std::vector<int> ids = uListToVector(uUniqueKeys(newS.getWords()));
-				std::vector<cv::Point3f> objectPoints(ids.size());
-				std::vector<cv::Point2f> imagePoints(ids.size());
-				int oi=0;
-				std::vector<int> matches(ids.size());
-				for(unsigned int i=0; i<ids.size(); ++i)
-				{
-					if(oldS.getWords3().count(ids[i]) == 1)
-					{
-						pcl::PointXYZ pt = oldS.getWords3().find(ids[i])->second;
-						if(pcl::isFinite(pt))
-						{
-							objectPoints[oi].x = pt.x;
-							objectPoints[oi].y = pt.y;
-							objectPoints[oi].z = pt.z;
-							imagePoints[oi] = newS.getWords().find(ids[i])->second.pt;
-							matches[oi++] = ids[i];
-						}
-					}
-				}
 
-				objectPoints.resize(oi);
+			// we only need the camera transform, send guess words3 for scale estimation
-				imagePoints.resize(oi);
-				matches.resize(oi);
-
-				if((int)matches.size() >= _bowMinInliers)
-				{
-					//PnPRansac
-					Transform guess = localTransform.inverse();
-					cv::Mat R = (cv::Mat_<double>(3,3) <<
-							(double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
-							(double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
-							(double)guess.r31(), (double)guess.r32(), (double)guess.r33());
-					cv::Mat rvec(1,3, CV_64FC1);
-					cv::Rodrigues(R, rvec);
-					cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guess.x(), (double)guess.y(), (double)guess.z());
-					std::vector<int> inliersV;
-					cv::solvePnPRansac(objectPoints,
-							imagePoints,
-							K,
-							cv::Mat(),
-							rvec,
-							tvec,
-							true,
-							_bowIterations,
-							_bowPnPReprojError,
-							0,
-							inliersV,
-							_bowPnPFlags);
-
-					if(inliers)
-					{
-						*inliers = (int)inliersV.size();
-					}
-					if((int)inliersV.size() >= _bowMinInliers)
-					{
-						cv::Rodrigues(rvec, R);
-						Transform pnp(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvec.at<double>(0),
-									   R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
-									   R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
-
-						transform = localTransform * pnp;
-
-						UDEBUG("Odom transform = %s", transform.prettyPrint().c_str());
-
-						// compute variance (like in PCL computeVariance() method of sac_model.h)
-						if(varianceOut)
-						{
-							std::vector<float> errorSqrdDists(inliersV.size());
-							oi = 0;
-							for(unsigned int i=0; i<inliersV.size(); ++i)
-							{
-								std::multimap<int, pcl::PointXYZ>::const_iterator iter = newS.getWords3().find(matches[inliersV[i]]);
-								if(iter != newS.getWords3().end() && pcl::isFinite(iter->second))
-								{
-									const cv::Point3f & objPt = objectPoints[inliersV[i]];
-									pcl::PointXYZ newPt = util3d::transformPoint(iter->second, transform);
-									errorSqrdDists[oi++] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
-								}
-							}
-							errorSqrdDists.resize(oi);
-							*varianceOut= 0;
-							if(errorSqrdDists.size())
-							{
-								std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
-								double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
-								*varianceOut = 2.1981 * median_error_sqr;
-							}
-						}
-					}
-					else
-					{
-						msg = uFormat("PnP not enough inliers (%d[%d] < %d), rejecting the transform...",
-								(int)inliersV.size(), (int)matches.size(), _bowMinInliers);
-						UINFO(msg.c_str());
-					}
-				}
-				else
-				{
-					msg = uFormat("Not enough inliers %d < %d", (int)matches.size(), _bowMinInliers);
-					UINFO(msg.c_str());
-				}
-			}
-			else
-			{
-				msg = uFormat("Not enough features in the new image (old=%d new=%d min=%d)",
-						(int)oldS.getWords3().size(), (int)newS.getWords().size(), _bowMinInliers);
-				UINFO(msg.c_str());
-			}
-		}
-
-	}
-	else if(_bowEpipolarGeometry)
-	{
-		// we only need the camera transform, send guess words3 for scale estimation
-		if(oldS.getWords3().size() && oldS.sensorData().cameraModels().size())
-		{
 			Transform cameraTransform;
-			double variance = 1;
 			std::multimap<int, pcl::PointXYZ> inliers3D = util3d::generateWords3DMono(
 					oldS.getWords(),
 					newS.getWords(),
-					oldS.sensorData().cameraModels()[0],
+					cameraModel,
 					cameraTransform,
-					100,
+					_bowIterations,
-					4.0f,
+					_bowPnPReprojError,
-					0, // cv::SOLVEPNP_ITERATIVE
+					_bowPnPFlags, // cv::SOLVEPNP_ITERATIVE
 					1.0f,
 					0.99f,
-					oldS.getWords3(),
+					oldS.getWords3(), // for scale estimation
 					&variance);
-			if(varianceOut)
+
-			{
+			inliersCount = (int)inliers3D.size();
-				*varianceOut = variance;
-			}
-			if(inliers)
-			{
-				*inliers = (int)inliers3D.size();
-			}
 
 			if(!cameraTransform.isNull())
 			{
@@ -2197,13 +2058,6 @@ Transform Memory::computeVisualTransform(
 					if(variance <= _bowEpipolarGeometryVar)
 					{
 						transform = cameraTransform.inverse();
-						if(_bowForce2D)
-						{
-							UDEBUG("Forcing 2D...");
-							float x,y,z,r,p,yaw;
-							transform.getTranslationAndEulerAngles(x,y,z, r,p,yaw);
-							transform = Transform(x,y,0, 0, 0, yaw);
-						}
 					}
 					else
 					{
@@ -2234,87 +2088,105 @@ Transform Memory::computeVisualTransform(
 			UWARN(msg.c_str());
 		}
 	}
-	else
+	else if(_bowPnPEstimation)
 	{
-		// 3D -> 3D
+		if(_bowEpipolarGeometry)
-		if(!oldS.getWords3().empty() && !newS.getWords3().empty())
 		{
-			pcl::PointCloud<pcl::PointXYZ>::Ptr inliersOld(new pcl::PointCloud<pcl::PointXYZ>);
+			UWARN("PnP estimation and Epipolar geometry estimation are set, only PnP is used.");
-			pcl::PointCloud<pcl::PointXYZ>::Ptr inliersNew(new pcl::PointCloud<pcl::PointXYZ>);
+		}
-			util3d::findCorrespondences(
-					oldS.getWords3(),
-					newS.getWords3(),
-					*inliersOld,
-					*inliersNew,
-					_bowMaxDepth);
 
-			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs2d;
+		if(!newS.sensorData().stereoCameraModel().isValid() &&
-			EpipolarGeometry::findPairsUnique(oldS.getWords(), newS.getWords(), pairs2d);
+		   (newS.sensorData().cameraModels().size() != 1 ||
-
+			!newS.sensorData().cameraModels()[0].isValid()))
-			UDEBUG("3D unique Correspondences = %d (2D unique pairs=%d)  words=%d and %d",
+		{
-					(int)inliersOld->size(), (int)pairs2d.size(), (int)oldS.getWords3().size(), (int)newS.getWords3().size());
+			UERROR("Calibrated camera required (multi-cameras not supported).");
-
+		}
-			if((int)inliersOld->size() >= _bowMinInliers)
+		else
+		{
+			// 3D to 2D
+			if((int)oldS.getWords3().size() >= _bowMinInliers &&
+			   (int)newS.getWords().size() >= _bowMinInliers)
 			{
+				UASSERT(newS.sensorData().stereoCameraModel().isValid() || (newS.sensorData().cameraModels().size() == 1 && newS.sensorData().cameraModels()[0].isValid()));
+				const CameraModel & cameraModel = newS.sensorData().stereoCameraModel().isValid()?newS.sensorData().stereoCameraModel().left():newS.sensorData().cameraModels()[0];
 
-				int inliersCount = 0;
 				std::vector<int> inliersV;
-				Transform t = util3d::transformFromXYZCorrespondences(
+				transform = util3d::estimateMotion3DTo2D(
-						inliersOld,
+						oldS.getWords3(),
-						inliersNew,
+						newS.getWords(),
-						_bowInlierDistance,
+						cameraModel,
+						_bowMinInliers,
 						_bowIterations,
-						true, 3.0, 10,
+						_bowPnPReprojError,
-						&inliersV,
+						_bowPnPFlags,
-						varianceOut);
+						Transform::getIdentity(),
+						newS.getWords3(),
+						&variance,
+						0,
+						&inliersV);
 				inliersCount = (int)inliersV.size();
-				if(!t.isNull() && inliersCount >= _bowMinInliers)
+				if(transform.isNull())
 				{
-					transform = t;
+					msg = uFormat("Not enough inliers %d/%d between %d and %d",
-					if(_bowForce2D)
+							inliersCount, _bowMinInliers, oldS.id(), newS.id());
-					{
-						UDEBUG("Forcing 2D...");
-						float x,y,z,r,p,yaw;
-						transform.getTranslationAndEulerAngles(x,y,z, r,p,yaw);
-						transform = Transform(x,y,0, 0, 0, yaw);
-					}
-				}
-				else if(inliersCount < _bowMinInliers)
-				{
-					msg = uFormat("Not enough inliers (after RANSAC) %d/%d between %d and %d", inliersCount, _bowMinInliers, oldS.id(), newS.id());
 					UINFO(msg.c_str());
 				}
-				else if(inliersCount == (int)inliersOld->size())
+				else
 				{
-					msg = uFormat("Rejected identity with full inliers.");
+					transform = transform.inverse();
-					UINFO(msg.c_str());
-				}
-
-				if(inliers)
-				{
-					*inliers = inliersCount;
 				}
 			}
 			else
 			{
-				msg = uFormat("Not enough inliers %d/%d between %d and %d", (int)inliersOld->size(), _bowMinInliers, oldS.id(), newS.id());
+				msg = uFormat("Not enough features in images (old=%d, new=%d, min=%d)",
+						(int)oldS.getWords3().size(), (int)newS.getWords().size(), _bowMinInliers);
 				UINFO(msg.c_str());
 			}
 		}
-		else if(!oldS.isBadSignature() && !newS.isBadSignature() && (oldS.getWords3().size()==0 || newS.getWords3().size()==0))
+
+	}
+	else
+	{
+		// 3D -> 3D
+		if((int)oldS.getWords3().size() >= _bowMinInliers &&
+		   (int)newS.getWords3().size() >= _bowMinInliers)
 		{
-			msg = uFormat("Words 3D empty?!? olds=%d=%d newS=%d=%d",
+			std::vector<int> inliersV;
-					oldS.id(), (int)oldS.getWords3().size(),
+			transform = util3d::estimateMotion3DTo3D(
-					newS.id(), (int)newS.getWords3().size());
+					oldS.getWords3(),
-			UWARN(msg.c_str());
+					newS.getWords3(),
+					_bowMinInliers,
+					_bowInlierDistance,
+					_bowIterations,
+					10,
+					&variance,
+					0,
+					&inliersV);
+			inliersCount = (int)inliersV.size();
+			if(transform.isNull())
+			{
+				msg = uFormat("Not enough inliers %d/%d between %d and %d",
+						inliersCount, _bowMinInliers, oldS.id(), newS.id());
+				UINFO(msg.c_str());
+			}
+			else
+			{
+				transform = transform.inverse();
+			}
+		}
+		else
+		{
+			msg = uFormat("Not enough 3D features in images (old=%d, new=%d, min=%d)",
+					(int)oldS.getWords3().size(), (int)newS.getWords3().size(), _bowMinInliers);
+			UINFO(msg.c_str());
 		}
 	}
 
 	if(!transform.isNull())
 	{
 		// verify if it is a 180 degree transform, well verify > 90
-		float roll,pitch,yaw;
+		float x,y,z, roll,pitch,yaw;
-		transform.getEulerAngles(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)
@@ -2324,12 +2196,25 @@ Transform Memory::computeVisualTransform(
 					roll, pitch, yaw);
 			UWARN(msg.c_str());
 		}
+		else if(_bowForce2D)
+		{
+			UDEBUG("Forcing 2D...");
+			transform = Transform(x,y,0, 0, 0, yaw);
+		}
 	}
 
 	if(rejectedMsg)
 	{
 		*rejectedMsg = msg;
 	}
+	if(inliersOut)
+	{
+		*inliersOut = inliersCount;
+	}
+	if(varianceOut)
+	{
+		*varianceOut = variance;
+	}
 	UDEBUG("transform=%s", transform.prettyPrint().c_str());
 	return transform;
 }

corelib/src/OdometryBOW.cpp

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util3d_transforms.h"
 #include "rtabmap/core/util3d_registration.h"
 #include "rtabmap/core/util3d_correspondences.h"
+#include "rtabmap/core/util3d_motion_estimation.h"
 #include "rtabmap/core/Graph.h"
 #include "rtabmap/core/VWDictionary.h"
 #include "rtabmap/utilite/ULogger.h"
@@ -208,7 +209,7 @@ Transform OdometryBOW::computeTransform(
 	}
 
 	double variance = 0;
-	int inliers = 0;
+	int inliersCount = 0;
 	int correspondences = 0;
 	int nFeatures = 0;
 
@@ -229,8 +230,11 @@ Transform OdometryBOW::computeTransform(
 			Transform transform;
 			if((int)localMap_.size() >= this->getMinInliers())
 			{
+				std::vector<int> matches, inliers;
+				Transform t;
 				if(this->isPnPEstimationUsed())
 				{
+					// 3D to 2D
 					if(data.cameraModels().size() > 1)
 					{
 						UERROR("PnP cannot be used on multi-cameras setup.");
@@ -240,117 +244,19 @@ Transform OdometryBOW::computeTransform(
 						UASSERT(data.stereoCameraModel().isValid() || (data.cameraModels().size() == 1 && data.cameraModels()[0].isValid()));
 						const CameraModel & cameraModel = data.stereoCameraModel().isValid()?data.stereoCameraModel().left():data.cameraModels()[0];
 
-						// find correspondences
+						t = util3d::estimateMotion3DTo2D(
-						std::vector<int> ids = uListToVector(uUniqueKeys(newSignature->getWords()));
+								localMap_,
-						std::vector<cv::Point3f> objectPoints(ids.size());
+								newSignature->getWords(),
-						std::vector<cv::Point2f> imagePoints(ids.size());
+								cameraModel,
-						int oi=0;
+								this->getMinInliers(),
-						std::vector<int> matches(ids.size());
+								this->getIterations(),
-						for(unsigned int i=0; i<ids.size(); ++i)
+								this->getPnPReprojError(),
-						{
+								this->getPnPFlags(),
-							if(localMap_.count(ids[i]) == 1)
+								this->getPose(),
-							{
+								newSignature->getWords3(),
-								pcl::PointXYZ pt = localMap_.find(ids[i])->second;
+								&variance,
-								objectPoints[oi].x = pt.x;
+								&matches,
-								objectPoints[oi].y = pt.y;
+								&inliers);
-								objectPoints[oi].z = pt.z;
-								imagePoints[oi] = newSignature->getWords().find(ids[i])->second.pt;
-								matches[oi++] = ids[i];
-							}
-						}
-
-						objectPoints.resize(oi);
-						imagePoints.resize(oi);
-						matches.resize(oi);
-
-						if(this->isInfoDataFilled() && info)
-						{
-							info->wordMatches.insert(info->wordMatches.end(), matches.begin(), matches.end());
-						}
-						correspondences = (int)matches.size();
-
-						if((int)matches.size() >= this->getMinInliers())
-						{
-							//PnPRansac
-							cv::Mat K = cameraModel.K();
-							Transform guess = (this->getPose() * cameraModel.localTransform()).inverse();
-							cv::Mat R = (cv::Mat_<double>(3,3) <<
-									(double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
-									(double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
-									(double)guess.r31(), (double)guess.r32(), (double)guess.r33());
-							cv::Mat rvec(1,3, CV_64FC1);
-							cv::Rodrigues(R, rvec);
-							cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guess.x(), (double)guess.y(), (double)guess.z());
-							std::vector<int> inliersV;
-							cv::solvePnPRansac(objectPoints,
-									imagePoints,
-									K,
-									cv::Mat(),
-									rvec,
-									tvec,
-									true,
-									this->getIterations(),
-									this->getPnPReprojError(),
-									0,
-									inliersV,
-									this->getPnPFlags());
-
-							inliers = (int)inliersV.size();
-							if((int)inliersV.size() >= this->getMinInliers())
-							{
-								cv::Rodrigues(rvec, R);
-								Transform pnp(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvec.at<double>(0),
-											   R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
-											   R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
-
-								// make it incremental
-								transform = (cameraModel.localTransform() * pnp * this->getPose()).inverse();
-
-								UDEBUG("Odom transform = %s", transform.prettyPrint().c_str());
-
-								// compute variance (like in PCL computeVariance() method of sac_model.h)
-								std::vector<float> errorSqrdDists(inliersV.size());
-								oi = 0;
-								for(unsigned int i=0; i<inliersV.size(); ++i)
-								{
-									std::multimap<int, pcl::PointXYZ>::const_iterator iter = newSignature->getWords3().find(matches[inliersV[i]]);
-									if(iter != newSignature->getWords3().end() && pcl::isFinite(iter->second))
-									{
-										const cv::Point3f & objPt = objectPoints[inliersV[i]];
-										pcl::PointXYZ newPt = util3d::transformPoint(iter->second, this->getPose()*transform);
-										errorSqrdDists[oi++] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
-									}
-								}
-								errorSqrdDists.resize(oi);
-								if(errorSqrdDists.size())
-								{
-									std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
-									double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
-									variance = 2.1981 * median_error_sqr;
-								}
-								else
-								{
-									variance = 1;
-								}
-							}
-							else
-							{
-								UWARN("PnP not enough inliers (%d < %d), rejecting the transform...", (int)inliersV.size(), this->getMinInliers());
-							}
-
-							if(this->isInfoDataFilled() && info && inliersV.size())
-							{
-								info->wordInliers.resize(inliersV.size());
-								for(unsigned int i=0; i<inliersV.size(); ++i)
-								{
-									info->wordInliers[i] = matches[inliersV[i]];
-								}
-							}
-						}
-						else
-						{
-							UWARN("Not enough correspondences (%d < %d)", correspondences, this->getMinInliers());
-						}
 					}
 					else
 					{
@@ -359,76 +265,51 @@ Transform OdometryBOW::computeTransform(
 				}
 				else
 				{
+					// 3D to 3D
 					if((int)newSignature->getWords3().size() >= this->getMinInliers())
 					{
-						pcl::PointCloud<pcl::PointXYZ>::Ptr inliers1(new pcl::PointCloud<pcl::PointXYZ>); // previous
+						t = util3d::estimateMotion3DTo3D(
-						pcl::PointCloud<pcl::PointXYZ>::Ptr inliers2(new pcl::PointCloud<pcl::PointXYZ>); // new
-
-						// No need to set max depth here, it is already applied in extractKeypointsAndDescriptors() above.
-						// Also! the localMap_ have points not in camera frame anymore (in local map frame), so filtering
-						// by depth here is wrong!
-						std::set<int> uniqueCorrespondences;
-						util3d::findCorrespondences(
 								localMap_,
 								newSignature->getWords3(),
-								*inliers1,
+								this->getMinInliers(),
-								*inliers2,
+								this->getInlierDistance(),
-								0,
+								this->getIterations(),
-								&uniqueCorrespondences);
+								this->getRefineIterations(),
-
+								&variance,
-						UDEBUG("localMap=%d, new=%d, unique correspondences=%d", (int)localMap_.size(), (int)newSignature->getWords3().size(), (int)uniqueCorrespondences.size());
+								&matches,
-
+								&inliers);
-						if(this->isInfoDataFilled() && info)
-						{
-							info->wordMatches.insert(info->wordMatches.end(), uniqueCorrespondences.begin(), uniqueCorrespondences.end());
-						}
-
-						correspondences = (int)inliers1->size();
-						if((int)inliers1->size() >= this->getMinInliers())
-						{
-							// the transform returned is global odometry pose, not incremental one
-							std::vector<int> inliersV;
-							Transform t = util3d::transformFromXYZCorrespondences(
-									inliers2,
-									inliers1,
-									this->getInlierDistance(),
-									this->getIterations(),
-									this->getRefineIterations()>0, 3.0, this->getRefineIterations(),
-									&inliersV,
-									&variance);
-
-							inliers = (int)inliersV.size();
-							if(!t.isNull() && inliers >= this->getMinInliers())
-							{
-								// make it incremental
-								transform = this->getPose().inverse() * t;
-
-								UDEBUG("Odom transform = %s", transform.prettyPrint().c_str());
-							}
-							else
-							{
-								UWARN("Transform not valid (inliers = %d/%d)", inliers, correspondences);
-							}
-
-							if(this->isInfoDataFilled() && info && inliersV.size())
-							{
-								info->wordInliers.resize(inliersV.size());
-								for(unsigned int i=0; i<inliersV.size(); ++i)
-								{
-									info->wordInliers[i] = info->wordMatches[inliersV[i]];
-								}
-							}
-						}
-						else
-						{
-							UWARN("Not enough inliers %d < %d", (int)inliers1->size(), this->getMinInliers());
-						}
 					}
 					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())
+				{
+					UWARN("Not enough correspondences (%d < %d)", correspondences, this->getMinInliers());
+				}
+				else if(inliersCount < this->getMinInliers())
+				{
+					UWARN("Not enough inliers (%d < %d)", inliersCount, this->getMinInliers());
+				}
+				else
+				{
+					UWARN("Unknown estimation error");
+				}
 			}
 			else
 			{
@@ -534,7 +415,7 @@ Transform OdometryBOW::computeTransform(
 	if(info)
 	{
 		info->variance = variance;
-		info->inliers = inliers;
+		info->inliers = inliersCount;
 		info->matches = correspondences;
 		info->features = nFeatures;
 		info->localMapSize = (int)localMap_.size();
@@ -544,7 +425,7 @@ Transform OdometryBOW::computeTransform(
 			timer.elapsed(),
 			output.isNull()?"true":"false",
 			nFeatures,
-			inliers,
+			inliersCount,
 			correspondences,
 			variance,
 			(int)localMap_.size(),

corelib/src/util3d_correspondences.cpp

@@ -355,12 +355,16 @@ void findCorrespondences(
 		pcl::PointCloud<pcl::PointXYZ> & inliers1,
 		pcl::PointCloud<pcl::PointXYZ> & inliers2,
 		float maxDepth,
-		std::set<int> * uniqueCorrespondences)
+		std::vector<int> * uniqueCorrespondences)
 {
 	std::list<int> ids = uUniqueKeys(words1);
 	// Find pairs
 	inliers1.resize(ids.size());
 	inliers2.resize(ids.size());
+	if(uniqueCorrespondences)
+	{
+		uniqueCorrespondences->resize(ids.size());
+	}
 
 	int oi=0;
 	for(std::list<int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
@@ -375,16 +379,20 @@ void findCorrespondences(
 			   (inliers2[oi].x != 0 || inliers2[oi].y != 0 || inliers2[oi].z != 0) &&
 			   (maxDepth <= 0 || (inliers1[oi].x > 0 && inliers1[oi].x <= maxDepth && inliers2[oi].x>0 &&inliers2[oi].x<=maxDepth)))
 			{
-				++oi;
 				if(uniqueCorrespondences)
 				{
-					uniqueCorrespondences->insert(*iter);
+					uniqueCorrespondences->at(oi) = *iter;
 				}
+				++oi;
 			}
 		}
 	}
 	inliers1.resize(oi);
 	inliers2.resize(oi);
+	if(uniqueCorrespondences)
+	{
+		uniqueCorrespondences->resize(oi);
+	}
 }
 
 }

corelib/src/util3d_features.cpp

@@ -351,18 +351,6 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
 						}
 					}
 
-					if(!useCameraTransformGuess)
-					{
-						cv::Mat R, T;
-						EpipolarGeometry::findRTFromP(P, R, T);
-
-						Transform t(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), T.at<double>(0),
-									R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), T.at<double>(1),
-									R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), T.at<double>(2));
-
-						cameraTransform = (cameraModel.localTransform() * t).inverse() * cameraModel.localTransform();
-					}
-
 					if(refGuess3D.size())
 					{
 						// scale estimation
@@ -489,7 +477,6 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
 								else
 								{
 									UWARN("No inliers after PnP!");
-									cameraTransform = Transform();
 								}
 							}
 						}
@@ -498,6 +485,17 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
 							UWARN("Cannot compute the scale, no points corresponding between the generated ref words and words guess");
 						}
 					}
+					else if(!useCameraTransformGuess)
+					{
+						cv::Mat R, T;
+						EpipolarGeometry::findRTFromP(P, R, T);
+
+						Transform t(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), T.at<double>(0),
+									R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), T.at<double>(1),
+									R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), T.at<double>(2));
+
+						cameraTransform = (cameraModel.localTransform() * t).inverse() * cameraModel.localTransform();
+					}
 				}
 			}
 		}

corelib/src/util3d_motion_estimation.cpp

@@ -0,0 +1,235 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/core/util3d_motion_estimation.h"
+
+#include "rtabmap/utilite/UStl.h"
+#include "rtabmap/utilite/UMath.h"
+#include "rtabmap/core/util3d_transforms.h"
+#include "rtabmap/core/util3d_registration.h"
+#include "rtabmap/core/util3d_correspondences.h"
+
+namespace rtabmap
+{
+
+namespace util3d
+{
+
+Transform estimateMotion3DTo2D(
+			const std::multimap<int, pcl::PointXYZ> & words3A,
+			const std::multimap<int, cv::KeyPoint> & words2B,
+			const CameraModel & cameraModel,
+			int minInliers,
+			int iterations,
+			double reprojError,
+			int flagsPnP,
+			const Transform & guess,
+			const std::multimap<int, pcl::PointXYZ> & words3B,
+			double * varianceOut,
+			std::vector<int> * matchesOut,
+			std::vector<int> * inliersOut)
+{
+
+	Transform transform;
+	std::vector<int> matches, inliers;
+
+	if(varianceOut)
+	{
+		*varianceOut = 1.0;
+	}
+
+	// find correspondences
+	std::vector<int> ids = uListToVector(uUniqueKeys(words2B));
+	std::vector<cv::Point3f> objectPoints(ids.size());
+	std::vector<cv::Point2f> imagePoints(ids.size());
+	int oi=0;
+	matches.resize(ids.size());
+	for(unsigned int i=0; i<ids.size(); ++i)
+	{
+		if(words3A.count(ids[i]) == 1)
+		{
+			pcl::PointXYZ pt = words3A.find(ids[i])->second;
+			objectPoints[oi].x = pt.x;
+			objectPoints[oi].y = pt.y;
+			objectPoints[oi].z = pt.z;
+			imagePoints[oi] = words2B.find(ids[i])->second.pt;
+			matches[oi++] = ids[i];
+		}
+	}
+
+	objectPoints.resize(oi);
+	imagePoints.resize(oi);
+	matches.resize(oi);
+
+	if((int)matches.size() >= minInliers)
+	{
+		//PnPRansac
+		cv::Mat K = cameraModel.K();
+		Transform guessCameraFrame = (guess * cameraModel.localTransform()).inverse();
+		cv::Mat R = (cv::Mat_<double>(3,3) <<
+				(double)guessCameraFrame.r11(), (double)guessCameraFrame.r12(), (double)guessCameraFrame.r13(),
+				(double)guessCameraFrame.r21(), (double)guessCameraFrame.r22(), (double)guessCameraFrame.r23(),
+				(double)guessCameraFrame.r31(), (double)guessCameraFrame.r32(), (double)guessCameraFrame.r33());
+
+		cv::Mat rvec(1,3, CV_64FC1);
+		cv::Rodrigues(R, rvec);
+		cv::Mat tvec = (cv::Mat_<double>(1,3) <<
+				(double)guessCameraFrame.x(), (double)guessCameraFrame.y(), (double)guessCameraFrame.z());
+
+		cv::solvePnPRansac(
+				objectPoints,
+				imagePoints,
+				K,
+				cv::Mat(),
+				rvec,
+				tvec,
+				true,
+				iterations,
+				reprojError,
+				0,
+				inliers,
+				flagsPnP);
+
+		if((int)inliers.size() >= minInliers)
+		{
+			cv::Rodrigues(rvec, R);
+			Transform pnp(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvec.at<double>(0),
+						   R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
+						   R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
+
+			transform = (cameraModel.localTransform() * pnp).inverse();
+
+			// compute variance (like in PCL computeVariance() method of sac_model.h)
+			if(varianceOut && words3B.size())
+			{
+				std::vector<float> errorSqrdDists(inliers.size());
+				oi = 0;
+				for(unsigned int i=0; i<inliers.size(); ++i)
+				{
+					std::multimap<int, pcl::PointXYZ>::const_iterator iter = words3B.find(matches[inliers[i]]);
+					if(iter != words3B.end() && pcl::isFinite(iter->second))
+					{
+						const cv::Point3f & objPt = objectPoints[inliers[i]];
+						pcl::PointXYZ newPt = util3d::transformPoint(iter->second, transform);
+						errorSqrdDists[oi++] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
+					}
+				}
+				errorSqrdDists.resize(oi);
+				if(errorSqrdDists.size())
+				{
+					std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
+					double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
+					*varianceOut = 2.1981 * median_error_sqr;
+				}
+			}
+		}
+	}
+
+	if(matchesOut)
+	{
+		*matchesOut = matches;
+	}
+	if(inliersOut)
+	{
+		inliersOut->resize(inliers.size());
+		for(unsigned int i=0; i<inliers.size(); ++i)
+		{
+			inliersOut->at(i) = matches[inliers[i]];
+		}
+	}
+
+	return transform;
+}
+
+Transform estimateMotion3DTo3D(
+			const std::multimap<int, pcl::PointXYZ> & words3A,
+			const std::multimap<int, pcl::PointXYZ> & words3B,
+			int minInliers,
+			double inliersDistance,
+			int iterations,
+			int refineIterations,
+			double * varianceOut,
+			std::vector<int> * matchesOut,
+			std::vector<int> * inliersOut)
+{
+	Transform transform;
+	pcl::PointCloud<pcl::PointXYZ>::Ptr inliers1(new pcl::PointCloud<pcl::PointXYZ>); // previous
+	pcl::PointCloud<pcl::PointXYZ>::Ptr inliers2(new pcl::PointCloud<pcl::PointXYZ>); // new
+
+	std::vector<int> matches;
+	util3d::findCorrespondences(
+			words3A,
+			words3B,
+			*inliers1,
+			*inliers2,
+			0,
+			&matches);
+
+	if(varianceOut)
+	{
+		*varianceOut = 1.0;
+	}
+
+	if((int)inliers1->size() >= minInliers)
+	{
+		std::vector<int> inliers;
+		Transform t = util3d::transformFromXYZCorrespondences(
+				inliers2,
+				inliers1,
+				inliersDistance,
+				iterations,
+				refineIterations>0,
+				3.0,
+				refineIterations,
+				&inliers,
+				varianceOut);
+
+		if(!t.isNull() && (int)inliers.size() >= minInliers)
+		{
+			transform = t;
+		}
+
+		if(matchesOut)
+		{
+			*matchesOut = matches;
+		}
+
+		if(inliersOut)
+		{
+			inliersOut->resize(inliers.size());
+			for(unsigned int i=0; i<inliers.size(); ++i)
+			{
+				inliersOut->at(i) = matches[inliers[i]];
+			}
+		}
+	}
+	return transform;
+}
+
+}
+
+}