147 lines
3.5 KiB
C++
147 lines
3.5 KiB
C++
/*
|
|
* 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 <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.
|
|
*/
|
|
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
|