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();
+}