moved tensors to ransac library. run cmake!
Chris Beall
parent
2db389b8cb
commit
0581fe1b24
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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>
|
||||
|
||||
|
||||
|
|
@ -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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
@ -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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
@ -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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
Loading…
Reference in New Issue