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

adding trifocal tensor factorized

parent 0dd7c0bf
No related branches found
No related tags found
No related merge requests found
......@@ -138,6 +138,9 @@ bool TrifocalTensor::computeTensor(const Eigen::MatrixXd& list1, const Eigen::Ma
// Normalize points
Eigen::MatrixXd l1(list1), l2(list2), l3(list3);
// Eigen::MatrixXd T1 = normalizePointsIsotrop(list1, l1);
// Eigen::MatrixXd T2 = normalizePointsIsotrop(list1, l2);
// Eigen::MatrixXd T3 = normalizePointsIsotrop(list1, l3);
Eigen::MatrixXd A;
computeMatrixA(l1, l2, l3, A);
......@@ -159,6 +162,96 @@ bool TrifocalTensor::computeTensor(const Eigen::MatrixXd& list1, const Eigen::Ma
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);
// Eigen::MatrixXd T1 = normalizePointsIsotrop(list1, l1);
// Eigen::MatrixXd T2 = normalizePointsIsotrop(list1, l2);
// Eigen::MatrixXd T3 = normalizePointsIsotrop(list1, 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);
// getting singular value 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();
// FIX: SOLVE USING THIS
// Compute tensor
Eigen::VectorXd tensorVector = (D*V.inverse()).inverse()*D_pu_diag;
Eigen::MatrixXd A = P*Q*L;
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();
......@@ -219,20 +312,25 @@ bool TrifocalTensor::computeTensorRansac(const Eigen::MatrixXd& list1, const Eig
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 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);
// 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);
}
}
}
......@@ -294,4 +392,17 @@ void TrifocalTensor::transfer(const Eigen::Vector3d& p1 , Eigen::Vector3d& p2, E
p3(2) = 1;
}
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;
}
} /* namespace vision_utils */
......@@ -38,6 +38,7 @@ public:
*/
bool computeTensor(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3);
bool computeTensorRansac(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3);
bool computeTensorFactorized(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3);
/**
* \brief Compute the coordinates of the point p3 on third img corresponding to p1 and p2 of others images
......@@ -93,6 +94,11 @@ private:
* \brief Get random candidates
*/
Eigen::VectorXd 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);
/**
* \brief Compute tensor product of two matrices
*/
Eigen::MatrixXd tensorProduct(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B);
};
} /* namespace vision_utils */
......
......@@ -483,6 +483,7 @@ TEST(TrifocalTensor, ComputeTensorReal)
vision_utils::TrifocalTensor tensor(1000,max_error_reprojection,0.75);
// bool tensor_ok = tensor.computeTensorRansac(list1, list2, list3);
bool tensor_ok = tensor.computeTensor(list1, list2, list3);
// bool tensor_ok = tensor.computeTensorFactorized(list1, list2, list3);
ASSERT_TRUE(tensor_ok);
#ifdef _VU_DEBUG
......
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