diff --git a/gtsam/geometry/Tensor1.h b/gtsam/geometry/Tensor1.h deleted file mode 100644 index e7dc6bfb3..000000000 --- a/gtsam/geometry/Tensor1.h +++ /dev/null @@ -1,85 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file Tensor1.h - * @brief Rank 1 tensors based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf - * @date Feb 10, 2010 - * @author Frank Dellaert - */ - -#pragma once -#include - -namespace tensors { - - /** - * A rank 1 tensor. Actually stores data. - * @addtogroup tensors - * \nosubgrouping - */ - template - class Tensor1 { - double T[N]; ///< Storage - - public: - - /// @name Standard Constructors - /// @{ - - /** default constructor */ - Tensor1() { - } - - /** construct from data */ - Tensor1(const double* data) { - for (int i = 0; i < N; i++) - T[i] = data[i]; - } - - /** construct from expression */ - template - Tensor1(const Tensor1Expression >& a) { - for (int i = 0; i < N; i++) - T[i] = a(i); - } - - /// @} - /// @name Standard Interface - /// @{ - - /** return data */ - inline int dim() const { - return N; - } - - /** return data */ - inline const double& operator()(int i) const { - return T[i]; - } - - /** return data */ - inline double& operator()(int i) { - return T[i]; - } - - /// return an expression associated with an index - template Tensor1Expression > operator()( - Index index) const { - return Tensor1Expression >(*this); - } - - /// @} - - }; -// Tensor1 - -}// namespace tensors diff --git a/gtsam/geometry/Tensor1Expression.h b/gtsam/geometry/Tensor1Expression.h deleted file mode 100644 index 37b5809c8..000000000 --- a/gtsam/geometry/Tensor1Expression.h +++ /dev/null @@ -1,181 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file Tensor1Expression.h - * @brief Tensor expression templates based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf - * @date Feb 10, 2010 - * @author Frank Dellaert - */ - -#pragma once - -#include -#include -#include -#include - -namespace tensors { - - /** - * Templated class to provide a rank 1 tensor interface to a class. - * This class does not store any data but the result of an expression. - * It is associated with an index. - * @addtogroup tensors - * \nosubgrouping - */ - template class Tensor1Expression { - - private: - - A iter; - - typedef Tensor1Expression This; - - /** Helper class for multiplying with a double */ - class TimesDouble_ { - A iter; - const double s; - public: - /// Constructor - TimesDouble_(const A &a, double s_) : - iter(a), s(s_) { - } - /// Element access - inline double operator()(int i) const { - return iter(i) * s; - } - }; - - public: - - /// @name Standard Constructors - /// @{ - - /** constructor */ - Tensor1Expression(const A &a) : - iter(a) { - } - - /// @} - /// @name Testable - /// @{ - - /** Print */ - void print(const std::string s = "") const { - std::cout << s << "{"; - std::cout << (*this)(0); - for (int i = 1; i < I::dim; i++) - std::cout << ", "<< (*this)(i); - std::cout << "}" << std::endl; - } - - /// equality - template - bool equals(const Tensor1Expression & q, double tol) const { - for (int i = 0; i < I::dim; i++) - if (fabs((*this)(i) - q(i)) > tol) return false; - return true; - } - - /// @} - /// @name Standard Interface - /// @{ - - /** norm */ - double norm() const { - double sumsqr = 0.0; - for (int i = 0; i < I::dim; i++) - sumsqr += iter(i) * iter(i); - return sqrt(sumsqr); - } - - /// test equivalence - template - bool equivalent(const Tensor1Expression & q, double tol = 1e-9) const { - return ((*this) * (1.0 / norm())).equals(q * (1.0 / q.norm()), tol) - || ((*this) * (-1.0 / norm())).equals(q * (1.0 / q.norm()), tol); - } - - /** Check if two expressions are equal */ - template - bool operator==(const Tensor1Expression& e) const { - for (int i = 0; i < I::dim; i++) - if (iter(i) != e(i)) return false; - return true; - } - - /** element access */ - double operator()(int i) const { - return iter(i); - } - - /** mutliply with a double. */ - inline Tensor1Expression operator*(double s) const { - return TimesDouble_(iter, s); - } - - /** Class for contracting two rank 1 tensor expressions, yielding a double. */ - template - inline double operator*(const Tensor1Expression &b) const { - double sum = 0.0; - for (int i = 0; i < I::dim; i++) - sum += (*this)(i) * b(i); - return sum; - } - - }; // Tensor1Expression - - /// @} - /// @name Advanced Interface - /// @{ - - /** Print a rank 1 expression */ - template - void print(const Tensor1Expression& T, const std::string s = "") { - T.print(s); - } - - /** norm */ - template - double norm(const Tensor1Expression& T) { - return T.norm(); - } - - /** - * This template works for any two expressions - */ - template - bool assert_equality(const Tensor1Expression& expected, - const Tensor1Expression& actual, double tol = 1e-9) { - if (actual.equals(expected, tol)) return true; - std::cout << "Not equal:\n"; - expected.print("expected:\n"); - actual.print("actual:\n"); - return false; - } - - /** - * This template works for any two expressions - */ - template - bool assert_equivalent(const Tensor1Expression& expected, - const Tensor1Expression& actual, double tol = 1e-9) { - if (actual.equivalent(expected, tol)) return true; - std::cout << "Not equal:\n"; - expected.print("expected:\n"); - actual.print("actual:\n"); - return false; - } - - /// @} - -} // namespace tensors diff --git a/gtsam/geometry/Tensor2.h b/gtsam/geometry/Tensor2.h deleted file mode 100644 index 825b57b3e..000000000 --- a/gtsam/geometry/Tensor2.h +++ /dev/null @@ -1,84 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file Tensor2.h - * @brief Rank 2 Tensor based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf - * @date Feb 10, 2010 - * @author Frank Dellaert - */ - -#pragma once -#include - -namespace tensors { - -/** - * Rank 2 Tensor - * @addtogroup tensors - * \nosubgrouping - */ -template -class Tensor2 { -protected: - Tensor1 T[N2]; ///< Storage - -public: - - /// @name Standard Constructors - /// @{ - - /** default constructor */ - Tensor2() { - } - - /// construct from data - expressed in row major form - Tensor2(const double data[N2][N1]) { - for (int j = 0; j < N2; j++) - T[j] = Tensor1 (data[j]); - } - - /** construct from expression */ - template - Tensor2(const Tensor2Expression , Index >& a) { - for (int j = 0; j < N2; j++) - T[j] = a(j); - } - - /// @} - /// @name Standard Interface - /// @{ - - /** dimension - TODO: is this right for anything other than 3x3? */ - size_t dim() const {return N1 * N2;} - - /// const element access - const double & operator()(int i, int j) const { - return T[j](i); - } - - /// element access - double & operator()(int i, int j) { - return T[j](i); - } - - /** convert to expression */ - template Tensor2Expression , Index< - N2, J> > operator()(Index i, Index j) const { - return Tensor2Expression , Index > (*this); - } - - /// @} - -}; - -} // namespace tensors - diff --git a/gtsam/geometry/Tensor2Expression.h b/gtsam/geometry/Tensor2Expression.h deleted file mode 100644 index 5b07bad37..000000000 --- a/gtsam/geometry/Tensor2Expression.h +++ /dev/null @@ -1,310 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file Tensor2Expression.h - * @brief Tensor expression templates based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf - * @date Feb 10, 2010 - * @author Frank Dellaert - */ - -#pragma once - -#include -#include -#include - -namespace tensors { - - /** - * Templated class to hold a rank 2 tensor expression. - * @addtogroup tensors - * \nosubgrouping - */ - template class Tensor2Expression { - - private: - - A iter; - - typedef Tensor2Expression This; - - /** Helper class for instantiating one index */ - class FixJ_ { - const int j; - const A iter; - public: - FixJ_(int j_, const A &a) : - j(j_), iter(a) { - } - double operator()(int i) const { - return iter(i, j); - } - }; - - /** Helper class for swapping indices */ - class Swap_ { - const A iter; - public: - /// Constructor - Swap_(const A &a) : - iter(a) { - } - /// Element access - double operator()(int j, int i) const { - return iter(i, j); - } - }; - - /** Helper class for multiplying with a double */ - class TimesDouble_ { - A iter; - const double s; - public: - /// Constructor - TimesDouble_(const A &a, double s_) : - iter(a), s(s_) { - } - /// Element access - inline double operator()(int i, int j) const { - return iter(i, j) * s; - } - }; - - /** Helper class for contracting index I with rank 1 tensor */ - template class ITimesRank1_ { - const This a; - const Tensor1Expression b; - public: - /// Constructor - ITimesRank1_(const This &a_, const Tensor1Expression &b_) : - a(a_), b(b_) { - } - /// Element access - double operator()(int j) const { - double sum = 0.0; - for (int i = 0; i < I::dim; i++) - sum += a(i, j) * b(i); - return sum; - } - }; - - /** Helper class for contracting index J with rank 1 tensor */ - template class JTimesRank1_ { - const This a; - const Tensor1Expression b; - public: - /// Constructor - JTimesRank1_(const This &a_, const Tensor1Expression &b_) : - a(a_), b(b_) { - } - /// Element access - double operator()(int i) const { - double sum = 0.0; - for (int j = 0; j < J::dim; j++) - sum += a(i, j) * b(j); - return sum; - } - }; - - /** Helper class for contracting index I with rank 2 tensor */ - template class ITimesRank2_ { - const This a; - const Tensor2Expression b; - public: - /// Constructor - ITimesRank2_(const This &a_, const Tensor2Expression &b_) : - a(a_), b(b_) { - } - /// Element access - double operator()(int j, int k) const { - double sum = 0.0; - for (int i = 0; i < I::dim; i++) - sum += a(i, j) * b(i, k); - return sum; - } - }; - - public: - - /// @name Standard Constructors - /// @{ - - /** constructor */ - Tensor2Expression(const A &a) : - iter(a) { - } - - /// @} - /// @name Testable - /// @{ - - /** Print */ - void print(const std::string& s = "Tensor2:") const { - std::cout << s << "{"; - (*this)(0).print(); - for (int j = 1; j < J::dim; j++) { - std::cout << ","; - (*this)(j).print(""); - } - std::cout << "}" << std::endl; - } - - /// test equality - template - bool equals(const Tensor2Expression & q, double tol) const { - for (int j = 0; j < J::dim; j++) - if (!(*this)(j).equals(q(j), tol)) - return false; - return true; - } - - /// @} - /// @name Standard Interface - /// @{ - - /** norm */ - double norm() const { - double sumsqr = 0.0; - for (int i = 0; i < I::dim; i++) - for (int j = 0; j < J::dim; j++) - sumsqr += iter(i, j) * iter(i, j); - return sqrt(sumsqr); - } - - /// test equivalence - template - bool equivalent(const Tensor2Expression & q, double tol) const { - return ((*this) * (1.0 / norm())).equals(q * (1.0 / q.norm()), tol) - || ((*this) * (-1.0 / norm())).equals(q * (1.0 / q.norm()), tol); - } - - /** element access */ - double operator()(int i, int j) const { - return iter(i, j); - } - - /** swap indices */ - typedef Tensor2Expression Swapped; - /// Return Swap_ helper class - Swapped swap() { - return Swap_(iter); - } - - /** mutliply with a double. */ - inline Tensor2Expression operator*(double s) const { - return TimesDouble_(iter, s); - } - - /** Fix a single index */ - Tensor1Expression operator()(int j) const { - return FixJ_(j, iter); - } - - /** Check if two expressions are equal */ - template - bool operator==(const Tensor2Expression& T) const { - for (int i = 0; i < I::dim; i++) - for (int j = 0; j < J::dim; j++) - if (iter(i, j) != T(i, j)) - return false; - return true; - } - - /// @} - /// @name Advanced Interface - /// @{ - - /** c(j) = a(i,j)*b(i) */ - template - inline Tensor1Expression, J> operator*( - const Tensor1Expression& p) { - return ITimesRank1_(*this, p); - } - - /** c(i) = a(i,j)*b(j) */ - template - inline Tensor1Expression, I> operator*( - const Tensor1Expression &p) { - return JTimesRank1_(*this, p); - } - - /** c(j,k) = a(i,j)*T(i,k) */ - template - inline Tensor2Expression , J, K> operator*( - const Tensor2Expression& p) { - return ITimesRank2_(*this, p); - } - - }; - // Tensor2Expression - - /** Print */ - template - void print(const Tensor2Expression& T, const std::string& s = - "Tensor2:") { - T.print(s); - } - - /** Helper class for multiplying two covariant tensors */ - template class Rank1Rank1_ { - const Tensor1Expression iterA; - const Tensor1Expression iterB; - public: - /// Constructor - Rank1Rank1_(const Tensor1Expression &a, - const Tensor1Expression &b) : - iterA(a), iterB(b) { - } - /// element access - double operator()(int i, int j) const { - return iterA(i) * iterB(j); - } - }; - - /** Multiplying two different indices yields an outer product */ - template - inline Tensor2Expression , I, J> operator*( - const Tensor1Expression &a, const Tensor1Expression &b) { - return Rank1Rank1_(a, b); - } - - /** - * This template works for any two expressions - */ - template - bool assert_equality(const Tensor2Expression& expected, - const Tensor2Expression& actual, double tol = 1e-9) { - if (actual.equals(expected, tol)) - return true; - std::cout << "Not equal:\n"; - expected.print("expected:\n"); - actual.print("actual:\n"); - return false; - } - - /** - * This template works for any two expressions - */ - template - bool assert_equivalent(const Tensor2Expression& expected, - const Tensor2Expression& actual, double tol = 1e-9) { - if (actual.equivalent(expected, tol)) - return true; - std::cout << "Not equivalent:\n"; - expected.print("expected:\n"); - actual.print("actual:\n"); - return false; - } - - /// @} - -} // namespace tensors diff --git a/gtsam/geometry/Tensor3.h b/gtsam/geometry/Tensor3.h deleted file mode 100644 index cfd96f7f6..000000000 --- a/gtsam/geometry/Tensor3.h +++ /dev/null @@ -1,106 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file Tensor3.h - * @brief Rank 3 tensors based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf - * @date Feb 10, 2010 - * @author: Frank Dellaert - */ - -#pragma once -#include - -namespace tensors { - - /** - * Rank 3 Tensor - * @addtogroup tensors - * \nosubgrouping - */ - template - class Tensor3 { - Tensor2 T[N3]; ///< Storage - - public: - - /// @name Standard Constructors - /// @{ - - /** default constructor */ - Tensor3() { - } - - /** construct from data */ - Tensor3(const double data[N3][N2][N1]) { - for (int k = 0; k < N3; k++) - T[k] = data[k]; - } - - /// @} - /// @name Advanced Constructors - /// @{ - - /** construct from expression */ - template - Tensor3(const Tensor3Expression , Index , Index >& a) { - for (int k = 0; k < N3; k++) - T[k] = a(k); - } - - /// @} - /// @name Standard Interface - /// @{ - - /// element access - double operator()(int i, int j, int k) const { - return T[k](i, j); - } - - /** convert to expression */ - template Tensor3Expression , - Index , Index > operator()(const Index& i, - const Index& j, const Index& k) { - return Tensor3Expression , Index , Index > (*this); - } - - /** convert to expression */ - template Tensor3Expression , - Index , Index > operator()(const Index& i, - const Index& j, const Index& k) const { - return Tensor3Expression , Index , Index > (*this); - } - }; // Tensor3 - - /** Rank 3 permutation tensor */ - struct Eta3 { - - /** calculate value. TODO: wasteful to actually use this */ - double operator()(int i, int j, int k) const { - return ((j - i) * (k - i) * (k - j)) / 2; - } - - /** create expression */ - template Tensor3Expression , - Index<3, J> , Index<3, K> > operator()(const Index<3, I>& i, - const Index<3, J>& j, const Index<3, K>& k) const { - return Tensor3Expression , Index<3, J> , Index<3, K> > ( - *this); - } - - }; // Eta - - /// @} - -} // namespace tensors diff --git a/gtsam/geometry/Tensor3Expression.h b/gtsam/geometry/Tensor3Expression.h deleted file mode 100644 index b7474bdfb..000000000 --- a/gtsam/geometry/Tensor3Expression.h +++ /dev/null @@ -1,194 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file Tensor3Expression.h - * @brief Tensor expression templates based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf - * @date Feb 10, 2010 - * @author Frank Dellaert - */ - -#pragma once - -#include -#include - -namespace tensors { - - /** - * templated class to interface to an object A as a rank 3 tensor - * @addtogroup tensors - * \nosubgrouping - */ - template class Tensor3Expression { - A iter; - - typedef Tensor3Expression This; - - /** Helper class for instantiating one index */ - class FixK_ { - const int k; - const A iter; - public: - FixK_(int k_, const A &a) : - k(k_), iter(a) { - } - double operator()(int i, int j) const { - return iter(i, j, k); - } - }; - - /** Helper class for contracting rank3 and rank1 tensor */ - template class TimesRank1_ { - typedef Tensor1Expression Rank1; - const This T; - const Rank1 t; - public: - TimesRank1_(const This &a, const Rank1 &b) : - T(a), t(b) { - } - double operator()(int j, int k) const { - double sum = 0.0; - for (int i = 0; i < I::dim; i++) - sum += T(i, j, k) * t(i); - return sum; - } - }; - - public: - - /// @name Standard Constructors - /// @{ - - /** constructor */ - Tensor3Expression(const A &a) : - iter(a) { - } - - /// @} - /// @name Standard Interface - /// @{ - - /** Print */ - void print(const std::string& s = "Tensor3:") const { - std::cout << s << "{"; - (*this)(0).print(""); - for (int k = 1; k < K::dim; k++) { - std::cout << ","; - (*this)(k).print(""); - } - std::cout << "}" << std::endl; - } - - /// test equality - template - bool equals(const Tensor3Expression & q, double tol) const { - for (int k = 0; k < K::dim; k++) - if (!(*this)(k).equals(q(k), tol)) return false; - return true; - } - - /** element access */ - double operator()(int i, int j, int k) const { - return iter(i, j, k); - } - - /** Fix a single index */ - Tensor2Expression operator()(int k) const { - return FixK_(k, iter); - } - - /** Contracting with rank1 tensor */ - template - inline Tensor2Expression , J, K> operator*( - const Tensor1Expression &b) const { - return TimesRank1_ (*this, b); - } - - }; // Tensor3Expression - - /// @} - /// @name Advanced Interface - /// @{ - - /** Print */ - template - void print(const Tensor3Expression& T, const std::string& s = - "Tensor3:") { - T.print(s); - } - - /** Helper class for outer product of rank2 and rank1 tensor */ - template - class Rank2Rank1_ { - typedef Tensor2Expression Rank2; - typedef Tensor1Expression Rank1; - const Rank2 iterA; - const Rank1 iterB; - public: - /// Constructor - Rank2Rank1_(const Rank2 &a, const Rank1 &b) : - iterA(a), iterB(b) { - } - /// Element access - double operator()(int i, int j, int k) const { - return iterA(i, j) * iterB(k); - } - }; - - /** outer product of rank2 and rank1 tensor */ - template - inline Tensor3Expression , I, J, K> operator*( - const Tensor2Expression& a, const Tensor1Expression &b) { - return Rank2Rank1_ (a, b); - } - - /** Helper class for outer product of rank1 and rank2 tensor */ - template - class Rank1Rank2_ { - typedef Tensor1Expression Rank1; - typedef Tensor2Expression Rank2; - const Rank1 iterA; - const Rank2 iterB; - public: - /// Constructor - Rank1Rank2_(const Rank1 &a, const Rank2 &b) : - iterA(a), iterB(b) { - } - /// Element access - double operator()(int i, int j, int k) const { - return iterA(i) * iterB(j, k); - } - }; - - /** outer product of rank2 and rank1 tensor */ - template - inline Tensor3Expression , I, J, K> operator*( - const Tensor1Expression& a, const Tensor2Expression &b) { - return Rank1Rank2_ (a, b); - } - - /** - * This template works for any two expressions - */ - template - bool assert_equality(const Tensor3Expression& expected, - const Tensor3Expression& actual, double tol = 1e-9) { - if (actual.equals(expected, tol)) return true; - std::cout << "Not equal:\n"; - expected.print("expected:\n"); - actual.print("actual:\n"); - return false; - } - - /// @} - -} // namespace tensors diff --git a/gtsam/geometry/Tensor4.h b/gtsam/geometry/Tensor4.h deleted file mode 100644 index e8bfb0186..000000000 --- a/gtsam/geometry/Tensor4.h +++ /dev/null @@ -1,58 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file Tensor4.h - * @brief Rank 4 tensors based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf - * @date Feb 12, 2010 - * @author Frank Dellaert - */ - -#pragma once -#include - -namespace tensors { - - /** - * Rank 4 Tensor - * @addtogroup tensors - * \nosubgrouping - */ - template - class Tensor4 { - - private: - - Tensor3 T[N4]; ///< Storage - - public: - - /// @name Standard Constructors - /// @{ - - /** default constructor */ - Tensor4() { - } - - /// @} - /// @name Standard Interface - /// @{ - - /// element access - double operator()(int i, int j, int k, int l) const { - return T[l](i, j, k); - } - - /// @} - - }; // Tensor4 - -} // namespace tensors diff --git a/gtsam/geometry/Tensor5.h b/gtsam/geometry/Tensor5.h deleted file mode 100644 index a19784d84..000000000 --- a/gtsam/geometry/Tensor5.h +++ /dev/null @@ -1,75 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file Tensor5.h - * @brief Rank 5 tensors based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf - * @date Feb 12, 2010 - * @author Frank Dellaert - */ - -#pragma once -#include - -namespace tensors { - - /** - * Rank 5 Tensor - * @addtogroup tensors - * \nosubgrouping - */ - template - class Tensor5 { - - private: - - Tensor4 T[N5]; ///< Storage - - public: - - /// @name Standard Constructors - /// @{ - - /** default constructor */ - Tensor5() { - } - - /// @} - /// @name Standard Interface - /// @{ - - /** construct from expression */ - template - Tensor5(const Tensor5Expression , Index , Index , Index , Index >& a) { - for (int m = 0; m < N5; m++) - T[m] = a(m); - } - - /// element access - double operator()(int i, int j, int k, int l, int m) const { - return T[m](i, j, k, l); - } - - /** convert to expression */ - template Tensor5Expression , Index , Index , Index , - Index > operator()(Index i, Index j, - Index k, Index l, Index m) { - return Tensor5Expression , Index , Index , Index , Index > (*this); - } - - /// @} - - }; // Tensor5 - -} // namespace tensors diff --git a/gtsam/geometry/Tensor5Expression.h b/gtsam/geometry/Tensor5Expression.h deleted file mode 100644 index 6a6018e22..000000000 --- a/gtsam/geometry/Tensor5Expression.h +++ /dev/null @@ -1,135 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file Tensor5Expression.h - * @brief Tensor expression templates based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf - * @date Feb 10, 2010 - * @author Frank Dellaert - */ - -#pragma once - -#include -#include - -namespace tensors { - - /** - * templated class to interface to an object A as a rank 5 tensor - * @addtogroup tensors - * \nosubgrouping - */ - template class Tensor5Expression { - A iter; - - typedef Tensor5Expression This; - - /** Helper class for swapping indices 3 and 4 :-) */ - class Swap34_ { - const A iter; - public: - /// Constructor - Swap34_(const A &a) : - iter(a) { - } - /// swapping element access - double operator()(int i, int j, int k, int l, int m) const { - return iter(i, j, l, k, m); - } - }; - - public: - - /// @name Standard Constructors - /// @{ - - /** constructor */ - Tensor5Expression(const A &a) : - iter(a) { - } - - /// @} - /// @name Standard Interface - /// @{ - - /** Print */ - void print(const std::string& s = "Tensor5:") const { - std::cout << s << std::endl; - for (int m = 0; m < M::dim; m++) - for (int l = 0; l < L::dim; l++) - for (int k = 0; k < K::dim; k++) { - std::cout << "(m,l,k) = (" << m << "," << l << "," << k << ")" - << std::endl; - for (int j = 0; j < J::dim; j++) { - for (int i = 0; i < I::dim; i++) - std::cout << " " << (*this)(i, j, k, l, m); - std::cout << std::endl; - } - } - std::cout << std::endl; - } - - /** swap indices */ - typedef Tensor5Expression Swapped; - /// create Swap34_ helper class - Swapped swap34() { - return Swap34_(iter); - } - - /** element access */ - double operator()(int i, int j, int k, int l, int m) const { - return iter(i, j, k, l, m); - } - - }; - // Tensor5Expression - - /// @} - /// @name Advanced Interface - /// @{ - - /** Print */ - template - void print(const Tensor5Expression& T, - const std::string& s = "Tensor5:") { - T.print(s); - } - - /** Helper class for outer product of rank3 and rank2 tensor */ - template - class Rank3Rank2_ { - typedef Tensor3Expression Rank3; - typedef Tensor2Expression Rank2; - const Rank3 iterA; - const Rank2 iterB; - public: - /// Constructor - Rank3Rank2_(const Rank3 &a, const Rank2 &b) : - iterA(a), iterB(b) { - } - /// Element access - double operator()(int i, int j, int k, int l, int m) const { - return iterA(i, j, k) * iterB(l, m); - } - }; - - /** outer product of rank2 and rank1 tensor */ - template - inline Tensor5Expression , I, J, K, L, M> operator*( - const Tensor3Expression& a, - const Tensor2Expression &b) { - return Rank3Rank2_(a, b); - } - - /// @} - -} // namespace tensors diff --git a/gtsam/geometry/projectiveGeometry.cpp b/gtsam/geometry/projectiveGeometry.cpp deleted file mode 100644 index 31c975c43..000000000 --- a/gtsam/geometry/projectiveGeometry.cpp +++ /dev/null @@ -1,71 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file projectiveGeometry.cpp - * @brief Projective geometry, implemented using tensor library - * @date Feb 12, 2010 - * @author: Frank Dellaert - */ - -#include -#include - -#include -#include - -namespace gtsam { - - using namespace std; - using namespace tensors; - - /* ************************************************************************* */ - Point2h point2h(double x, double y, double w) { - double data[3]; - data[0] = x; - data[1] = y; - data[2] = w; - return data; - } - - /* ************************************************************************* */ - Line2h line2h(double a, double b, double c) { - double data[3]; - data[0] = a; - data[1] = b; - data[2] = c; - return data; - } - - /* ************************************************************************* */ - Point3h point3h(double X, double Y, double Z, double W) { - double data[4]; - data[0] = X; - data[1] = Y; - data[2] = Z; - data[3] = W; - return data; - } - - /* ************************************************************************* */ - Plane3h plane3h(double a, double b, double c, double d) { - double data[4]; - data[0] = a; - data[1] = b; - data[2] = c; - data[3] = d; - return data; - } - - - /* ************************************************************************* */ - -} // namespace gtsam diff --git a/gtsam/geometry/projectiveGeometry.h b/gtsam/geometry/projectiveGeometry.h deleted file mode 100644 index 2be02a349..000000000 --- a/gtsam/geometry/projectiveGeometry.h +++ /dev/null @@ -1,124 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file projectiveGeometry.h - * @brief Projective geometry, implemented using tensor library - * @date Feb 12, 2010 - * @author Frank Dellaert - */ - -#pragma once - -#include -#include - -namespace gtsam { - - /** - * 2D Point in homogeneous coordinates - * @addtogroup geometry - */ - typedef tensors::Tensor1<3> Point2h; - Point2h point2h(double x, double y, double w); ///< create Point2h - - /** - * 2D Line in homogeneous coordinates - * @addtogroup geometry - */ - typedef tensors::Tensor1<3> Line2h; - Line2h line2h(double a, double b, double c); ///< create Line2h - - /** - * 2D (homegeneous) Point correspondence - * @addtogroup geometry - */ - struct Correspondence { - Point2h first; ///< First point - Point2h second; ///< Second point - - /// Create a correspondence pair - Correspondence(const Point2h &p1, const Point2h &p2) : - first(p1), second(p2) { - } - /// Swap points - Correspondence swap() const { - return Correspondence(second, first); - } - /// print - void print() { - tensors::Index<3, 'i'> i; - tensors::print(first(i), "first :"); - tensors::print(second(i), "second:"); - } - }; - - /** - * 2D-2D Homography - * @addtogroup geometry - */ - typedef tensors::Tensor2<3, 3> Homography2; - - /** - * Fundamental Matrix - * @addtogroup geometry - */ - typedef tensors::Tensor2<3, 3> FundamentalMatrix; - - /** - * Triplet of (homogeneous) 2D points - * @addtogroup geometry - */ - struct Triplet { - Point2h first; ///< First point - Point2h second; ///< Second point - Point2h third; ///< Third point - - /// Create a Triplet correspondence - Triplet(const Point2h &p1, const Point2h &p2, const Point2h &p3) : - first(p1), second(p2), third(p3) { - } - /// print - void print() { - tensors::Index<3, 'i'> i; - tensors::print(first(i), "first :"); - tensors::print(second(i), "second:"); - tensors::print(third(i), "third :"); - } - }; - - /** - * Trifocal Tensor - * @addtogroup geometry - */ - typedef tensors::Tensor3<3, 3, 3> TrifocalTensor; - - /** - * 3D Point in homogeneous coordinates - * @addtogroup geometry - */ - typedef tensors::Tensor1<4> Point3h; - Point3h point3h(double X, double Y, double Z, double W); ///< create Point3h - - /** - * 3D Plane in homogeneous coordinates - * @addtogroup geometry - */ - typedef tensors::Tensor1<4> Plane3h; - Plane3h plane3h(double a, double b, double c, double d); ///< create Plane3h - - /** - * 3D to 2D projective camera - * @addtogroup geometry - */ - typedef tensors::Tensor2<3, 4> ProjectiveCamera; - -} // namespace gtsam diff --git a/gtsam/geometry/tensorInterface.h b/gtsam/geometry/tensorInterface.h deleted file mode 100644 index 7a2c144d7..000000000 --- a/gtsam/geometry/tensorInterface.h +++ /dev/null @@ -1,120 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file tensorInterface.h - * @brief Interfacing tensors template library and gtsam - * @date Feb 12, 2010 - * @author Frank Dellaert - */ - -#pragma once - -#include -#include - -namespace gtsam { - - /** Reshape rank 2 tensor into Matrix */ - template - Matrix reshape(const tensors::Tensor2Expression& T, int m, int n) { - if (m * n != I::dim * J::dim) throw std::invalid_argument( - "reshape: incompatible dimensions"); - MatrixRowMajor 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 Matrix(M); - } - - /** Reshape rank 2 tensor into Vector */ - template - Vector toVector(const tensors::Tensor2Expression& T) { - Vector v(I::dim * J::dim); - size_t t = 0; - for (int j = 0; j < J::dim; j++) - for (int i = 0; i < I::dim; i++) - v(t++) = T(i, j); - return v; - } - - /** 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[N2][N1]; - 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 Matrix into rank 2 tensor */ - template - tensors::Tensor2 reshape2matrix(const Matrix& m) { - if (m.rows() * m.cols() != N1 * N2) throw std::invalid_argument( - "reshape2: incompatible dimensions"); - double data[N2][N1]; - for (int j = 0; j < N2; j++) - for (int i = 0; i < N1; i++) - data[j][i] = m(j,i); - 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 i = 0; i < I::dim; i++) - for (int j = 0; j < J::dim; j++) - 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[N3][N2][N1]; - 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 i = 0; i < I::dim; i++) - for (int j = 0; j < J::dim; j++) - R.data()[t++] = T(i, j, k, l, m); - return R; - } - -} // namespace gtsam diff --git a/gtsam/geometry/tensors.h b/gtsam/geometry/tensors.h deleted file mode 100644 index a7125775a..000000000 --- a/gtsam/geometry/tensors.h +++ /dev/null @@ -1,45 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file tensors.h - * @brief Tensor expression templates based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf - * @date Feb 10, 2010 - * @author Frank Dellaert - * @addtogroup tensors - */ - -#pragma once - -namespace tensors { - - /** index */ - template struct Index { - static const int dim = Dim; ///< dimension - }; - -} // namespace tensors - -// Expression templates -#include -#include -#include -// Tensor4 not needed so far -#include - -// Actual tensor classes -#include -#include -#include -#include -#include - - diff --git a/gtsam/geometry/tests/testFundamental.cpp b/gtsam/geometry/tests/testFundamental.cpp deleted file mode 100644 index c233cbfd0..000000000 --- a/gtsam/geometry/tests/testFundamental.cpp +++ /dev/null @@ -1,73 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testFundamental.cpp - * @brief try tensor expressions based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf - * @date Feb 13, 2010 - * @author: Frank Dellaert - */ - -#include -#include -#include // for operator += -using namespace boost::assign; - -#include - -#include -#include -#include - -using namespace std; -using namespace gtsam; -using namespace tensors; - -/* ************************************************************************* */ -// Indices - -static tensors::Index<3, 'a'> a; -static tensors::Index<3, 'b'> b; - -static tensors::Index<4, 'A'> A; -static tensors::Index<4, 'B'> B; - -/* ************************************************************************* */ -TEST( Tensors, FundamentalMatrix) -{ - double f[3][3] = { { 1, 0, 0 }, { 1, 2, 3 }, { 1, 2, 3 } }; - FundamentalMatrix F(f); - - Point2h p = point2h(1, 2, 3); // point p in view one - Point2h q = point2h(14, -1, 0); // point q in view two - - // points p and q are in correspondence - CHECK(F(a,b)*p(a)*q(b) == 0) - - // in detail, l1(b)*q(b)==0 - Line2h l1 = line2h(1, 14, 14); - CHECK(F(a,b)*p(a) == l1(b)) - CHECK(l1(b)*q(b) == 0); // q is on line l1 - - // and l2(a)*p(a)==0 - Line2h l2 = line2h(13, -2, -3); - CHECK(F(a,b)*q(b) == l2(a)) - CHECK(l2(a)*p(a) == 0); // p is on line l2 -} - - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ - diff --git a/gtsam/geometry/tests/testHomography2.cpp b/gtsam/geometry/tests/testHomography2.cpp deleted file mode 100644 index 85681baa4..000000000 --- a/gtsam/geometry/tests/testHomography2.cpp +++ /dev/null @@ -1,188 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testHomography2.cpp - * @brief Test and estimate 2D homographies - * @date Feb 13, 2010 - * @author Frank Dellaert - */ - -#include -#include -#include // for operator += -using namespace boost::assign; - -#include - -#include -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; -using namespace tensors; - -/* ************************************************************************* */ -// Indices - -static tensors::Index<3, 'a'> a, _a; -static tensors::Index<3, 'b'> b, _b; -static tensors::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))) -} - -/* ************************************************************************* */ -/** - * Computes the homography H(I,_T) from template to image - * given the pose tEc of the camera in the template coordinate frame. - * Assumption is Z is normal to the template, template texture in X-Y plane. - */ -Homography2 patchH(const Pose3& tEc) { - Pose3 cEt = tEc.inverse(); - Rot3 cRt = cEt.rotation(); - Point3 r1 = cRt.r1(), r2 = cRt.r2(), t = cEt.translation(); - - // TODO cleanup !!!! - // column 1 - double H11 = r1.x(); - double H21 = r1.y(); - double H31 = r1.z(); - // column 2 - double H12 = r2.x(); - double H22 = r2.y(); - double H32 = r2.z(); - // column 3 - double H13 = t.x(); - double H23 = t.y(); - double H33 = t.z(); - double data2[3][3] = { { H11, H21, H31 }, { H12, H22, H32 }, - { H13, H23, H33 } }; - return Homography2(data2); -} - -/* ************************************************************************* */ -namespace gtsam { -// size_t dim(const tensors::Tensor2<3, 3>& H) {return 9;} - Vector toVector(const tensors::Tensor2<3, 3>& H) { - tensors::Index<3, 'T'> _T; // covariant 2D template - tensors::Index<3, 'C'> I; // contravariant 2D camera - return toVector(H(I,_T)); - } - Vector localCoordinates(const tensors::Tensor2<3, 3>& A, const tensors::Tensor2<3, 3>& B) { - return toVector(A)-toVector(B); // TODO correct order ? - } -} - -#include - -/* ************************************************************************* */ -TEST( Homography2, patchH) -{ - tensors::Index<3, 'T'> _T; // covariant 2D template - tensors::Index<3, 'C'> I; // contravariant 2D camera - - // data[_T][I] - double data1[3][3] = {{1,0,0},{0,-1,0},{0,0,10}}; - Homography2 expected(data1); - - // camera rotation, looking in negative Z - Rot3 gRc(Point3(1,0,0),Point3(0,-1,0),Point3(0,0,-1)); - Point3 gTc(0,0,10); // Camera location, out on the Z axis - Pose3 gEc(gRc,gTc); // Camera pose - - Homography2 actual = patchH(gEc); - -// GTSAM_PRINT(expected(I,_T)); -// GTSAM_PRINT(actual(I,_T)); - CHECK(assert_equality(expected(I,_T),actual(I,_T))); - - // FIXME: this doesn't appear to be tested, and requires that Tensor2 be a Lie object -// Matrix D = numericalDerivative11(patchH, gEc); -// print(D,"D"); -} - -/* ************************************************************************* */ -TEST( Homography2, patchH2) -{ - tensors::Index<3, 'T'> _T; // covariant 2D template - tensors::Index<3, 'C'> I; // contravariant 2D camera - - // data[_T][I] - double data1[3][3] = {{1,0,0},{0,-1,0},{0,0,10}}; - Homography2 expected(data1); - - // camera rotation, looking in negative Z - Rot3 gRc(Point3(1,0,0),Point3(0,-1,0),Point3(0,0,-1)); - Point3 gTc(0,0,10); // Camera location, out on the Z axis - Pose3 gEc(gRc,gTc); // Camera pose - - Homography2 actual = patchH(gEc); - -// GTSAM_PRINT(expected(I,_T)); -// GTSAM_PRINT(actual(I,_T)); - CHECK(assert_equality(expected(I,_T),actual(I,_T))); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ - diff --git a/gtsam/geometry/tests/testTensors.cpp b/gtsam/geometry/tests/testTensors.cpp deleted file mode 100644 index 0e1c381cf..000000000 --- a/gtsam/geometry/tests/testTensors.cpp +++ /dev/null @@ -1,244 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testTensors.cpp - * @brief try tensor expressions based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf - * @date Feb 9, 2010 - * @author Frank Dellaert - */ - -#include -#include -#include // for operator += -using namespace boost::assign; - -#include - -#include -#include -#include - -using namespace std; -using namespace gtsam; -using namespace tensors; - -/* ************************************************************************* */ -// Indices - -tensors::Index<3, 'a'> a, _a; -tensors::Index<3, 'b'> b, _b; -tensors::Index<3, 'c'> c, _c; - -tensors::Index<4, 'A'> A; -tensors::Index<4, 'B'> B; - -/* ************************************************************************* */ -// Tensor1 -/* ************************************************************************* */ -TEST(Tensor1, Basics) -{ - // you can create 1-tensors corresponding to 2D homogeneous points - // using the function point2h in projectiveGeometry.* - Point2h p = point2h(1, 2, 3), q = point2h(2, 4, 6); - - // equality tests always take tensor expressions, not tensors themselves - // the difference is that a tensor expression has indices - CHECK(p(a)==p(a)) - CHECK(assert_equality(p(a),p(a))) - CHECK(assert_equality(p(a)*2,q(a))) - CHECK(assert_equivalent(p(a),q(a))) // projectively equivalent - - // and you can take a norm, typically for normalization to the sphere - DOUBLES_EQUAL(sqrt(14.0),norm(p(a)),1e-9) -} - -/* ************************************************************************* */ -TEST( Tensor1, Incidence2D) -{ - // 2D lines are created with line2h - Line2h l = line2h(-13, 5, 1); - Point2h p = point2h(1, 2, 3), q = point2h(2, 5, 1); - - // Incidence between a line and a point is checked with simple contraction - // It does not matter which index you use, but it has to be of dimension 3 - DOUBLES_EQUAL(l(a)*p(a),0,1e-9) - DOUBLES_EQUAL(l(b)*q(b),0,1e-9) - DOUBLES_EQUAL(p(a)*l(a),0,1e-9) - DOUBLES_EQUAL(q(a)*l(a),0,1e-9) -} - -/* ************************************************************************* */ -TEST( Tensor1, Incidence3D) -{ - // similar constructs exist for 3D points and planes - Plane3h pi = plane3h(0, 1, 0, -2); - Point3h P = point3h(0, 2, 0, 1), Q = point3h(1, 2, 0, 1); - - // Incidence is checked similarly - DOUBLES_EQUAL(pi(A)*P(A),0,1e-9) - DOUBLES_EQUAL(pi(A)*Q(A),0,1e-9) - DOUBLES_EQUAL(P(A)*pi(A),0,1e-9) - DOUBLES_EQUAL(Q(A)*pi(A),0,1e-9) -} - -/* ************************************************************************* */ -// Tensor2 -/* ************************************************************************* */ -TEST( Tensor2, Outer33) -{ - Line2h l1 = line2h(1, 2, 3), l2 = line2h(1, 3, 5); - - // We can also create tensors directly from data - double data[3][3] = { { 1, 2, 3 }, { 3, 6, 9 }, {5, 10, 15} }; - Tensor2<3, 3> expected(data); - // in this case expected(0) == {1,2,3} - Line2h l0 = expected(a,b)(0); - CHECK(l0(a) == l1(a)) - - // And we create rank 2 tensors from the outer product of two rank 1 tensors - CHECK(expected(a,b) == l1(a) * l2(b)) - - // swap just swaps how you access a tensor, but note the data is the same - CHECK(assert_equality(expected(a,b).swap(), l2(b) * l1(a))); -} - -/* ************************************************************************* */ -TEST( Tensor2, AnotherOuter33) -{ - // first cube point from testFundamental, projected in left and right -// Point2h p = point2h(0, -1, 2), q = point2h(-2, -1, 2); -// print(p(a)*q(b)); -// print(p(b)*q(a)); -// print(q(a)*p(b)); -// print(q(b)*p(a)); -} - -/* ************************************************************************* */ -TEST( Tensor2, Outer34) -{ - Line2h l = line2h(1, 2, 3); - Plane3h pi = plane3h(1, 3, 5, 7); - double - data[4][3] = { { 1, 2, 3 }, { 3, 6, 9 }, { 5, 10, 15 }, { 7, 14, 21 } }; - Tensor2<3, 4> expected(data); - CHECK(assert_equality(expected(a,B),l(a) * pi(B))) - CHECK(assert_equality(expected(a,B).swap(),pi(B) * l(a))) -} - -/* ************************************************************************* */ -TEST( Tensor2, SpecialContract) -{ - double data[3][3] = { { 1, 2, 3 }, { 2, 4, 6 }, { 3, 6, 9 } }; - Tensor2<3, 3> S(data), T(data); - //print(S(a, b) * T(a, c)); // contract a -> b,c - // S(a,0)*T(a,0) = [1 2 3] . [1 2 3] = 14 - // S(a,0)*T(a,2) = [1 2 3] . [3 6 9] = 3+12+27 = 42 - double data2[3][3] = { { 14, 28, 42 }, { 28, 56, 84 }, { 42, 84, 126 } }; - Tensor2<3, 3> expected(data2); - CHECK(assert_equality(expected(b,c), S(a, b) * T(a, c))); -} - -/* ************************************************************************* */ -TEST( Tensor2, ProjectiveCamera) -{ - Point2h p = point2h(1 + 2, 2, 5); - Point3h P = point3h(1, 2, 5, 1); - double data[4][3] = { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }, { 2, 0, 0 } }; - ProjectiveCamera M(data); - CHECK(assert_equality(p(a),M(a,A)*P(A))) -} - -/* ************************************************************************* */ -namespace camera { - // to specify the tensor M(a,A), we need to give four 2D points - double data[4][3] = { { 1, 2, 3 }, { 4, 5, 6 }, { 7, 8, 9 }, { 10, 11, 12 } }; - ProjectiveCamera M(data); - Matrix matrix = Matrix_(4,3,1.,2.,3.,4.,5.,6.,7.,8.,9.,10.,11.,12.); - Vector vector = Vector_( 12,1.,2.,3.,4.,5.,6.,7.,8.,9.,10.,11.,12.); -} - -/* ************************************************************************* */ -TEST( Tensor2, reshape ) -{ - // it is annoying that a camera can only be reshaped to a 4*3 -// print(camera::M(a,A)); - Matrix actual = reshape(camera::M(a,A),4,3); - EQUALITY(camera::matrix,actual); -} - -/* ************************************************************************* */ -TEST( Tensor2, toVector ) -{ - // Vectors are created with the leftmost indices iterating the fastest - Vector actual = toVector(camera::M(a,A)); - CHECK(assert_equal(camera::vector,actual)); -} - -/* ************************************************************************* */ -TEST( Tensor2, reshape2 ) -{ - Tensor2<3,4> actual = reshape2<3,4>(camera::vector); - CHECK(assert_equality(camera::M(a,A),actual(a,A))); - - // reshape Matrix to rank 2 tensor - Tensor2<3,4> actual_m = reshape2matrix<3,4>(camera::matrix); - CHECK(assert_equality(camera::M(a,A), actual_m(a,A))); -} - -/* ************************************************************************* */ -TEST( Tensor2, reshape_33_to_9 ) -{ - double data[3][3] = { { 1, 2, 3 }, { 4, 5, 6 }, { 7, 8, 9 } }; - FundamentalMatrix F(data); - Matrix matrix = Matrix_(1,9,1.,2.,3.,4.,5.,6.,7.,8.,9.); - Matrix actual = reshape(F(a,b),1,9); - EQUALITY(matrix,actual); - Vector v = Vector_( 9,1.,2.,3.,4.,5.,6.,7.,8.,9.); - CHECK(assert_equality(F(a,b),reshape2<3, 3> (v)(a,b))); -} - -/* ************************************************************************* */ -// Tensor3 -/* ************************************************************************* */ -TEST( Tensor3, Join) -{ - Line2h l = line2h(-13, 5, 1); - Point2h p = point2h(1, 2, 3), q = point2h(2, 5, 1); - - // join points into line - Eta3 e; - CHECK(assert_equality(e(a, b, c) * p(a) * q(b), l(c))) -} - -/* ************************************************************************* */ -TEST( Tensor5, Outer32) -{ - double t[3][3][3] = { { { 0, 0, 3 }, { 0, 8, -125 }, { -3, 125, 1 } }, { { 0, - 0, 3 }, { 0, 8, -125 }, { -3, 125, 1 } }, { { 0, 0, 3 }, { 0, 8, -125 }, - { -3, 125, 1 } } }; - TrifocalTensor T(t); - - double data[3][3] = { { 0, 0, 3 }, { 0, 8, -125 }, { -3, 125, 1 } }; - FundamentalMatrix F(data); - - //Index<3, 'd'> d, _d; - //Index<3, 'e'> e, _e; - //print(T(_a,b,c)*F(_d,_e)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ -