trifocaltensor.cpp

#include "trifocaltensor.h"

#include <cstdlib>
#include <iostream>

namespace vision_utils {

TrifocalTensor::TrifocalTensor(const int& max_iterations, const double& max_reprojection_error, const double& percentage_of_correct_reprojected_points):
		comp_time_(0),
		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");

	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;
}

TrifocalTensor::TrifocalTensor():
		comp_time_(0),
		max_iterations_(-1),
		max_reprojection_error_(-1),
		percentage_of_correct_reprojected_points_(-1)
{
	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;
}

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;
	}
}

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

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);

	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::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) );

	// DEBUG
	std::cout << "=============" << std::endl;
	std::cout << "best point idxs: " << idxs_best.transpose() << std::endl;
	std::cout << "=============" << std::endl;

	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)
{
	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 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*p_idx + 2*ii + ll, 9*kk + 3*2  + ll) = list1(p_idx, kk) * list2(p_idx, ii) * list3(p_idx, 2);
					A(4*p_idx + 2*ii + ll, 9*kk + 3*ii + ll) = -list1(p_idx, kk) * list2(p_idx, 2) * list3(p_idx, 2);
					A(4*p_idx + 2*ii + ll, 9*kk + 3*2  + 2) = -list1(p_idx, kk) * list2(p_idx, ii) * list3(p_idx, ll);
					A(4*p_idx + 2*ii + ll, 9*kk + 3*ii + 2) = list1(p_idx, kk) * list2(p_idx, 2) * list3(p_idx, ll);
				}
			}
		}
	}
}

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;
	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 */