Updated EpipolarGeometry methods

Experimental: added OdometryMono in epipolar geometry test

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

corelib/include/rtabmap/core/EpipolarGeometry.h

@@ -65,10 +65,10 @@ public:
 			cv::Vec3d & e1,
 			cv::Vec3d & e2);
 
-	static cv::Mat findPFromF(
+	static cv::Mat findPFromE(
-			const cv::Mat & fundamentalMatrix,
+			const cv::Mat & E,
-			const cv::Mat & x1,
+			const cv::Mat & x,
-			const cv::Mat & x2);
+			const cv::Mat & xp);
 
 	// return fundamental matrix
 	// status -> inliers = 1, outliers = 0
@@ -126,8 +126,8 @@ public:
 			const cv::Matx34d & P1);  //camera 2 matrix 3x4 double
 
 	static double triangulatePoints(
-			const std::vector<cv::Point2f>& pt_set1,
+			const cv::Mat& pt_set1, //2xN double
-			const std::vector<cv::Point2f>& pt_set2,
+			const cv::Mat& pt_set2, //2xN double
 			const cv::Mat& P, // 3x4 double
 			const cv::Mat& P1, // 3x4 double
 			pcl::PointCloud<pcl::PointXYZ>::Ptr & pointcloud,

corelib/include/rtabmap/core/Odometry.h

@@ -127,16 +127,15 @@ public:
 
 	virtual void reset(const Transform & initialPose = Transform::getIdentity());
 
-	const cv::Mat & getLastFrame() const {return lastFrame_;}
+	const cv::Mat & getLastFrame() const {return refFrame_;}
-	const std::vector<cv::Point2f> & getLastCorners() const {return lastCorners_;}
+	const std::vector<cv::Point2f> & getLastCorners() const {return refCorners_;}
-	const pcl::PointCloud<pcl::PointXYZ>::Ptr & getLastCorners3D() const {return lastCorners3D_;}
+	const pcl::PointCloud<pcl::PointXYZ>::Ptr & getLastCorners3D() const {return refCorners3D_;}
-
-	cv::Mat imgMatches_;
 
 private:
 	virtual Transform computeTransform(const SensorData & image, int * quality = 0, int * features = 0, int * localMapSize = 0);
 	Transform computeTransformStereo(const SensorData & image, int * quality, int * features);
 	Transform computeTransformRGBD(const SensorData & image, int * quality, int * features);
+	Transform computeTransformMono(const SensorData & image, int * quality, int * features);
 private:
 	//Parameters:
 	int flowWinSize_;
@@ -151,11 +150,10 @@ private:
 
 	Feature2D * feature2D_;
 
-	cv::Mat lastFrame_;
+	cv::Mat refFrame_;
-	cv::Mat lastRightFrame_;
+	cv::Mat refRightFrame_;
-	std::vector<cv::Point2f> lastCorners_;
+	std::vector<cv::Point2f> refCorners_;
-	pcl::PointCloud<pcl::PointXYZ>::Ptr lastCorners3D_;
+	pcl::PointCloud<pcl::PointXYZ>::Ptr refCorners3D_;
-	Transform savedLastRefFrameTransform_;
 };
 
 class RTABMAP_EXP OdometryICP : public Odometry

corelib/include/rtabmap/core/SensorData.h

@@ -86,7 +86,7 @@ public:
 	const cv::Mat & depthOrRightImage() const {return _depthOrRightImage;}
 	const cv::Mat & depth2d() const {return _depth2d;}
 	float fx() const {return _fx;}
-	float fy() const {return (_depthOrRightImage.type()==CV_32FC1 || _depthOrRightImage.type()==CV_16UC1)?_fyOrBaseline:0;}
+	float fy() const {return (_depthOrRightImage.type()==CV_8UC1)?0:_fyOrBaseline;}
 	float cx() const {return _cx;}
 	float cy() const {return _cy;}
 	float baseline() const {return _depthOrRightImage.type()==CV_8UC1?_fyOrBaseline:0;}

corelib/include/rtabmap/core/VWDictionary.h

@@ -86,6 +86,7 @@ public:
 	std::vector<int> getUnusedWordIds() const;
 	unsigned int getUnusedWordsSize() const {return (int)_unusedWords.size();}
 	void removeWords(const std::vector<VisualWord*> & words); // caller must delete the words
+	void deleteUnusedWords();
 
 protected:
 	int getNextId();

corelib/src/EpipolarGeometry.cpp

@@ -129,170 +129,159 @@ void EpipolarGeometry::findEpipolesFromF(const cv::Mat & fundamentalMatrix, cv::
 	e2[2] = u.at<double>(2,2);// /u->data.db[2*3+2];
 }
 
-//Assuming P0 = [eye(3) zeros(3,1)]
+int inFrontOfBothCameras(const cv::Mat & x, const cv::Mat & xp, const cv::Mat & R, const cv::Mat & T)
-// x1 and x2 are 2D points
-// return camera matrix P (3x4) matrix
-cv::Mat EpipolarGeometry::findPFromF(const cv::Mat & fundamentalMatrix, const cv::Mat & x1, const cv::Mat & x2)
 {
-
-	if(fundamentalMatrix.rows != 3 || fundamentalMatrix.cols != 3)
-	{
-		ULOGGER_ERROR("Matrices are not the good size... ");
-		return cv::Mat();
-	}
-
-	if(fundamentalMatrix.type() != CV_64FC1)
-	{
-		ULOGGER_ERROR("Matrices are not the good type...");
-		return cv::Mat();
-	}
-
-	// P matrix 3x4
-	cv::Mat p = cv::Mat::zeros(3, 4, CV_64FC1);
-
 	// P0 matrix 3X4
 	cv::Mat p0 = cv::Mat::zeros(3, 4, CV_64FC1);
 	p0.at<double>(0,0) = 1;
 	p0.at<double>(1,1) = 1;
 	p0.at<double>(2,2) = 1;
+	cv::Mat p = cv::Mat::zeros(3, 4, CV_64FC1);
+	p.at<double>(0,0) = R.at<double>(0,0);
+	p.at<double>(0,1) = R.at<double>(0,1);
+	p.at<double>(0,2) = R.at<double>(0,2);
+	p.at<double>(1,0) = R.at<double>(1,0);
+	p.at<double>(1,1) = R.at<double>(1,1);
+	p.at<double>(1,2) = R.at<double>(1,2);
+	p.at<double>(2,0) = R.at<double>(2,0);
+	p.at<double>(2,1) = R.at<double>(2,1);
+	p.at<double>(2,2) = R.at<double>(2,2);
+	p.at<double>(0,3) = T.at<double>(0,0);
+	p.at<double>(1,3) = T.at<double>(1,0);
+	p.at<double>(2,3) = T.at<double>(2,0);
 
-	// cv::SVD doesn't five same results as cvSVD ?!? cvSVD return same values as in MatLab
+	cv::Mat pts4D;
-	/*cv::SVD svd(fundamentalMatrix);
+	cv::triangulatePoints(p0, p, x, xp, pts4D);
-	cv::Mat u = svd.u;
-	cv::Mat v = svd.vt;
-	cv::Mat s = svd.w;
-	cv::Mat e = u.col(2);*/
 
-	CvMat F  = fundamentalMatrix;
+    //http://en.wikipedia.org/wiki/Essential_matrix#3D_points_from_corresponding_image_points
-	cv::Mat u(3,3,CV_64F);
+	int nValid = 0;
-	cv::Mat v(3,3,CV_64F);
+    for(int i=0; i<x.cols; ++i)
-	cv::Mat s(3,3,CV_64F);
+    {
-	CvMat U  = u;
+        if(pts4D.at<double>(2,i)/pts4D.at<double>(3,i) > 5)
-	CvMat S  = s;
+        {
-	CvMat V  = v;
+        	++nValid;
-	cvSVD(&F, &S, &U, &V, CV_SVD_U_T|CV_SVD_V_T); // F = U D V^T
+        }
-	u = u.t();
+    }
-	//
+    UDEBUG("nValid=%d/%d", nValid,  x.cols);
-	// INFO: may be required to multiply by -1 the last column of U
-	// TODO: Is any way to detect when it is required to do that ? When
-	//       it is wrong, triangulated points have their Z value below 1 (between 0 and 1)...
-	//
-	/*u.at<double>(0,2) = -u.at<double>(0,2);
-	u.at<double>(1,2) = -u.at<double>(1,2);
-	u.at<double>(2,2) = -u.at<double>(2,2);*/
-	v = v.t();
-	cv::Mat e = u.col(2);
 
-	//std::cout << "u=" << u << std::endl;
+    return nValid;
-	//std::cout << "v=" << v << std::endl;
+}
-	//std::cout << "s=" << s << std::endl;
+
+//Assuming P0 = [eye(3) zeros(3,1)]
+// x1 and x2 are 2D points
+// return camera matrix P (3x4) matrix
+//http://www.robots.ox.ac.uk/~vgg/hzbook/hzbook2/HZepipolar.pdf
+cv::Mat EpipolarGeometry::findPFromE(const cv::Mat & E,
+		const cv::Mat & x,
+		const cv::Mat & xp)
+{
+	UDEBUG("begin");
+	UASSERT(E.rows == 3 && E.cols == 3);
+	UASSERT(E.type() == CV_64FC1);
+	UASSERT(x.rows == 2 && x.cols>0 && x.type() == CV_64FC1);
+	UASSERT(xp.rows == 2 && xp.cols>0 && x.type() == CV_64FC1);
 
 	// skew matrix 3X3
-	cv::Mat skew = cv::Mat::zeros( 3, 3, CV_64FC1);
+	cv::Mat w = cv::Mat::zeros( 3, 3, CV_64FC1);
-	skew.at<double>(0,1) = -1;
+	w.at<double>(0,1) = -1;
-	skew.at<double>(1,0) = 1;
+	w.at<double>(1,0) = 1;
-	skew.at<double>(2,2) = 1;
+	w.at<double>(2,2) = 1;
+	//std::cout << "W=" << w << std::endl;
 
-	cv::Mat r;
+	cv::Mat e = E;
-	cv::Mat x4d;
+	cv::SVD svd(e,cv::SVD::MODIFY_A);
+	cv::Mat u = svd.u;
+	cv::Mat vt = svd.vt;
+	cv::Mat s = svd.w;
 
-	cv::Mat x = x1.col(0); // just take one point
+	//std::cout << "u=" << u << std::endl;
-	cv::Mat xp = x2.col(0); // just take one point
+	//std::cout << "vt=" << vt << std::endl;
+	//std::cout << "s=" << s << std::endl;
 
-	// INFO: There 4 cases of P, only one have the points in
+	// E = u*diag(1,1,0)*vt
+	cv::Mat diag = cv::Mat::eye(3,3,CV_64FC1);
+	diag.at<double>(2,2) = 0;
+	e = u*diag*vt;
+	svd(e,cv::SVD::MODIFY_A);
+	u = svd.u;
+	vt = svd.vt;
+	s = svd.w;
+
+	cv::Mat r = u*w*vt;
+	if(cv::determinant(r)+1.0 < 1e-09) {
+		//according to http://en.wikipedia.org/wiki/Essential_matrix#Showing_that_it_is_valid
+		UWARN("det(R) == -1 [%f]: flip E's sign", cv::determinant(r));
+		e = -E;
+		svd(e,cv::SVD::MODIFY_A);
+		u = svd.u;
+		vt = svd.vt;
+		s = svd.w;
+	}
+	cv::Mat wt = w.t();
+
+	// INFO: There 4 cases of P, only one have all the points in
 	// front of the two cameras (positive z).
 
-	// Case 1 : P = [U*W*V' e];
+	cv::Mat r1 = u*w*vt;
-	r = u*skew*v.t();
+	cv::Mat r2 = u*wt*vt;
-	p.at<double>(0,0) = r.at<double>(0,0);
-	p.at<double>(0,1) = r.at<double>(0,1);
-	p.at<double>(0,2) = r.at<double>(0,2);
-	p.at<double>(1,0) = r.at<double>(1,0);
-	p.at<double>(1,1) = r.at<double>(1,1);
-	p.at<double>(1,2) = r.at<double>(1,2);
-	p.at<double>(2,0) = r.at<double>(2,0);
-	p.at<double>(2,1) = r.at<double>(2,1);
-	p.at<double>(2,2) = r.at<double>(2,2);
-	p.at<double>(0,3) = e.at<double>(0,0);
-	p.at<double>(1,3) = e.at<double>(1,0);
-	p.at<double>(2,3) = e.at<double>(2,0);
 
-	cv::triangulatePoints(p0, p, x, xp, x4d);
+	cv::Mat t1 = u.col(2);
-	x4d.at<double>(0) = x4d.at<double>(0)/x4d.at<double>(3);
+	cv::Mat t2 = u.col(2)*-1;
-	x4d.at<double>(1) = x4d.at<double>(1)/x4d.at<double>(3);
-	x4d.at<double>(2) = x4d.at<double>(2)/x4d.at<double>(3);
-	x4d.at<double>(3) = x4d.at<double>(3)/x4d.at<double>(3);
 
-	cv::Mat xt1 = p0*x4d;
+	int max = 0;
-	cv::Mat xt2 = p*x4d;
+	int maxIndex = 1;
+	int maxTmp;
+	cv::Mat R=r1,T=t1;
 
-	if(xt1.at<double>(2,0) < 0 || xt2.at<double>(2,0) < 0)
+	// Case 1 : P = [U*W*V' t];
+	max = inFrontOfBothCameras(x, xp, r1, t1);
+	// Case 2 : P = [U*W*V' -t];
+	maxTmp = inFrontOfBothCameras(x, xp, r1, t2);
+	if(maxTmp > max)
 	{
-		// Case 2 : P = [U*W*V' -e];
+		maxIndex = 2;
-		p.at<double>(0,3) = -e.at<double>(0,0);
+		max = maxTmp;
-		p.at<double>(1,3) = -e.at<double>(1,0);
+		R=r1,T=t2;
-		p.at<double>(2,3) = -e.at<double>(2,0);
-		cv::triangulatePoints(p0, p, x, xp, x4d);
-		x4d.at<double>(0) = x4d.at<double>(0)/x4d.at<double>(3);
-		x4d.at<double>(1) = x4d.at<double>(1)/x4d.at<double>(3);
-		x4d.at<double>(2) = x4d.at<double>(2)/x4d.at<double>(3);
-		x4d.at<double>(3) = x4d.at<double>(3)/x4d.at<double>(3);
-		xt1 = p0*x4d;
-		xt2 = p*x4d;
-		if(xt1.at<double>(2,0) < 0 || xt2.at<double>(2,0) < 0)
-		{
-			// Case 3 : P = [U*W'*V' e];
-			r = u*skew.t()*v.t();
-			p.at<double>(0,0) = r.at<double>(0,0);
-			p.at<double>(0,1) = r.at<double>(0,1);
-			p.at<double>(0,2) = r.at<double>(0,2);
-			p.at<double>(1,0) = r.at<double>(1,0);
-			p.at<double>(1,1) = r.at<double>(1,1);
-			p.at<double>(1,2) = r.at<double>(1,2);
-			p.at<double>(2,0) = r.at<double>(2,0);
-			p.at<double>(2,1) = r.at<double>(2,1);
-			p.at<double>(2,2) = r.at<double>(2,2);
-			p.at<double>(0,3) = e.at<double>(0,0);
-			p.at<double>(1,3) = e.at<double>(1,0);
-			p.at<double>(2,3) = e.at<double>(2,0);
-			p.col(3) = e;
-			cv::triangulatePoints(p0, p, x, xp, x4d);
-			x4d.at<double>(0) = x4d.at<double>(0)/x4d.at<double>(3);
-			x4d.at<double>(1) = x4d.at<double>(1)/x4d.at<double>(3);
-			x4d.at<double>(2) = x4d.at<double>(2)/x4d.at<double>(3);
-			x4d.at<double>(3) = x4d.at<double>(3)/x4d.at<double>(3);
-			xt1 = p0*x4d;
-			xt2 = p*x4d;
-			if(xt1.at<double>(2,0) < 0 || xt2.at<double>(2,0) < 0)
-			{
-				// Case 4 : P = [U*W'*V' -e];
-				p.at<double>(0,3) = -e.at<double>(0,0);
-				p.at<double>(1,3) = -e.at<double>(1,0);
-				p.at<double>(2,3) = -e.at<double>(2,0);
-				cv::triangulatePoints(p0, p, x, xp, x4d);
-				x4d.at<double>(0) = x4d.at<double>(0)/x4d.at<double>(3);
-				x4d.at<double>(1) = x4d.at<double>(1)/x4d.at<double>(3);
-				x4d.at<double>(2) = x4d.at<double>(2)/x4d.at<double>(3);
-				x4d.at<double>(3) = x4d.at<double>(3)/x4d.at<double>(3);
-				xt1 = p0*x4d;
-				xt2 = p*x4d;
-				UDEBUG("Case 4");
-			}
-			else
-			{
-				UDEBUG("Case 3");
-			}
-		}
-		else
-		{
-			UDEBUG("Case 2");
-		}
 	}
-	else
+	// Case 3 : P = [U*W'*V' t];
+	maxTmp = inFrontOfBothCameras(x, xp, r2, t1);
+	if(maxTmp > max)
 	{
-		UDEBUG("Case 1");
+		maxIndex = 3;
+		max = maxTmp;
+		R=r2,T=t1;
 	}
-	return p;
+	// Case 4 : P = [U*W'*V' -t];
+	maxTmp = inFrontOfBothCameras(x, xp, r2, t2);
+	if(maxTmp > max)
+	{
+		maxIndex = 4;
+		max = maxTmp;
+		R=r2,T=t2;
+	}
+
+	if(max > 0)
+	{
+		UDEBUG("Case %d", maxIndex);
+
+		// P matrix 3x4
+		cv::Mat p = cv::Mat::zeros(3, 4, CV_64FC1);
+		p.at<double>(0,0) = R.at<double>(0,0);
+		p.at<double>(0,1) = R.at<double>(0,1);
+		p.at<double>(0,2) = R.at<double>(0,2);
+		p.at<double>(1,0) = R.at<double>(1,0);
+		p.at<double>(1,1) = R.at<double>(1,1);
+		p.at<double>(1,2) = R.at<double>(1,2);
+		p.at<double>(2,0) = R.at<double>(2,0);
+		p.at<double>(2,1) = R.at<double>(2,1);
+		p.at<double>(2,2) = R.at<double>(2,2);
+		p.at<double>(0,3) = T.at<double>(0);
+		p.at<double>(1,3) = T.at<double>(1);
+		p.at<double>(2,3) = T.at<double>(2);
+		return p;
+	}
+
+	return cv::Mat();
 }
 
 cv::Mat EpipolarGeometry::findFFromWords(
@@ -387,9 +376,10 @@ void EpipolarGeometry::findRTFromP(
 	UDEBUG("");
 	r = cv::Mat(p, cv::Range(0,3), cv::Range(0,3));
 	UDEBUG("");
-	r = -r.inv();
+	//r = -r.inv();
 	UDEBUG("r=%d %d, t=%d", r.cols, r.rows, p.col(3).rows);
-	t = r*p.col(3);
+	//t = r*p.col(3);
+	t = p.col(3);
 	UDEBUG("");
 }
 
@@ -510,6 +500,7 @@ int EpipolarGeometry::findPairsAll(const std::multimap<int, cv::KeyPoint> & word
 /**
  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
+ return 1x3 double
  */
 cv::Mat EpipolarGeometry::linearLSTriangulation(
 		cv::Point3d u,   //homogenous image point (u,v,1)
@@ -536,12 +527,13 @@ cv::Mat EpipolarGeometry::linearLSTriangulation(
     cv::Mat X;
     solve(A,B,X,cv::DECOMP_SVD);
 
-    return X;
+    return X; // return 1x3 double
 }
 
  /**
  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
+ return 4x1 double
  */
 cv::Mat EpipolarGeometry::iterativeLinearLSTriangulation(
 		cv::Point3d u,            //homogenous image point (u,v,1)
@@ -552,14 +544,17 @@ cv::Mat EpipolarGeometry::iterativeLinearLSTriangulation(
     double wi = 1, wi1 = 1;
 	double EPSILON = 0.0001;
 
-	cv::Mat_<double> X(4,1);
+	cv::Mat X(4,1,CV_64FC1);
-	cv::Mat_<double> X_ = linearLSTriangulation(u,P,u1,P1);
+	cv::Mat X_ = linearLSTriangulation(u,P,u1,P1);
-	X(0) = X_(0); X(1) = X_(1); X(2) = X_(2); X_(3) = 1.0;
+	X.at<double>(0) = X_.at<double>(0);
+	X.at<double>(1) = X_.at<double>(1);
+	X.at<double>(2) = X_.at<double>(2);
+	X.at<double>(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 p2x = cv::Mat(cv::Mat(P).row(2)*X).at<double>(0);
-    	double p2x1 = cv::Mat(cv::Mat(P1).row(2)*cv::Mat(X)).at<double>(0);
+    	double p2x1 = cv::Mat(cv::Mat(P1).row(2)*X).at<double>(0);
 
         //breaking point
         if(fabs(wi - p2x) <= EPSILON && fabs(wi1 - p2x1) <= EPSILON) break;
@@ -580,9 +575,12 @@ cv::Mat EpipolarGeometry::iterativeLinearLSTriangulation(
 				-(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;
+        X.at<double>(0) = X_.at<double>(0);
+		X.at<double>(1) = X_.at<double>(1);
+		X.at<double>(2) = X_.at<double>(2);
+		X.at<double>(3) = 1.0;
     }
-    return X;
+    return X; //  return 4x1 double
 }
 
 /**
@@ -590,8 +588,8 @@ cv::Mat EpipolarGeometry::iterativeLinearLSTriangulation(
  */
 //Triagulate points
 double EpipolarGeometry::triangulatePoints(
-		const std::vector<cv::Point2f>& pt_set1,
+		const cv::Mat& pt_set, //2xN double
-		const std::vector<cv::Point2f>& pt_set2,
+		const cv::Mat& pt_set1, //2xN double
 		const cv::Mat& P, // 3x4 double
 		const cv::Mat& P1, // 3x4 double
 		pcl::PointCloud<pcl::PointXYZ>::Ptr & pointcloud,
@@ -599,24 +597,25 @@ double EpipolarGeometry::triangulatePoints(
 {
 	pointcloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
 
-	unsigned int pts_size = pt_set1.size();
+	unsigned int pts_size = pt_set.cols;
 
 	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 u(pt_set.at<double>(0,i),pt_set.at<double>(1,i),1.0);
-		cv::Point3d u1(pt_set2[i].x,pt_set2[i].y,1.0);
+		cv::Point3d u1(pt_set1.at<double>(0,i),pt_set1.at<double>(1,i),1.0);
 
-		cv::Mat_<double> X = iterativeLinearLSTriangulation(u,P,u1,P1);
+		cv::Mat X = iterativeLinearLSTriangulation(u,P,u1,P1);
 
-		cv::Mat_<double> xPt_img = P1 * X;				//reproject
+		cv::Mat x_proj = P * X;				//reproject
-		cv::Point2f xPt_img_(xPt_img(0)/xPt_img(2),xPt_img(1)/xPt_img(2));
+		x_proj = x_proj / x_proj.at<double>(2);
+		cv::Point3d xPt_img_(x_proj.at<double>(0), x_proj.at<double>(1), 1.0);
 
-		double reprj_err = norm(xPt_img_-pt_set1[i]);
+		double reprj_err = norm(xPt_img_ - u);
 		reproj_errors[i] = reprj_err;
-		pointcloud->at(i) = pcl::PointXYZ(X(0),X(1),X(2));
+		pointcloud->at(i) = pcl::PointXYZ(X.at<double>(0),X.at<double>(1),X.at<double>(2));
 	}
 
 	return cv::mean(reproj_errors)[0]; // mean reproj error

corelib/src/Odometry.cpp

@@ -486,7 +486,7 @@ OdometryOpticalFlow::OdometryOpticalFlow(const ParametersMap & parameters) :
 	subPixWinSize_(Parameters::defaultOdomSubPixWinSize()),
 	subPixIterations_(Parameters::defaultOdomSubPixIterations()),
 	subPixEps_(Parameters::defaultOdomSubPixEps()),
-	lastCorners3D_(new pcl::PointCloud<pcl::PointXYZ>)
+	refCorners3D_(new pcl::PointCloud<pcl::PointXYZ>)
 {
 	Parameters::parse(parameters, Parameters::kOdomFlowWinSize(), flowWinSize_);
 	Parameters::parse(parameters, Parameters::kOdomFlowIterations(), flowIterations_);
@@ -515,9 +515,9 @@ OdometryOpticalFlow::~OdometryOpticalFlow()
 void OdometryOpticalFlow::reset(const Transform & initialPose)
 {
 	Odometry::reset(initialPose);
-	lastFrame_ = cv::Mat();
+	refFrame_ = cv::Mat();
-	lastCorners_.clear();
+	refCorners_.clear();
-	lastCorners3D_->clear();
+	refCorners3D_->clear();
 }
 
 // return not null transform if odometry is correctly computed
@@ -551,7 +551,6 @@ Transform OdometryOpticalFlow::computeTransformStereo(
 
 	int inliers = 0;
 	int correspondences = 0;
-	imgMatches_ = cv::Mat();
 
 	cv::Mat newLeftFrame;
 	// convert to grayscale
@@ -566,8 +565,8 @@ Transform OdometryOpticalFlow::computeTransformStereo(
 	cv::Mat newRightFrame = data.rightImage().clone();
 
 	std::vector<cv::Point2f> newCorners;
-	UDEBUG("lastCorners_.size()=%d lastFrame_=%d lastRightFrame_=%d", (int)lastCorners_.size(), lastFrame_.empty()?0:1, lastRightFrame_.empty()?0:1);
+	UDEBUG("lastCorners_.size()=%d lastFrame_=%d lastRightFrame_=%d", (int)refCorners_.size(), refFrame_.empty()?0:1, refRightFrame_.empty()?0:1);
-	if(!lastFrame_.empty() && !lastRightFrame_.empty() && lastCorners_.size())
+	if(!refFrame_.empty() && !refRightFrame_.empty() && refCorners_.size())
 	{
 		UDEBUG("");
 		// Find features in the new left image
@@ -575,9 +574,9 @@ Transform OdometryOpticalFlow::computeTransformStereo(
 		std::vector<float> err;
 		UDEBUG("cv::calcOpticalFlowPyrLK() begin");
 		cv::calcOpticalFlowPyrLK(
-				lastFrame_,
+				refFrame_,
 				newLeftFrame,
-				lastCorners_,
+				refCorners_,
 				newCorners,
 				status,
 				err,
@@ -593,7 +592,7 @@ Transform OdometryOpticalFlow::computeTransformStereo(
 		{
 			if(status[i])
 			{
-				lastCornersKept[ki] = lastCorners_[i];
+				lastCornersKept[ki] = refCorners_[i];
 				newCornersKept[ki] = newCorners[i];
 				++ki;
 			}
@@ -608,8 +607,8 @@ Transform OdometryOpticalFlow::computeTransformStereo(
 			std::vector<float> errLast;
 			std::vector<cv::Point2f> lastCornersKeptRight;
 			cv::calcOpticalFlowPyrLK(
-						lastFrame_,
+						refFrame_,
-						lastRightFrame_,
+						refRightFrame_,
 						lastCornersKept,
 						lastCornersKeptRight,
 						statusLast,
@@ -697,7 +696,7 @@ Transform OdometryOpticalFlow::computeTransformStereo(
 			lastKpts.resize(oi);
 			newKpts.resize(oi);
 			correspondences = oi;
-			lastCorners3D_ = correspondencesNew;
+			refCorners3D_ = correspondencesNew;
 			UDEBUG("Getting correspondences end, kept %d/%d", correspondences, (int)statusLast.size());
 
 			/*good_matches.resize(lastKpts.size());
@@ -763,9 +762,6 @@ Transform OdometryOpticalFlow::computeTransformStereo(
 	newCorners.clear();
 	if(!output.isNull())
 	{
-		// Update frame, reset saved last transform
-		savedLastRefFrameTransform_.setNull();
-
 		// Copy or generate new keypoints
 		if(data.keypoints().size())
 		{
@@ -799,16 +795,16 @@ Transform OdometryOpticalFlow::computeTransformStereo(
 			}
 		}
 
-		if(lastCorners_.size() && newCorners.size() < (unsigned int)(this->getFeaturesRatio() * float(lastCorners_.size())))
+		if(refCorners_.size() && newCorners.size() < (unsigned int)(this->getFeaturesRatio() * float(refCorners_.size())))
 		{
 			UWARN("At least %f%% keypoints of the last image required. New=%d last=%d",
-					this->getFeaturesRatio()*100.0f, newCorners.size(), lastCorners_.size());
+					this->getFeaturesRatio()*100.0f, newCorners.size(), refCorners_.size());
 		}
 		else if((int)newCorners.size() > this->getMinInliers())
 		{
-			lastFrame_ = newLeftFrame;
+			refFrame_ = newLeftFrame;
-			lastRightFrame_ = newRightFrame;
+			refRightFrame_ = newRightFrame;
-			lastCorners_ = newCorners;
+			refCorners_ = newCorners;
 		}
 		else
 		{
@@ -841,7 +837,6 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
 
 	int inliers = 0;
 	int correspondences = 0;
-	imgMatches_ = cv::Mat();
 
 	cv::Mat newFrame;
 	// convert to grayscale
@@ -858,15 +853,15 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
 	bool updateFrame = false;
 
 	std::vector<cv::Point2f> newCorners;
-	if(!lastFrame_.empty() && lastCorners_.size() && lastCorners3D_->size())
+	if(!refFrame_.empty() && refCorners_.size() && refCorners3D_->size())
 	{
 		std::vector<unsigned char> status;
 		std::vector<float> err;
 		UDEBUG("cv::calcOpticalFlowPyrLK() begin");
 		cv::calcOpticalFlowPyrLK(
-				lastFrame_,
+				refFrame_,
 				newFrame,
-				lastCorners_,
+				refCorners_,
 				newCorners,
 				status,
 				err,
@@ -877,20 +872,20 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
 
 		pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesLast(new pcl::PointCloud<pcl::PointXYZ>);
 		pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesNew(new pcl::PointCloud<pcl::PointXYZ>);
-		correspondencesLast->resize(lastCorners_.size());
+		correspondencesLast->resize(refCorners_.size());
-		correspondencesNew->resize(lastCorners_.size());
+		correspondencesNew->resize(refCorners_.size());
 		int oi=0;
 
-		std::vector<cv::KeyPoint> lastKpts(lastCorners_.size());
+		std::vector<cv::KeyPoint> lastKpts(refCorners_.size());
-		std::vector<cv::KeyPoint> newKpts(lastCorners_.size());
+		std::vector<cv::KeyPoint> newKpts(refCorners_.size());
 
-		UASSERT(lastCorners_.size() == lastCorners3D_->size());
+		UASSERT(refCorners_.size() == refCorners3D_->size());
-		UDEBUG("lastCorners3D_ = %d", lastCorners3D_->size());
+		UDEBUG("lastCorners3D_ = %d", refCorners3D_->size());
 		float sumSqrdDistance = 0.0f;
 		int flowInliers = 0;
 		for(unsigned int i=0; i<status.size(); ++i)
 		{
-			if(status[i] && pcl::isFinite(lastCorners3D_->at(i)) &&
+			if(status[i] && pcl::isFinite(refCorners3D_->at(i)) &&
 				uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols-1)) &&
 				uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows-1)))
 			{
@@ -903,13 +898,13 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
 					uIsInBounds(pt.z, 0.0f, this->getMaxDepth()))))
 				{
 					pt = util3d::transformPoint(pt, data.localTransform());
-					correspondencesLast->at(oi) = lastCorners3D_->at(i);
+					correspondencesLast->at(oi) = refCorners3D_->at(i);
 					correspondencesNew->at(oi) = pt;
 
-					cv::Point2f diff = newCorners[i]-lastCorners_[i];
+					cv::Point2f diff = newCorners[i]-refCorners_[i];
 					sumSqrdDistance += diff.x*diff.x + diff.y*diff.y;
 
-					lastKpts[oi].pt = lastCorners_[i];
+					lastKpts[oi].pt = refCorners_[i];
 					newKpts[oi].pt = newCorners[i];
 
 					++oi;
@@ -1021,10 +1016,10 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
 			}
 		}
 
-		if(lastCorners_.size() && newCorners.size() < (unsigned int)(this->getFeaturesRatio() * float(lastCorners_.size())))
+		if(refCorners_.size() && newCorners.size() < (unsigned int)(this->getFeaturesRatio() * float(refCorners_.size())))
 		{
 			UWARN("At least %f%% keypoints of the last image required. New=%d last=%d",
-					this->getFeaturesRatio()*100.0f, newCorners.size(), lastCorners_.size());
+					this->getFeaturesRatio()*100.0f, newCorners.size(), refCorners_.size());
 		}
 		else if((int)newCorners.size() > this->getMinInliers())
 		{
@@ -1057,14 +1052,14 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
 			newCorners3D->resize(oi);
 			if((int)newCornersFiltered.size() > this->getMinInliers())
 			{
-				lastFrame_ = newFrame;
+				refFrame_ = newFrame;
-				lastCorners_ = newCornersFiltered;
+				refCorners_ = newCornersFiltered;
-				lastCorners3D_ = newCorners3D;
+				refCorners3D_ = newCorners3D;
 			}
 			else
 			{
 				UWARN("Too low 3D corners (%d/%d, minCorners=%d), ignoring new frame...",
-						(int)newCornersFiltered.size(), (int)lastCorners3D_->size(), this->getMinInliers());
+						(int)newCornersFiltered.size(), (int)refCorners3D_->size(), this->getMinInliers());
 			}
 		}
 		else

corelib/src/VWDictionary.cpp

@@ -380,6 +380,8 @@ void VWDictionary::removeAllWordRef(int wordId, int signatureId)
 std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
 							   int signatureId)
 {
+	UASSERT(signatureId > 0);
+
 	UDEBUG("id=%d descriptors=%d", signatureId, descriptors.rows);
 	UTimer timer;
 	std::list<int> wordIds;
@@ -389,11 +391,12 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
 		return wordIds;
 	}
 	int dim = 0;
-	int type = 0;
+	int type = -1;
 	if(_visualWords.size())
 	{
 		dim = _visualWords.begin()->second->getDescriptor().cols;
 		type = _visualWords.begin()->second->getDescriptor().type();
+		UASSERT(type == CV_32F || type == CV_8U);
 	}
 
 	if(dim && dim != descriptors.cols)
@@ -403,7 +406,7 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
 	}
 	dim = descriptors.cols;
 
-	if(type && type != descriptors.type())
+	if(type>=0 && type != descriptors.type())
 	{
 		UERROR("Descriptors (type=%d) are not the same type as already added words in dictionary(type=%d)", descriptors.type(), type);
 		return wordIds;
@@ -859,6 +862,16 @@ void VWDictionary::removeWords(const std::vector<VisualWord*> & words)
 	}
 }
 
+void VWDictionary::deleteUnusedWords()
+{
+	std::vector<VisualWord*> unusedWords = uValues(_unusedWords);
+	removeWords(unusedWords);
+	for(unsigned int i=0; i<unusedWords.size(); ++i)
+	{
+		delete unusedWords[i];
+	}
+}
+
 void VWDictionary::exportDictionary(const char * fileNameReferences, const char * fileNameDescriptors) const
 {
     FILE* foutRef = 0;

corelib/src/VisualWord.h

@@ -34,8 +34,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap
 {
 
-class SignatureSurf;
-
 class RTABMAP_EXP VisualWord
 {
 public:

tools/EpipolarGeometry/main.cpp

@@ -40,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Features2d.h"
 #include "rtabmap/core/EpipolarGeometry.h"
 #include "rtabmap/core/VWDictionary.h"
+#include "rtabmap/core/Odometry.h"
 #include "rtabmap/gui/UCv2Qt.h"
 
 #include "rtabmap/gui/ImageView.h"
@@ -62,6 +63,570 @@ void showUsage()
 	exit(1);
 }
 
+class RTABMAP_EXP OdometryMono : public Odometry
+{
+public:
+	OdometryMono(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap()) :
+		Odometry(parameters),
+		flowWinSize_(Parameters::defaultOdomFlowWinSize()),
+		flowIterations_(Parameters::defaultOdomFlowIterations()),
+		flowEps_(Parameters::defaultOdomFlowEps()),
+		flowMaxLevel_(Parameters::defaultOdomFlowMaxLevel()),
+		subPixWinSize_(Parameters::defaultOdomSubPixWinSize()),
+		subPixIterations_(Parameters::defaultOdomSubPixIterations()),
+		subPixEps_(Parameters::defaultOdomSubPixEps()),
+		refCorners3D_(new pcl::PointCloud<pcl::PointXYZ>)
+	{
+		Parameters::parse(parameters, Parameters::kOdomFlowWinSize(), flowWinSize_);
+		Parameters::parse(parameters, Parameters::kOdomFlowIterations(), flowIterations_);
+		Parameters::parse(parameters, Parameters::kOdomFlowEps(), flowEps_);
+		Parameters::parse(parameters, Parameters::kOdomFlowMaxLevel(), flowMaxLevel_);
+		Parameters::parse(parameters, Parameters::kOdomSubPixWinSize(), subPixWinSize_);
+		Parameters::parse(parameters, Parameters::kOdomSubPixIterations(), subPixIterations_);
+		Parameters::parse(parameters, Parameters::kOdomSubPixEps(), subPixEps_);
+
+		ParametersMap::const_iterator iter;
+		Feature2D::Type detectorStrategy = (Feature2D::Type)Parameters::defaultOdomFeatureType();
+		if((iter=parameters.find(Parameters::kOdomFeatureType())) != parameters.end())
+		{
+			detectorStrategy = (Feature2D::Type)std::atoi((*iter).second.c_str());
+		}
+		feature2D_ = Feature2D::create(detectorStrategy, parameters);
+
+		ParametersMap customParameters;
+		customParameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uValue(parameters, Parameters::kOdomBowNNType(), uNumber2Str(Parameters::defaultOdomBowNNType()))));
+		customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uValue(parameters, Parameters::kOdomBowNNDR(), uNumber2Str(Parameters::defaultOdomBowNNDR()))));
+		customParameters.insert(ParametersPair(Parameters::kKpNewWordsComparedTogether(), "false"));
+		dictionary_ = new VWDictionary(customParameters);
+	}
+
+	virtual ~OdometryMono()
+	{
+		delete feature2D_;
+		delete dictionary_;
+	}
+
+private:
+	virtual Transform computeTransform(const SensorData & data, int * quality = 0, int * features = 0, int * localMapSize = 0)
+	{
+		UTimer timer;
+		Transform output;
+
+		int inliers = 0;
+		int correspondences = 0;
+
+		cv::Mat newFrame;
+		// convert to grayscale
+		if(data.image().channels() > 1)
+		{
+			cv::cvtColor(data.image(), newFrame, cv::COLOR_BGR2GRAY);
+		}
+		else
+		{
+			newFrame = data.image().clone();
+		}
+
+		UDEBUG("lastCorners_.size()=%d lastFrame_=%d", (int)refCorners_.size(), refFrame_.empty()?0:1);
+		if(!refFrame_.empty() && refCorners_.size())
+		{
+			if(refCorners3D_->size())
+			{
+				//PnP
+				UDEBUG("PnP");
+
+				std::vector<cv::KeyPoint> newKpts;
+				std::vector<cv::Point2f> newCorners;
+				cv::Mat newDescriptors;
+				if(data.keypoints().size())
+				{
+					cv::KeyPoint::convert(data.keypoints(), newCorners);
+					newKpts = data.keypoints();
+					newDescriptors = data.descriptors();
+				}
+				else
+				{
+					// generate kpts
+					cv::Rect roi = Feature2D::computeRoi(newFrame, this->getRoiRatios());
+					newKpts = feature2D_->generateKeypoints(newFrame, this->getMaxFeatures(), roi);
+					Feature2D::limitKeypoints(newKpts, this->getMaxFeatures());
+
+					if(newKpts.size())
+					{
+						//extract descriptors (before subpixel)
+						newDescriptors = feature2D_->generateDescriptors(newFrame, newKpts);
+
+						cv::KeyPoint::convert(newKpts, newCorners);
+
+						if(subPixWinSize_ > 0 && subPixIterations_ > 0)
+						{
+							UDEBUG("cv::cornerSubPix() begin");
+							cv::cornerSubPix(newFrame, newCorners,
+								cv::Size( subPixWinSize_, subPixWinSize_ ),
+								cv::Size( -1, -1 ),
+								cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, subPixIterations_, subPixEps_ ) );
+							UDEBUG("cv::cornerSubPix() end");
+
+							for(unsigned int i=0; i<newCorners.size(); ++i)
+							{
+								newKpts[i].pt = newCorners[i];
+							}
+						}
+					}
+				}
+
+				//matching using visual words dictionary
+				std::vector<int> newWordIds = uListToVector(dictionary_->addNewWords(newDescriptors, 2));
+				UDEBUG("");
+				UASSERT((int)newKpts.size() == newDescriptors.rows);
+				UASSERT(newKpts.size() == newWordIds.size());
+				std::multimap<int, cv::KeyPoint> newWords;
+				for(unsigned int i=0; i<newWordIds.size(); ++i)
+				{
+					newWords.insert(std::make_pair(newWordIds[i], newKpts[i]));
+				}
+				UDEBUG("newWords=%d", (int)newWords.size());
+				std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
+				if(EpipolarGeometry::findPairsUnique(refWords_, newWords, pairs) > this->getMinInliers())
+				{
+					UDEBUG("pairs = %d", (int)pairs.size());
+					// now that we have correspondences, set data for PnP
+					std::vector<cv::Point3f> objectPoints(pairs.size());
+					std::vector<cv::Point2f> imagePoints(pairs.size());
+					int i=0;
+					std::vector<cv::KeyPoint> a,b;
+					for(std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > >::iterator iter = pairs.begin();
+						iter!=pairs.end();
+						++iter)
+					{
+						pcl::PointXYZ pt3 = refCorners3D_->at(iter->first-1); // id and index should match
+						objectPoints[i] = cv::Point3f(pt3.x, pt3.y, pt3.z);
+						imagePoints[i] = iter->second.second.pt;
+						UDEBUG("ref (%f %f) new (%f %f) pt (%f %f %f)",
+								iter->second.first.pt.x, iter->second.first.pt.y,
+								iter->second.second.pt.x, iter->second.second.pt.y,
+								pt3.x, pt3.y, pt3.z);
+						a.push_back(iter->second.first);
+						b.push_back(iter->second.second);
+						++i;
+					}
+
+					UDEBUG("");
+					cv::Mat K = (cv::Mat_<double>(3,3) <<
+						data.fx(), 0, data.cx(),
+						0, data.fyOrBaseline(), data.cy(),
+						0, 0, 1);
+					cv::Mat rvec, tvec;
+					std::vector<int> inliers;
+					cv::solvePnPRansac(objectPoints, imagePoints, K, cv::Mat(), rvec, tvec, false, 100, 8., 100, inliers);
+					UDEBUG("");
+					UDEBUG("inliers=%d/%d", (int)inliers.size(), (int)objectPoints.size());
+
+					/*
+					/// Debug draw matches
+					std::vector<cv::DMatch> good_matches(inliers.size());
+					for(i=0; i<(int)good_matches.size(); ++i)
+					{
+						good_matches[i].trainIdx = inliers[i];
+						good_matches[i].queryIdx = inliers[i];
+					}
+
+					cv::Mat imgInliers;
+					cv::drawMatches( refFrame_, a, newFrame, b,
+								   good_matches, imgInliers, cv::Scalar::all(-1), cv::Scalar::all(-1),
+								   std::vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
+					UWARN("saved test.png");
+					cv::imwrite("test.png", imgInliers);
+					cv::imwrite("testa.png", refFrame_);
+					cv::imwrite("testb.png", newFrame);
+					/// Debug draw matches
+					 */
+
+					if((int)inliers.size() > this->getMinInliers())
+					{
+						cv::Mat R(3,3,CV_64FC1);
+						cv::Rodrigues(rvec, R);
+
+						std::cout << "R: " << R << std::endl;
+						std::cout << "T: " << tvec << std::endl;
+
+						//R = R.t();  // rotation of inverse
+						//tvec = -R * tvec; // translation of inverse
+
+						//UDEBUG("camera movement:");
+						//std::cout << "R: " << R << std::endl;
+						//std::cout << "T: " << tvec << std::endl;
+
+						output = Transform(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));
+						output = data.localTransform() * output.inverse() * data.localTransform().inverse();
+						output = this->getPose().inverse() * refCorners3DPose_ * output;
+					}
+					else
+					{
+						UWARN("PnP not enough inliers (%d < %d), rejecting the transform...", (int)inliers.size(), this->getMinInliers());
+					}
+				}
+				else
+				{
+					UWARN("Not enough pairs found (%d)...", (int)pairs.size());
+				}
+
+				// remove new words from dictionary
+				for(unsigned int i=0; i<newWordIds.size(); ++i)
+				{
+					dictionary_->removeAllWordRef(newWordIds[i], 2);
+				}
+				dictionary_->deleteUnusedWords();
+			}
+			else
+			{
+				//flow
+
+				UDEBUG("flow");
+				// Find features in the new left image
+				std::vector<unsigned char> status;
+				std::vector<float> err;
+				std::vector<cv::Point2f> flowCorners = refCornersGuess_;
+				std::vector<cv::Point2f> refCorners = refCorners_;
+				std::vector<cv::KeyPoint> refKpts = refKpts_;
+				cv::Mat refDescriptors = refDescriptors_;
+				UDEBUG("cv::calcOpticalFlowPyrLK() begin (ref=%d guess=%d)", (int)refCorners.size(), (int)flowCorners.size());
+				cv::calcOpticalFlowPyrLK(
+						refFrame_,
+						newFrame,
+						refCorners,
+						flowCorners,
+						status,
+						err,
+						cv::Size(flowWinSize_, flowWinSize_), flowMaxLevel_,
+						cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations_, flowEps_),
+						cv::OPTFLOW_LK_GET_MIN_EIGENVALS | cv::OPTFLOW_USE_INITIAL_FLOW, 1e-4);
+				UDEBUG("cv::calcOpticalFlowPyrLK() end");
+
+				UDEBUG("Filtering optical flow outliers...");
+				std::vector<cv::Point2f> tmpFlowCorners(status.size());
+				std::vector<cv::Point2f> tmpRefCorners(status.size());
+				std::vector<cv::KeyPoint> tmpRefKpts(status.size());
+				cv::Mat tmpRefDescriptors;
+				int oi = 0;
+				float flow = 0;
+				float minFlow = 50;
+
+				UASSERT(flowCorners.size() == status.size());
+				UASSERT(refCorners.size() == status.size());
+				UASSERT(refKpts.size() == status.size());
+				UASSERT(refDescriptors.rows == (int)status.size());
+				for(unsigned int i=0; i<status.size(); ++i)
+				{
+					if(status[i] && refCornersMask_[i])
+					{
+						float dx = refCorners[i].x - flowCorners[i].x;
+						float dy = refCorners[i].y - flowCorners[i].y;
+						float tmp = std::sqrt(dx*dx + dy*dy);
+						flow+=tmp;
+
+						tmpFlowCorners[oi] = flowCorners[i];
+						tmpRefCorners[oi] = refCorners[i];
+						tmpRefKpts[oi] = refKpts[i];
+						tmpRefDescriptors.push_back(refDescriptors.row(i));
+						++oi;
+
+						UDEBUG("%d = ref(%f %f) flow(%f %f) = %f", i,
+							refCorners[i].x, refCorners[i].y,
+							flowCorners[i].x, flowCorners[i].y,
+							tmp);
+					}
+					else
+					{
+						refCornersMask_[i] = 0;
+					}
+				}
+				if(oi)
+				{
+					flow /=float(oi);
+				}
+				tmpFlowCorners.resize(oi);
+				tmpRefCorners.resize(oi);
+				tmpRefKpts.resize(oi);
+				UDEBUG("Filtering optical flow outliers...done! (inliers=%d/%d)", oi, (int)status.size());
+
+				if(flow > minFlow && oi > this->getMinInliers())
+				{
+					flowCorners = tmpFlowCorners;
+					refCorners = tmpRefCorners;
+					refKpts = tmpRefKpts;
+					refDescriptors = tmpRefDescriptors;
+
+					UDEBUG("flow=%f", flow);
+					// compute fundamental matrix
+					UDEBUG("Find fundamental matrix");
+					status.clear();
+					cv::Mat F = cv::findFundamentalMat(refCorners, flowCorners, status, cv::RANSAC, 3.0, 0.99);
+					std::cout << "F=" << F << std::endl;
+
+					if(!F.empty())
+					{
+						UDEBUG("Filtering fundamental matrix outliers...");
+						tmpFlowCorners.resize(status.size());
+						tmpRefCorners.resize(status.size());
+						tmpRefKpts.resize(status.size());
+						tmpRefDescriptors = cv::Mat();
+						oi = 0;
+						UASSERT(flowCorners.size() == status.size());
+						UASSERT(refCorners.size() == status.size());
+						UASSERT(refKpts.size() == status.size());
+						UASSERT(refDescriptors.rows == (int)status.size());
+						for(unsigned int i=0; i<status.size(); ++i)
+						{
+							if(status[i])
+							{
+								tmpFlowCorners[oi] = flowCorners[i];
+								tmpRefCorners[oi] = refCorners[i];
+								tmpRefKpts[oi] = refKpts[i];
+								tmpRefDescriptors.push_back(refDescriptors.row(i));
+								++oi;
+							}
+						}
+						tmpFlowCorners.resize(oi);
+						tmpRefCorners.resize(oi);
+						tmpRefKpts.resize(oi);
+						flowCorners = tmpFlowCorners;
+						refCorners = tmpRefCorners;
+						refKpts = tmpRefKpts;
+						refDescriptors = tmpRefDescriptors;
+						UDEBUG("Filtering fundamental matrix outliers...done! (inliers=%d/%d)", oi, (int)status.size());
+
+						if(refCorners.size())
+						{
+							std::vector<cv::Point2f> lastCornersRefined;
+							std::vector<cv::Point2f> newCornersRefined;
+							//UDEBUG("Correcting matches...");
+							cv::correctMatches(F, refCorners, flowCorners, lastCornersRefined, newCornersRefined);
+							refCorners = lastCornersRefined;
+							flowCorners = newCornersRefined;
+							//UDEBUG("Correcting matches...done!");
+
+							UDEBUG("Computing P...");
+							cv::Mat K = (cv::Mat_<double>(3,3) <<
+										data.fx(), 0, data.cx(),
+										0, data.fyOrBaseline(), data.cy(),
+										0, 0, 1);
+							//std::cout << "K=" << K << std::endl;
+							cv::Mat Kinv = K.inv();
+							//std::cout << "Kinv=" << Kinv << std::endl;
+							cv::Mat E = K.t()*F*K;
+							std::cout << "E=" << E << std::endl;
+
+							//normalize coordinates
+							cv::Mat x(3, refCorners.size(), CV_64FC1);
+							cv::Mat xp(3, refCorners.size(), CV_64FC1);
+							for(unsigned int i=0; i<refCorners.size(); ++i)
+							{
+								x.at<double>(0, i) = refCorners[i].x;
+								x.at<double>(1, i) = refCorners[i].y;
+								x.at<double>(2, i) = 1;
+
+								xp.at<double>(0, i) = flowCorners[i].x;
+								xp.at<double>(1, i) = flowCorners[i].y;
+								xp.at<double>(2, i) = 1;
+
+								//UDEBUG("ptA= %f %f %f", ptA.at<double>(0, i), ptA.at<double>(1, i), ptA.at<double>(2, i));
+							}
+
+							cv::Mat x_norm = Kinv * x;
+							cv::Mat xp_norm = Kinv * xp;
+							x_norm = x_norm.rowRange(0,2);
+							xp_norm = xp_norm.rowRange(0,2);
+							x = x.rowRange(0,2);
+							xp = xp.rowRange(0,2);
+
+							cv::Mat P = EpipolarGeometry::findPFromE(E, x_norm, xp_norm);
+							if(!P.empty())
+							{
+								cv::Mat P0 = cv::Mat::zeros(3, 4, CV_64FC1);
+								P0.at<double>(0,0) = 1;
+								P0.at<double>(1,1) = 1;
+								P0.at<double>(2,2) = 1;
+
+								UDEBUG("Computing P...done!");
+								std::cout << "P=" << P << std::endl;
+
+								//scale
+								//P.col(3) /= 10.0;
+
+								cv::Mat R, T;
+								EpipolarGeometry::findRTFromP(P, R, T);
+								//std::cout << "R=" << R << std::endl;
+								//std::cout << "T=" << T << std::endl;
+
+								UDEBUG("");
+								//cv::Mat pts4D;
+								std::vector<double> reprojErrors;
+								pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
+								EpipolarGeometry::triangulatePoints(x_norm, xp_norm, P0, P, cloud, reprojErrors);
+								//cv::triangulatePoints(P0, P, x_norm, xp_norm, pts4D);
+
+								tmpRefCorners.resize(cloud->size());
+								tmpRefKpts.resize(cloud->size());
+								tmpRefDescriptors = cv::Mat();
+								refCorners3D_->resize(cloud->size());
+								oi = 0;
+								UASSERT(refCorners.size() == cloud->size());
+								UASSERT(refKpts.size() == cloud->size());
+								UASSERT(refDescriptors.rows == (int)cloud->size());
+								for(unsigned int i=0; i<cloud->size(); ++i)
+								{
+									if(cloud->at(i).z>0)
+									{
+										refCorners3D_->at(oi) = cloud->at(i);
+										tmpRefCorners[oi] = refCorners[i];
+										tmpRefKpts[oi] = refKpts[i];
+										tmpRefDescriptors.push_back(refDescriptors.row(i));
+										++oi;
+									}
+								}
+								refCorners3D_->resize(oi);
+								tmpRefCorners.resize(oi);
+								tmpRefKpts.resize(oi);
+								refCorners = tmpRefCorners;
+								refKpts = tmpRefKpts;
+								refDescriptors = tmpRefDescriptors;
+								UDEBUG("Filtering triangulation outliers...done! (inliers=%d/%d)", oi, (int)cloud->size());
+
+								//refCorners3D_ = util3d::transformPointCloud<pcl::PointXYZ>(refCorners3D_, data.localTransform());
+
+								refCorners3DPose_ = this->getPose();
+								dictionary_->clear();
+								refCorners_ = refCorners;
+								refKpts_ = refKpts;
+								refDescriptors_ = refDescriptors;
+								std::vector<int> wordsId = uListToVector(dictionary_->addNewWords(refDescriptors_, 1));
+								refWords_.clear();
+								UASSERT(wordsId.size() == refCorners_.size());
+								for(unsigned int i=0; i<wordsId.size(); ++i)
+								{
+									refWords_.insert(std::make_pair(wordsId[i], refKpts[i]));
+								}
+								dictionary_->update();
+
+								output = Transform(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), T.at<double>(0)/*/T.at<double>(3)*/,
+												   R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), T.at<double>(1)/*/T.at<double>(3)*/,
+												   R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), T.at<double>(2)/*/T.at<double>(3)*/);
+								output = data.localTransform() * output.inverse() * data.localTransform().inverse();
+							}
+							else
+							{
+								UFATAL("No valid camera matrix found!");
+							}
+						}
+					}
+				}
+				else
+				{
+					UWARN("Flow not enough high! flow=%f ki=%d", flow, oi);
+					refCornersGuess_ = flowCorners;
+				}
+			}
+		}
+		else
+		{
+			//return Identity
+			output = Transform::getIdentity();
+
+			std::vector<cv::KeyPoint> newKpts;
+			std::vector<cv::Point2f> newCorners;
+			cv::Mat newDescriptors;
+			if(data.keypoints().size())
+			{
+				cv::KeyPoint::convert(data.keypoints(), newCorners);
+				newKpts = data.keypoints();
+				newDescriptors = data.descriptors();
+			}
+			else
+			{
+				// generate kpts
+				cv::Rect roi = Feature2D::computeRoi(newFrame, this->getRoiRatios());
+				newKpts = feature2D_->generateKeypoints(newFrame, this->getMaxFeatures(), roi);
+				Feature2D::limitKeypoints(newKpts, this->getMaxFeatures());
+
+				if(newKpts.size())
+				{
+					//extract descriptors (before subpixel)
+					newDescriptors = feature2D_->generateDescriptors(newFrame, newKpts);
+
+					cv::KeyPoint::convert(newKpts, newCorners);
+
+					if(subPixWinSize_ > 0 && subPixIterations_ > 0)
+					{
+						UDEBUG("cv::cornerSubPix() begin");
+						cv::cornerSubPix(newFrame, newCorners,
+							cv::Size( subPixWinSize_, subPixWinSize_ ),
+							cv::Size( -1, -1 ),
+							cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, subPixIterations_, subPixEps_ ) );
+						UDEBUG("cv::cornerSubPix() end");
+
+						for(unsigned int i=0; i<newCorners.size(); ++i)
+						{
+							newKpts[i].pt = newCorners[i];
+						}
+					}
+				}
+			}
+
+			if((int)newCorners.size() > this->getMinInliers())
+			{
+				refFrame_ = newFrame;
+				refCorners_ = newCorners;
+				refKpts_ = newKpts;
+				refDescriptors_ = newDescriptors;
+				refCornersGuess_ = newCorners;
+				refCornersMask_.resize(newCorners.size(), 1);
+				UASSERT(refCorners_.size() == refKpts_.size());
+				UASSERT(refDescriptors_.rows == (int)refKpts_.size());
+			}
+			else
+			{
+				UWARN("Too low 2D corners (%d), ignoring new frame...",
+						(int)newCorners.size());
+			}
+		}
+
+		UINFO("Odom update time = %fs tf=[%s] inliers=%d/%d, transform accepted=%s",
+				timer.elapsed(),
+				output.prettyPrint().c_str(),
+				inliers,
+				correspondences,
+				!output.isNull()?"true":"false");
+
+		return output;
+
+	}
+private:
+	//Parameters:
+	int flowWinSize_;
+	int flowIterations_;
+	double flowEps_;
+	int flowMaxLevel_;
+
+	int subPixWinSize_;
+	int subPixIterations_;
+	double subPixEps_;
+
+	Feature2D * feature2D_;
+	VWDictionary * dictionary_;
+
+	cv::Mat refFrame_;
+	std::vector<cv::Point2f> refCorners_;
+	std::vector<cv::Point2f> refCornersGuess_;
+	std::vector<unsigned char> refCornersMask_;
+	pcl::PointCloud<pcl::PointXYZ>::Ptr refCorners3D_;
+	Transform refCorners3DPose_;
+	std::vector<cv::KeyPoint> refKpts_;
+	cv::Mat refDescriptors_;
+	std::multimap<int, cv::KeyPoint> refWords_;
+};
+
 class MainWidget : public QWidget
 {
 public:
@@ -327,7 +892,7 @@ int main(int argc, char** argv)
 		std::cout<<"K=" << k << std::endl;
 		timer.start();
 		//std::cout<<"e=" << e << std::endl;
-		cv::Mat p = EpipolarGeometry::findPFromF(e, x1, x2);
+		cv::Mat p = EpipolarGeometry::findPFromE(e, x1, x2);
 		cv::Mat p0 = cv::Mat::zeros(3, 4, CV_64FC1);
 		p0.at<double>(0,0) = 1;
 		p0.at<double>(1,1) = 1;