diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 8525bbb609e6012bff4cb182c33d509979431891..6000975813f082960f579f3d3d12a54f77212d61 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -18,6 +18,7 @@ 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 @@ -99,7 +100,8 @@ SET(headers_main algorithms.h) SET(headers_com common_class/buffer.h - common_class/frame.h) + common_class/frame.h + common_class/trifocaltensor.h) SET(headers_sen sensors/sensor_factory.h sensors/sensor_base.h) diff --git a/src/common_class/trifocaltensor.cpp b/src/common_class/trifocaltensor.cpp new file mode 100644 index 0000000000000000000000000000000000000000..2afe25d3f140e8324a5cd52b676adba0fd191f62 --- /dev/null +++ b/src/common_class/trifocaltensor.cpp @@ -0,0 +1,127 @@ +#include "trifocaltensor.h" + +#include <cstdlib> +#include <iostream> + +namespace vision_utils { + +TrifocalTensor::TrifocalTensor() +{ + for(size_t kk = 0; kk<3; ++kk) + for(size_t jj = 0; jj<3; ++jj) + for(size_t ii = 0; ii<3; ++ii) + tensor_[ii][jj][kk] = 0; +} + +TrifocalTensor::~TrifocalTensor() +{ +} + +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; + } +} + +// compute elements of the tensor +void TrifocalTensor::computeTensor(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3) +{ + Eigen::MatrixXd A; + computeMatrixA(list1, list2, list3, A); + // resolve equation : A*t = 0 where t is unknown + Eigen::VectorXd tensorVector = resolveEquationWithSVD(A, 26); + fillTensor(tensorVector); +} + +void TrifocalTensor::computeMatrixA(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3, Eigen::MatrixXd& A) +{ + size_t nbCorrespondence = list1.rows(); + // fill A by zeros + A.setZero(4 * nbCorrespondence, 27); + // for each correspondence (= 3 pixel, 1 per image) + for(size_t pp = 0; pp<nbCorrespondence; ++pp) { + // we have 4 equations per correspondence + for(size_t ii = 0; ii<2; ++ii) { + for(size_t ll = 0; ll<2; ++ll) { + // we change 12 (4 * 3) elements per row + for(size_t kk = 0; kk<3; ++kk) { + A(4*pp + 2*ii + ll, 9*kk + 3*2 + ll) = list1(pp, kk) * list2(pp, ii) * list3(pp, 2); + A(4*pp + 2*ii + ll, 9*kk + 3*ii + ll) = -list1(pp, kk) * list2(pp, 2) * list3(pp, 2); + A(4*pp + 2*ii + ll, 9*kk + 3*2 + 2) = -list1(pp, kk) * list2(pp, ii) * list3(pp, ll); + A(4*pp + 2*ii + ll, 9*kk + 3*ii + 2) = list1(pp, kk) * list2(pp, 2) * list3(pp, ll); + } + } + } + } +} + +Eigen::VectorXd TrifocalTensor::resolveEquationWithSVD(Eigen::MatrixXd& A, size_t indexOfLastColumnOfV) +{ + Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV); + + const Eigen::MatrixXd U = svd.matrixU(); + const Eigen::VectorXd D_diag = svd.singularValues(); + Eigen::MatrixXd V = svd.matrixV(); + + Eigen::VectorXd tensorVector; + tensorVector.Zero(27); + tensorVector = V.col(indexOfLastColumnOfV); + + return tensorVector; +} + +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::computeMatrixB(const float* p1, const float* p2, Eigen::MatrixXd& B, Eigen::VectorXd& b) +{ + B.setZero(4, 2); + b.setZero(4, 1); + for(size_t i = 0; i<2; ++i) { + for(size_t l = 0; l<2; ++l) { + for(size_t k = 0; k<3; ++k) { + //fill the 1st and 2nd column of B + B(2*i+l, l) += p1[k] * (p2[2] * tensor_[i][2][k] - p2[i] * tensor_[2][2][k]);//compute coefficients for x''1 and x''2 + //fill b + b(2*i+l) += - p1[k] * (p2[i] * tensor_[2][l][k] - p2[2]* tensor_[i][l][k]); + + } + } + } +} + +void TrifocalTensor::transfert(const float* p1 , const float* p2, float* p3) { + Eigen::MatrixXd B; + Eigen::VectorXd b; + computeMatrixB(p1, p2, 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); + + p3[0] = searchedPoint[0]; + p3[1] = searchedPoint[1]; + p3[2] = 1; +} + +} /* namespace vision_utils */ diff --git a/src/common_class/trifocaltensor.h b/src/common_class/trifocaltensor.h new file mode 100644 index 0000000000000000000000000000000000000000..6f68217354d4b5522fe9a82df8e2fd0be94df52c --- /dev/null +++ b/src/common_class/trifocaltensor.h @@ -0,0 +1,76 @@ +#ifndef _TRIFOCALTENSOR_H_ +#define _TRIFOCALTENSOR_H_ + +#include <Eigen/Dense> + +#include "../vision_utils.h" + +namespace vision_utils { + +VU_PTR_TYPEDEFS(TrifocalTensor); + +/** + * \brief TrifocalTensor class + */ +class TrifocalTensor { +public: + + /** + * \brief Constructor + */ + TrifocalTensor(); + + /** + * \brief Destructor + */ + virtual ~TrifocalTensor(); + + /** + * \brief Compute trifocal tensor + */ + void computeTensor(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3); + +private: + + float tensor_[3][3][3]; // Main Tensor matrix (3x3x3) + + /** + * \brief Overloading () for a easy acces to the elements of the tensor + */ + float operator()(size_t row, size_t col, size_t depth); + + /** + * \brief Compute elements of the matrix A in the equation A*tensor = 0 + */ + void computeMatrixA(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3, Eigen::MatrixXd& A); + + /** + * \brief Resolve equation by finding tensor in A*tensor = 0 excluding the solution t = 0 with SVD decomposition + */ + Eigen::VectorXd resolveEquationWithSVD(Eigen::MatrixXd& A, size_t indexOfLastColumnOfV); + + /** + * \brief Fill elements of the tensor with the solution found + */ + void fillTensor(const Eigen::VectorXd& t); + + /** + * \brief Compute B matrix and b vector in the equation Bx = b from the coordinates of two pixels + */ + void computeMatrixB(const float* p1, const float* p2, Eigen::MatrixXd& B, Eigen::VectorXd& b); + + /** + * \brief Compute the coordinates of the point p3 on third img corresponding to p1 and p2 of others images + */ + void transfert(const float* p1 , const float* p2, float* p3); + + /** + * \biref Display the tensor, just for debug + */ + void print() const; +}; + +} /* namespace vision_utils */ + +#endif /* _TRIFOCALTENSOR_H_ */ + diff --git a/src/vision_utils.h b/src/vision_utils.h index 180f56863f50a72b9ced9a968953d53f42319a3a..f81f989653fdcacfb0e1b31a0f09b72109513b1a 100644 --- a/src/vision_utils.h +++ b/src/vision_utils.h @@ -26,6 +26,8 @@ // OpenCV #include <opencv2/opencv.hpp> +#include <opencv2/imgproc.hpp> +#include <opencv2/highgui.hpp> #include <opencv2/core/eigen.hpp> #include <opencv2/line_descriptor.hpp> #include <opencv2/xfeatures2d.hpp>