From c16e6fc0d545974c208c222436287d154563536e Mon Sep 17 00:00:00 2001 From: Manohar Paluri Date: Sun, 14 Feb 2010 07:25:03 +0000 Subject: [PATCH] Moved Tensor related Files from CitySLAM --- cpp/tensorInterface.cpp | 37 ++++++++++++++ cpp/tensorInterface.h | 90 ++++++++++++++++++++++++++++++++++ cpp/tensors.h | 33 +++++++++++++ cpp/testHomography2.cpp | 104 ++++++++++++++++++++++++++++++++++++++++ 4 files changed, 264 insertions(+) create mode 100644 cpp/tensorInterface.cpp create mode 100644 cpp/tensorInterface.h create mode 100644 cpp/tensors.h create mode 100644 cpp/testHomography2.cpp diff --git a/cpp/tensorInterface.cpp b/cpp/tensorInterface.cpp new file mode 100644 index 000000000..b66f74b0a --- /dev/null +++ b/cpp/tensorInterface.cpp @@ -0,0 +1,37 @@ +/* + * tensorInterface.cpp + * @brief Interfacing tensors template library and gtsam + * Created on: Feb 12, 2010 + * @author: Frank Dellaert + */ + +#include "tensorInterface.h" + +using namespace std; +using namespace tensors; + +namespace gtsam { + + boost::tuple DLT(const Matrix& A) { + + // Do SVD on A + Matrix U, V; + Vector S; + svd(A, U, S, V); + + // 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)); + } + +} // namespace gtsam diff --git a/cpp/tensorInterface.h b/cpp/tensorInterface.h new file mode 100644 index 000000000..8fddc1de3 --- /dev/null +++ b/cpp/tensorInterface.h @@ -0,0 +1,90 @@ +/* + * tensorInterface.h + * @brief Interfacing tensors template library and gtsam + * Created on: Feb 12, 2010 + * @author: Frank Dellaert + */ + +#include "tensors.h" +#include "Matrix.h"" + +namespace gtsam { + + /** Reshape 3*3 rank 2 tensor into Matrix */ + template + Matrix reshape(const tensors::Tensor2Expression& T, int m, int n) { + if (m * n != 9) throw std::invalid_argument( + "reshape: incompatible dimensions"); + Matrix M(m, n); + size_t t = 0; + for (int j = 0; j < J::dim; j++) + for (int i = 0; i < I::dim; i++) + M.data()[t++] = T(i, j); + return M; + } + + /** Reshape Vector into rank 2 tensor */ + template + tensors::Tensor2 reshape2(const Vector& v) { + if (v.size() != N1 * N2) throw std::invalid_argument( + "reshape2: incompatible dimensions"); + double data[N1][N2]; + int t = 0; + for (int j = 0; j < N2; j++) + for (int i = 0; i < N1; i++) + data[j][i] = v(t++); + return tensors::Tensor2(data); + } + + /** Reshape rank 3 tensor into Matrix */ + template + Matrix reshape(const tensors::Tensor3Expression& T, int m, int n) { + if (m * n != I::dim * J::dim * K::dim) throw std::invalid_argument( + "reshape: incompatible dimensions"); + Matrix M(m, n); + int t = 0; + for (int k = 0; k < K::dim; k++) + for (int j = 0; j < J::dim; j++) + for (int i = 0; i < I::dim; i++) + M.data()[t++] = T(i, j, k); + return M; + } + + /** Reshape Vector into rank 3 tensor */ + template + tensors::Tensor3 reshape3(const Vector& v) { + if (v.size() != N1 * N2 * N3) throw std::invalid_argument( + "reshape3: incompatible dimensions"); + double data[N1][N2][N3]; + int t = 0; + for (int k = 0; k < N3; k++) + for (int j = 0; j < N2; j++) + for (int i = 0; i < N1; i++) + data[k][j][i] = v(t++); + return tensors::Tensor3(data); + } + + /** Reshape rank 5 tensor into Matrix */ + template + Matrix reshape(const tensors::Tensor5Expression& T, int m, + int n) { + if (m * n != I::dim * J::dim * K::dim * L::dim * M::dim) throw std::invalid_argument( + "reshape: incompatible dimensions"); + Matrix R(m, n); + int t = 0; + for (int m = 0; m < M::dim; m++) + for (int l = 0; l < L::dim; l++) + for (int k = 0; k < K::dim; k++) + for (int j = 0; j < J::dim; j++) + for (int i = 0; i < I::dim; i++) + R.data()[t++] = T(i, j, k, l, m); + return R; + } + + /** + * Find Vector v that minimizes algebraic error A*v + * Returns rank of A, minimum error, and corresponding eigenvector + */ + boost::tuple DLT(const Matrix& A); + +} // namespace gtsam diff --git a/cpp/tensors.h b/cpp/tensors.h new file mode 100644 index 000000000..b7712f02c --- /dev/null +++ b/cpp/tensors.h @@ -0,0 +1,33 @@ +/* + * tensors.h + * @brief Tensor expression templates based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf + * Created on: Feb 10, 2010 + * @author: Frank Dellaert + */ + +#pragma once + +namespace tensors { + + /** index */ + template struct Index { + enum { dim = Dim }; + }; + +} // namespace tensors + +// Expression templates +#include "Tensor1Expression.h" +#include "Tensor2Expression.h" +#include "Tensor3Expression.h" +// Tensor4 not needed so far +#include "Tensor5Expression.h" + +// Actual tensor classes +#include "Tensor1.h" +#include "Tensor2.h" +#include "Tensor3.h" +#include "Tensor4.h" +#include "Tensor5.h" + + diff --git a/cpp/testHomography2.cpp b/cpp/testHomography2.cpp new file mode 100644 index 000000000..decc1922b --- /dev/null +++ b/cpp/testHomography2.cpp @@ -0,0 +1,104 @@ +/* + * testHomography2.cpp + * @brief Test and estimate 2D homographies + * Created on: Feb 13, 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; + +/* ************************************************************************* */ +TEST( Homography2, RealImages) +{ + // 4 point correspondences MATLAB from the floor of bt001.png and bt002.png + Correspondence p1(point2h(216.841, 443.220, 1), point2h(213.528, 414.671, 1)); + Correspondence p2(point2h(252.119, 363.481, 1), point2h(244.614, 348.842, 1)); + Correspondence p3(point2h(316.614, 414.768, 1), point2h(303.128, 390.000, 1)); + Correspondence p4(point2h(324.165, 465.463, 1), point2h(308.614, 431.129, 1)); + + // Homography obtained using MATLAB code + double h[3][3] = { { 0.9075, 0.0031, -0 }, { -0.1165, 0.8288, -0.0004 }, { + 30.8472, 16.0449, 1 } }; + Homography2 H(h); + + // CHECK whether they are equivalent + CHECK(assert_equivalent(p1.second(b),H(b,a)*p1.first(a),0.05)) + CHECK(assert_equivalent(p2.second(b),H(b,a)*p2.first(a),0.05)) + CHECK(assert_equivalent(p3.second(b),H(b,a)*p3.first(a),0.05)) + CHECK(assert_equivalent(p4.second(b),H(b,a)*p4.first(a),0.05)) +} + +/* ************************************************************************* */ +// Homography test case +// 4 trivial correspondences of a translating square +Correspondence p1(point2h(0, 0, 1), point2h(4, 5, 1)); +Correspondence p2(point2h(1, 0, 1), point2h(5, 5, 1)); +Correspondence p3(point2h(1, 1, 1), point2h(5, 6, 1)); +Correspondence p4(point2h(0, 1, 1), point2h(4, 6, 1)); + +double h[3][3] = { { 1, 0, 4 }, { 0, 1, 5 }, { 0, 0, 1 } }; +Homography2 H(h); + +/* ************************************************************************* */ +TEST( Homography2, TestCase) +{ + // Check homography + list correspondences; + correspondences += p1, p2, p3, p4; + BOOST_FOREACH(const Correspondence& p, correspondences) + CHECK(assert_equality(p.second(b),H(_a,b) * p.first(a))) + + // Check a line + Line2h l1 = line2h(1, 0, -1); // in a + Line2h l2 = line2h(1, 0, -5); // x==5 in b + CHECK(assert_equality(l1(a), H(a,b)*l2(b))) +} + +/* ************************************************************************* */ +TEST( Homography2, Estimate) +{ + list correspondences; + correspondences += p1, p2, p3, p4; + Homography2 estimatedH = estimateHomography2(correspondences); + CHECK(assert_equivalent(H(_a,b),estimatedH(_a,b))); +} + +/* ************************************************************************* */ +TEST( Homography2, EstimateReverse) +{ + double h[3][3] = { { 1, 0, -4 }, { 0, 1, -5 }, { 0, 0, 1 } }; + Homography2 reverse(h); + + list correspondences; + correspondences += p1.swap(), p2.swap(), p3.swap(), p4.swap(); + Homography2 estimatedH = estimateHomography2(correspondences); + CHECK(assert_equality(reverse(_a,b),estimatedH(_a,b)*(1.0/estimatedH(2,2)))); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ +