WIP: Adding trifocal tensor

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)

src/common_class/trifocaltensor.cpp

+#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 */

src/common_class/trifocaltensor.h

+#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_ */
+

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>