From c51256cf7312f2dd2c4a1cc400ac55a73ba355e7 Mon Sep 17 00:00:00 2001
From: asantamaria <asantamaria@iri.upc.edu>
Date: Wed, 31 Jan 2018 09:09:16 +0100
Subject: [PATCH] remove T_norm as templated member. starting with first
 templated constructor

---
 src/CMakeLists.txt                |  3 ++-
 src/common_class/trifocaltensor.h | 43 +++++++++++++++++++------------
 src/vision_utils.cpp              |  4 +--
 src/vision_utils.h                |  2 +-
 4 files changed, 31 insertions(+), 21 deletions(-)

diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 5c8528b..f3d8fa8 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -90,7 +90,8 @@ SET(sources
 	algorithms/activesearch/alg_activesearch.cpp
 	algorithms/activesearch/alg_activesearch_load_yaml.cpp
 	algorithms/residualtrilinearity/alg_residualtrilinearity.cpp
-	algorithms/residualtrilinearity/alg_residualtrilinearity_load_yaml.cpp)
+	algorithms/residualtrilinearity/alg_residualtrilinearity_load_yaml.cpp
+	)
 	
 # application header files
 SET(headers_main 
diff --git a/src/common_class/trifocaltensor.h b/src/common_class/trifocaltensor.h
index 73574f9..8c751c7 100644
--- a/src/common_class/trifocaltensor.h
+++ b/src/common_class/trifocaltensor.h
@@ -25,7 +25,8 @@ public:
 	 */
 	TrifocalTensor();
 	TrifocalTensor(const int& max_iterations, const double& max_reprojection_error, const double& percentage_of_correct_reprojected_points);
-	TrifocalTensor(const Eigen::MatrixXd& P1, const Eigen::MatrixXd& P2, const Eigen::MatrixXd& P3);
+	template<typename D2>
+	TrifocalTensor(const Eigen::MatrixBase<D2>& P1, const Eigen::MatrixBase<D2>& P2, const Eigen::MatrixBase<D2>& P3);
 
 	/**
 	 * \brief Destructor
@@ -36,7 +37,8 @@ public:
 	 * \brief Compute trifocal tensor
 	 */
 	bool computeTensor(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3);
-	bool computeTensorFromProjectionMat(const Eigen::MatrixXd& P1, const Eigen::MatrixXd& P2, const Eigen::MatrixXd& P3);
+	template<typename D2>
+	bool computeTensorFromProjectionMat(const Eigen::MatrixBase<D2>& P1, const Eigen::MatrixBase<D2>& P2, const Eigen::MatrixBase<D2>& P3);
 	bool computeTensorRansac(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3);
 	bool computeTensorFactorized(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3);
 
@@ -45,7 +47,7 @@ public:
 	/**
 	 * \brief Compute the coordinates of the point p3 on third img corresponding to p1 and p2 of others images
 	 */
-	void transfer(const Eigen::Vector3d& p1 , Eigen::Vector3d& p2, Eigen::Vector3d& p3);
+	void transfer(const Eigen::Vector3d& p1 , const Eigen::Vector3d& p2, Eigen::Vector3d& p3, const Eigen::Matrix3d& T_norm = Eigen::Matrix3d::Identity());
 
 	/**
 	 * \brief Get error of trifocal tensor using all points matched in three frames
@@ -61,8 +63,6 @@ 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
@@ -112,7 +112,7 @@ private:
 	/**
 	 * \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);
+	void normalizeIsotrop(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3, Eigen::MatrixXd& l1, Eigen::MatrixXd& l2, Eigen::MatrixXd& l3, Eigen::Matrix3d& T_norm);
 };
 
 inline TrifocalTensor::TrifocalTensor()
@@ -128,14 +128,18 @@ inline TrifocalTensor::TrifocalTensor(const int& max_iterations, const double& m
 	iniClassVars(max_iterations,max_reprojection_error,percentage_of_correct_reprojected_points);
 }
 
-inline TrifocalTensor::TrifocalTensor(const Eigen::MatrixXd& P1, const Eigen::MatrixXd& P2, const Eigen::MatrixXd& P3)
+template<typename D2>
+inline TrifocalTensor::TrifocalTensor(const Eigen::MatrixBase<D2>& P1, const Eigen::MatrixBase<D2>& P2, const Eigen::MatrixBase<D2>& P3)
 {
+	MatrixSizeCheck<3,4>::check(P1);
+	MatrixSizeCheck<3,4>::check(P2);
+	MatrixSizeCheck<3,4>::check(P3);
+
 	iniClassVars(-1,-1,-1);
 
 	computeTensorFromProjectionMat(P1,P2,P3);
 }
 
-
 inline TrifocalTensor::~TrifocalTensor()
 {
 }
@@ -143,7 +147,6 @@ inline TrifocalTensor::~TrifocalTensor()
 inline void TrifocalTensor::iniClassVars(const int& max_iterations, const double& max_reprojection_error, const double& percentage_of_correct_reprojected_points)
 {
 	comp_time_ = 0.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;
@@ -278,8 +281,13 @@ inline bool TrifocalTensor::computeTensor(const Eigen::MatrixXd& list1, const Ei
 	return true;
 }
 
-inline bool TrifocalTensor::computeTensorFromProjectionMat(const Eigen::MatrixXd& P1, const Eigen::MatrixXd& P2, const Eigen::MatrixXd& P3)
+template<typename Derived>
+inline bool TrifocalTensor::computeTensorFromProjectionMat(const Eigen::MatrixBase<Derived>& P1, const Eigen::MatrixBase<Derived>& P2, const Eigen::MatrixBase<Derived>& P3)
 {
+	MatrixSizeCheck<3,4>::check(P1);
+	MatrixSizeCheck<3,4>::check(P2);
+	MatrixSizeCheck<3,4>::check(P3);
+
 	Eigen::MatrixXd c2Pc1(3,4), c3Pc1(3,4);
 	if(P1.block(0,0,3,3).isIdentity(EPS) && P1.block(0,3,3,1).isZero(EPS))
 	{
@@ -324,7 +332,8 @@ inline bool TrifocalTensor::computeTensorFactorized(const Eigen::MatrixXd& list1
 
 	// Normalize points
 	Eigen::MatrixXd l1(list1), l2(list2), l3(list3);
-	normalizeIsotrop(list1, list2, list3, l1, l2, l3);
+	Eigen::Matrix3d T_norm; // Normalization transform used in isotropic normalization. TODO: Extract it to be used externally
+	normalizeIsotrop(list1, list2, list3, l1, l2, l3, T_norm);
 
 	int np = l1.rows();
 	Eigen::MatrixXd P = Eigen::MatrixXd::Zero(4*np,12*np);
@@ -517,14 +526,14 @@ inline void TrifocalTensor::computeMatrixB(const Eigen::Vector3d& p1, const Eige
 	}
 }
 
-inline void TrifocalTensor::transfer(const Eigen::Vector3d& p1 , Eigen::Vector3d& p2, Eigen::Vector3d& p3) {
+inline void TrifocalTensor::transfer(const Eigen::Vector3d& p1 , const Eigen::Vector3d& p2, Eigen::Vector3d& p3, const Eigen::Matrix3d& T_norm) {
 
 	Eigen::MatrixXd B;
 	Eigen::VectorXd b;
 
 	// normalize
-	Eigen::Vector3d pn1 = T_norm_*p1;
-	Eigen::Vector3d pn2 = T_norm_*p2;
+	Eigen::Vector3d pn1 = T_norm*p1;
+	Eigen::Vector3d pn2 = T_norm*p2;
 
 	computeMatrixB(pn1, pn2, B, b);
 
@@ -540,7 +549,7 @@ inline void TrifocalTensor::transfer(const Eigen::Vector3d& p1 , Eigen::Vector3d
 	pn3(2) = 1;
 
 	// reverse normalization if any
-	p3 = T_norm_.inverse()*pn3;
+	p3 = T_norm.inverse()*pn3;
 }
 
 inline Eigen::MatrixXd TrifocalTensor::tensorProduct(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B)
@@ -556,14 +565,14 @@ inline Eigen::MatrixXd TrifocalTensor::tensorProduct(const Eigen::MatrixXd& A, c
 	return C;
 }
 
-inline void TrifocalTensor::normalizeIsotrop(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3, Eigen::MatrixXd& l1, Eigen::MatrixXd& l2, Eigen::MatrixXd& l3)
+inline 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::Matrix3d& T_norm)
 {
 	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);
+	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);
diff --git a/src/vision_utils.cpp b/src/vision_utils.cpp
index ef0a0fc..947a845 100644
--- a/src/vision_utils.cpp
+++ b/src/vision_utils.cpp
@@ -331,7 +331,7 @@ double factorial(const int& n)
 	return fact;
 }
 
-Eigen::MatrixXd normalizePointsIsotrop(const Eigen::MatrixXd& l_in, Eigen::MatrixXd& l_out)
+Eigen::Matrix3d normalizePointsIsotrop(const Eigen::MatrixXd& l_in, Eigen::MatrixXd& l_out)
 {
 	// Centroid
 	Eigen::Vector3d centroid = l_in.colwise().mean();
@@ -359,7 +359,7 @@ Eigen::MatrixXd normalizePointsIsotrop(const Eigen::MatrixXd& l_in, Eigen::Matri
 	}
 
 	// Composition of the normalization matrix
-	Eigen::MatrixXd T = Eigen::MatrixXd::Identity(3,3);
+	Eigen::Matrix3d T = Eigen::Matrix3d::Identity();
 	T(0,0) = scale;
 	T(1,1) = scale;
 	T(0,2) = -scale*centroid(0);
diff --git a/src/vision_utils.h b/src/vision_utils.h
index bfeed0f..c7b0d7d 100644
--- a/src/vision_utils.h
+++ b/src/vision_utils.h
@@ -255,7 +255,7 @@ bool next_combination(const Iterator first, Iterator k, const Iterator last)
    return false;
 }
 
-Eigen::MatrixXd normalizePointsIsotrop(const Eigen::MatrixXd& list1, Eigen::MatrixXd& l1);
+Eigen::Matrix3d normalizePointsIsotrop(const Eigen::MatrixXd& list1, Eigen::MatrixXd& l1);
 
 //////////////////////////////////////////////////////////
 /** Check Eigen Matrix sizes, both statically and dynamically
-- 
GitLab