Moved Tensor related Files from CitySLAM
Manohar Paluri
parent
44094b494e
commit
1923778750
|
|
@ -0,0 +1,60 @@
|
|||
/*
|
||||
* Tensor1.h
|
||||
* @brief Rank 1 tensors based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf
|
||||
* Created on: Feb 10, 2010
|
||||
* @author: Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include "tensors.h"
|
||||
|
||||
namespace tensors {
|
||||
|
||||
/** A rank 1 tensor. Actually stores data. */
|
||||
template<int N>
|
||||
class Tensor1 {
|
||||
double T[N];
|
||||
|
||||
public:
|
||||
|
||||
/** 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);
|
||||
}
|
||||
|
||||
/** 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
|
||||
|
|
@ -0,0 +1,146 @@
|
|||
/*
|
||||
* Tensor1Expression.h
|
||||
* @brief Tensor expression templates based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf
|
||||
* Created on: Feb 10, 2010
|
||||
* @author: Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <math.h>
|
||||
#include <iostream>
|
||||
#include <stdexcept>
|
||||
#include "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.
|
||||
*/
|
||||
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:
|
||||
TimesDouble_(const A &a, double s_) :
|
||||
iter(a), s(s_) {
|
||||
}
|
||||
inline double operator()(int i) const {
|
||||
return iter(i) * s;
|
||||
}
|
||||
};
|
||||
|
||||
public:
|
||||
|
||||
/** constructor */
|
||||
Tensor1Expression(const A &a) :
|
||||
iter(a) {
|
||||
}
|
||||
|
||||
/** 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;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
/** norm */
|
||||
double norm() const {
|
||||
double sumsqr = 0.0;
|
||||
for (int i = 0; i < I::dim; i++)
|
||||
sumsqr += iter(i) * iter(i);
|
||||
return sqrt(sumsqr);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
/** 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
|
||||
|
||||
/** 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
|
||||
|
|
@ -0,0 +1,50 @@
|
|||
/*
|
||||
* Tensor2.h
|
||||
* @brief Rank 2 Tensor based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf
|
||||
* Created on: Feb 10, 2010
|
||||
* @author: Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include "tensors.h"
|
||||
|
||||
namespace tensors {
|
||||
|
||||
/** Rank 2 Tensor */
|
||||
template<int N1, int N2>
|
||||
class Tensor2 {
|
||||
protected:
|
||||
Tensor1<N1> T[N2];
|
||||
|
||||
public:
|
||||
|
||||
/** default constructor */
|
||||
Tensor2() {
|
||||
}
|
||||
|
||||
/* construct from data */
|
||||
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);
|
||||
}
|
||||
|
||||
double operator()(int i, int j) const {
|
||||
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
|
||||
|
||||
|
|
@ -0,0 +1,251 @@
|
|||
/*
|
||||
* Tensor2Expression.h
|
||||
* @brief Tensor expression templates based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf
|
||||
* Created on: Feb 10, 2010
|
||||
* @author: Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdexcept>
|
||||
#include <iostream>
|
||||
#include "tensors.h"
|
||||
|
||||
namespace tensors {
|
||||
|
||||
/** Templated class to hold a rank 2 tensor expression. */
|
||||
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:
|
||||
Swap_(const A &a) :
|
||||
iter(a) {
|
||||
}
|
||||
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:
|
||||
TimesDouble_(const A &a, double s_) :
|
||||
iter(a), s(s_) {
|
||||
}
|
||||
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:
|
||||
ITimesRank1_(const This &a_, const Tensor1Expression<B, I> &b_) :
|
||||
a(a_), b(b_) {
|
||||
}
|
||||
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:
|
||||
JTimesRank1_(const This &a_, const Tensor1Expression<B, J> &b_) :
|
||||
a(a_), b(b_) {
|
||||
}
|
||||
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:
|
||||
ITimesRank2_(const This &a_, const Tensor2Expression<B, I, K> &b_) :
|
||||
a(a_), b(b_) {
|
||||
}
|
||||
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:
|
||||
|
||||
/** constructor */
|
||||
Tensor2Expression(const A &a) :
|
||||
iter(a) {
|
||||
}
|
||||
|
||||
/** 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;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
// TODO: broken !!!! Should not treat Tensor1's separately: one scale only!
|
||||
template<class B>
|
||||
bool equivalent(const Tensor2Expression<B, I, J> & q, double tol) const {
|
||||
for (int j = 0; j < J::dim; j++)
|
||||
if (!(*this)(j).equivalent(q(j), tol)) return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
/** element access */
|
||||
double operator()(int i, int j) const {
|
||||
return iter(i, j);
|
||||
}
|
||||
|
||||
/** swap indices */
|
||||
typedef Tensor2Expression<Swap_, J, I> Swapped;
|
||||
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;
|
||||
}
|
||||
|
||||
/** 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:
|
||||
Rank1Rank1_(const Tensor1Expression<A, I> &a,
|
||||
const Tensor1Expression<B, J> &b) :
|
||||
iterA(a), iterB(b) {
|
||||
}
|
||||
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 equal:\n";
|
||||
expected.print("expected:\n");
|
||||
actual.print("actual:\n");
|
||||
return false;
|
||||
}
|
||||
|
||||
} // namespace tensors
|
||||
|
|
@ -0,0 +1,69 @@
|
|||
/*
|
||||
* Tensor3.h
|
||||
* @brief Rank 3 tensors based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf
|
||||
* Created on: Feb 10, 2010
|
||||
* @author: Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include "tensors.h"
|
||||
|
||||
namespace tensors {
|
||||
|
||||
/** Rank 3 Tensor */
|
||||
template<int N1, int N2, int N3>
|
||||
class Tensor3 {
|
||||
Tensor2<N1, N2> T[N3];
|
||||
|
||||
public:
|
||||
|
||||
/** default constructor */
|
||||
Tensor3() {
|
||||
}
|
||||
|
||||
/** construct from data */
|
||||
Tensor3(const double data[N3][N2][N1]) {
|
||||
for (int k = 0; k < N3; k++)
|
||||
T[k] = data[k];
|
||||
}
|
||||
|
||||
/** 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);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
}; // 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
|
||||
|
|
@ -0,0 +1,161 @@
|
|||
/*
|
||||
* Tensor3Expression.h
|
||||
* @brief Tensor expression templates based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf
|
||||
* Created on: Feb 10, 2010
|
||||
* @author: Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <iostream>
|
||||
#include "tensors.h"
|
||||
|
||||
namespace tensors {
|
||||
|
||||
/** templated class to interface to an object A as a rank 3 tensor */
|
||||
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:
|
||||
|
||||
/** constructor */
|
||||
Tensor3Expression(const A &a) :
|
||||
iter(a) {
|
||||
}
|
||||
|
||||
/** 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;
|
||||
}
|
||||
|
||||
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) {
|
||||
return TimesRank1_<B> (*this, b);
|
||||
}
|
||||
|
||||
}; // Tensor3Expression
|
||||
|
||||
/** 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:
|
||||
Rank2Rank1_(const Rank2 &a, const Rank1 &b) :
|
||||
iterA(a), iterB(b) {
|
||||
}
|
||||
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:
|
||||
Rank1Rank2_(const Rank1 &a, const Rank2 &b) :
|
||||
iterA(a), iterB(b) {
|
||||
}
|
||||
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
|
||||
|
|
@ -0,0 +1,33 @@
|
|||
/*
|
||||
* Tensor4.h
|
||||
* @brief Rank 4 tensors based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf
|
||||
* Created on: Feb 12, 2010
|
||||
* @author: Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include "tensors.h"
|
||||
|
||||
namespace tensors {
|
||||
|
||||
/** Rank 3 Tensor */
|
||||
template<int N1, int N2, int N3, int N4>
|
||||
class Tensor4 {
|
||||
|
||||
private:
|
||||
|
||||
Tensor3<N1, N2, N3> T[N4];
|
||||
|
||||
public:
|
||||
|
||||
/** default constructor */
|
||||
Tensor4() {
|
||||
}
|
||||
|
||||
double operator()(int i, int j, int k, int l) const {
|
||||
return T[l](i, j, k);
|
||||
}
|
||||
|
||||
}; // Tensor4
|
||||
|
||||
} // namespace tensors
|
||||
|
|
@ -0,0 +1,49 @@
|
|||
/*
|
||||
* Tensor5.h
|
||||
* @brief Rank 5 tensors based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf
|
||||
* Created on: Feb 12, 2010
|
||||
* @author: Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include "tensors.h"
|
||||
|
||||
namespace tensors {
|
||||
|
||||
/** Rank 3 Tensor */
|
||||
template<int N1, int N2, int N3, int N4, int N5>
|
||||
class Tensor5 {
|
||||
|
||||
private:
|
||||
|
||||
Tensor4<N1, N2, N3, N4> T[N5];
|
||||
|
||||
public:
|
||||
|
||||
/** default constructor */
|
||||
Tensor5() {
|
||||
}
|
||||
|
||||
/** 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);
|
||||
}
|
||||
|
||||
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
|
||||
|
|
@ -0,0 +1,101 @@
|
|||
/*
|
||||
* Tensor5Expression.h
|
||||
* @brief Tensor expression templates based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf
|
||||
* Created on: Feb 10, 2010
|
||||
* @author: Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <iostream>
|
||||
#include "tensors.h"
|
||||
|
||||
namespace tensors {
|
||||
|
||||
/** templated class to interface to an object A as a rank 3 tensor */
|
||||
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:
|
||||
Swap34_(const A &a) :
|
||||
iter(a) {
|
||||
}
|
||||
double operator()(int i, int j, int k, int l, int m) const {
|
||||
return iter(i, j, l, k, m);
|
||||
}
|
||||
};
|
||||
|
||||
public:
|
||||
|
||||
/** constructor */
|
||||
Tensor5Expression(const A &a) :
|
||||
iter(a) {
|
||||
}
|
||||
|
||||
/** 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;
|
||||
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
|
||||
|
||||
/** 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:
|
||||
Rank3Rank2_(const Rank3 &a, const Rank2 &b) :
|
||||
iterA(a), iterB(b) {
|
||||
}
|
||||
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
|
||||
|
|
@ -0,0 +1,118 @@
|
|||
/*
|
||||
* projectiveGeometry.cpp
|
||||
* @brief Projective geometry, implemented using tensor library
|
||||
* Created on: Feb 12, 2010
|
||||
* @author: Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include "tensorInterface.h"
|
||||
#include "projectiveGeometry.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
using namespace std;
|
||||
using namespace tensors;
|
||||
|
||||
static Eta3 eta;
|
||||
static Index<3, 'a'> a, _a;
|
||||
static Index<3, 'b'> b, _b;
|
||||
static Index<3, 'c'> c, _c;
|
||||
static Index<3, 'd'> d, _d;
|
||||
static Index<3, 'd'> e, _e;
|
||||
static Index<3, 'f'> f, _f;
|
||||
static Index<3, 'g'> g, _g;
|
||||
|
||||
/* ************************************************************************* */
|
||||
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[3];
|
||||
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[3];
|
||||
data[0] = a;
|
||||
data[1] = b;
|
||||
data[2] = c;
|
||||
data[3] = d;
|
||||
return data;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Homography2 estimateHomography2(const list<Correspondence>& correspondences) {
|
||||
// Generate entries of A matrix for linear estimation
|
||||
Matrix A;
|
||||
BOOST_FOREACH(const Correspondence& p, correspondences) {
|
||||
Matrix Ap = reshape(p.first(a) * (eta(_b, _c, _d) * p.second(b)),3,9);
|
||||
A = stack(2,&A,&Ap);
|
||||
}
|
||||
|
||||
// Call DLT and reshape to Homography
|
||||
int rank; double error; Vector v;
|
||||
boost::tie(rank, error, v) = DLT(A);
|
||||
if (rank < 8) throw invalid_argument("estimateHomography2: not enough data");
|
||||
return reshape2<3, 3> (v);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
FundamentalMatrix estimateFundamentalMatrix(const list<Correspondence>& correspondences) {
|
||||
// Generate entries of A matrix for linear estimation
|
||||
Matrix A;
|
||||
BOOST_FOREACH(const Correspondence& p, correspondences) {
|
||||
Matrix Ap = reshape(p.first(a) * p.second(b),1,9);
|
||||
A = stack(2,&A,&Ap);
|
||||
}
|
||||
|
||||
// Call DLT and reshape to Homography
|
||||
int rank; double error; Vector v;
|
||||
boost::tie(rank, error, v) = DLT(A);
|
||||
if (rank < 8) throw invalid_argument("estimateFundamentalMatrix: not enough data");
|
||||
return reshape2<3, 3> (v);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TrifocalTensor estimateTrifocalTensor(const list<Triplet>& triplets) {
|
||||
// Generate entries of A matrix for linear estimation
|
||||
Matrix A;
|
||||
BOOST_FOREACH(const Triplet& p, triplets) {
|
||||
Tensor3<3,3,3> T3 = p.first(a)* (eta(_d,_b,_e) * p.second(d));
|
||||
Tensor2<3,3> T2 = eta(_f,_c,_g) * p.third(f);
|
||||
// Take outer product of rank 3 and rank 2, then swap indices 3,4 for reshape
|
||||
// We get a rank 5 tensor T5(a,_b,_c,_e,_g), where _e and _g index the rows...
|
||||
Matrix Ap = reshape((T3(a,_b,_e) * T2(_c,_g)).swap34(), 9, 27);
|
||||
A = stack(2,&A,&Ap);
|
||||
}
|
||||
|
||||
// Call DLT and reshape to Homography
|
||||
int rank; double error; Vector v;
|
||||
boost::tie(rank, error, v) = DLT(A);
|
||||
if (rank < 26) throw invalid_argument("estimateTrifocalTensor: not enough data");
|
||||
return reshape3<3,3,3>(v);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,91 @@
|
|||
/*
|
||||
* projectiveGeometry.h
|
||||
* @brief Projective geometry, implemented using tensor library
|
||||
* Created on: Feb 12, 2010
|
||||
* @author: Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <list>
|
||||
#include "tensors.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/** 2D Point */
|
||||
typedef tensors::Tensor1<3> Point2h;
|
||||
Point2h point2h(double x, double y, double w);
|
||||
|
||||
/** 2D Line */
|
||||
typedef tensors::Tensor1<3> Line2h;
|
||||
Line2h line2h(double a, double b, double c);
|
||||
|
||||
/** 2D Point corrrespondence */
|
||||
struct Correspondence {
|
||||
Point2h first, second;
|
||||
Correspondence(const Point2h &p1, const Point2h &p2) :
|
||||
first(p1), second(p2) {
|
||||
}
|
||||
Correspondence swap() const {
|
||||
return Correspondence(second, first);
|
||||
}
|
||||
void print() {
|
||||
tensors::Index<3, 'i'> i;
|
||||
tensors::print(first(i), "first :");
|
||||
tensors::print(second(i), "second:");
|
||||
}
|
||||
};
|
||||
|
||||
/** 2D-2D Homography */
|
||||
typedef tensors::Tensor2<3, 3> Homography2;
|
||||
|
||||
/**
|
||||
* Estimate homography from point correspondences
|
||||
* Result is H(_a,b) s.t. p.second(b) = H(_a,b) * p.first(a) for all p
|
||||
*/
|
||||
Homography2 estimateHomography2(
|
||||
const std::list<Correspondence>& correspondences);
|
||||
|
||||
/** Fundamental Matrix */
|
||||
typedef tensors::Tensor2<3, 3> FundamentalMatrix;
|
||||
|
||||
/**
|
||||
* Estimate fundamental matrix from point correspondences
|
||||
* Result is F(_a,_b) s.t. H(_a,_b) * p.first(a) * p.second(b) == 0 for all p
|
||||
*/
|
||||
FundamentalMatrix estimateFundamentalMatrix(
|
||||
const std::list<Correspondence>& correspondences);
|
||||
|
||||
/** Triplet of points */
|
||||
struct Triplet {
|
||||
Point2h first, second, third;
|
||||
Triplet(const Point2h &p1, const Point2h &p2, const Point2h &p3) :
|
||||
first(p1), second(p2), third(p3) {
|
||||
}
|
||||
void print() {
|
||||
tensors::Index<3, 'i'> i;
|
||||
tensors::print(first(i), "first :");
|
||||
tensors::print(second(i), "second:");
|
||||
tensors::print(third(i), "third :");
|
||||
}
|
||||
};
|
||||
|
||||
/** Trifocal Tensor */
|
||||
typedef tensors::Tensor3<3, 3, 3> TrifocalTensor;
|
||||
|
||||
/**
|
||||
* Estimate trifocal Tensor from point triplets
|
||||
* Result is T(_a,b,c)
|
||||
*/
|
||||
TrifocalTensor estimateTrifocalTensor(const std::list<Triplet>& triplets);
|
||||
|
||||
/** 3D Point */
|
||||
typedef tensors::Tensor1<4> Point3h;
|
||||
Point3h point3h(double X, double Y, double Z, double W);
|
||||
|
||||
/** 3D Plane */
|
||||
typedef tensors::Tensor1<4> Plane3h;
|
||||
Plane3h plane3h(double a, double b, double c, double d);
|
||||
|
||||
/** 3D to 2D projective camera */
|
||||
typedef tensors::Tensor2<3, 4> ProjectiveCamera;
|
||||
|
||||
} // namespace gtsam
|
||||
Loading…
Reference in New Issue