Skip to content
Snippets Groups Projects
Commit 945e7738 authored by Angel Santamaria-Navarro's avatar Angel Santamaria-Navarro
Browse files

Before templatizing trifocaltensor header

parent 987b0fcb
No related branches found
No related tags found
No related merge requests found
...@@ -18,7 +18,6 @@ ENDIF() ...@@ -18,7 +18,6 @@ ENDIF()
# library source files # library source files
SET(sources SET(sources
vision_utils.cpp vision_utils.cpp
common_class/trifocaltensor.cpp
sensors/sensor_base.cpp sensors/sensor_base.cpp
sensors/usb_cam/usb_cam.cpp sensors/usb_cam/usb_cam.cpp
sensors/usb_cam/usb_cam_load_yaml.cpp sensors/usb_cam/usb_cam_load_yaml.cpp
......
#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 */
...@@ -20,8 +20,6 @@ public: ...@@ -20,8 +20,6 @@ public:
double comp_time_; // Detection time double comp_time_; // Detection time
std::vector<std::vector<int> > getAllComb(const int& nelements);
/** /**
* \brief Constructor * \brief Constructor
*/ */
...@@ -74,7 +72,7 @@ private: ...@@ -74,7 +72,7 @@ private:
/** /**
* \brief Initialize Class variables * \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 * \brief Overloading () for a easy acces to the elements of the tensor
...@@ -117,6 +115,461 @@ private: ...@@ -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); 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 */ } /* namespace vision_utils */
#endif /* _TRIFOCALTENSOR_H_ */ #endif /* _TRIFOCALTENSOR_H_ */
......
#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();
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment