diff --git a/cpp/Makefile.am b/cpp/Makefile.am index a89c05e3a..457d107cd 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -227,11 +227,13 @@ headers += tensors.h Tensor1.h Tensor2.h Tensor3.h Tensor4.h Tensor5.h headers += Tensor1Expression.h Tensor2Expression.h Tensor3Expression.h Tensor5Expression.h headers += projectiveGeometry.h tensorInterface.h sources += projectiveGeometry.cpp tensorInterface.cpp -check_PROGRAMS += testTensors testHomography2 +check_PROGRAMS += testTensors testHomography2 testTrifocal testHomography2_SOURCES = testHomography2.cpp testHomography2_LDADD = libgtsam.la testTensors_SOURCES = testTensors.cpp testTensors_LDADD = libgtsam.la +testTrifocal_SOURCES = testTrifocal.cpp +testTrifocal_LDADD = libgtsam.la # Visual SLAM sources += visualSLAM.cpp diff --git a/cpp/tensorInterface.cpp b/cpp/tensorInterface.cpp index b66f74b0a..f5a0c3c8b 100644 --- a/cpp/tensorInterface.cpp +++ b/cpp/tensorInterface.cpp @@ -12,26 +12,27 @@ using namespace tensors; namespace gtsam { - boost::tuple DLT(const Matrix& A) { +boost::tuple DLT(const Matrix& A) { - // Do SVD on A - Matrix U, V; - Vector S; - svd(A, U, S, V); + // Do SVD on A + Matrix U, V; + Vector S; + svd(A, U, S, V, false); - // Find minimum column - int n = V.size2(), min_j = n - 1, rank = 0; - for (int j = 0; j < n; j++) - if (S(j) > 1e-9) rank++; - double min_S = S(min_j); - for (int j = 0; j < n - 1; j++) - if (S(j) < min_S) { - min_j = j; - min_S = S(j); - } + // Find minimum column + int n = V.size2(), min_j = n - 1, rank = 0; + for (int j = 0; j < n; j++) + if (S(j) > 1e-9) + rank++; + double min_S = S(min_j); + for (int j = 0; j < n - 1; j++) + if (S(j) < min_S) { + min_j = j; + min_S = S(j); + } - // Return minimum column - return boost::tuple(rank, min_S, column_(V, min_j)); - } + // Return minimum column + return boost::tuple(rank, min_S, column_(V, min_j)); +} } // namespace gtsam diff --git a/cpp/testTrifocal.cpp b/cpp/testTrifocal.cpp new file mode 100644 index 000000000..5bfd07730 --- /dev/null +++ b/cpp/testTrifocal.cpp @@ -0,0 +1,113 @@ +/* + * testTrifocal.cpp + * @brief trifocal tensor estimation + * Created on: Feb 9, 2010 + * @author: Frank Dellaert + */ + +#include +#include +#include // for operator += +using namespace boost::assign; + +#include + +#include "tensors.h" +#include "tensorInterface.h" +#include "projectiveGeometry.h" + +using namespace std; +using namespace gtsam; +using namespace tensors; + +/* ************************************************************************* */ +// Indices + +Index<3, 'a'> a, _a; +Index<3, 'b'> b, _b; +Index<3, 'c'> c, _c; +Index<3, 'd'> d, _d; +Index<3, 'e'> e, _e; +Index<3, 'f'> f, _f; +Index<3, 'g'> g, _g; + +Index<4, 'A'> A; + +/* ************************************************************************* */ +// 3 Camera setup in trifocal stereo setup, -1,0,1 +/* ************************************************************************* */ +double left__[4][3] = { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }, { -1, 0, 0 } }; +double middle[4][3] = { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }, { +0, 0, 0 } }; +double right_[4][3] = { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }, { +1, 0, 0 } }; +ProjectiveCamera ML(left__), MM(middle), MR(right_); + +// Cube +Point3h P1 = point3h(-1, -1, 3 - 1, 1); +Point3h P2 = point3h(-1, -1, 3 + 1, 1); +Point3h P3 = point3h(-1, +1, 3 - 1, 1); +Point3h P4 = point3h(-1, +1, 3 + 1, 1); +Point3h P5 = point3h(+1, -1, 3 - 1, 1); +Point3h P6 = point3h(+1, -1, 3 + 1, 1); +Point3h P7 = point3h(+1, +1, 3 - 1, 1); +Point3h P8 = point3h(+1, +1, 3 + 1, 1); + +/* ************************************************************************* */ +// Manohar's homework +TEST(Tensors, TrifocalTensor) +{ + // Checked with MATLAB ! + double t[3][3][3] = { + { { -0.301511, 0, 0 }, { 0, -0.603023, 0 }, { 0, 0,-0.603023 } }, + { { 0, 0.301511, 0 }, { 0, 0, 0 }, { 0, 0, 0 } }, + { { 0, 0, 0.301511 }, { 0, 0, 0 }, { 0, 0, 0 } } + }; + TrifocalTensor T(t); + + list points; + points += P1, P2, P3, P4, P5, P6, P7, P8; + + Eta3 eta; + + list triplets; + double data[3][3] = { { 0, 0, 0 }, { 0, 0, 0 }, { 0, 0, 0 } }; + Tensor2<3,3> zero(data); + BOOST_FOREACH(const Point3h& P, points) { + // form triplet + Triplet p(ML(a,A)*P(A), MM(b,A)*P(A), MR(c,A)*P(A)); + // check trifocal constraint + Tensor2<3,3> T1 = T(_a,b,c) * p.first(a); + Tensor2<3,3> T2 = eta(_d,_b,_e) * p.second(d); + Tensor2<3,3> T3 = eta(_f,_c,_g) * p.third(f); + CHECK(assert_equality(zero(_e,_g), (T1(b,c) * T2(_b,_e)) * T3(_c,_g),1e-4)); + triplets += p; + } + + // We will form the rank 5 tensor by multiplying a rank 3 and rank 2 + // Let's check the answer for the first point: + Triplet p = triplets.front(); + + // This checks the rank 3 (with answer checked in MATLAB); + double matlab3[3][3][3] = { + { { -0, -0, 0}, { 4, 2, -4}, { 2, 1, -2}}, + { { -4, -2, 4}, {-0, -0, 0}, {-2, -1, 2}}, + { { -2, -1, 2}, { 2, 1, -2}, {-0, -0, 0}} + }; + Tensor3<3,3,3> expected3(matlab3); + CHECK(assert_equality(expected3(a,_b,_e), p.first(a)* (eta(_d,_b,_e) * p.second(d)))); + + // This checks the rank 2 (with answer checked in MATLAB); + double matlab2[3][3] = { {0, -2, -1}, {2, 0, 0}, {1, 0, 0}}; + Tensor2<3,3> expected2(matlab2); + CHECK(assert_equality(expected2(_c,_g), eta(_f,_c,_g) * p.third(f))); + + TrifocalTensor actual = estimateTrifocalTensor(triplets); + CHECK(assert_equality(T(_a,b,c),actual(_a,b,c),1e-6)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ +