Using solvePnPRansac() of OpenCV2.4 in OpenCV3 (for some reasons RANSAC is better in OpenCV2.4, tested on Mac OS X). Updated default 3D->3D estimation parameters

2026-10-03 16:27:46 +08:00 · 2015-10-01 21:00:16 -04:00
parent 0c8309bb28
commit ee76a02117
4 changed files with 410 additions and 73 deletions
@@ -320,18 +320,18 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Odom, FeatureType,            int, 6, 	    "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
 	RTABMAP_PARAM(Odom, EstimationType,  		int, 0,    	    "Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP)");
 	RTABMAP_PARAM(Odom, MaxFeatures,            int, 1000, 		"0 no limits.");
-	RTABMAP_PARAM(Odom, InlierDistance,         float, 0.02, 	"Maximum distance for visual word correspondences.");
+	RTABMAP_PARAM(Odom, InlierDistance,         float, 0.1,     "Maximum distance for visual word correspondences. Used by 3D->3D estimation approach.");
 	RTABMAP_PARAM(Odom, MinInliers,             int, 20, 		"Minimum visual word correspondences to compute geometry transform.");
 	RTABMAP_PARAM(Odom, Iterations,             int, 100, 		"Maximum iterations to compute the transform from visual words.");
-	RTABMAP_PARAM(Odom, RefineIterations,       int, 5,        "Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.");
-	RTABMAP_PARAM(Odom, MaxDepth,               float, 4.0, 	"Max depth of the words (0 means no limit).");
+	RTABMAP_PARAM(Odom, RefineIterations,       int, 5,         "Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.");
+	RTABMAP_PARAM(Odom, MaxDepth,               float, 0,       "Max depth of the words (0 means no limit).");
 	RTABMAP_PARAM(Odom, ResetCountdown,         int, 0,         "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).");
 	RTABMAP_PARAM_STR(Odom, RoiRatios,          "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
 	RTABMAP_PARAM(Odom, Force2D, 		        bool, false,     "Force 2D transform (3Dof: x,y and yaw).");
 	RTABMAP_PARAM(Odom, Holonomic, 		        bool, true,     "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)).");
 	RTABMAP_PARAM(Odom, FillInfoData, 		    bool, true,     "Fill info with data (inliers/outliers features).");
 	RTABMAP_PARAM(Odom, ImageBufferSize,        unsigned int, 1, "Data buffer size (0 min inf).");
-	RTABMAP_PARAM(Odom, VarianceFromInliersCount, bool, false,   "Set variance as the inverse of the number of inliers. Otherwise, the variance is computed as the average 3D position error of the inliers.");
+	RTABMAP_PARAM(Odom, VarianceFromInliersCount, bool, true,   "Set variance as the inverse of the number of inliers. Otherwise, the variance is computed as the average 3D position error of the inliers.");
 	RTABMAP_PARAM(Odom, PnPReprojError, 		double, 5.0,     "PnP reprojection error.");
 	RTABMAP_PARAM(Odom, PnPFlags, 				int, 1,    	     "PnP flags: 0=Iterative, 1=EPNP, 2=P3P");
 	RTABMAP_PARAM(Odom, ParticleFiltering, 		bool, false,     "Particle filtering to smooth the odometry trajectory.");
@@ -371,7 +371,7 @@ class RTABMAP_EXP Parameters

 	RTABMAP_PARAM(LccBow, EstimationType,  int, 0,    	    "Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP), 2:2D->2D (Epipolar Geometry)");
 	RTABMAP_PARAM(LccBow, MinInliers,      int, 10, 		"Minimum visual word correspondences to compute geometry transform.");
-	RTABMAP_PARAM(LccBow, InlierDistance,  float, 0.05, 	"Maximum distance for visual word correspondences.");
+	RTABMAP_PARAM(LccBow, InlierDistance,  float, 0.1,      "Maximum distance for visual word correspondences. Used by 3D->3D estimation approach.");
 	RTABMAP_PARAM(LccBow, Iterations,      int, 100, 		"Maximum iterations to compute the transform from visual words.");
 	RTABMAP_PARAM(LccBow, RefineIterations,  int, 10,        "Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.");
 	RTABMAP_PARAM(LccBow, Force2D, 		   bool, false,     "Force 2D transform (3Dof: x,y and yaw).");
@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 #include "rtabmap/utilite/UStl.h"
 #include "rtabmap/utilite/UMath.h"
+#include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/core/util3d_transforms.h"
 #include "rtabmap/core/util3d_registration.h"
 #include "rtabmap/core/util3d_correspondences.h"
@@ -39,6 +40,14 @@ namespace rtabmap
 namespace util3d
 {

+#if CV_MAJOR_VERSION >= 3
+void solvePnPRansac(cv::InputArray _opoints, cv::InputArray _ipoints,
+		cv::InputArray _cameraMatrix, cv::InputArray _distCoeffs,
+		cv::OutputArray _rvec, cv::OutputArray _tvec, bool useExtrinsicGuess,
+        int iterationsCount, float reprojectionError, int minInliersCount,
+        cv::OutputArray _inliers, int flags);
+#endif
+
 Transform estimateMotion3DTo2D(
 			const std::map<int, pcl::PointXYZ> & words3A,
 			const std::map<int, cv::KeyPoint> & words2B,
@@ -100,7 +109,11 @@ Transform estimateMotion3DTo2D(
 		cv::Mat tvec = (cv::Mat_<double>(1,3) <<
 				(double)guessCameraFrame.x(), (double)guessCameraFrame.y(), (double)guessCameraFrame.z());

+#if CV_MAJOR_VERSION >= 3
+		solvePnPRansac(
+#else
 		cv::solvePnPRansac(
+#endif
 				objectPoints,
 				imagePoints,
 				K,
@@ -110,11 +123,7 @@ Transform estimateMotion3DTo2D(
 				true,
 				iterations,
 				reprojError,
-#if CV_MAJOR_VERSION < 3
 				0, // min inliers
-#else
-				0.99, // confidence
-#endif
 				inliers,
 				flagsPnP);

@@ -234,6 +243,297 @@ Transform estimateMotion3DTo3D(
 	return transform;
 }

+// Don't know why, but the RANSAC implementation in OpenCV 3 gives me far
+// more wrong results than the 2.4x implementation.
+// Here is a copy of the RANSAC implementation from OpenCV 2.4.x version
+#if CV_MAJOR_VERSION >= 3
+namespace pnpransac
+{
+	const int MIN_POINTS_COUNT = 4;
+
+	static void project3dPoints(const cv::Mat& points, const cv::Mat& rvec, const cv::Mat& tvec, cv::Mat& modif_points)
+	{
+		modif_points.create(1, points.cols, CV_32FC3);
+		cv::Mat R(3, 3, CV_64FC1);
+		cv::Rodrigues(rvec, R);
+		cv::Mat transformation(3, 4, CV_64F);
+		cv::Mat r = transformation.colRange(0, 3);
+		R.copyTo(r);
+		cv::Mat t = transformation.colRange(3, 4);
+		tvec.copyTo(t);
+		transform(points, modif_points, transformation);
+	}
+
+	struct CameraParameters
+	{
+		void init(cv::Mat _intrinsics, cv::Mat _distCoeffs)
+		{
+			_intrinsics.copyTo(intrinsics);
+			_distCoeffs.copyTo(distortion);
+		}
+
+		cv::Mat intrinsics;
+		cv::Mat distortion;
+	};
+
+	struct Parameters
+	{
+		int iterationsCount;
+		float reprojectionError;
+		int minInliersCount;
+		bool useExtrinsicGuess;
+		int flags;
+		CameraParameters camera;
+	};
+
+	template <typename OpointType, typename IpointType>
+	static void pnpTask(const int curIndex, const std::vector<char>& pointsMask, const cv::Mat& objectPoints, const cv::Mat& imagePoints,
+				 const Parameters& params, std::vector<int>& inliers, int& bestIndex, cv::Mat& rvec, cv::Mat& tvec,
+				 const cv::Mat& rvecInit, const cv::Mat& tvecInit, cv::Mutex& resultsMutex)
+	{
+		cv::Mat modelObjectPoints(1, MIN_POINTS_COUNT, CV_MAKETYPE(cv::DataDepth<OpointType>::value, 3));
+		cv::Mat modelImagePoints(1, MIN_POINTS_COUNT, CV_MAKETYPE(cv::DataDepth<IpointType>::value, 2));
+		for (int i = 0, colIndex = 0; i < (int)pointsMask.size(); i++)
+		{
+			if (pointsMask[i])
+			{
+				cv::Mat colModelImagePoints = modelImagePoints(cv::Rect(colIndex, 0, 1, 1));
+				imagePoints.col(i).copyTo(colModelImagePoints);
+				cv::Mat colModelObjectPoints = modelObjectPoints(cv::Rect(colIndex, 0, 1, 1));
+				objectPoints.col(i).copyTo(colModelObjectPoints);
+				colIndex = colIndex+1;
+			}
+		}
+
+		//filter same 3d points, hang in solvePnP
+		double eps = 1e-10;
+		int num_same_points = 0;
+		for (int i = 0; i < MIN_POINTS_COUNT; i++)
+			for (int j = i + 1; j < MIN_POINTS_COUNT; j++)
+			{
+				if (norm(modelObjectPoints.at<cv::Vec<OpointType,3> >(0, i) - modelObjectPoints.at<cv::Vec<OpointType,3> >(0, j)) < eps)
+					num_same_points++;
+			}
+		if (num_same_points > 0)
+			return;
+
+		cv::Mat localRvec, localTvec;
+		rvecInit.copyTo(localRvec);
+		tvecInit.copyTo(localTvec);
+
+		// OpenCV 3
+		cv::solvePnP(
+				modelObjectPoints,
+				modelImagePoints,
+				params.camera.intrinsics,
+				params.camera.distortion,
+				localRvec,
+				localTvec,
+				 params.useExtrinsicGuess,
+				 params.flags);
+
+
+		std::vector<cv::Point_<OpointType> > projected_points;
+		projected_points.resize(objectPoints.cols);
+		projectPoints(objectPoints, localRvec, localTvec, params.camera.intrinsics, params.camera.distortion, projected_points);
+
+		cv::Mat rotatedPoints;
+		project3dPoints(objectPoints, localRvec, localTvec, rotatedPoints);
+
+		std::vector<int> localInliers;
+		for (int i = 0; i < objectPoints.cols; i++)
+		{
+			//Although p is a 2D point it needs the same type as the object points to enable the norm calculation
+			cv::Point_<OpointType> p((OpointType)imagePoints.at<cv::Vec<IpointType,2> >(0, i)[0],
+								 (OpointType)imagePoints.at<cv::Vec<IpointType,2> >(0, i)[1]);
+			if ((norm(p - projected_points[i]) < params.reprojectionError)
+				&& (rotatedPoints.at<cv::Vec<OpointType,3> >(0, i)[2] > 0)) //hack
+			{
+				localInliers.push_back(i);
+			}
+		}
+
+		resultsMutex.lock();
+		if ( (localInliers.size() > inliers.size()) || (localInliers.size() == inliers.size() && curIndex > bestIndex))
+		{
+			inliers.clear();
+			inliers.resize(localInliers.size());
+			memcpy(&inliers[0], &localInliers[0], sizeof(int) * localInliers.size());
+			localRvec.copyTo(rvec);
+			localTvec.copyTo(tvec);
+			bestIndex = curIndex;
+		}
+		resultsMutex.unlock();
+	}
+
+	static void pnpTask(const int curIndex, const std::vector<char>& pointsMask, const cv::Mat& objectPoints, const cv::Mat& imagePoints,
+		const Parameters& params, std::vector<int>& inliers, int& bestIndex, cv::Mat& rvec, cv::Mat& tvec,
+		const cv::Mat& rvecInit, const cv::Mat& tvecInit, cv::Mutex& resultsMutex)
+	{
+		CV_Assert(objectPoints.depth() == CV_64F ||  objectPoints.depth() == CV_32F);
+		CV_Assert(imagePoints.depth() == CV_64F ||  imagePoints.depth() == CV_32F);
+		const bool objectDoublePrecision = objectPoints.depth() == CV_64F;
+		const bool imageDoublePrecision = imagePoints.depth() == CV_64F;
+		if(objectDoublePrecision)
+		{
+			if(imageDoublePrecision)
+				pnpTask<double, double>(curIndex, pointsMask, objectPoints, imagePoints, params, inliers, bestIndex, rvec, tvec, rvecInit, tvecInit, resultsMutex);
+			else
+				pnpTask<double, float>(curIndex, pointsMask, objectPoints, imagePoints, params, inliers, bestIndex, rvec, tvec, rvecInit, tvecInit, resultsMutex);
+		}
+		else
+		{
+			if(imageDoublePrecision)
+				pnpTask<float, double>(curIndex, pointsMask, objectPoints, imagePoints, params, inliers, bestIndex, rvec, tvec, rvecInit, tvecInit, resultsMutex);
+			else
+				pnpTask<float, float>(curIndex, pointsMask, objectPoints, imagePoints, params, inliers, bestIndex, rvec, tvec, rvecInit, tvecInit, resultsMutex);
+		}
+	}
+
+	// TBB removed
+	class PnPSolver
+	{
+	public:
+		void operator()(int begin, int end) const
+		{
+			std::vector<char> pointsMask(objectPoints.cols, 0);
+			for( int i=begin; i!=end; ++i )
+			{
+				memset(&pointsMask[0], 0, objectPoints.cols );
+				memset(&pointsMask[0], 1, MIN_POINTS_COUNT );
+				generateVar(pointsMask, rng_base_seed + i);
+				pnpTask(i, pointsMask, objectPoints, imagePoints, parameters,
+						inliers, bestIndex, rvec, tvec, initRvec, initTvec, syncMutex);
+				if ((int)inliers.size() >= parameters.minInliersCount)
+				{
+					break;
+				}
+			}
+		}
+		PnPSolver(const cv::Mat& _objectPoints, const cv::Mat& _imagePoints, const Parameters& _parameters,
+				cv::Mat& _rvec, cv::Mat& _tvec, std::vector<int>& _inliers, int& _bestIndex, uint64 _rng_base_seed):
+		objectPoints(_objectPoints), imagePoints(_imagePoints), parameters(_parameters),
+		rvec(_rvec), tvec(_tvec), inliers(_inliers), bestIndex(_bestIndex), rng_base_seed(_rng_base_seed)
+		{
+			bestIndex = -1;
+			rvec.copyTo(initRvec);
+			tvec.copyTo(initTvec);
+		}
+	private:
+		PnPSolver& operator=(const PnPSolver&);
+
+		const cv::Mat& objectPoints;
+		const cv::Mat& imagePoints;
+		const Parameters& parameters;
+		cv::Mat &rvec, &tvec;
+		std::vector<int>& inliers;
+		int& bestIndex;
+		const uint64 rng_base_seed;
+		cv::Mat initRvec, initTvec;
+
+		static cv::Mutex syncMutex;
+
+	  void generateVar(std::vector<char>& mask, uint64 rng_seed) const
+		{
+		    cv::RNG generator(rng_seed);
+			int size = (int)mask.size();
+			for (int i = 0; i < size; i++)
+			{
+				int i1 = generator.uniform(0, size);
+				int i2 = generator.uniform(0, size);
+				char curr = mask[i1];
+				mask[i1] = mask[i2];
+				mask[i2] = curr;
+			}
+		}
+	};
+
+	cv::Mutex PnPSolver::syncMutex;
+
+}
+
+void solvePnPRansac(cv::InputArray _opoints, cv::InputArray _ipoints,
+		cv::InputArray _cameraMatrix, cv::InputArray _distCoeffs,
+		cv::OutputArray _rvec, cv::OutputArray _tvec, bool useExtrinsicGuess,
+        int iterationsCount, float reprojectionError, int minInliersCount,
+        cv::OutputArray _inliers, int flags)
+{
+    const int _rng_seed = 0;
+    cv::Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
+    cv::Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();
+
+    CV_Assert(opoints.isContinuous());
+    CV_Assert(opoints.depth() == CV_32F || opoints.depth() == CV_64F);
+    CV_Assert((opoints.rows == 1 && opoints.channels() == 3) || opoints.cols*opoints.channels() == 3);
+    CV_Assert(ipoints.isContinuous());
+    CV_Assert(ipoints.depth() == CV_32F || ipoints.depth() == CV_64F);
+    CV_Assert((ipoints.rows == 1 && ipoints.channels() == 2) || ipoints.cols*ipoints.channels() == 2);
+
+    _rvec.create(3, 1, CV_64FC1);
+    _tvec.create(3, 1, CV_64FC1);
+    cv::Mat rvec = _rvec.getMat();
+    cv::Mat tvec = _tvec.getMat();
+
+    cv::Mat objectPoints = opoints.reshape(3, 1), imagePoints = ipoints.reshape(2, 1);
+
+    if (minInliersCount <= 0)
+        minInliersCount = objectPoints.cols;
+    pnpransac::Parameters params;
+    params.iterationsCount = iterationsCount;
+    params.minInliersCount = minInliersCount;
+    params.reprojectionError = reprojectionError;
+    params.useExtrinsicGuess = useExtrinsicGuess;
+    params.camera.init(cameraMatrix, distCoeffs);
+    params.flags = flags;
+
+    std::vector<int> localInliers;
+    cv::Mat localRvec, localTvec;
+    rvec.copyTo(localRvec);
+    tvec.copyTo(localTvec);
+    int bestIndex;
+
+    // TBB not used
+    if (objectPoints.cols >= pnpransac::MIN_POINTS_COUNT)
+    {
+    	pnpransac::PnPSolver solver(objectPoints, imagePoints, params,
+							   localRvec, localTvec, localInliers, bestIndex,
+							   _rng_seed);
+    	solver(0, iterationsCount);
+    }
+
+    if (localInliers.size() >= (size_t)pnpransac::MIN_POINTS_COUNT)
+    {
+        if (flags != CV_P3P)
+        {
+            int i, pointsCount = (int)localInliers.size();
+            cv::Mat inlierObjectPoints(1, pointsCount, CV_MAKE_TYPE(opoints.depth(), 3)), inlierImagePoints(1, pointsCount, CV_MAKE_TYPE(ipoints.depth(), 2));
+            for (i = 0; i < pointsCount; i++)
+            {
+                int index = localInliers[i];
+                cv::Mat colInlierImagePoints = inlierImagePoints(cv::Rect(i, 0, 1, 1));
+                imagePoints.col(index).copyTo(colInlierImagePoints);
+                cv::Mat colInlierObjectPoints = inlierObjectPoints(cv::Rect(i, 0, 1, 1));
+                objectPoints.col(index).copyTo(colInlierObjectPoints);
+            }
+            solvePnP(inlierObjectPoints, inlierImagePoints, params.camera.intrinsics, params.camera.distortion, localRvec, localTvec, false, flags);
+        }
+        localRvec.copyTo(rvec);
+        localTvec.copyTo(tvec);
+        if (_inliers.needed())
+        	cv::Mat(localInliers).copyTo(_inliers);
+    }
+    else
+    {
+        tvec.setTo(cv::Scalar(0));
+        cv::Mat R = cv::Mat::eye(3, 3, CV_64F);
+        Rodrigues(R, rvec);
+        if( _inliers.needed() )
+            _inliers.release();
+    }
+    return;
+}
+#endif
+
 }

 }