added EpipolarGeometry::triangulatePoints() (LS and iterative LS)

Added option to see words3d in constraint view

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1974 f169173b-cf89-36c8-b27e-44dbe73f0c83

corelib/include/rtabmap/core/EpipolarGeometry.h

@@ -31,6 +31,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Parameters.h"
 #include <opencv2/core/core.hpp>
 #include <opencv2/features2d/features2d.hpp>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
 #include <list>
 #include <vector>
 
@@ -111,6 +113,26 @@ public:
 			const std::multimap<int, cv::KeyPoint> & wordsB,
 			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs);
 
+	static cv::Mat linearLSTriangulation(
+			cv::Point3d u,    //homogenous image point (u,v,1)
+			cv::Matx34d P,        //camera 1 matrix 3x4 double
+			cv::Point3d u1,   //homogenous image point in 2nd camera
+			cv::Matx34d P1);       //camera 2 matrix 3x4 double
+
+	static cv::Mat iterativeLinearLSTriangulation(
+			cv::Point3d u,        //homogenous image point (u,v,1)
+			const cv::Matx34d & P,    //camera 1 matrix 3x4 double
+			cv::Point3d u1,        //homogenous image point in 2nd camera
+			const cv::Matx34d & P1);  //camera 2 matrix 3x4 double
+
+	static double triangulatePoints(
+			const std::vector<cv::Point2f>& pt_set1,
+			const std::vector<cv::Point2f>& pt_set2,
+			const cv::Mat& P, // 3x4 double
+			const cv::Mat& P1, // 3x4 double
+			pcl::PointCloud<pcl::PointXYZ>::Ptr & pointcloud,
+			std::vector<double> & reproj_errors);
+
 private:
 	int _matchCountMinAccepted;
 	double _ransacParam1;

corelib/src/EpipolarGeometry.cpp

@@ -395,7 +395,7 @@ void EpipolarGeometry::findRTFromP(
 
 cv::Mat EpipolarGeometry::findFFromCalibratedStereoCameras(double fx, double fy, double cx, double cy, double Tx, double Ty)
 {
-	cv::Mat R = cv::Mat::ones(3, 3, CV_64FC1);
+	cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
 
 	double Bx = Tx/-fx;
 	double By = Ty/-fy;
@@ -408,11 +408,11 @@ cv::Mat EpipolarGeometry::findFFromCalibratedStereoCameras(double fx, double fy,
 	cv::Mat K = (cv::Mat_<double>(3,3) <<
 			fx, 0, cx,
 			0, fy, cy,
-			0, 0, 0);
+			0, 0, 1);
 
 	cv::Mat E = tx*R;
 
-	return K.t().inv()*E*K.inv();
+	return K.inv().t()*E*K.inv();
 }
 
 /**
@@ -505,4 +505,121 @@ int EpipolarGeometry::findPairsAll(const std::multimap<int, cv::KeyPoint> & word
 	return realPairsCount;
 }
 
+
+
+/**
+ source = SfM toy library: https://github.com/royshil/SfM-Toy-Library
+ From "Triangulation", Hartley, R.I. and Sturm, P., Computer vision and image understanding, 1997
+ */
+cv::Mat EpipolarGeometry::linearLSTriangulation(
+		cv::Point3d u,   //homogenous image point (u,v,1)
+		cv::Matx34d P,       //camera 1 matrix 3x4 double
+		cv::Point3d u1,  //homogenous image point in 2nd camera
+		cv::Matx34d P1       //camera 2 matrix 3x4 double
+                                   )
+{
+    //build matrix A for homogenous equation system Ax = 0
+    //assume X = (x,y,z,1), for Linear-LS method
+    //which turns it into a AX = B system, where A is 4x3, X is 3x1 and B is 4x1
+    cv::Mat A = (cv::Mat_<double>(4,3) <<
+    		u.x*P(2,0)-P(0,0),    u.x*P(2,1)-P(0,1),      u.x*P(2,2)-P(0,2),
+			u.y*P(2,0)-P(1,0),    u.y*P(2,1)-P(1,1),      u.y*P(2,2)-P(1,2),
+			u1.x*P1(2,0)-P1(0,0), u1.x*P1(2,1)-P1(0,1),   u1.x*P1(2,2)-P1(0,2),
+			u1.y*P1(2,0)-P1(1,0), u1.y*P1(2,1)-P1(1,1),   u1.y*P1(2,2)-P1(1,2)
+              );
+    cv::Mat B = (cv::Mat_<double>(4,1) <<
+			-(u.x*P(2,3)    -P(0,3)),
+			-(u.y*P(2,3)  -P(1,3)),
+			-(u1.x*P1(2,3)    -P1(0,3)),
+			-(u1.y*P1(2,3)    -P1(1,3)));
+
+    cv::Mat X;
+    solve(A,B,X,cv::DECOMP_SVD);
+
+    return X;
+}
+
+ /**
+ source = SfM toy library: https://github.com/royshil/SfM-Toy-Library
+ From "Triangulation", Hartley, R.I. and Sturm, P., Computer vision and image understanding, 1997
+ */
+cv::Mat EpipolarGeometry::iterativeLinearLSTriangulation(
+		cv::Point3d u,            //homogenous image point (u,v,1)
+		const cv::Matx34d & P,   //camera 1 matrix 3x4 double
+		cv::Point3d u1,           //homogenous image point in 2nd camera
+		const cv::Matx34d & P1)   //camera 2 matrix 3x4 double
+{
+    double wi = 1, wi1 = 1;
+	double EPSILON = 0.0001;
+
+	cv::Mat_<double> X(4,1);
+	cv::Mat_<double> X_ = linearLSTriangulation(u,P,u1,P1);
+	X(0) = X_(0); X(1) = X_(1); X(2) = X_(2); X_(3) = 1.0;
+	for (int i=0; i<10; i++)  //Hartley suggests 10 iterations at most
+	{
+        //recalculate weights
+    	double p2x = cv::Mat(cv::Mat(P).row(2)*cv::Mat(X)).at<double>(0);
+    	double p2x1 = cv::Mat(cv::Mat(P1).row(2)*cv::Mat(X)).at<double>(0);
+
+        //breaking point
+        if(fabs(wi - p2x) <= EPSILON && fabs(wi1 - p2x1) <= EPSILON) break;
+
+        wi = p2x;
+        wi1 = p2x1;
+
+        //reweight equations and solve
+        cv::Mat A = (cv::Mat_<double>(4,3) <<
+        		(u.x*P(2,0)-P(0,0))/wi,       (u.x*P(2,1)-P(0,1))/wi,         (u.x*P(2,2)-P(0,2))/wi,
+				(u.y*P(2,0)-P(1,0))/wi,       (u.y*P(2,1)-P(1,1))/wi,         (u.y*P(2,2)-P(1,2))/wi,
+				(u1.x*P1(2,0)-P1(0,0))/wi1,   (u1.x*P1(2,1)-P1(0,1))/wi1,     (u1.x*P1(2,2)-P1(0,2))/wi1,
+				(u1.y*P1(2,0)-P1(1,0))/wi1,   (u1.y*P1(2,1)-P1(1,1))/wi1,     (u1.y*P1(2,2)-P1(1,2))/wi1);
+        cv::Mat B = (cv::Mat_<double>(4,1) <<
+        		-(u.x*P(2,3)    -P(0,3))/wi,
+				-(u.y*P(2,3)  -P(1,3))/wi,
+				-(u1.x*P1(2,3)    -P1(0,3))/wi1,
+				-(u1.y*P1(2,3)    -P1(1,3))/wi1);
+
+        solve(A,B,X_,cv::DECOMP_SVD);
+        X(0) = X_(0); X(1) = X_(1); X(2) = X_(2); X_(3) = 1.0;
+    }
+    return X;
+}
+
+/**
+ source = SfM toy library: https://github.com/royshil/SfM-Toy-Library
+ */
+//Triagulate points
+double EpipolarGeometry::triangulatePoints(
+		const std::vector<cv::Point2f>& pt_set1,
+		const std::vector<cv::Point2f>& pt_set2,
+		const cv::Mat& P, // 3x4 double
+		const cv::Mat& P1, // 3x4 double
+		pcl::PointCloud<pcl::PointXYZ>::Ptr & pointcloud,
+		std::vector<double> & reproj_errors)
+{
+	pointcloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
+
+	unsigned int pts_size = pt_set1.size();
+
+	pointcloud->resize(pts_size);
+	reproj_errors.resize(pts_size);
+
+	for(unsigned int i=0; i<pts_size; i++)
+	{
+		cv::Point3d u(pt_set1[i].x,pt_set1[i].y,1.0);
+		cv::Point3d u1(pt_set2[i].x,pt_set2[i].y,1.0);
+
+		cv::Mat_<double> X = iterativeLinearLSTriangulation(u,P,u1,P1);
+
+		cv::Mat_<double> xPt_img = P1 * X;				//reproject
+		cv::Point2f xPt_img_(xPt_img(0)/xPt_img(2),xPt_img(1)/xPt_img(2));
+
+		double reprj_err = norm(xPt_img_-pt_set1[i]);
+		reproj_errors[i] = reprj_err;
+		pointcloud->at(i) = pcl::PointXYZ(X(0),X(1),X(2));
+	}
+
+	return cv::mean(reproj_errors)[0]; // mean reproj error
+}
+
 } // namespace rtabmap

corelib/src/Odometry.cpp

@@ -264,6 +264,9 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
 				{
 					correspondences = (int)inliers1->size();
 
+					// transform new words in local map referential
+					//inliers2 = util3d::transformPointCloud<pcl::PointXYZ>(inliers2, this->getPose());
+
 					// the transform returned is global odometry pose, not incremental one
 					std::vector<int> inliersV;
 					transform = util3d::transformFromXYZCorrespondences(
@@ -291,12 +294,43 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
 						//inliers2 = util3d::transformPointCloud(inliers2, transform);
 						//pcl::io::savePCDFile("to_t.pcd", *inliers2);
 					}
+
+					if(ULogger::level() == ULogger::kDebug)
+					{
+						float error3D = 0;
+					//	pcl::PointCloud<pcl::PointXYZ>::Ptr inliers1Cloud(new pcl::PointCloud<pcl::PointXYZ>), inliers2Cloud(new pcl::PointCloud<pcl::PointXYZ>);
+						for(unsigned int i=0; i<inliersV.size(); ++i)
+						{
+							pcl::PointXYZ pt = util3d::transformPoint(inliers2->at(inliersV[i]), this->getPose()*transform);
+							error3D+=pcl::euclideanDistance(inliers1->at(inliersV[i]), pt);
+						//	inliers1Cloud->push_back(inliers1->at(inliersV[i]));
+						//	inliers2Cloud->push_back(pt);
+						}
+						error3D/=float(inliersV.size());
+						UDEBUG("3D error = %f", error3D);
+					}
+
+					/*if(inliersV.size() < 30 || fabs(transform.x()) > 0.3 || fabs(transform.y()) > 0.3 || fabs(transform.z()) > 0.3)
+					{
+						UWARN("Saved from.pcd, to.pcd and to_t.pcd");
+						pcl::io::savePCDFile("from.pcd", *inliers1);
+						pcl::io::savePCDFile("to.pcd", *inliers2);
+						inliers2 = util3d::transformPointCloud<pcl::PointXYZ>(inliers2, transform);
+						pcl::io::savePCDFile("to_t.pcd", *inliers2);
+
+						pcl::io::savePCDFile("inliersFrom.pcd", *inliers1Cloud);
+						pcl::io::savePCDFile("inliersTo.pcd", *inliers2Cloud);
+						inliers2Cloud = util3d::transformPointCloud<pcl::PointXYZ>(inliers2Cloud, transform);
+						pcl::io::savePCDFile("inliersTo_t.pcd", *inliers2Cloud);
+						exit(-1);
+					}*/
 					/*pcl::io::savePCDFile("from.pcd", *inliers1);
 					inliers2 = util3d::transformPointCloud(inliers2, this->getPose());
 					pcl::io::savePCDFile("to.pcd", *inliers2);
 					inliers2 = util3d::transformPointCloud(inliers2, transform);
 					pcl::io::savePCDFile("to_t.pcd", *inliers2);*/
 
+
 					/*
 					//refine ICP test
 					bool hasConverged;
@@ -428,8 +462,9 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
 		*localMapSize = (int)localMap_.size();
 	}
 
-	UINFO("Odom update time = %fs features=%d inliers=%d/%d local_map=%d[%d] dict=%d nodes=%d",
+	UINFO("Odom update time = %fs out=[%s] features=%d inliers=%d/%d local_map=%d[%d] dict=%d nodes=%d",
 			timer.elapsed(),
+			output.prettyPrint().c_str(),
 			nFeatures,
 			inliers,
 			correspondences,

corelib/src/util3d.cpp

@@ -1571,8 +1571,8 @@ Transform transformFromXYZCorrespondences(
 					double variance = model->computeVariance ();
 					error_threshold = sqrt (std::min (inlier_distance_threshold_sqr, sigma_sqr * variance));
 
-					UDEBUG ("RANSAC refineModel: New estimated error threshold: %f on iteration %d out of %d.",
+					UDEBUG ("RANSAC refineModel: New estimated error threshold: %f (variance=%f) on iteration %d out of %d.",
-						  error_threshold, refine_iterations, refineModelIterations);
+						  error_threshold, variance, refine_iterations, refineModelIterations);
 					inlier_changed = false;
 					std::swap (prev_inliers, new_inliers);
 

guilib/src/DatabaseViewer.cpp

@@ -1454,51 +1454,120 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & link,
 
 
 			//cloud 3d
-			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFrom;
+			if(!ui_->checkBox_show3DWords->isChecked())
-			if(dataFrom.getDepthRaw().type() == CV_8UC1)
 			{
-				cloudFrom = rtabmap::util3d::cloudFromStereoImages(
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFrom;
-						dataFrom.getImageRaw(),
+				if(dataFrom.getDepthRaw().type() == CV_8UC1)
-						dataFrom.getDepthRaw(),
+				{
-						dataFrom.getDepthCx(), dataFrom.getDepthCy(),
+					cloudFrom = rtabmap::util3d::cloudFromStereoImages(
-						dataFrom.getDepthFx(), dataFrom.getDepthFy(),
+							dataFrom.getImageRaw(),
-						1);
+							dataFrom.getDepthRaw(),
+							dataFrom.getDepthCx(), dataFrom.getDepthCy(),
+							dataFrom.getDepthFx(), dataFrom.getDepthFy(),
+							1);
+				}
+				else
+				{
+					cloudFrom = rtabmap::util3d::cloudFromDepthRGB(
+							dataFrom.getImageRaw(),
+							dataFrom.getDepthRaw(),
+							dataFrom.getDepthCx(), dataFrom.getDepthCy(),
+							dataFrom.getDepthFx(), dataFrom.getDepthFy(),
+							1);
+				}
+
+				cloudFrom = rtabmap::util3d::removeNaNFromPointCloud<pcl::PointXYZRGB>(cloudFrom);
+				cloudFrom = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloudFrom, dataFrom.getLocalTransform());
+
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudTo;
+				if(dataTo.getDepthRaw().type() == CV_8UC1)
+				{
+					cloudTo = rtabmap::util3d::cloudFromStereoImages(
+							dataTo.getImageRaw(),
+							dataTo.getDepthRaw(),
+							dataTo.getDepthCx(), dataTo.getDepthCy(),
+							dataTo.getDepthFx(), dataTo.getDepthFy(),
+							1);
+				}
+				else
+				{
+					cloudTo = rtabmap::util3d::cloudFromDepthRGB(
+							dataTo.getImageRaw(),
+							dataTo.getDepthRaw(),
+							dataTo.getDepthCx(), dataTo.getDepthCy(),
+							dataTo.getDepthFx(), dataTo.getDepthFy(),
+							1);
+				}
+
+				cloudTo = rtabmap::util3d::removeNaNFromPointCloud<pcl::PointXYZRGB>(cloudTo);
+				cloudTo = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloudTo, t*dataTo.getLocalTransform());
+
+				if(cloudFrom->size())
+				{
+					ui_->constraintsViewer->addOrUpdateCloud("cloud0", cloudFrom);
+				}
+				if(cloudTo->size())
+				{
+					ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo);
+				}
 			}
 			else
 			{
-				cloudFrom = rtabmap::util3d::cloudFromDepthRGB(
+				const Signature * sFrom = memory_->getSignature(link.from());
-						dataFrom.getImageRaw(),
+				const Signature * sTo = memory_->getSignature(link.to());
-						dataFrom.getDepthRaw(),
+				if(sFrom && sTo)
-						dataFrom.getDepthCx(), dataFrom.getDepthCy(),
+				{
-						dataFrom.getDepthFx(), dataFrom.getDepthFy(),
+					pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFrom(new pcl::PointCloud<pcl::PointXYZ>);
-						1);
+					pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTo(new pcl::PointCloud<pcl::PointXYZ>);
-			}
+					cloudFrom->resize(sFrom->getWords3().size());
+					cloudTo->resize(sTo->getWords3().size());
+					int i=0;
+					for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=sFrom->getWords3().begin();
+						iter!=sFrom->getWords3().end();
+						++iter)
+					{
+						cloudFrom->at(i++) = iter->second;
+					}
+					i=0;
+					for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=sTo->getWords3().begin();
+						iter!=sTo->getWords3().end();
+						++iter)
+					{
+						cloudTo->at(i++) = iter->second;
+					}
 
-			cloudFrom = rtabmap::util3d::removeNaNFromPointCloud<pcl::PointXYZRGB>(cloudFrom);
+					if(cloudFrom->size())
-			cloudFrom = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloudFrom, dataFrom.getLocalTransform());
+					{
+						cloudFrom = rtabmap::util3d::removeNaNFromPointCloud<pcl::PointXYZ>(cloudFrom);
+					}
+					if(cloudTo->size())
+					{
+						cloudTo = rtabmap::util3d::removeNaNFromPointCloud<pcl::PointXYZ>(cloudTo);
+						cloudTo = rtabmap::util3d::transformPointCloud<pcl::PointXYZ>(cloudTo, t);
+					}
 
-			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudTo;
+					if(cloudFrom->size())
-			if(dataTo.getDepthRaw().type() == CV_8UC1)
+					{
-			{
+						ui_->constraintsViewer->addOrUpdateCloud("cloud0", cloudFrom);
-				cloudTo = rtabmap::util3d::cloudFromStereoImages(
+					}
-						dataTo.getImageRaw(),
+					else
-						dataTo.getDepthRaw(),
+					{
-						dataTo.getDepthCx(), dataTo.getDepthCy(),
+						UWARN("Empty 3D words for node %d", link.from());
-						dataTo.getDepthFx(), dataTo.getDepthFy(),
+					}
-						1);
+					if(cloudTo->size())
+					{
+						ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo);
+					}
+					else
+					{
+						UWARN("Empty 3D words for node %d", link.to());
+					}
+				}
+				else
+				{
+					UERROR("Not found signature %d or %d in RAM", link.from(), link.to());
+				}
 			}
-			else
-			{
-				cloudTo = rtabmap::util3d::cloudFromDepthRGB(
-						dataTo.getImageRaw(),
-						dataTo.getDepthRaw(),
-						dataTo.getDepthCx(), dataTo.getDepthCy(),
-						dataTo.getDepthFx(), dataTo.getDepthFy(),
-						1);
-			}
-
-			cloudTo = rtabmap::util3d::removeNaNFromPointCloud<pcl::PointXYZRGB>(cloudTo);
-			cloudTo = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloudTo, t*dataTo.getLocalTransform());
 
 			//cloud 2d
 			pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
@@ -1506,14 +1575,6 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & link,
 			scanB = rtabmap::util3d::depth2DToPointCloud(dataTo.getDepth2DRaw());
 			scanB = rtabmap::util3d::transformPointCloud<pcl::PointXYZ>(scanB, t);
 
-			if(cloudFrom->size())
-			{
-				ui_->constraintsViewer->addOrUpdateCloud("cloud0", cloudFrom);
-			}
-			if(cloudTo->size())
-			{
-				ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo);
-			}
 			if(scanA->size())
 			{
 				ui_->constraintsViewer->addOrUpdateCloud("scan0", scanA);

guilib/src/ui/DatabaseViewer.ui

@@ -382,6 +382,20 @@
          </property>
         </widget>
        </item>
+       <item row="4" column="0">
+        <widget class="QLabel" name="label_32">
+         <property name="text">
+          <string>Show 3D words</string>
+         </property>
+        </widget>
+       </item>
+       <item row="4" column="1">
+        <widget class="QCheckBox" name="checkBox_show3DWords">
+         <property name="text">
+          <string/>
+         </property>
+        </widget>
+       </item>
       </layout>
      </item>
      <item>

tools/EpipolarGeometry/CMakeLists.txt

@@ -8,12 +8,14 @@ SET(INCLUDE_DIRS
 	${PROJECT_SOURCE_DIR}/corelib/include
 	${PROJECT_SOURCE_DIR}/guilib/include
     ${OpenCV_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}
 )
 
 INCLUDE(${QT_USE_FILE})
 
 SET(LIBRARIES
 	${OpenCV_LIBRARIES}
+	${PCL_LIBRARIES} 
 	${QT_LIBRARIES} 
 )