added trifocal tensor factorization

src/common_class/trifocaltensor.cpp

@@ -7,6 +7,7 @@ namespace vision_utils {
 
 TrifocalTensor::TrifocalTensor(const int& max_iterations, const double& max_reprojection_error, const double& percentage_of_correct_reprojected_points):
 		comp_time_(0),
+		T_norm_(Eigen::Matrix3d::Identity()),
 		max_iterations_(max_iterations),
 		max_reprojection_error_(max_reprojection_error),
 		percentage_of_correct_reprojected_points_(percentage_of_correct_reprojected_points)
@@ -22,6 +23,7 @@ TrifocalTensor::TrifocalTensor(const int& max_iterations, const double& max_repr
 
 TrifocalTensor::TrifocalTensor():
 		comp_time_(0),
+		T_norm_(Eigen::Matrix3d::Identity()),
 		max_iterations_(-1),
 		max_reprojection_error_(-1),
 		percentage_of_correct_reprojected_points_(-1)
@@ -138,9 +140,7 @@ bool TrifocalTensor::computeTensor(const Eigen::MatrixXd& list1, const Eigen::Ma
 
 	// Normalize points
 	Eigen::MatrixXd l1(list1), l2(list2), l3(list3);
-//	Eigen::MatrixXd T1 = normalizePointsIsotrop(list1, l1);
-//	Eigen::MatrixXd T2 = normalizePointsIsotrop(list1, l2);
-//	Eigen::MatrixXd T3 = normalizePointsIsotrop(list1, l3);
+//	normalizeIsotrop(list1, list2, list3, l1, l2, l3);
 
 	Eigen::MatrixXd A;
 	computeMatrixA(l1, l2, l3, A);
@@ -171,9 +171,7 @@ bool TrifocalTensor::computeTensorFactorized(const Eigen::MatrixXd& list1, const
 
 	// Normalize points
 	Eigen::MatrixXd l1(list1), l2(list2), l3(list3);
-//	Eigen::MatrixXd T1 = normalizePointsIsotrop(list1, l1);
-//	Eigen::MatrixXd T2 = normalizePointsIsotrop(list1, l2);
-//	Eigen::MatrixXd T3 = normalizePointsIsotrop(list1, l3);
+	normalizeIsotrop(list1, list2, list3, l1, l2, l3);
 
 	int np = l1.rows();
 	Eigen::MatrixXd P = Eigen::MatrixXd::Zero(4*np,12*np);
@@ -222,7 +220,7 @@ bool TrifocalTensor::computeTensorFactorized(const Eigen::MatrixXd& list1, const
 	for (int ii=0;ii<27;++ii)
 		D(ii,ii)=D_diag(ii);
 
-	// getting singular value of P*U_hat
+	// SVD of P*U_hat
 	Eigen::JacobiSVD<Eigen::MatrixXd> svd_pu(P*U, Eigen::ComputeThinU | Eigen::ComputeThinV);
 	// Ensure that a poor choice of points are not selected to compute the Trifocal Tensor.
 	// The check is based on the condition number of the matrices involved in computing the trifocal tensor
@@ -230,21 +228,13 @@ bool TrifocalTensor::computeTensorFactorized(const Eigen::MatrixXd& list1, const
 	if (std::isnan(cond) || cond < 1e-6)
 		return false;
 	const Eigen::VectorXd D_pu_diag = svd_pu.singularValues();
+	Eigen::MatrixXd V_pu = svd_pu.matrixV();
 
-	// FIX: SOLVE USING THIS
 	// Compute tensor
-	Eigen::VectorXd tensorVector = (D*V.inverse()).inverse()*D_pu_diag;
+	Eigen::VectorXd tensorVector = (D*V.inverse()).inverse()*V_pu.col(26);
 
-	Eigen::MatrixXd A = P*Q*L;
-	if ( resolveEquationWithSVD(A, tensorVector) )
-	{
-		fillTensor(tensorVector);
-		memcpy( tensor_final, tensor_, 27*sizeof(int) );
-	}
-	else
-		return false;
-
-	memcpy( tensor_, tensor_final, 27*sizeof(int) );
+	fillTensor(tensorVector);
+	memcpy( tensor_final, tensor_, 27*sizeof(int) );
 
 	comp_time_ = (double)(clock() - tStart) / CLOCKS_PER_SEC;
 
@@ -379,7 +369,12 @@ void TrifocalTensor::transfer(const Eigen::Vector3d& p1 , Eigen::Vector3d& p2, E
 
 	Eigen::MatrixXd B;
 	Eigen::VectorXd b;
-	computeMatrixB(p1, p2, B, b);
+
+	// normalize
+	Eigen::Vector3d pn1 = T_norm_*p1;
+	Eigen::Vector3d pn2 = T_norm_*p2;
+
+	computeMatrixB(pn1, pn2, B, b);
 
 	Eigen::JacobiSVD<Eigen::MatrixXd> svd(B, Eigen::ComputeThinU | Eigen::ComputeThinV);
 	Eigen::VectorXd searchedPoint;
@@ -387,9 +382,13 @@ void TrifocalTensor::transfer(const Eigen::Vector3d& p1 , Eigen::Vector3d& p2, E
 	// returns the solution x in Bx=b where x has two coordinates
 	searchedPoint = svd.solve(b);
 
-	p3(0) = searchedPoint[0];
-	p3(1) = searchedPoint[1];
-	p3(2) = 1;
+	Eigen::Vector3d pn3;
+	pn3(0) = searchedPoint[0];
+	pn3(1) = searchedPoint[1];
+	pn3(2) = 1;
+
+	// reverse normalization if any
+	p3 = T_norm_.inverse()*pn3;
 }
 
 Eigen::MatrixXd TrifocalTensor::tensorProduct(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B)
@@ -405,4 +404,17 @@ Eigen::MatrixXd TrifocalTensor::tensorProduct(const Eigen::MatrixXd& A, const Ei
 	return C;
 }
 
+void TrifocalTensor::normalizeIsotrop(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3, Eigen::MatrixXd& l1, Eigen::MatrixXd& l2, Eigen::MatrixXd& l3)
+{
+	Eigen::MatrixXd list(list1.rows()+list2.rows()+list3.rows(),3);
+	list.block(0,0,list1.rows(),3) = list1;
+	list.block(list1.rows(),0,list2.rows(),3) = list2;
+	list.block(list1.rows()+list2.rows(),0,list3.rows(),3) = list3;
+	Eigen::MatrixXd listnorm(list);
+	T_norm_ = normalizePointsIsotrop(list, listnorm);
+	l1 = listnorm.block(0,0,list1.rows(),3);
+	l2 = listnorm.block(list1.rows(),0,list2.rows(),3);
+	l3 = listnorm.block(list1.rows()+list2.rows(),0,list3.rows(),3);
+}
+
 } /* namespace vision_utils */

src/common_class/trifocaltensor.h

@@ -59,6 +59,8 @@ private:
 
 	float tensor_[3][3][3];	// Main Tensor matrix (3x3x3)
 
+	Eigen::Matrix3d T_norm_;
+
 	int max_iterations_;	// Maximum number of iterations when sampling the candidate points (matched in three frames)
 
 	double max_reprojection_error_; // Maximum reprojection error to consider correct the trifocal tensor when checking solutions
@@ -99,6 +101,11 @@ private:
 	 * \brief Compute tensor product of two matrices
 	 */
 	Eigen::MatrixXd tensorProduct(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B);
+
+	/**
+	 * \brief Normalize points of 3 lists isotropically
+	 */
+	void normalizeIsotrop(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3, Eigen::MatrixXd& l1, Eigen::MatrixXd& l2, Eigen::MatrixXd& l3);
 };
 
 } /* namespace vision_utils */

src/test/gtest_trifocaltensor.cpp

@@ -483,7 +483,7 @@ TEST(TrifocalTensor, ComputeTensorReal)
     vision_utils::TrifocalTensor tensor(1000,max_error_reprojection,0.75);
     // bool tensor_ok = tensor.computeTensorRansac(list1, list2, list3);
     bool tensor_ok = tensor.computeTensor(list1, list2, list3);
-//    bool tensor_ok = tensor.computeTensorFactorized(list1, list2, list3);
+    // bool tensor_ok = tensor.computeTensorFactorized(list1, list2, list3);
     ASSERT_TRUE(tensor_ok);
 
 #ifdef _VU_DEBUG