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