12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

moved tensors to ransac library. run cmake!

release/4.3a0
Chris Beall 2012-06-21 17:22:05 +00:00
parent 2db389b8cb
commit 0581fe1b24
16 changed files with 0 additions and 2093 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

85
gtsam/geometry/Tensor1.h
View File

@ -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 <gtsam/geometry/tensors.h>
namespace tensors {
/**
* A rank 1 tensor. Actually stores data.
* @addtogroup tensors
* \nosubgrouping
*/
template<int N>
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<class A, char I>
Tensor1(const Tensor1Expression<A, Index<N, I> >& 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<char I> Tensor1Expression<Tensor1, Index<N, I> > operator()(
Index<N, I> index) const {
return Tensor1Expression<Tensor1, Index<N, I> >(*this);
}
/// @}
};
// Tensor1
}// namespace tensors

181
gtsam/geometry/Tensor1Expression.h
View File

@ -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 <cmath>
#include <iostream>
#include <stdexcept>
#include <gtsam/geometry/tensors.h>
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 A, class I> class Tensor1Expression {
private:
A iter;
typedef Tensor1Expression<A, I> 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<class B>
bool equals(const Tensor1Expression<B, I> & 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<class B>
bool equivalent(const Tensor1Expression<B, I> & 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<class B>
bool operator==(const Tensor1Expression<B, I>& 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<TimesDouble_, I> operator*(double s) const {
return TimesDouble_(iter, s);
}
/** Class for contracting two rank 1 tensor expressions, yielding a double. */
template<class B>
inline double operator*(const Tensor1Expression<B, I> &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<class A, class I>
void print(const Tensor1Expression<A, I>& T, const std::string s = "") {
T.print(s);
}
/** norm */
template<class A, class I>
double norm(const Tensor1Expression<A, I>& T) {
return T.norm();
}
/**
* This template works for any two expressions
*/
template<class A, class B, class I>
bool assert_equality(const Tensor1Expression<A, I>& expected,
const Tensor1Expression<B, I>& 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<class A, class B, class I>
bool assert_equivalent(const Tensor1Expression<A, I>& expected,
const Tensor1Expression<B, I>& 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

84
gtsam/geometry/Tensor2.h
View File

@ -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 <gtsam/geometry/tensors.h>
namespace tensors {
/**
* Rank 2 Tensor
* @addtogroup tensors
* \nosubgrouping
*/
template<int N1, int N2>
class Tensor2 {
protected:
Tensor1<N1> 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<N1> (data[j]);
}
/** construct from expression */
template<class A, char I, char J>
Tensor2(const Tensor2Expression<A, Index<N1, I> , Index<N2, J> >& 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<char I, char J> Tensor2Expression<Tensor2, Index<N1, I> , Index<
N2, J> > operator()(Index<N1, I> i, Index<N2, J> j) const {
return Tensor2Expression<Tensor2, Index<N1, I> , Index<N2, J> > (*this);
}
/// @}
};
} // namespace tensors

310
gtsam/geometry/Tensor2Expression.h
View File

@ -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 <stdexcept>
#include <iostream>
#include <gtsam/geometry/tensors.h>
namespace tensors {
/**
* Templated class to hold a rank 2 tensor expression.
* @addtogroup tensors
* \nosubgrouping
*/
template<class A, class I, class J> class Tensor2Expression {
private:
A iter;
typedef Tensor2Expression<A, I, J> 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 B> class ITimesRank1_ {
const This a;
const Tensor1Expression<B, I> b;
public:
/// Constructor
ITimesRank1_(const This &a_, const Tensor1Expression<B, I> &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 B> class JTimesRank1_ {
const This a;
const Tensor1Expression<B, J> b;
public:
/// Constructor
JTimesRank1_(const This &a_, const Tensor1Expression<B, J> &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 B, class K> class ITimesRank2_ {
const This a;
const Tensor2Expression<B, I, K> b;
public:
/// Constructor
ITimesRank2_(const This &a_, const Tensor2Expression<B, I, K> &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<class B>
bool equals(const Tensor2Expression<B, I, J> & 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<class B>
bool equivalent(const Tensor2Expression<B, I, J> & 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<Swap_, J, I> Swapped;
/// Return Swap_ helper class
Swapped swap() {
return Swap_(iter);
}
/** mutliply with a double. */
inline Tensor2Expression<TimesDouble_, I, J> operator*(double s) const {
return TimesDouble_(iter, s);
}
/** Fix a single index */
Tensor1Expression<FixJ_, I> operator()(int j) const {
return FixJ_(j, iter);
}
/** Check if two expressions are equal */
template<class B>
bool operator==(const Tensor2Expression<B, I, J>& 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<class B>
inline Tensor1Expression<ITimesRank1_<B>, J> operator*(
const Tensor1Expression<B, I>& p) {
return ITimesRank1_<B>(*this, p);
}
/** c(i) = a(i,j)*b(j) */
template<class B>
inline Tensor1Expression<JTimesRank1_<B>, I> operator*(
const Tensor1Expression<B, J> &p) {
return JTimesRank1_<B>(*this, p);
}
/** c(j,k) = a(i,j)*T(i,k) */
template<class B, class K>
inline Tensor2Expression<ITimesRank2_<B, K> , J, K> operator*(
const Tensor2Expression<B, I, K>& p) {
return ITimesRank2_<B, K>(*this, p);
}
};
// Tensor2Expression
/** Print */
template<class A, class I, class J>
void print(const Tensor2Expression<A, I, J>& T, const std::string& s =
"Tensor2:") {
T.print(s);
}
/** Helper class for multiplying two covariant tensors */
template<class A, class I, class B, class J> class Rank1Rank1_ {
const Tensor1Expression<A, I> iterA;
const Tensor1Expression<B, J> iterB;
public:
/// Constructor
Rank1Rank1_(const Tensor1Expression<A, I> &a,
const Tensor1Expression<B, J> &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<class A, class I, class B, class J>
inline Tensor2Expression<Rank1Rank1_<A, I, B, J> , I, J> operator*(
const Tensor1Expression<A, I> &a, const Tensor1Expression<B, J> &b) {
return Rank1Rank1_<A, I, B, J>(a, b);
}
/**
* This template works for any two expressions
*/
template<class A, class B, class I, class J>
bool assert_equality(const Tensor2Expression<A, I, J>& expected,
const Tensor2Expression<B, I, J>& 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<class A, class B, class I, class J>
bool assert_equivalent(const Tensor2Expression<A, I, J>& expected,
const Tensor2Expression<B, I, J>& 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

106
gtsam/geometry/Tensor3.h
View File

@ -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 <gtsam/geometry/tensors.h>
namespace tensors {
/**
* Rank 3 Tensor
* @addtogroup tensors
* \nosubgrouping
*/
template<int N1, int N2, int N3>
class Tensor3 {
Tensor2<N1, N2> 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<class A, char I, char J, char K>
Tensor3(const Tensor3Expression<A, Index<N1, I> , Index<N2, J> , Index<N3,
K> >& 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<char I, char J, char K> Tensor3Expression<Tensor3, Index<N1, I> ,
Index<N2, J> , Index<N3, K> > operator()(const Index<N1, I>& i,
const Index<N2, J>& j, const Index<N3, K>& k) {
return Tensor3Expression<Tensor3, Index<N1, I> , Index<N2, J> , Index<N3,
K> > (*this);
}
/** convert to expression */
template<char I, char J, char K> Tensor3Expression<const Tensor3, Index<N1, I> ,
Index<N2, J> , Index<N3, K> > operator()(const Index<N1, I>& i,
const Index<N2, J>& j, const Index<N3, K>& k) const {
return Tensor3Expression<const Tensor3, Index<N1, I> , Index<N2, J> , Index<N3,
K> > (*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<char I, char J, char K> Tensor3Expression<Eta3, Index<3, I> ,
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<Eta3, Index<3, I> , Index<3, J> , Index<3, K> > (
*this);
}
}; // Eta
/// @}
} // namespace tensors

194
gtsam/geometry/Tensor3Expression.h
View File

@ -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 <iostream>
#include <gtsam/geometry/tensors.h>
namespace tensors {
/**
* templated class to interface to an object A as a rank 3 tensor
* @addtogroup tensors
* \nosubgrouping
*/
template<class A, class I, class J, class K> class Tensor3Expression {
A iter;
typedef Tensor3Expression<A, I, J, K> 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 B> class TimesRank1_ {
typedef Tensor1Expression<B, I> 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<class B>
bool equals(const Tensor3Expression<B, I, J, K> & 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<FixK_, I, J> operator()(int k) const {
return FixK_(k, iter);
}
/** Contracting with rank1 tensor */
template<class B>
inline Tensor2Expression<TimesRank1_<B> , J, K> operator*(
const Tensor1Expression<B, I> &b) const {
return TimesRank1_<B> (*this, b);
}
}; // Tensor3Expression
/// @}
/// @name Advanced Interface
/// @{
/** Print */
template<class A, class I, class J, class K>
void print(const Tensor3Expression<A, I, J, K>& T, const std::string& s =
"Tensor3:") {
T.print(s);
}
/** Helper class for outer product of rank2 and rank1 tensor */
template<class A, class I, class J, class B, class K>
class Rank2Rank1_ {
typedef Tensor2Expression<A, I, J> Rank2;
typedef Tensor1Expression<B, K> 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<class A, class I, class J, class B, class K>
inline Tensor3Expression<Rank2Rank1_<A, I, J, B, K> , I, J, K> operator*(
const Tensor2Expression<A, I, J>& a, const Tensor1Expression<B, K> &b) {
return Rank2Rank1_<A, I, J, B, K> (a, b);
}
/** Helper class for outer product of rank1 and rank2 tensor */
template<class A, class I, class B, class J, class K>
class Rank1Rank2_ {
typedef Tensor1Expression<A, I> Rank1;
typedef Tensor2Expression<B, J, K> 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<class A, class I, class J, class B, class K>
inline Tensor3Expression<Rank1Rank2_<A, I, B, J, K> , I, J, K> operator*(
const Tensor1Expression<A, I>& a, const Tensor2Expression<B, J, K> &b) {
return Rank1Rank2_<A, I, B, J, K> (a, b);
}
/**
* This template works for any two expressions
*/
template<class A, class B, class I, class J, class K>
bool assert_equality(const Tensor3Expression<A, I, J, K>& expected,
const Tensor3Expression<B, I, J, K>& 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

58
gtsam/geometry/Tensor4.h
View File

@ -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 <gtsam/geometry/tensors.h>
namespace tensors {
/**
* Rank 4 Tensor
* @addtogroup tensors
* \nosubgrouping
*/
template<int N1, int N2, int N3, int N4>
class Tensor4 {
private:
Tensor3<N1, N2, N3> 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

75
gtsam/geometry/Tensor5.h
View File

@ -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 <gtsam/geometry/tensors.h>
namespace tensors {
/**
* Rank 5 Tensor
* @addtogroup tensors
* \nosubgrouping
*/
template<int N1, int N2, int N3, int N4, int N5>
class Tensor5 {
private:
Tensor4<N1, N2, N3, N4> T[N5]; ///< Storage
public:
/// @name Standard Constructors
/// @{
/** default constructor */
Tensor5() {
}
/// @}
/// @name Standard Interface
/// @{
/** construct from expression */
template<class A, char I, char J, char K, char L, char M>
Tensor5(const Tensor5Expression<A, Index<N1, I> , Index<N2, J> , Index<N3,
K> , Index<N4, L> , Index<N5, M> >& 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<char I, char J, char K, char L, char M> Tensor5Expression<Tensor5,
Index<N1, I> , Index<N2, J> , Index<N3, K> , Index<N4, L> ,
Index<N5, M> > operator()(Index<N1, I> i, Index<N2, J> j,
Index<N3, K> k, Index<N4, L> l, Index<N5, M> m) {
return Tensor5Expression<Tensor5, Index<N1, I> , Index<N2, J> , Index<N3,
K> , Index<N4, L> , Index<N5, M> > (*this);
}
/// @}
}; // Tensor5
} // namespace tensors

135
gtsam/geometry/Tensor5Expression.h
View File

@ -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 <iostream>
#include <gtsam/geometry/tensors.h>
namespace tensors {
/**
* templated class to interface to an object A as a rank 5 tensor
* @addtogroup tensors
* \nosubgrouping
*/
template<class A, class I, class J, class K, class L, class M> class Tensor5Expression {
A iter;
typedef Tensor5Expression<A, I, J, K, L, M> 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<Swap34_, I, J, L, K, M> 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<class A, class I, class J, class K, class L, class M>
void print(const Tensor5Expression<A, I, J, K, L, M>& T,
const std::string& s = "Tensor5:") {
T.print(s);
}
/** Helper class for outer product of rank3 and rank2 tensor */
template<class A, class I, class J, class K, class B, class L, class M>
class Rank3Rank2_ {
typedef Tensor3Expression<A, I, J, K> Rank3;
typedef Tensor2Expression<B, L, M> 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<class A, class I, class J, class K, class B, class L, class M>
inline Tensor5Expression<Rank3Rank2_<A, I, J, K, B, L, M> , I, J, K, L, M> operator*(
const Tensor3Expression<A, I, J, K>& a,
const Tensor2Expression<B, L, M> &b) {
return Rank3Rank2_<A, I, J, K, B, L, M>(a, b);
}
/// @}
} // namespace tensors

71
gtsam/geometry/projectiveGeometry.cpp
View File

@ -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 <boost/foreach.hpp>
#include <gtsam/base/Matrix.h>
#include <gtsam/geometry/tensorInterface.h>
#include <gtsam/geometry/projectiveGeometry.h>
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

124
gtsam/geometry/projectiveGeometry.h
View File

@ -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 <list>
#include <gtsam/geometry/tensors.h>
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

120
gtsam/geometry/tensorInterface.h
View File

@ -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 <gtsam/geometry/tensors.h>
#include <gtsam/base/Matrix.h>
namespace gtsam {
/** Reshape rank 2 tensor into Matrix */
template<class A, class I, class J>
Matrix reshape(const tensors::Tensor2Expression<A, I, J>& 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<class A, class I, class J>
Vector toVector(const tensors::Tensor2Expression<A, I, J>& 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<int N1, int N2>
tensors::Tensor2<N1, N2> 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<N1, N2>(data);
}
/** Reshape Matrix into rank 2 tensor */
template<int N1, int N2>
tensors::Tensor2<N1, N2> 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<N1, N2>(data);
}
/** Reshape rank 3 tensor into Matrix */
template<class A, class I, class J, class K>
Matrix reshape(const tensors::Tensor3Expression<A, I, J, K>& 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<int N1, int N2, int N3>
tensors::Tensor3<N1, N2, N3> 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<N1, N2, N3>(data);
}
/** Reshape rank 5 tensor into Matrix */
template<class A, class I, class J, class K, class L, class M>
Matrix reshape(const tensors::Tensor5Expression<A, I, J, K, L, M>& 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

45
gtsam/geometry/tensors.h
View File

@ -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<int Dim, char C> struct Index {
static const int dim = Dim; ///< dimension
};
} // namespace tensors
// Expression templates
#include <gtsam/geometry/Tensor1Expression.h>
#include <gtsam/geometry/Tensor2Expression.h>
#include <gtsam/geometry/Tensor3Expression.h>
// Tensor4 not needed so far
#include <gtsam/geometry/Tensor5Expression.h>
// Actual tensor classes
#include <gtsam/geometry/Tensor1.h>
#include <gtsam/geometry/Tensor2.h>
#include <gtsam/geometry/Tensor3.h>
#include <gtsam/geometry/Tensor4.h>
#include <gtsam/geometry/Tensor5.h>

73
gtsam/geometry/tests/testFundamental.cpp
View File

@ -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 <iostream>
#include <boost/foreach.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include <gtsam/geometry/tensors.h>
#include <gtsam/geometry/tensorInterface.h>
#include <gtsam/geometry/projectiveGeometry.h>
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);
}
/* ************************************************************************* */

188
gtsam/geometry/tests/testHomography2.cpp
View File

@ -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 <iostream>
#include <boost/foreach.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/geometry/tensors.h>
#include <gtsam/geometry/tensorInterface.h>
#include <gtsam/geometry/projectiveGeometry.h>
#include <gtsam/geometry/Pose3.h>
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<Correspondence> 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 <gtsam/base/numericalDerivative.h>
/* ************************************************************************* */
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<Homography2,Pose3>(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);
}
/* ************************************************************************* */

244
gtsam/geometry/tests/testTensors.cpp
View File

@ -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 <iostream>
#include <boost/foreach.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include <gtsam/geometry/tensors.h>
#include <gtsam/geometry/tensorInterface.h>
#include <gtsam/geometry/projectiveGeometry.h>
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);
}
/* ************************************************************************* */