diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 6b6149d63e80be5b146e961c867850230cea2647..5c8528b59a5abc9754af74ead9bf53e0c76874bb 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -18,7 +18,6 @@ ENDIF() # library source files SET(sources vision_utils.cpp - common_class/trifocaltensor.cpp sensors/sensor_base.cpp sensors/usb_cam/usb_cam.cpp sensors/usb_cam/usb_cam_load_yaml.cpp diff --git a/src/common_class/trifocaltensor.cpp b/src/common_class/trifocaltensor.cpp deleted file mode 100644 index 5d840b79816250cd07215cd9b08aae393f485cc6..0000000000000000000000000000000000000000 --- a/src/common_class/trifocaltensor.cpp +++ /dev/null @@ -1,484 +0,0 @@ -#include "trifocaltensor.h" - -#include <cstdlib> -#include <iostream> - -namespace vision_utils { - -TrifocalTensor::TrifocalTensor(): - comp_time_(0), - T_norm_(Eigen::Matrix3d::Identity()), - max_iterations_(-1), - max_reprojection_error_(-1), - percentage_of_correct_reprojected_points_(-1) -{ - iniClassVars(); -} - -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) -{ - if (percentage_of_correct_reprojected_points_<0.0 || percentage_of_correct_reprojected_points_>1.0) - VU_ERROR("Incorrect parameter: percentage_of_correct_reprojected_points. Values must be between 0~1"); - - iniClassVars(); -} - -TrifocalTensor::TrifocalTensor(const Eigen::MatrixXd& P1, const Eigen::MatrixXd& P2, const Eigen::MatrixXd& P3): - comp_time_(0), - T_norm_(Eigen::Matrix3d::Identity()), - max_iterations_(-1), - max_reprojection_error_(-1), - percentage_of_correct_reprojected_points_(-1) -{ - iniClassVars(); - - computeTensorFromProjectionMat(P1,P2,P3); -} - - -TrifocalTensor::~TrifocalTensor() -{ -} - -void TrifocalTensor::iniClassVars(void) -{ - for(size_t kk = 0; kk<3; ++kk) - for(size_t ii = 0; ii<3; ++ii) - for(size_t jj = 0; jj<3; ++jj) - tensor_[ii][jj][kk] = 0; -} - -float TrifocalTensor::operator()(size_t row, size_t col, size_t depth) -{ - return tensor_[row][col][depth]; -} - -void TrifocalTensor::print() const -{ - std::cout << "Trifocal tensor : " << std::endl; - for(size_t ii = 0; ii<3; ++ii) - { - std::cout << " [" << tensor_[ii][0][0] << "," << tensor_[ii][1][0] << "," << tensor_[ii][2][0] << "] "; - std::cout << "[" << tensor_[ii][0][1] << "," << tensor_[ii][1][1] << "," << tensor_[ii][2][1] << "] "; - std::cout << "[" << tensor_[ii][0][2] << "," << tensor_[ii][1][2] << "," << tensor_[ii][2][2] << "]" << std::endl; - } -} - -void TrifocalTensor::fillTensor(const Eigen::VectorXd& tensorVector) -{ - for(size_t kk = 0; kk<3; ++kk) { - for(size_t ii = 0; ii<3; ++ii) { - for(size_t jj = 0; jj<3; ++jj) { - tensor_[ii][jj][kk] = tensorVector(jj + ii*3 + kk*9); - } - } - } -} - -void TrifocalTensor::getLayers(Eigen::Matrix3d& T1, Eigen::Matrix3d& T2, Eigen::Matrix3d& T3) -{ - for(size_t ii = 0; ii<3; ++ii) - { - for(size_t jj = 0; jj<3; ++jj) - { - T1(ii,jj) = tensor_[ii][jj][0]; - T2(ii,jj) = tensor_[ii][jj][1]; - T3(ii,jj) = tensor_[ii][jj][2]; - } - } -} - -std::vector<std::vector<int> > TrifocalTensor::getAllComb(const int& nelements) -{ - std::vector<std::vector<int> > list; - - std::vector<int> comb(nelements); - for (int ii=0;ii<nelements;++ii) comb[ii]=ii; - do { - list.push_back(comb); - } while(std::next_permutation(comb.begin(), comb.end())); - - return list; -} - -Eigen::VectorXd TrifocalTensor::getRandomCandidates(const int& min_points, const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3, Eigen::MatrixXd& l1, Eigen::MatrixXd& l2, Eigen::MatrixXd& l3) -{ - std::random_device rd; //Will be used to obtain a seed for the random number engine - std::mt19937 gen(rd()); //Standard mersenne_twister_engine seeded with rd() - std::uniform_int_distribution<> dis(0, list1.rows()-1); - - Eigen::VectorXd idx = Eigen::VectorXd::Zero(min_points); - for (int n=0; n<min_points; ++n) - { - int num = dis(gen); - idx(n) = num; - for (int ii=0; ii<n; ++ii) - { - if (idx(ii)==num) - { - n = n-1; - break; - } - } - } - - l1 = Eigen::MatrixXd::Zero(min_points,3); - l2 = Eigen::MatrixXd::Zero(min_points,3); - l3 = Eigen::MatrixXd::Zero(min_points,3); - - for (int ii=0; ii<l1.rows(); ++ii) - { - l1.row(ii) = list1.row(idx(ii)); - l2.row(ii) = list2.row(idx(ii)); - l3.row(ii) = list3.row(idx(ii)); - } - - return idx; -} - -double TrifocalTensor::getErrorTrifocal(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3) -{ - double count = 0.0; - for (int ii=0; ii<list1.rows(); ++ii) - { - Eigen::Vector3d p1,p2,p3,p3est; - p1 << list1(ii,0),list1(ii,1),list1(ii,2); - p2 << list2(ii,0),list2(ii,1),list2(ii,2); - p3 << list3(ii,0),list3(ii,1),list3(ii,2); - transfer(p1, p2, p3est); - p3est = p3est/p3est(2); - Eigen::Vector3d res = p3est - p3; - if (res.norm() < max_reprojection_error_) - count = count+1; - } - - return count/list1.rows(); -} - -bool TrifocalTensor::computeTensor(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3) -{ - clock_t tStart = clock(); - - float tensor_final[3][3][3]; - - // Normalize points - Eigen::MatrixXd l1(list1), l2(list2), l3(list3); -// normalizeIsotrop(list1, list2, list3, l1, l2, l3); - - Eigen::MatrixXd A; - computeMatrixA(l1, l2, l3, A); - - Eigen::VectorXd tensorVector; - if ( resolveEquationWithSVD(A, tensorVector) ) - { - fillTensor(tensorVector); - - memcpy( tensor_final, tensor_, 27*sizeof(int) ); - } - else - return false; - - memcpy( tensor_, tensor_final, 27*sizeof(int) ); - - comp_time_ = (double)(clock() - tStart) / CLOCKS_PER_SEC; - - return true; -} - -bool TrifocalTensor::computeTensorFromProjectionMat(const Eigen::MatrixXd& P1, const Eigen::MatrixXd& P2, const Eigen::MatrixXd& 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)) - { - c2Pc1 = P2; - c3Pc1 = P3; - } - else - { - Eigen::Matrix3d wRc1 = P1.block(0,0,3,3); - Eigen::Matrix3d wRc2 = P2.block(0,0,3,3); - Eigen::Matrix3d wRc3 = P3.block(0,0,3,3); - Eigen::Vector3d wtc1 = P1.block(0,3,3,1); - Eigen::Vector3d wtc2 = P2.block(0,3,3,1); - Eigen::Vector3d wtc3 = P3.block(0,3,3,1); - - Eigen::Matrix3d c1Rc2 = wRc1.transpose()*wRc2; - Eigen::Matrix3d c1Rc3 = wRc1.transpose()*wRc3; - Eigen::Vector3d c1tc2 = wRc1.transpose()*(wtc2-wtc1); - Eigen::Vector3d c1tc3 = wRc1.transpose()*(wtc3-wtc1); - - c2Pc1.block(0,0,3,3) = c1Rc2.transpose(); - c2Pc1.block(0,3,3,1) = -c1Rc2.transpose()*c1tc2; - c3Pc1.block(0,0,3,3) = c1Rc3.transpose(); - c3Pc1.block(0,3,3,1) = -c1Rc3.transpose()*c1tc3; - } - - - // Trifocal tensor from Projection matrices (c1->c2 and c1->c3) - for(size_t ii = 0; ii<3; ++ii) - for(size_t jj = 0; jj<3; ++jj) - for(size_t kk = 0; kk<3; ++kk) - tensor_[kk][jj][ii] = c2Pc1(kk,jj)*c3Pc1(ii,3)-c2Pc1(kk,3)*c3Pc1(ii,jj); - - return true; -} - -bool TrifocalTensor::computeTensorFactorized(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3) -{ - clock_t tStart = clock(); - - float tensor_final[3][3][3]; - - // Normalize points - Eigen::MatrixXd l1(list1), l2(list2), l3(list3); - normalizeIsotrop(list1, list2, list3, l1, l2, l3); - - int np = l1.rows(); - Eigen::MatrixXd P = Eigen::MatrixXd::Zero(4*np,12*np); - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(12*np,18*np); - Eigen::MatrixXd L = Eigen::MatrixXd::Zero(18*np,27); - Eigen::MatrixXd I4 = Eigen::MatrixXd::Identity(4,4); - Eigen::MatrixXd I6 = Eigen::MatrixXd::Identity(6,6); - Eigen::MatrixXd I9 = Eigen::MatrixXd::Identity(9,9); - - // for each correspondence (= 3 pixel, 1 per image) - for(size_t p_idx = 0; p_idx<np; ++p_idx) - { - // Factorized method - Eigen::MatrixXd X(1,3),U1(1,3),V1(1,3),U2(1,3),V2(1,3); - X << l1.row(p_idx); - U1 << 1,0,-l2(p_idx,0); - V1 << 0,1,-l2(p_idx,1); - U2 << -1,0,l3(p_idx,0); - V2 << 0,-1,l3(p_idx,1); - - // Matrix P - P.block(p_idx*4,p_idx*12,4,12) = tensorProduct(I4,X); - // Matrix Q - Eigen::MatrixXd Qtmp = Eigen::MatrixXd::Zero(12,18); - Qtmp.block(0,0,6,18) = tensorProduct(I6,U1); - Qtmp.block(6,0,6,18) = tensorProduct(I6,V1); - Q.block(p_idx*12,p_idx*18,12,18) = Qtmp; - // Matrix L - Eigen::MatrixXd Ltmp = Eigen::MatrixXd::Zero(18,27); - Ltmp.block(0,0,9,27) = tensorProduct(I9,U2); - Ltmp.block(9,0,9,27) = tensorProduct(I9,V2); - L.block(p_idx*18,0,18,27) = Ltmp; - } - - // SVD of QL - Eigen::JacobiSVD<Eigen::MatrixXd> svd(Q*L, 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 - double cond = svd.singularValues()(0) / svd.singularValues()(svd.singularValues().size()-1); - if (std::isnan(cond) || cond < 1e-6) - return false; - const Eigen::MatrixXd U = svd.matrixU(); - const Eigen::MatrixXd D_diag = svd.singularValues(); - Eigen::MatrixXd V = svd.matrixV(); - Eigen::MatrixXd D = Eigen::MatrixXd::Zero(27,27); - for (int ii=0;ii<27;++ii) - D(ii,ii)=D_diag(ii); - - // 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 - cond = svd_pu.singularValues()(0) / svd_pu.singularValues()(svd_pu.singularValues().size()-1); - 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(); - - // Compute tensor - Eigen::VectorXd tensorVector = (D*V.inverse()).inverse()*V_pu.col(26); - - fillTensor(tensorVector); - memcpy( tensor_final, tensor_, 27*sizeof(int) ); - - comp_time_ = (double)(clock() - tStart) / CLOCKS_PER_SEC; - - return true; -} - -bool TrifocalTensor::computeTensorRansac(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3) -{ - clock_t tStart = clock(); - - int min_points = 8; - - // Checks - if (list1.rows()<min_points || list2.rows()<min_points || list3.rows()<min_points) - { - VU_ERROR("[computeTensorRansac] : Not enough matched points. Min = 8."); - return false; - } - if (max_iterations_<0 || max_reprojection_error_<0 || percentage_of_correct_reprojected_points_<0) - { - VU_ERROR("[computeTensorRansac] : Cannot use RANSAC without defining max_iterations, max_reprojection_error and percentage_of_correct_reprojected_points in while constructing the tensor object."); - return false; - } - - double percentage_of_correct_reprojected_points = 0; - int iter = 0; - float tensor_final[3][3][3]; - - // DEBUG - Eigen::VectorXd idxs_best; - - while (iter<max_iterations_ && percentage_of_correct_reprojected_points<percentage_of_correct_reprojected_points_) - { - iter=iter+1; - - Eigen::MatrixXd l1(min_points,3), l2(min_points,3), l3(min_points,3); - Eigen::VectorXd idxs = getRandomCandidates(min_points, list1, list2, list3, l1, l2, l3); - - Eigen::MatrixXd A; - computeMatrixA(l1, l2, l3, A); - - Eigen::VectorXd tensorVector; - if ( resolveEquationWithSVD(A, tensorVector) ) - { - fillTensor(tensorVector); - - double percentage = getErrorTrifocal(list1, list2, list3); - - if (percentage_of_correct_reprojected_points<percentage) - { - percentage_of_correct_reprojected_points = percentage; - memcpy( tensor_final, tensor_, 27*sizeof(int) ); - } - } - else - return false; - } - memcpy( tensor_, tensor_final, 27*sizeof(int) ); - - comp_time_ = (double)(clock() - tStart) / CLOCKS_PER_SEC; - - return true; -} - -void TrifocalTensor::computeMatrixA(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3, Eigen::MatrixXd& A) -{ - // DLT algorithm (check summary in - //L. Wang and F. C. Wu, "A factorization algorithm for trifocal tensor estimation," - //2008 19th International Conference on Pattern Recognition, Tampa, FL, 2008, pp. 1-4. - - size_t nbCorrespondence = list1.rows(); - // fill A with zeros - A.setZero(4 * nbCorrespondence, 27); - // for each correspondence (= 3 pixel, 1 per image) - for(size_t p_idx = 0; p_idx<nbCorrespondence; ++p_idx) { - // we change 12 (4 * 3) elements per row. cimg1 are coordinate sin image 1 - for(size_t cimg1 = 0; cimg1<3; ++cimg1) { - // we have 4 equations per correspondence. cimg2 are coordinates in image 2 - for(size_t cimg2 = 0; cimg2<2; ++cimg2) { - // iterate over coordinates in image 3 (cimg3) - for(size_t cimg3 = 0; cimg3<2; ++cimg3) { - A(4*p_idx + 2*cimg2 + cimg3, 9*cimg1 + 3*2 + cimg3) = list1(p_idx, cimg1) * list2(p_idx, cimg2) * list3(p_idx, 2); - A(4*p_idx + 2*cimg2 + cimg3, 9*cimg1 + 3*cimg2 + cimg3) = -list1(p_idx, cimg1) * list2(p_idx, 2) * list3(p_idx, 2); - A(4*p_idx + 2*cimg2 + cimg3, 9*cimg1 + 3*2 + 2) = -list1(p_idx, cimg1) * list2(p_idx, cimg2) * list3(p_idx, cimg3); - A(4*p_idx + 2*cimg2 + cimg3, 9*cimg1 + 3*cimg2 + 2) = list1(p_idx, cimg1) * list2(p_idx, 2) * list3(p_idx, cimg3); - } - } - } - } -} - -bool TrifocalTensor::resolveEquationWithSVD(Eigen::MatrixXd& A, Eigen::VectorXd& tensorVector) -{ - Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, 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 - double cond = svd.singularValues()(0) / svd.singularValues()(svd.singularValues().size()-1); - - if (std::isnan(cond) || cond < 1e-6) - return false; - - const Eigen::MatrixXd U = svd.matrixU(); - const Eigen::VectorXd D_diag = svd.singularValues(); - Eigen::MatrixXd V = svd.matrixV(); - - tensorVector.Zero(27); - tensorVector = V.col(26); - - return true; -} - - -void TrifocalTensor::computeMatrixB(const Eigen::Vector3d& p1, const Eigen::Vector3d& p2, Eigen::MatrixXd& B, Eigen::VectorXd& b) -{ - B.setZero(4, 2); - b.setZero(4, 1); - for(size_t ii = 0; ii<2; ++ii) { - for(size_t ll = 0; ll<2; ++ll) { - for(size_t kk = 0; kk<3; ++kk) { - //fill the 1st and 2nd column of B - B(2*ii+ll, ll) += p1(kk) * (p2(2) * tensor_[ii][2][kk] - p2(ii) * tensor_[2][2][kk]);//compute coefficients for x''1 and x''2 - //fill b - b(2*ii+ll) += -p1(kk) * (p2(ii) * tensor_[2][ll][kk] - p2(2)* tensor_[ii][ll][kk]); - } - } - } -} - -void TrifocalTensor::transfer(const Eigen::Vector3d& p1 , Eigen::Vector3d& p2, Eigen::Vector3d& p3) { - - Eigen::MatrixXd B; - Eigen::VectorXd 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; - searchedPoint.Zero(2); - // returns the solution x in Bx=b where x has two coordinates - searchedPoint = svd.solve(b); - - 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) -{ - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(A.rows()*B.rows(),A.cols()*B.cols()); - for (int ra=0;ra<A.rows();ra++) - { - for (int ca=0;ca<A.cols();ca++) - { - C.block(ra*B.rows(),ca*B.cols(),B.rows(),B.cols()) = A(ra,ca)*B; - } - } - 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 */ diff --git a/src/common_class/trifocaltensor.h b/src/common_class/trifocaltensor.h index 2037b88a21f11207a11d95097fb7580a06c91372..73574f9a8b60de1a602a0bceb21a85f131e9ea58 100644 --- a/src/common_class/trifocaltensor.h +++ b/src/common_class/trifocaltensor.h @@ -20,8 +20,6 @@ public: double comp_time_; // Detection time - std::vector<std::vector<int> > getAllComb(const int& nelements); - /** * \brief Constructor */ @@ -74,7 +72,7 @@ private: /** * \brief Initialize Class variables */ - void iniClassVars(void); + void iniClassVars(const int& max_iterations, const double& max_reprojection_error, const double& percentage_of_correct_reprojected_points); /** * \brief Overloading () for a easy acces to the elements of the tensor @@ -117,6 +115,461 @@ private: void normalizeIsotrop(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3, Eigen::MatrixXd& l1, Eigen::MatrixXd& l2, Eigen::MatrixXd& l3); }; +inline TrifocalTensor::TrifocalTensor() +{ + iniClassVars(-1,-1,-1); +} + +inline TrifocalTensor::TrifocalTensor(const int& max_iterations, const double& max_reprojection_error, const double& percentage_of_correct_reprojected_points) +{ + if (percentage_of_correct_reprojected_points<0.0 || percentage_of_correct_reprojected_points>1.0) + VU_ERROR("Incorrect parameter: percentage_of_correct_reprojected_points. Values must be between 0~1"); + + 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) +{ + iniClassVars(-1,-1,-1); + + computeTensorFromProjectionMat(P1,P2,P3); +} + + +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; + + for(size_t kk = 0; kk<3; ++kk) + for(size_t ii = 0; ii<3; ++ii) + for(size_t jj = 0; jj<3; ++jj) + tensor_[ii][jj][kk] = 0; +} + +inline float TrifocalTensor::operator()(size_t row, size_t col, size_t depth) +{ + return tensor_[row][col][depth]; +} + +inline void TrifocalTensor::print() const +{ + std::cout << "Trifocal tensor : " << std::endl; + for(size_t ii = 0; ii<3; ++ii) + { + std::cout << " [" << tensor_[ii][0][0] << "," << tensor_[ii][1][0] << "," << tensor_[ii][2][0] << "] "; + std::cout << "[" << tensor_[ii][0][1] << "," << tensor_[ii][1][1] << "," << tensor_[ii][2][1] << "] "; + std::cout << "[" << tensor_[ii][0][2] << "," << tensor_[ii][1][2] << "," << tensor_[ii][2][2] << "]" << std::endl; + } +} + +inline void TrifocalTensor::fillTensor(const Eigen::VectorXd& tensorVector) +{ + for(size_t kk = 0; kk<3; ++kk) { + for(size_t ii = 0; ii<3; ++ii) { + for(size_t jj = 0; jj<3; ++jj) { + tensor_[ii][jj][kk] = tensorVector(jj + ii*3 + kk*9); + } + } + } +} + +inline void TrifocalTensor::getLayers(Eigen::Matrix3d& T1, Eigen::Matrix3d& T2, Eigen::Matrix3d& T3) +{ + for(size_t ii = 0; ii<3; ++ii) + { + for(size_t jj = 0; jj<3; ++jj) + { + T1(ii,jj) = tensor_[ii][jj][0]; + T2(ii,jj) = tensor_[ii][jj][1]; + T3(ii,jj) = tensor_[ii][jj][2]; + } + } +} + +inline Eigen::VectorXd TrifocalTensor::getRandomCandidates(const int& min_points, const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3, Eigen::MatrixXd& l1, Eigen::MatrixXd& l2, Eigen::MatrixXd& l3) +{ + std::random_device rd; //Will be used to obtain a seed for the random number engine + std::mt19937 gen(rd()); //Standard mersenne_twister_engine seeded with rd() + std::uniform_int_distribution<> dis(0, list1.rows()-1); + + Eigen::VectorXd idx = Eigen::VectorXd::Zero(min_points); + for (int n=0; n<min_points; ++n) + { + int num = dis(gen); + idx(n) = num; + for (int ii=0; ii<n; ++ii) + { + if (idx(ii)==num) + { + n = n-1; + break; + } + } + } + + l1 = Eigen::MatrixXd::Zero(min_points,3); + l2 = Eigen::MatrixXd::Zero(min_points,3); + l3 = Eigen::MatrixXd::Zero(min_points,3); + + for (int ii=0; ii<l1.rows(); ++ii) + { + l1.row(ii) = list1.row(idx(ii)); + l2.row(ii) = list2.row(idx(ii)); + l3.row(ii) = list3.row(idx(ii)); + } + + return idx; +} + +inline double TrifocalTensor::getErrorTrifocal(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3) +{ + double count = 0.0; + for (int ii=0; ii<list1.rows(); ++ii) + { + Eigen::Vector3d p1,p2,p3,p3est; + p1 << list1(ii,0),list1(ii,1),list1(ii,2); + p2 << list2(ii,0),list2(ii,1),list2(ii,2); + p3 << list3(ii,0),list3(ii,1),list3(ii,2); + transfer(p1, p2, p3est); + p3est = p3est/p3est(2); + Eigen::Vector3d res = p3est - p3; + if (res.norm() < max_reprojection_error_) + count = count+1; + } + + return count/list1.rows(); +} + +inline bool TrifocalTensor::computeTensor(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3) +{ + clock_t tStart = clock(); + + float tensor_final[3][3][3]; + + // Normalize points + Eigen::MatrixXd l1(list1), l2(list2), l3(list3); +// normalizeIsotrop(list1, list2, list3, l1, l2, l3); + + Eigen::MatrixXd A; + computeMatrixA(l1, l2, l3, A); + + Eigen::VectorXd tensorVector; + if ( resolveEquationWithSVD(A, tensorVector) ) + { + fillTensor(tensorVector); + + memcpy( tensor_final, tensor_, 27*sizeof(int) ); + } + else + return false; + + memcpy( tensor_, tensor_final, 27*sizeof(int) ); + + comp_time_ = (double)(clock() - tStart) / CLOCKS_PER_SEC; + + return true; +} + +inline bool TrifocalTensor::computeTensorFromProjectionMat(const Eigen::MatrixXd& P1, const Eigen::MatrixXd& P2, const Eigen::MatrixXd& 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)) + { + c2Pc1 = P2; + c3Pc1 = P3; + } + else + { + Eigen::Matrix3d wRc1 = P1.block(0,0,3,3); + Eigen::Matrix3d wRc2 = P2.block(0,0,3,3); + Eigen::Matrix3d wRc3 = P3.block(0,0,3,3); + Eigen::Vector3d wtc1 = P1.block(0,3,3,1); + Eigen::Vector3d wtc2 = P2.block(0,3,3,1); + Eigen::Vector3d wtc3 = P3.block(0,3,3,1); + + Eigen::Matrix3d c1Rc2 = wRc1.transpose()*wRc2; + Eigen::Matrix3d c1Rc3 = wRc1.transpose()*wRc3; + Eigen::Vector3d c1tc2 = wRc1.transpose()*(wtc2-wtc1); + Eigen::Vector3d c1tc3 = wRc1.transpose()*(wtc3-wtc1); + + c2Pc1.block(0,0,3,3) = c1Rc2.transpose(); + c2Pc1.block(0,3,3,1) = -c1Rc2.transpose()*c1tc2; + c3Pc1.block(0,0,3,3) = c1Rc3.transpose(); + c3Pc1.block(0,3,3,1) = -c1Rc3.transpose()*c1tc3; + } + + + // Trifocal tensor from Projection matrices (c1->c2 and c1->c3) + for(size_t ii = 0; ii<3; ++ii) + for(size_t jj = 0; jj<3; ++jj) + for(size_t kk = 0; kk<3; ++kk) + tensor_[kk][jj][ii] = c2Pc1(kk,jj)*c3Pc1(ii,3)-c2Pc1(kk,3)*c3Pc1(ii,jj); + + return true; +} + +inline bool TrifocalTensor::computeTensorFactorized(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3) +{ + clock_t tStart = clock(); + + float tensor_final[3][3][3]; + + // Normalize points + Eigen::MatrixXd l1(list1), l2(list2), l3(list3); + normalizeIsotrop(list1, list2, list3, l1, l2, l3); + + int np = l1.rows(); + Eigen::MatrixXd P = Eigen::MatrixXd::Zero(4*np,12*np); + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(12*np,18*np); + Eigen::MatrixXd L = Eigen::MatrixXd::Zero(18*np,27); + Eigen::MatrixXd I4 = Eigen::MatrixXd::Identity(4,4); + Eigen::MatrixXd I6 = Eigen::MatrixXd::Identity(6,6); + Eigen::MatrixXd I9 = Eigen::MatrixXd::Identity(9,9); + + // for each correspondence (= 3 pixel, 1 per image) + for(size_t p_idx = 0; p_idx<np; ++p_idx) + { + // Factorized method + Eigen::MatrixXd X(1,3),U1(1,3),V1(1,3),U2(1,3),V2(1,3); + X << l1.row(p_idx); + U1 << 1,0,-l2(p_idx,0); + V1 << 0,1,-l2(p_idx,1); + U2 << -1,0,l3(p_idx,0); + V2 << 0,-1,l3(p_idx,1); + + // Matrix P + P.block(p_idx*4,p_idx*12,4,12) = tensorProduct(I4,X); + // Matrix Q + Eigen::MatrixXd Qtmp = Eigen::MatrixXd::Zero(12,18); + Qtmp.block(0,0,6,18) = tensorProduct(I6,U1); + Qtmp.block(6,0,6,18) = tensorProduct(I6,V1); + Q.block(p_idx*12,p_idx*18,12,18) = Qtmp; + // Matrix L + Eigen::MatrixXd Ltmp = Eigen::MatrixXd::Zero(18,27); + Ltmp.block(0,0,9,27) = tensorProduct(I9,U2); + Ltmp.block(9,0,9,27) = tensorProduct(I9,V2); + L.block(p_idx*18,0,18,27) = Ltmp; + } + + // SVD of QL + Eigen::JacobiSVD<Eigen::MatrixXd> svd(Q*L, 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 + double cond = svd.singularValues()(0) / svd.singularValues()(svd.singularValues().size()-1); + if (std::isnan(cond) || cond < 1e-6) + return false; + const Eigen::MatrixXd U = svd.matrixU(); + const Eigen::MatrixXd D_diag = svd.singularValues(); + Eigen::MatrixXd V = svd.matrixV(); + Eigen::MatrixXd D = Eigen::MatrixXd::Zero(27,27); + for (int ii=0;ii<27;++ii) + D(ii,ii)=D_diag(ii); + + // 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 + cond = svd_pu.singularValues()(0) / svd_pu.singularValues()(svd_pu.singularValues().size()-1); + 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(); + + // Compute tensor + Eigen::VectorXd tensorVector = (D*V.inverse()).inverse()*V_pu.col(26); + + fillTensor(tensorVector); + memcpy( tensor_final, tensor_, 27*sizeof(int) ); + + comp_time_ = (double)(clock() - tStart) / CLOCKS_PER_SEC; + + return true; +} + +inline bool TrifocalTensor::computeTensorRansac(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3) +{ + clock_t tStart = clock(); + + int min_points = 8; + + // Checks + if (list1.rows()<min_points || list2.rows()<min_points || list3.rows()<min_points) + { + VU_ERROR("[computeTensorRansac] : Not enough matched points. Min = 8."); + return false; + } + if (max_iterations_<0 || max_reprojection_error_<0 || percentage_of_correct_reprojected_points_<0) + { + VU_ERROR("[computeTensorRansac] : Cannot use RANSAC without defining max_iterations, max_reprojection_error and percentage_of_correct_reprojected_points in while constructing the tensor object."); + return false; + } + + double percentage_of_correct_reprojected_points = 0; + int iter = 0; + float tensor_final[3][3][3]; + + // DEBUG + Eigen::VectorXd idxs_best; + + while (iter<max_iterations_ && percentage_of_correct_reprojected_points<percentage_of_correct_reprojected_points_) + { + iter=iter+1; + + Eigen::MatrixXd l1(min_points,3), l2(min_points,3), l3(min_points,3); + Eigen::VectorXd idxs = getRandomCandidates(min_points, list1, list2, list3, l1, l2, l3); + + Eigen::MatrixXd A; + computeMatrixA(l1, l2, l3, A); + + Eigen::VectorXd tensorVector; + if ( resolveEquationWithSVD(A, tensorVector) ) + { + fillTensor(tensorVector); + + double percentage = getErrorTrifocal(list1, list2, list3); + + if (percentage_of_correct_reprojected_points<percentage) + { + percentage_of_correct_reprojected_points = percentage; + memcpy( tensor_final, tensor_, 27*sizeof(int) ); + } + } + else + return false; + } + memcpy( tensor_, tensor_final, 27*sizeof(int) ); + + comp_time_ = (double)(clock() - tStart) / CLOCKS_PER_SEC; + + return true; +} + +inline void TrifocalTensor::computeMatrixA(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3, Eigen::MatrixXd& A) +{ + // DLT algorithm (check summary in + //L. Wang and F. C. Wu, "A factorization algorithm for trifocal tensor estimation," + //2008 19th International Conference on Pattern Recognition, Tampa, FL, 2008, pp. 1-4. + + size_t nbCorrespondence = list1.rows(); + // fill A with zeros + A.setZero(4 * nbCorrespondence, 27); + // for each correspondence (= 3 pixel, 1 per image) + for(size_t p_idx = 0; p_idx<nbCorrespondence; ++p_idx) { + // we change 12 (4 * 3) elements per row. cimg1 are coordinate sin image 1 + for(size_t cimg1 = 0; cimg1<3; ++cimg1) { + // we have 4 equations per correspondence. cimg2 are coordinates in image 2 + for(size_t cimg2 = 0; cimg2<2; ++cimg2) { + // iterate over coordinates in image 3 (cimg3) + for(size_t cimg3 = 0; cimg3<2; ++cimg3) { + A(4*p_idx + 2*cimg2 + cimg3, 9*cimg1 + 3*2 + cimg3) = list1(p_idx, cimg1) * list2(p_idx, cimg2) * list3(p_idx, 2); + A(4*p_idx + 2*cimg2 + cimg3, 9*cimg1 + 3*cimg2 + cimg3) = -list1(p_idx, cimg1) * list2(p_idx, 2) * list3(p_idx, 2); + A(4*p_idx + 2*cimg2 + cimg3, 9*cimg1 + 3*2 + 2) = -list1(p_idx, cimg1) * list2(p_idx, cimg2) * list3(p_idx, cimg3); + A(4*p_idx + 2*cimg2 + cimg3, 9*cimg1 + 3*cimg2 + 2) = list1(p_idx, cimg1) * list2(p_idx, 2) * list3(p_idx, cimg3); + } + } + } + } +} + +inline bool TrifocalTensor::resolveEquationWithSVD(Eigen::MatrixXd& A, Eigen::VectorXd& tensorVector) +{ + Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, 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 + double cond = svd.singularValues()(0) / svd.singularValues()(svd.singularValues().size()-1); + + if (std::isnan(cond) || cond < 1e-6) + return false; + + const Eigen::MatrixXd U = svd.matrixU(); + const Eigen::VectorXd D_diag = svd.singularValues(); + Eigen::MatrixXd V = svd.matrixV(); + + tensorVector.Zero(27); + tensorVector = V.col(26); + + return true; +} + + +inline void TrifocalTensor::computeMatrixB(const Eigen::Vector3d& p1, const Eigen::Vector3d& p2, Eigen::MatrixXd& B, Eigen::VectorXd& b) +{ + B.setZero(4, 2); + b.setZero(4, 1); + for(size_t ii = 0; ii<2; ++ii) { + for(size_t ll = 0; ll<2; ++ll) { + for(size_t kk = 0; kk<3; ++kk) { + //fill the 1st and 2nd column of B + B(2*ii+ll, ll) += p1(kk) * (p2(2) * tensor_[ii][2][kk] - p2(ii) * tensor_[2][2][kk]);//compute coefficients for x''1 and x''2 + //fill b + b(2*ii+ll) += -p1(kk) * (p2(ii) * tensor_[2][ll][kk] - p2(2)* tensor_[ii][ll][kk]); + } + } + } +} + +inline void TrifocalTensor::transfer(const Eigen::Vector3d& p1 , Eigen::Vector3d& p2, Eigen::Vector3d& p3) { + + Eigen::MatrixXd B; + Eigen::VectorXd 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; + searchedPoint.Zero(2); + // returns the solution x in Bx=b where x has two coordinates + searchedPoint = svd.solve(b); + + Eigen::Vector3d pn3; + pn3(0) = searchedPoint[0]; + pn3(1) = searchedPoint[1]; + pn3(2) = 1; + + // reverse normalization if any + p3 = T_norm_.inverse()*pn3; +} + +inline Eigen::MatrixXd TrifocalTensor::tensorProduct(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B) +{ + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(A.rows()*B.rows(),A.cols()*B.cols()); + for (int ra=0;ra<A.rows();ra++) + { + for (int ca=0;ca<A.cols();ca++) + { + C.block(ra*B.rows(),ca*B.cols(),B.rows(),B.cols()) = A(ra,ca)*B; + } + } + 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) +{ + 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 */ #endif /* _TRIFOCALTENSOR_H_ */ diff --git a/src/test/gtest_algorithm_residualtrilinearity.cpp b/src/test/gtest_algorithm_residualtrilinearity.cpp new file mode 100644 index 0000000000000000000000000000000000000000..520929e27faa082667b01846897a27fdc61d84f1 --- /dev/null +++ b/src/test/gtest_algorithm_residualtrilinearity.cpp @@ -0,0 +1,63 @@ +#include "utils_gtest.h" + +// vision utils includes +#include "../vision_utils.h" +#include "../algorithms.h" +#include "../common_class/trifocaltensor.h" +#include "../common_class/rotations.h" + +TEST(ResidualTrilinearity, getResidualEpipolarLinesAndPLP) +{ + // Point in three images (normalized 3D coordinates, not projected) + Eigen::MatrixXd feat(3,3); + feat.col(0) << 0.0,-0.088388347648318,1.0; + feat.col(1) << 0.139866460329444, 0.078845527832412, 1.0; + feat.col(2) << -0.115385095362789,-0.267742935981736,1.0; + + // Instrinsics + Eigen::Matrix3d K; + K.row(0) << 426.6667, 0.0, 320.0000; + K.row(1) << 0.0, 426.6667, 240.0000; + K.row(2) << 0.0, 0.0, 1.0; + + // Covariance + double cov = 1.0; // 1 pixel + Eigen::Matrix3d Q = cov*Eigen::Matrix3d::Identity(); + Eigen::MatrixXd feat_cov = K.inverse()*Q*K.inverse().transpose(); + + //Extrisinsics + Eigen::Vector3d wtc1, wtc2, wtc3; + wtc1 << 0,0,5; + wtc2 << 5,0,6; + wtc3 << 10,0,4; + + Eigen::Vector3d angc1, angc2, angc3; + angc1 << -90.0,0.0,-45.0; + angc1 = angc1*M_PI/180.0; + angc2 << -90.0,20.0,20.0; + angc2 = angc2*M_PI/180.0; + angc3 << -90.0,-20.0,45.0; + angc3 = angc3*M_PI/180.0; + Eigen::Matrix3d wRc1 = vision_utils::matrixRollPitchYaw(angc1(0),angc1(1),angc1(2)); + Eigen::Matrix3d wRc2 = vision_utils::matrixRollPitchYaw(angc2(0),angc2(1),angc2(2)); + Eigen::Matrix3d wRc3 = vision_utils::matrixRollPitchYaw(angc3(0),angc3(1),angc3(2)); + + // Create algorithm and cast it to the correct type (residual trilinearity) + vision_utils::AlgorithmParamsRESIDUALTRILINEARITYPtr params_ptr = std::make_shared<vision_utils::AlgorithmParamsRESIDUALTRILINEARITY>(); + std::string def_alg = "RESIDUALTRILINEARITY"; + vision_utils::AlgorithmBasePtr alg_ptr = setupAlgorithm(def_alg, def_alg + " algorithm", params_ptr); + vision_utils::AlgorithmRESIDUALTRILINEARITYPtr residual_alg_ptr = std::static_pointer_cast<vision_utils::AlgorithmRESIDUALTRILINEARITY>(alg_ptr); + + // Get residual using contraint: [epipolar line c1c2; epipolar line c2c3; point-line-point trilinearity] + Eigen::MatrixXd residual(4,1); + Eigen::MatrixXd residual_cov(4,4); + residual_alg_ptr->getResidualEpipolarLinesAndPLP(wRc1, wtc1, wRc2, wtc2, wRc3, wtc3, feat, feat_cov, residual, residual_cov, true); + + ASSERT_TRUE(residual(0,0)<vision_utils::EPS && residual(1,0)<vision_utils::EPS && residual(2,0)<vision_utils::EPS && residual(3,0)<vision_utils::EPS); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}