Skip to content
Snippets Groups Projects
Commit c51256cf authored by Angel Santamaria-Navarro's avatar Angel Santamaria-Navarro
Browse files

remove T_norm as templated member. starting with first templated constructor

parent 945e7738
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/CMakeLists.txt
+ 2
− 1
View file @ c51256cf
...@@ -90,7 +90,8 @@ SET(sources ...@@ -90,7 +90,8 @@ SET(sources
algorithms/activesearch/alg_activesearch.cpp algorithms/activesearch/alg_activesearch.cpp
algorithms/activesearch/alg_activesearch_load_yaml.cpp algorithms/activesearch/alg_activesearch_load_yaml.cpp
algorithms/residualtrilinearity/alg_residualtrilinearity.cpp algorithms/residualtrilinearity/alg_residualtrilinearity.cpp
algorithms/residualtrilinearity/alg_residualtrilinearity_load_yaml.cpp) algorithms/residualtrilinearity/alg_residualtrilinearity_load_yaml.cpp
)
# application header files # application header files
SET(headers_main SET(headers_main
......
src/common_class/trifocaltensor.h
+ 26
− 17
View file @ c51256cf
...@@ -25,7 +25,8 @@ public: ...@@ -25,7 +25,8 @@ public:
*/ */
TrifocalTensor(); TrifocalTensor();
TrifocalTensor(const int& max_iterations, const double& max_reprojection_error, const double& percentage_of_correct_reprojected_points); 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 * \brief Destructor
...@@ -36,7 +37,8 @@ public: ...@@ -36,7 +37,8 @@ public:
* \brief Compute trifocal tensor * \brief Compute trifocal tensor
*/ */
bool computeTensor(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3); 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 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); bool computeTensorFactorized(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3);
...@@ -45,7 +47,7 @@ public: ...@@ -45,7 +47,7 @@ public:
/** /**
* \brief Compute the coordinates of the point p3 on third img corresponding to p1 and p2 of others images * \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 * \brief Get error of trifocal tensor using all points matched in three frames
...@@ -61,8 +63,6 @@ private: ...@@ -61,8 +63,6 @@ private:
float tensor_[3][3][3]; // Main Tensor matrix (3x3x3) 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) 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 double max_reprojection_error_; // Maximum reprojection error to consider correct the trifocal tensor when checking solutions
...@@ -112,7 +112,7 @@ private: ...@@ -112,7 +112,7 @@ private:
/** /**
* \brief Normalize points of 3 lists isotropically * \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() inline TrifocalTensor::TrifocalTensor()
...@@ -128,14 +128,18 @@ inline TrifocalTensor::TrifocalTensor(const int& max_iterations, const double& m ...@@ -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); 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); iniClassVars(-1,-1,-1);
computeTensorFromProjectionMat(P1,P2,P3); computeTensorFromProjectionMat(P1,P2,P3);
} }
inline TrifocalTensor::~TrifocalTensor() inline TrifocalTensor::~TrifocalTensor()
{ {
} }
...@@ -143,7 +147,6 @@ 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) inline void TrifocalTensor::iniClassVars(const int& max_iterations, const double& max_reprojection_error, const double& percentage_of_correct_reprojected_points)
{ {
comp_time_ = 0.0; comp_time_ = 0.0;
T_norm_ = Eigen::Matrix3d::Identity();
max_iterations_ = max_iterations; max_iterations_ = max_iterations;
max_reprojection_error_ = max_reprojection_error; max_reprojection_error_ = max_reprojection_error;
percentage_of_correct_reprojected_points_ = percentage_of_correct_reprojected_points; percentage_of_correct_reprojected_points_ = percentage_of_correct_reprojected_points;
...@@ -278,8 +281,13 @@ inline bool TrifocalTensor::computeTensor(const Eigen::MatrixXd& list1, const Ei ...@@ -278,8 +281,13 @@ inline bool TrifocalTensor::computeTensor(const Eigen::MatrixXd& list1, const Ei
return true; 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); 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)) 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 ...@@ -324,7 +332,8 @@ inline bool TrifocalTensor::computeTensorFactorized(const Eigen::MatrixXd& list1
// Normalize points // Normalize points
Eigen::MatrixXd l1(list1), l2(list2), l3(list3); 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(); int np = l1.rows();
Eigen::MatrixXd P = Eigen::MatrixXd::Zero(4*np,12*np); Eigen::MatrixXd P = Eigen::MatrixXd::Zero(4*np,12*np);
...@@ -517,14 +526,14 @@ inline void TrifocalTensor::computeMatrixB(const Eigen::Vector3d& p1, const Eige ...@@ -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::MatrixXd B;
Eigen::VectorXd b; Eigen::VectorXd b;
// normalize // normalize
Eigen::Vector3d pn1 = T_norm_*p1; Eigen::Vector3d pn1 = T_norm*p1;
Eigen::Vector3d pn2 = T_norm_*p2; Eigen::Vector3d pn2 = T_norm*p2;
computeMatrixB(pn1, pn2, B, b); computeMatrixB(pn1, pn2, B, b);
...@@ -540,7 +549,7 @@ inline void TrifocalTensor::transfer(const Eigen::Vector3d& p1 , Eigen::Vector3d ...@@ -540,7 +549,7 @@ inline void TrifocalTensor::transfer(const Eigen::Vector3d& p1 , Eigen::Vector3d
pn3(2) = 1; pn3(2) = 1;
// reverse normalization if any // 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) 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 ...@@ -556,14 +565,14 @@ inline Eigen::MatrixXd TrifocalTensor::tensorProduct(const Eigen::MatrixXd& A, c
return 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); Eigen::MatrixXd list(list1.rows()+list2.rows()+list3.rows(),3);
list.block(0,0,list1.rows(),3) = list1; list.block(0,0,list1.rows(),3) = list1;
list.block(list1.rows(),0,list2.rows(),3) = list2; list.block(list1.rows(),0,list2.rows(),3) = list2;
list.block(list1.rows()+list2.rows(),0,list3.rows(),3) = list3; list.block(list1.rows()+list2.rows(),0,list3.rows(),3) = list3;
Eigen::MatrixXd listnorm(list); Eigen::MatrixXd listnorm(list);
T_norm_ = normalizePointsIsotrop(list, listnorm); T_norm = normalizePointsIsotrop(list, listnorm);
l1 = listnorm.block(0,0,list1.rows(),3); l1 = listnorm.block(0,0,list1.rows(),3);
l2 = listnorm.block(list1.rows(),0,list2.rows(),3); l2 = listnorm.block(list1.rows(),0,list2.rows(),3);
l3 = listnorm.block(list1.rows()+list2.rows(),0,list3.rows(),3); l3 = listnorm.block(list1.rows()+list2.rows(),0,list3.rows(),3);
......
src/vision_utils.cpp
+ 2
− 2
View file @ c51256cf
...@@ -331,7 +331,7 @@ double factorial(const int& n) ...@@ -331,7 +331,7 @@ double factorial(const int& n)
return fact; 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 // Centroid
Eigen::Vector3d centroid = l_in.colwise().mean(); Eigen::Vector3d centroid = l_in.colwise().mean();
...@@ -359,7 +359,7 @@ Eigen::MatrixXd normalizePointsIsotrop(const Eigen::MatrixXd& l_in, Eigen::Matri ...@@ -359,7 +359,7 @@ Eigen::MatrixXd normalizePointsIsotrop(const Eigen::MatrixXd& l_in, Eigen::Matri
} }
// Composition of the normalization matrix // Composition of the normalization matrix
Eigen::MatrixXd T = Eigen::MatrixXd::Identity(3,3); Eigen::Matrix3d T = Eigen::Matrix3d::Identity();
T(0,0) = scale; T(0,0) = scale;
T(1,1) = scale; T(1,1) = scale;
T(0,2) = -scale*centroid(0); T(0,2) = -scale*centroid(0);
......
src/vision_utils.h
+ 1
− 1
View file @ c51256cf
...@@ -255,7 +255,7 @@ bool next_combination(const Iterator first, Iterator k, const Iterator last) ...@@ -255,7 +255,7 @@ bool next_combination(const Iterator first, Iterator k, const Iterator last)
return false; 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 /** Check Eigen Matrix sizes, both statically and dynamically
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment