diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 5c8528b59a5abc9754af74ead9bf53e0c76874bb..f3d8fa8ca66c062a37e5b2aed7c354f3fe72a6b5 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 73574f9a8b60de1a602a0bceb21a85f131e9ea58..8c751c7371b3ad3ce9e7c9fb7b4a084d6915e647 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 ef0a0fc57108677ac8aa8203bb12e9815159b021..947a8456c6c26f6b69660b53315430da51f13465 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 bfeed0f524374eb0f66f1173e504e56e79939b0f..c7b0d7d6463d641af912aac1a74fc2f94899499f 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