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

Removed unused code - all moved to gtsam2

release/4.3a0
Alex Cunningham 2012-04-06 20:22:42 +00:00
parent 45f2101f48
commit 0e7f5e6d45
30 changed files with 0 additions and 3961 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

409
gtsam/base/BTree.h
View File

@ -1,409 +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 BTree.h
* @brief purely functional binary tree
* @author Chris Beall
* @author Frank Dellaert
* @date Feb 3, 2010
*/
#include <stack>
#include <sstream>
#include <boost/shared_ptr.hpp>
#include <boost/function.hpp>
namespace gtsam {
/**
* @brief Binary tree
* @ingroup base
*/
template<class KEY, class VALUE>
class BTree {
public:
typedef std::pair<KEY, VALUE> value_type;
private:
/**
* Node in a tree
*/
struct Node {
const size_t height_;
const value_type keyValue_;
const BTree left, right;
/** default constructor */
Node() {
}
/**
* Create leaf node with height 1
* @param keyValue (key,value) pair
*/
Node(const value_type& keyValue) :
height_(1), keyValue_(keyValue) {
}
/**
* Create a node from two subtrees and a key value pair
*/
Node(const BTree& l, const value_type& keyValue, const BTree& r) :
height_(l.height() >= r.height() ? l.height() + 1 : r.height() + 1),
keyValue_(keyValue), left(l), right(r) {
}
inline const KEY& key() const { return keyValue_.first;}
inline const VALUE& value() const { return keyValue_.second;}
}; // Node
// We store a shared pointer to the root of the functional tree
// composed of Node classes. If root_==NULL, the tree is empty.
typedef boost::shared_ptr<const Node> sharedNode;
sharedNode root_;
inline const value_type& keyValue() const { return root_->keyValue_;}
inline const KEY& key() const { return root_->key(); }
inline const VALUE& value() const { return root_->value(); }
inline const BTree& left() const { return root_->left; }
inline const BTree& right() const { return root_->right; }
/** create a new balanced tree out of two trees and a key-value pair */
static BTree balance(const BTree& l, const value_type& xd, const BTree& r) {
size_t hl = l.height(), hr = r.height();
if (hl > hr + 2) {
const BTree& ll = l.left(), lr = l.right();
if (ll.height() >= lr.height())
return BTree(ll, l.keyValue(), BTree(lr, xd, r));
else {
BTree _left(ll, l.keyValue(), lr.left());
BTree _right(lr.right(), xd, r);
return BTree(_left, lr.keyValue(), _right);
}
} else if (hr > hl + 2) {
const BTree& rl = r.left(), rr = r.right();
if (rr.height() >= rl.height())
return BTree(BTree(l, xd, rl), r.keyValue(), rr);
else {
BTree _left(l, xd, rl.left());
BTree _right(rl.right(), r.keyValue(), rr);
return BTree(_left, rl.keyValue(), _right);
}
} else
return BTree(l, xd, r);
}
public:
/** default constructor creates an empty tree */
BTree() {
}
/** copy constructor */
BTree(const BTree& other) :
root_(other.root_) {
}
/** create leaf from key-value pair */
BTree(const value_type& keyValue) :
root_(new Node(keyValue)) {
}
/** create from key-value pair and left, right subtrees */
BTree(const BTree& l, const value_type& keyValue, const BTree& r) :
root_(new Node(l, keyValue, r)) {
}
/** Check whether tree is empty */
bool empty() const {
return !root_;
}
/** add a key-value pair */
BTree add(const value_type& xd) const {
if (empty()) return BTree(xd);
const KEY& x = xd.first;
if (x == key())
return BTree(left(), xd, right());
else if (x < key())
return balance(left().add(xd), keyValue(), right());
else
return balance(left(), keyValue(), right().add(xd));
}
/** add a key-value pair */
BTree add(const KEY& x, const VALUE& d) const {
return add(std::make_pair(x, d));
}
/** member predicate */
bool mem(const KEY& x) const {
if (!root_) return false;
if (x == key()) return true;
if (x < key())
return left().mem(x);
else
return right().mem(x);
}
/** Check whether trees are *exactly* the same (occupy same memory) */
inline bool same(const BTree& other) const {
return (other.root_ == root_);
}
/**
* Check whether trees are structurally the same,
* i.e., contain the same values in same tree-structure.
*/
bool operator==(const BTree& other) const {
if (other.root_ == root_) return true; // if same, we're done
if (empty() && !other.empty()) return false;
if (!empty() && other.empty()) return false;
// both non-empty, recurse: check this key-value pair and subtrees...
return (keyValue() == other.keyValue()) && (left() == other.left())
&& (right() == other.right());
}
inline bool operator!=(const BTree& other) const {
return !operator==(other);
}
/** minimum key binding */
const value_type& min() const {
if (!root_) throw std::invalid_argument("BTree::min: empty tree");
if (left().empty()) return keyValue();
return left().min();
}
/** remove minimum key binding */
BTree remove_min() const {
if (!root_) throw std::invalid_argument("BTree::remove_min: empty tree");
if (left().empty()) return right();
return balance(left().remove_min(), keyValue(), right());
}
/** merge two trees */
static BTree merge(const BTree& t1, const BTree& t2) {
if (t1.empty()) return t2;
if (t2.empty()) return t1;
const value_type& xd = t2.min();
return balance(t1, xd, t2.remove_min());
}
/** remove a key-value pair */
BTree remove(const KEY& x) const {
if (!root_) return BTree();
if (x == key())
return merge(left(), right());
else if (x < key())
return balance(left().remove(x), keyValue(), right());
else
return balance(left(), keyValue(), right().remove(x));
}
/** Return height of the tree, 0 if empty */
size_t height() const {
return (root_ != NULL) ? root_->height_ : 0;
}
/** return size of the tree */
size_t size() const {
if (!root_) return 0;
return left().size() + 1 + right().size();
}
/**
* find a value given a key, throws exception when not found
* Optimized non-recursive version as [find] is crucial for speed
*/
const VALUE& find(const KEY& k) const {
const Node* node = root_.get();
while (node) {
const KEY& key = node->key();
if (k < key) node = node->left.root_.get();
else if (key < k) node = node->right.root_.get();
else return node->value();
}
throw std::invalid_argument("BTree::find: key not found");
}
/** print in-order */
void print(const std::string& s = "") const {
if (empty()) return;
KEY k = key();
std::stringstream ss;
ss << height();
k.print(s + ss.str() + " ");
left().print(s + "L ");
right().print(s + "R ");
}
/** iterate over tree */
void iter(boost::function<void(const KEY&, const VALUE&)> f) const {
if (!root_) return;
left().iter(f);
f(key(), value());
right().iter(f);
}
/** map key-values in tree over function f that computes a new value */
template<class TO>
BTree<KEY, TO> map(boost::function<TO(const KEY&, const VALUE&)> f) const {
if (empty()) return BTree<KEY, TO> ();
std::pair<KEY, TO> xd(key(), f(key(), value()));
return BTree<KEY, TO> (left().map(f), xd, right().map(f));
}
/**
* t.fold(f,a) computes [(f kN dN ... (f k1 d1 a)...)],
* where [k1 ... kN] are the keys of all bindings in [m],
* and [d1 ... dN] are the associated data.
* The associated values are passed to [f] in reverse sort order
*/
template<class ACC>
ACC fold(boost::function<ACC(const KEY&, const VALUE&, const ACC&)> f,
const ACC& a) const {
if (!root_) return a;
ACC ar = right().fold(f, a); // fold over right subtree
ACC am = f(key(), value(), ar); // apply f with current value
return left().fold(f, am); // fold over left subtree
}
/**
* @brief Const iterator
* Not trivial: iterator keeps a stack to indicate current path from root_
*/
class const_iterator {
private:
typedef const_iterator Self;
typedef std::pair<sharedNode, bool> flagged;
/** path to the iterator, annotated with flag */
std::stack<flagged> path_;
const sharedNode& current() const {
return path_.top().first;
}
bool done() const {
return path_.top().second;
}
// The idea is we already iterated through the left-subtree and current key-value.
// We now try pushing left subtree of right onto the stack. If there is no right
// sub-tree, we pop this node of the stack and the parent becomes the iterator.
// We avoid going down a right-subtree that was already visited by checking the flag.
void increment() {
if (path_.empty()) return;
sharedNode t = current()->right.root_;
if (!t || done()) {
// no right subtree, iterator becomes first parent with a non-visited right subtree
path_.pop();
while (!path_.empty() && done())
path_.pop();
} else {
path_.top().second = true; // flag we visited right
// push right root and its left-most path onto the stack
while (t) {
path_.push(std::make_pair(t, false));
t = t->left.root_;
}
}
}
public:
// traits for playing nice with STL
typedef ptrdiff_t difference_type;
typedef std::forward_iterator_tag iterator_category;
typedef std::pair<KEY, VALUE> value_type;
typedef const value_type* pointer;
typedef const value_type& reference;
/** initialize end */
const_iterator() {
}
/** initialize from root */
const_iterator(const sharedNode& root) {
sharedNode t = root;
while (t) {
path_.push(std::make_pair(t, false));
t = t->left.root_;
}
}
/** equality */
bool operator==(const Self& __x) const {
return path_ == __x.path_;
}
/** inequality */
bool operator!=(const Self& __x) const {
return path_ != __x.path_;
}
/** dereference */
reference operator*() const {
if (path_.empty()) throw std::invalid_argument(
"operator*: tried to dereference end");
return current()->keyValue_;
}
/** dereference */
pointer operator->() const {
if (path_.empty()) throw std::invalid_argument(
"operator->: tried to dereference end");
return &(current()->keyValue_);
}
/** pre-increment */
Self& operator++() {
increment();
return *this;
}
/** post-increment */
Self operator++(int) {
Self __tmp = *this;
increment();
return __tmp;
}
}; // const_iterator
// to make BTree work with BOOST_FOREACH
// We do *not* want a non-const iterator
typedef const_iterator iterator;
/** return iterator */
const_iterator begin() const {
return const_iterator(root_);
}
/** return iterator */
const_iterator end() const {
return const_iterator();
}
}; // BTree
} // namespace gtsam

174
gtsam/base/DSF.h
View File

@ -1,174 +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 DSF.h
* @date Mar 26, 2010
* @author Kai Ni
* @brief An implementation of Disjoint set forests (see CLR page 446 and up)
*/
#pragma once
#include <iostream>
#include <list>
#include <set>
#include <map>
#include <boost/foreach.hpp>
#include <gtsam/base/BTree.h>
namespace gtsam {
/**
* Disjoint Set Forest class
*
* Quoting from CLR: A disjoint-set data structure maintains a collection
* S = {S_1,S_2,...} of disjoint dynamic sets. Each set is identified by
* a representative, which is some member of the set.
*
* @ingroup base
*/
template <class KEY>
class DSF : protected BTree<KEY, KEY> {
public:
typedef KEY Label; // label can be different from key, but for now they are same
typedef DSF<KEY> Self;
typedef std::set<KEY> Set;
typedef BTree<KEY, Label> Tree;
typedef std::pair<KEY, Label> KeyLabel;
// constructor
DSF() : Tree() { }
// constructor
DSF(const Tree& tree) : Tree(tree) {}
// constructor with a list of unconnected keys
DSF(const std::list<KEY>& keys) : Tree() { BOOST_FOREACH(const KEY& key, keys) *this = this->add(key, key); }
// constructor with a set of unconnected keys
DSF(const std::set<KEY>& keys) : Tree() { BOOST_FOREACH(const KEY& key, keys) *this = this->add(key, key); }
// create a new singleton, does nothing if already exists
Self makeSet(const KEY& key) const { if (this->mem(key)) return *this; else return this->add(key, key); }
// find the label of the set in which {key} lives
Label findSet(const KEY& key) const {
KEY parent = this->find(key);
return parent == key ? key : findSet(parent); }
// return a new DSF where x and y are in the same set. Kai: the caml implementation is not const, and I followed
Self makeUnion(const KEY& key1, const KEY& key2) { return this->add(findSet_(key2), findSet_(key1)); }
// the in-place version of makeUnion
void makeUnionInPlace(const KEY& key1, const KEY& key2) { *this = this->add(findSet_(key2), findSet_(key1)); }
// create a new singleton with two connected keys
Self makePair(const KEY& key1, const KEY& key2) const { return makeSet(key1).makeSet(key2).makeUnion(key1, key2); }
// create a new singleton with a list of fully connected keys
Self makeList(const std::list<KEY>& keys) const {
Self t = *this;
BOOST_FOREACH(const KEY& key, keys)
t = t.makePair(key, keys.front());
return t;
}
// return a dsf in which all find_set operations will be O(1) due to path compression.
DSF flatten() const {
DSF t = *this;
BOOST_FOREACH(const KeyLabel& pair, (Tree)t)
t.findSet_(pair.first);
return t;
}
// maps f over all keys, must be invertible
DSF map(boost::function<KEY(const KEY&)> func) const {
DSF t;
BOOST_FOREACH(const KeyLabel& pair, (Tree)*this)
t = t.add(func(pair.first), func(pair.second));
return t;
}
// return the number of sets
size_t numSets() const {
size_t num = 0;
BOOST_FOREACH(const KeyLabel& pair, (Tree)*this)
if (pair.first == pair.second) num++;
return num;
}
// return the numer of keys
size_t size() const { return Tree::size(); }
// return all sets, i.e. a partition of all elements
std::map<Label, Set> sets() const {
std::map<Label, Set> sets;
BOOST_FOREACH(const KeyLabel& pair, (Tree)*this)
sets[findSet(pair.second)].insert(pair.first);
return sets;
}
// return a partition of the given elements {keys}
std::map<Label, Set> partition(const std::list<KEY>& keys) const {
std::map<Label, Set> partitions;
BOOST_FOREACH(const KEY& key, keys)
partitions[findSet(key)].insert(key);
return partitions;
}
// get the nodes in the tree with the given label
Set set(const Label& label) const {
Set set;
BOOST_FOREACH(const KeyLabel& pair, (Tree)*this) {
if (pair.second == label || findSet(pair.second) == label)
set.insert(pair.first);
}
return set;
}
/** equality */
bool operator==(const Self& t) const { return (Tree)*this == (Tree)t; }
/** inequality */
bool operator!=(const Self& t) const { return (Tree)*this != (Tree)t; }
// print the object
void print(const std::string& name = "DSF") const {
std::cout << name << std::endl;
BOOST_FOREACH(const KeyLabel& pair, (Tree)*this)
std::cout << (std::string)pair.first << " " << (std::string)pair.second << std::endl;
}
protected:
/**
* same as findSet except with path compression: After we have traversed the path to
* the root, each parent pointer is made to directly point to it
*/
KEY findSet_(const KEY& key) {
KEY parent = this->find(key);
if (parent == key)
return parent;
else {
KEY label = findSet_(parent);
*this = this->add(key, label);
return label;
}
}
};
// shortcuts
typedef DSF<int> DSFInt;
} // namespace gtsam

97
gtsam/base/DSFVector.cpp
View File

@ -1,97 +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 DSFVector.cpp
* @date Jun 25, 2010
* @author Kai Ni
* @brief a faster implementation for DSF, which uses vector rather than btree.
*/
#include <boost/make_shared.hpp>
#include <boost/foreach.hpp>
#include <gtsam/base/DSFVector.h>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
DSFVector::DSFVector (const size_t numNodes) {
v_ = boost::make_shared<V>(numNodes);
int index = 0;
keys_.reserve(numNodes);
for(V::iterator it = v_->begin(); it!=v_->end(); it++, index++) {
*it = index;
keys_.push_back(index);
}
}
/* ************************************************************************* */
DSFVector::DSFVector(const boost::shared_ptr<V>& v_in, const std::vector<size_t>& keys) : keys_(keys) {
v_ = v_in;
BOOST_FOREACH(const size_t key, keys)
(*v_)[key] = key;
}
/* ************************************************************************* */
bool DSFVector::isSingleton(const Label& label) const {
bool result = false;
V::const_iterator it = keys_.begin();
for (; it != keys_.end(); ++it) {
if(findSet(*it) == label) {
if (!result) // find the first occurrence
result = true;
else
return false;
}
}
return result;
}
/* ************************************************************************* */
std::set<size_t> DSFVector::set(const Label& label) const {
std::set<size_t> set;
V::const_iterator it = keys_.begin();
for (; it != keys_.end(); it++) {
if (findSet(*it) == label)
set.insert(*it);
}
return set;
}
/* ************************************************************************* */
std::map<DSFVector::Label, std::set<size_t> > DSFVector::sets() const {
std::map<Label, std::set<size_t> > sets;
V::const_iterator it = keys_.begin();
for (; it != keys_.end(); it++) {
sets[findSet(*it)].insert(*it);
}
return sets;
}
/* ************************************************************************* */
std::map<DSFVector::Label, std::vector<size_t> > DSFVector::arrays() const {
std::map<Label, std::vector<size_t> > arrays;
V::const_iterator it = keys_.begin();
for (; it != keys_.end(); it++) {
arrays[findSet(*it)].push_back(*it);
}
return arrays;
}
/* ************************************************************************* */
void DSFVector::makeUnionInPlace(const size_t& i1, const size_t& i2) {
(*v_)[findSet(i2)] = findSet(i1);
}
} // namespace

79
gtsam/base/DSFVector.h
View File

@ -1,79 +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 DSFVector.h
* @date Jun 25, 2010
* @author Kai Ni
* @brief A faster implementation for DSF, which uses vector rather than btree. As a result, the size of the forest is prefixed.
*/
#pragma once
#include <vector>
#include <map>
#include <set>
#include <boost/shared_ptr.hpp>
namespace gtsam {
/**
* A fast implementation of disjoint set forests that uses vector as underly data structure.
* @ingroup base
*/
class DSFVector {
public:
typedef std::vector<size_t> V; ///< Vector of ints
typedef size_t Label; ///< Label type
typedef V::const_iterator const_iterator; ///< const iterator over V
typedef V::iterator iterator;///< iterator over V
private:
// TODO could use existing memory to improve the efficiency
boost::shared_ptr<V> v_;
std::vector<size_t> keys_;
public:
/// constructor that allocate a new memory
DSFVector(const size_t numNodes);
/// constructor that uses the existing memory
DSFVector(const boost::shared_ptr<V>& v_in, const std::vector<size_t>& keys);
/// find the label of the set in which {key} lives
inline Label findSet(size_t key) const {
size_t parent = (*v_)[key];
while (parent != key) {
key = parent;
parent = (*v_)[key];
}
return parent;
}
/// find whether there is one and only one occurrence for the given {label}
bool isSingleton(const Label& label) const;
/// get the nodes in the tree with the given label
std::set<size_t> set(const Label& label) const;
/// return all sets, i.e. a partition of all elements
std::map<Label, std::set<size_t> > sets() const;
/// return all sets, i.e. a partition of all elements
std::map<Label, std::vector<size_t> > arrays() const;
/// the in-place version of makeUnion
void makeUnionInPlace(const size_t& i1, const size_t& i2);
};
}

118
gtsam/base/FixedVector.h
View File

@ -1,118 +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 FixedVector.h
* @brief Extension of boost's bounded_vector to allow for fixed size operation
* @author Alex Cunningham
*/
#pragma once
#include <stdarg.h>
#include <gtsam/base/Vector.h>
namespace gtsam {
/**
* Fixed size vectors - compatible with boost vectors, but with compile-type
* size checking.
*/
template<size_t N>
class FixedVector : public Eigen::Matrix<double, N, 1> {
public:
typedef Eigen::Matrix<double, N, 1> Base;
/** default constructor */
FixedVector() {}
/** copy constructors */
FixedVector(const FixedVector& v) : Base(v) {}
/** Convert from a variable-size vector to a fixed size vector */
FixedVector(const Vector& v) : Base(v) {}
/** Initialize with a C-style array */
FixedVector(const double* values) {
std::copy(values, values+N, this->data());
}
/**
* nice constructor, dangerous as number of arguments must be exactly right
* and you have to pass doubles !!! always use 0.0 never 0
*
* NOTE: this will throw warnings/explode if there is no argument
* before the variadic section, so there is a meaningless size argument.
*/
FixedVector(size_t n, ...) {
va_list ap;
va_start(ap, n);
for(size_t i = 0 ; i < N ; i++) {
double value = va_arg(ap, double);
(*this)(i) = value;
}
va_end(ap);
}
/**
* Create vector initialized to a constant value
* @param constant value
*/
inline static FixedVector repeat(double value) {
return FixedVector(Base::Constant(value));
}
/**
* Create basis vector of
* with a constant in spot i
* @param index of the one
* @param value is the value to insert into the vector
* @return delta vector
*/
inline static FixedVector delta(size_t i, double value) {
return FixedVector(Base::Unit(i) * value);
}
/**
* Create basis vector,
* with one in spot i
* @param index of the one
* @return basis vector
*/
inline static FixedVector basis(size_t i) { return FixedVector(Base::Unit(i)); }
/**
* Create zero vector
*/
inline static FixedVector zero() { return FixedVector(Base::Zero());}
/**
* Create vector initialized to ones
*/
inline static FixedVector ones() { return FixedVector(FixedVector::Ones());}
static size_t dim() { return Base::max_size; }
void print(const std::string& name="") const { gtsam::print(Vector(*this), name); }
template<size_t M>
bool equals(const FixedVector<M>& other, double tol=1e-9) const {
return false;
}
bool equals(const FixedVector& other, double tol=1e-9) const {
return equal_with_abs_tol(*this,other,tol);
}
};
} // \namespace

210
gtsam/base/tests/testBTree.cpp
View File

@ -1,210 +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 testBTree.cpp
* @date Feb 3, 2010
* @author Chris Beall
* @author Frank Dellaert
*/
#include <boost/shared_ptr.hpp>
#include <boost/foreach.hpp>
#include <boost/assign/std/list.hpp> // for +=
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/BTree.h>
using namespace std;
using namespace gtsam;
typedef pair<size_t, size_t> Range;
typedef BTree<string, Range> RangeTree;
typedef BTree<string, int> IntTree;
static std::stringstream ss;
static string x1("x1"), x2("x2"), x3("x3"), x4("x4"), x5("x5");
typedef pair<string, int> KeyInt;
KeyInt p1(x1, 1), p2(x2, 2), p3(x3, 3), p4(x4, 4), p5(x5, 5);
/* ************************************************************************* */
int f(const string& key, const Range& range) {
return range.first;
}
void g(const string& key, int i) {
ss << (string) key;
}
int add(const string& k, int v, int a) {
return v + a;
}
/* ************************************************************************* */
TEST( BTree, add )
{
RangeTree tree;
CHECK(tree.empty())
LONGS_EQUAL(0,tree.height())
// check the height of tree after adding an element
RangeTree tree1 = tree.add(x1, Range(1, 1));
LONGS_EQUAL(1,tree1.height())
LONGS_EQUAL(1,tree1.size())
CHECK(tree1.find(x1) == Range(1,1))
RangeTree tree2 = tree1.add(x5, Range(5, 2));
RangeTree tree3 = tree2.add(x3, Range(3, 3));
LONGS_EQUAL(3,tree3.size())
CHECK(tree3.find(x5) == Range(5,2))
CHECK(tree3.find(x3) == Range(3,3))
RangeTree tree4 = tree3.add(x2, Range(2, 4));
RangeTree tree5 = tree4.add(x4, Range(4, 5));
LONGS_EQUAL(5,tree5.size())
CHECK(tree5.find(x4) == Range(4,5))
// Test functional nature: tree5 and tree6 have different values for x4
RangeTree tree6 = tree5.add(x4, Range(6, 6));
CHECK(tree5.find(x4) == Range(4,5))
CHECK(tree6.find(x4) == Range(6,6))
// test assignment
RangeTree c5 = tree5;
LONGS_EQUAL(5,c5.size())
CHECK(c5.find(x4) == Range(4,5))
// test map
// After (map f tree5) tree contains (x1,1), (x2,2), etc...
IntTree mapped = tree5.map<int> (f);
LONGS_EQUAL(2,mapped.find(x2));
LONGS_EQUAL(4,mapped.find(x4));
}
/* ************************************************************************* */
TEST( BTree, equality )
{
IntTree tree1 = IntTree().add(p1).add(p2).add(p3).add(p4).add(p5);
CHECK(tree1==tree1)
CHECK(tree1.same(tree1))
IntTree tree2 = IntTree().add(p1).add(p2).add(p3).add(p4).add(p5);
CHECK(tree2==tree1)
CHECK(!tree2.same(tree1))
IntTree tree3 = IntTree().add(p1).add(p2).add(p3).add(p4);
CHECK(tree3!=tree1)
CHECK(tree3!=tree2)
CHECK(!tree3.same(tree1))
CHECK(!tree3.same(tree2))
IntTree tree4 = tree3.add(p5);
CHECK(tree4==tree1)
CHECK(!tree4.same(tree1))
IntTree tree5 = tree1;
CHECK(tree5==tree1)
CHECK(tree5==tree2)
CHECK(tree5.same(tree1))
CHECK(!tree5.same(tree2))
}
/* ************************************************************************* */
TEST( BTree, iterating )
{
IntTree tree = IntTree().add(p1).add(p2).add(p3).add(p4).add(p5);
// test iter
tree.iter(g);
CHECK(ss.str() == string("x1x2x3x4x5"));
// test fold
LONGS_EQUAL(25,tree.fold<int>(add,10))
// test iterator
BTree<string, int>::const_iterator it = tree.begin(), it2 = tree.begin();
CHECK(it==it2)
CHECK(*it == p1)
CHECK(it->first == x1)
CHECK(it->second == 1)
CHECK(*(++it) == p2)
CHECK(it!=it2)
CHECK(it==(++it2))
CHECK(*(++it) == p3)
CHECK(*(it++) == p3)
// post-increment, not so efficient
CHECK(*it == p4)
CHECK(*(++it) == p5)
CHECK((++it)==tree.end())
// acid iterator test: BOOST_FOREACH
int sum = 0;
BOOST_FOREACH(const KeyInt& p, tree)
sum += p.second;
LONGS_EQUAL(15,sum)
// STL iterator test
list<KeyInt> expected, actual;
expected += p1,p2,p3,p4,p5;
copy (tree.begin(),tree.end(),back_inserter(actual));
CHECK(actual==expected)
}
/* ************************************************************************* */
TEST( BTree, remove )
{
IntTree tree5 = IntTree().add(p1).add(p2).add(p3).add(p4).add(p5);
LONGS_EQUAL(5,tree5.size())
CHECK(tree5.mem(x3))
IntTree tree4 = tree5.remove(x3);
LONGS_EQUAL(4,tree4.size())
CHECK(!tree4.mem(x3))
}
/* ************************************************************************* */
TEST( BTree, stress )
{
RangeTree tree;
list<RangeTree::value_type> expected;
int N = 128;
for (int i = 1; i <= N; i++) {
string key('a', i);
Range value(i - 1, i);
tree = tree.add(key, value);
LONGS_EQUAL(i,tree.size())
CHECK(tree.find(key) == value)
expected += make_pair(key, value);
}
// Check height is log(N)
LONGS_EQUAL(8,tree.height())
// stress test iterator
list<RangeTree::value_type> actual;
copy(tree.begin(), tree.end(), back_inserter(actual));
CHECK(actual==expected)
// deconstruct the tree
for (int i = N; i >= N; i--) {
string key('a', i);
tree = tree.remove(key);
LONGS_EQUAL(i-1,tree.size())
CHECK(!tree.mem(key))
}
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

280
gtsam/base/tests/testDSF.cpp
View File

@ -1,280 +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 testDSF.cpp
* @date Mar 26, 2010
* @author nikai
* @brief unit tests for DSF
*/
#include <iostream>
#include <boost/assign/std/list.hpp>
#include <boost/assign/std/set.hpp>
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/DSF.h>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
TEST(DSF, makeSet) {
DSFInt dsf;
dsf = dsf.makeSet(5);
LONGS_EQUAL(1, dsf.size());
}
/* ************************************************************************* */
TEST(DSF, findSet) {
DSFInt dsf;
dsf = dsf.makeSet(5);
dsf = dsf.makeSet(6);
dsf = dsf.makeSet(7);
CHECK(dsf.findSet(5) != dsf.findSet(7));
}
/* ************************************************************************* */
TEST(DSF, makeUnion) {
DSFInt dsf;
dsf = dsf.makeSet(5);
dsf = dsf.makeSet(6);
dsf = dsf.makeSet(7);
dsf = dsf.makeUnion(5,7);
CHECK(dsf.findSet(5) == dsf.findSet(7));
}
/* ************************************************************************* */
TEST(DSF, makeUnion2) {
DSFInt dsf;
dsf = dsf.makeSet(5);
dsf = dsf.makeSet(6);
dsf = dsf.makeSet(7);
dsf = dsf.makeUnion(7,5);
CHECK(dsf.findSet(5) == dsf.findSet(7));
}
/* ************************************************************************* */
TEST(DSF, makeUnion3) {
DSFInt dsf;
dsf = dsf.makeSet(5);
dsf = dsf.makeSet(6);
dsf = dsf.makeSet(7);
dsf = dsf.makeUnion(5,6);
dsf = dsf.makeUnion(6,7);
CHECK(dsf.findSet(5) == dsf.findSet(7));
}
/* ************************************************************************* */
TEST(DSF, makePair) {
DSFInt dsf;
dsf = dsf.makePair(0, 1);
dsf = dsf.makePair(1, 2);
dsf = dsf.makePair(3, 2);
CHECK(dsf.findSet(0) == dsf.findSet(3));
}
/* ************************************************************************* */
TEST(DSF, makeList) {
DSFInt dsf;
list<int> keys; keys += 5, 6, 7;
dsf = dsf.makeList(keys);
CHECK(dsf.findSet(5) == dsf.findSet(7));
}
/* ************************************************************************* */
TEST(DSF, numSets) {
DSFInt dsf;
dsf = dsf.makeSet(5);
dsf = dsf.makeSet(6);
dsf = dsf.makeSet(7);
dsf = dsf.makeUnion(5,6);
LONGS_EQUAL(2, dsf.numSets());
}
/* ************************************************************************* */
TEST(DSF, sets) {
DSFInt dsf;
dsf = dsf.makeSet(5);
dsf = dsf.makeSet(6);
dsf = dsf.makeUnion(5,6);
map<int, set<int> > sets = dsf.sets();
LONGS_EQUAL(1, sets.size());
set<int> expected; expected += 5, 6;
CHECK(expected == sets[dsf.findSet(5)]);
}
/* ************************************************************************* */
TEST(DSF, sets2) {
DSFInt dsf;
dsf = dsf.makeSet(5);
dsf = dsf.makeSet(6);
dsf = dsf.makeSet(7);
dsf = dsf.makeUnion(5,6);
dsf = dsf.makeUnion(6,7);
map<int, set<int> > sets = dsf.sets();
LONGS_EQUAL(1, sets.size());
set<int> expected; expected += 5, 6, 7;
CHECK(expected == sets[dsf.findSet(5)]);
}
/* ************************************************************************* */
TEST(DSF, sets3) {
DSFInt dsf;
dsf = dsf.makeSet(5);
dsf = dsf.makeSet(6);
dsf = dsf.makeSet(7);
dsf = dsf.makeUnion(5,6);
map<int, set<int> > sets = dsf.sets();
LONGS_EQUAL(2, sets.size());
set<int> expected; expected += 5, 6;
CHECK(expected == sets[dsf.findSet(5)]);
}
/* ************************************************************************* */
TEST(DSF, partition) {
DSFInt dsf;
dsf = dsf.makeSet(5);
dsf = dsf.makeSet(6);
dsf = dsf.makeUnion(5,6);
list<int> keys; keys += 5;
map<int, set<int> > partitions = dsf.partition(keys);
LONGS_EQUAL(1, partitions.size());
set<int> expected; expected += 5;
CHECK(expected == partitions[dsf.findSet(5)]);
}
/* ************************************************************************* */
TEST(DSF, partition2) {
DSFInt dsf;
dsf = dsf.makeSet(5);
dsf = dsf.makeSet(6);
dsf = dsf.makeSet(7);
dsf = dsf.makeUnion(5,6);
list<int> keys; keys += 7;
map<int, set<int> > partitions = dsf.partition(keys);
LONGS_EQUAL(1, partitions.size());
set<int> expected; expected += 7;
CHECK(expected == partitions[dsf.findSet(7)]);
}
/* ************************************************************************* */
TEST(DSF, partition3) {
DSFInt dsf;
dsf = dsf.makeSet(5);
dsf = dsf.makeSet(6);
dsf = dsf.makeSet(7);
dsf = dsf.makeUnion(5,6);
list<int> keys; keys += 5, 7;
map<int, set<int> > partitions = dsf.partition(keys);
LONGS_EQUAL(2, partitions.size());
set<int> expected; expected += 5;
CHECK(expected == partitions[dsf.findSet(5)]);
}
/* ************************************************************************* */
TEST(DSF, set) {
DSFInt dsf;
dsf = dsf.makeSet(5);
dsf = dsf.makeSet(6);
dsf = dsf.makeSet(7);
dsf = dsf.makeUnion(5,6);
set<int> set = dsf.set(5);
LONGS_EQUAL(2, set.size());
std::set<int> expected; expected += 5, 6;
CHECK(expected == set);
}
/* ************************************************************************* */
TEST(DSF, set2) {
DSFInt dsf;
dsf = dsf.makeSet(5);
dsf = dsf.makeSet(6);
dsf = dsf.makeSet(7);
dsf = dsf.makeUnion(5,6);
dsf = dsf.makeUnion(6,7);
set<int> set = dsf.set(5);
LONGS_EQUAL(3, set.size());
std::set<int> expected; expected += 5, 6, 7;
CHECK(expected == set);
}
/* ************************************************************************* */
int func(const int& a) { return a + 10; }
TEST(DSF, map) {
DSFInt dsf;
dsf = dsf.makeSet(5);
dsf = dsf.makeSet(6);
dsf = dsf.makeSet(7);
dsf = dsf.makeUnion(5,6);
DSFInt actual = dsf.map(&func);
DSFInt expected;
expected = expected.makeSet(15);
expected = expected.makeSet(16);
expected = expected.makeSet(17);
expected = expected.makeUnion(15,16);
CHECK(actual == expected);
}
/* ************************************************************************* */
TEST(DSF, flatten) {
DSFInt dsf;
dsf = dsf.makePair(1, 2);
dsf = dsf.makePair(2, 3);
dsf = dsf.makePair(5, 6);
dsf = dsf.makePair(6, 7);
dsf = dsf.makeUnion(2, 6);
DSFInt actual = dsf.flatten();
DSFInt expected;
expected = expected.makePair(1, 2);
expected = expected.makePair(1, 3);
expected = expected.makePair(1, 5);
expected = expected.makePair(1, 6);
expected = expected.makePair(1, 7);
CHECK(actual == expected);
}
/* ************************************************************************* */
TEST(DSF, flatten2) {
static string x1("x1"), x2("x2"), x3("x3"), x4("x4");
list<string> keys; keys += x1,x2,x3,x4;
DSF<string> dsf(keys);
dsf = dsf.makeUnion(x1,x2);
dsf = dsf.makeUnion(x3,x4);
dsf = dsf.makeUnion(x1,x3);
CHECK(dsf != dsf.flatten());
DSF<string> expected2;
expected2 = expected2.makePair(x1, x2);
expected2 = expected2.makePair(x1, x3);
expected2 = expected2.makePair(x1, x4);
CHECK(expected2 == dsf.flatten());
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

171
gtsam/base/tests/testDSFVector.cpp
View File

@ -1,171 +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 testDSF.cpp
* @date June 25, 2010
* @author nikai
* @brief unit tests for DSF
*/
#include <iostream>
#include <boost/make_shared.hpp>
#include <boost/assign/std/list.hpp>
#include <boost/assign/std/set.hpp>
#include <boost/assign/std/vector.hpp>
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/DSFVector.h>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
TEST(DSFVectorVector, findSet) {
DSFVector dsf(3);
CHECK(dsf.findSet(0) != dsf.findSet(2));
}
/* ************************************************************************* */
TEST(DSFVectorVector, makeUnionInPlace) {
DSFVector dsf(3);
dsf.makeUnionInPlace(0,2);
CHECK(dsf.findSet(0) == dsf.findSet(2));
}
/* ************************************************************************* */
TEST(DSFVectorVector, makeUnionInPlace2) {
boost::shared_ptr<DSFVector::V> v = boost::make_shared<DSFVector::V>(5);
std::vector<size_t> keys; keys += 1, 3;
DSFVector dsf(v, keys);
dsf.makeUnionInPlace(1,3);
CHECK(dsf.findSet(1) == dsf.findSet(3));
}
/* ************************************************************************* */
TEST(DSFVector, makeUnion2) {
DSFVector dsf(3);
dsf.makeUnionInPlace(2,0);
CHECK(dsf.findSet(0) == dsf.findSet(2));
}
/* ************************************************************************* */
TEST(DSFVector, makeUnion3) {
DSFVector dsf(3);
dsf.makeUnionInPlace(0,1);
dsf.makeUnionInPlace(1,2);
CHECK(dsf.findSet(0) == dsf.findSet(2));
}
/* ************************************************************************* */
TEST(DSFVector, sets) {
DSFVector dsf(2);
dsf.makeUnionInPlace(0,1);
map<size_t, set<size_t> > sets = dsf.sets();
LONGS_EQUAL(1, sets.size());
set<size_t> expected; expected += 0, 1;
CHECK(expected == sets[dsf.findSet(0)]);
}
/* ************************************************************************* */
TEST(DSFVector, arrays) {
DSFVector dsf(2);
dsf.makeUnionInPlace(0,1);
map<size_t, vector<size_t> > arrays = dsf.arrays();
LONGS_EQUAL(1, arrays.size());
vector<size_t> expected; expected += 0, 1;
CHECK(expected == arrays[dsf.findSet(0)]);
}
/* ************************************************************************* */
TEST(DSFVector, sets2) {
DSFVector dsf(3);
dsf.makeUnionInPlace(0,1);
dsf.makeUnionInPlace(1,2);
map<size_t, set<size_t> > sets = dsf.sets();
LONGS_EQUAL(1, sets.size());
set<size_t> expected; expected += 0, 1, 2;
CHECK(expected == sets[dsf.findSet(0)]);
}
/* ************************************************************************* */
TEST(DSFVector, arrays2) {
DSFVector dsf(3);
dsf.makeUnionInPlace(0,1);
dsf.makeUnionInPlace(1,2);
map<size_t, vector<size_t> > arrays = dsf.arrays();
LONGS_EQUAL(1, arrays.size());
vector<size_t> expected; expected += 0, 1, 2;
CHECK(expected == arrays[dsf.findSet(0)]);
}
/* ************************************************************************* */
TEST(DSFVector, sets3) {
DSFVector dsf(3);
dsf.makeUnionInPlace(0,1);
map<size_t, set<size_t> > sets = dsf.sets();
LONGS_EQUAL(2, sets.size());
set<size_t> expected; expected += 0, 1;
CHECK(expected == sets[dsf.findSet(0)]);
}
/* ************************************************************************* */
TEST(DSFVector, arrays3) {
DSFVector dsf(3);
dsf.makeUnionInPlace(0,1);
map<size_t, vector<size_t> > arrays = dsf.arrays();
LONGS_EQUAL(2, arrays.size());
vector<size_t> expected; expected += 0, 1;
CHECK(expected == arrays[dsf.findSet(0)]);
}
/* ************************************************************************* */
TEST(DSFVector, set) {
DSFVector dsf(3);
dsf.makeUnionInPlace(0,1);
set<size_t> set = dsf.set(0);
LONGS_EQUAL(2, set.size());
std::set<size_t> expected; expected += 0, 1;
CHECK(expected == set);
}
/* ************************************************************************* */
TEST(DSFVector, set2) {
DSFVector dsf(3);
dsf.makeUnionInPlace(0,1);
dsf.makeUnionInPlace(1,2);
set<size_t> set = dsf.set(0);
LONGS_EQUAL(3, set.size());
std::set<size_t> expected; expected += 0, 1, 2;
CHECK(expected == set);
}
/* ************************************************************************* */
TEST(DSFVector, isSingleton) {
DSFVector dsf(3);
dsf.makeUnionInPlace(0,1);
CHECK(!dsf.isSingleton(0));
CHECK(!dsf.isSingleton(1));
CHECK( dsf.isSingleton(2));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

87
gtsam/base/tests/testFixedVector.cpp
View File

@ -1,87 +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 testFixedVector.cpp
* @author Alex Cunningham
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/FixedVector.h>
using namespace gtsam;
typedef FixedVector<5> Vector5;
typedef FixedVector<3> Vector3;
static const double tol = 1e-9;
/* ************************************************************************* */
TEST( testFixedVector, conversions ) {
double data1[] = {1.0, 2.0, 3.0};
Vector v1 = Vector_(3, data1);
Vector3 fv1(v1), fv2(data1);
Vector actFv2(fv2);
CHECK(assert_equal(v1, actFv2));
}
/* ************************************************************************* */
TEST( testFixedVector, variable_constructor ) {
Vector3 act(3, 1.0, 2.0, 3.0);
DOUBLES_EQUAL(1.0, act(0), tol);
DOUBLES_EQUAL(2.0, act(1), tol);
DOUBLES_EQUAL(3.0, act(2), tol);
}
/* ************************************************************************* */
TEST( testFixedVector, equals ) {
Vector3 vec1(3, 1.0, 2.0, 3.0), vec2(3, 1.0, 2.0, 3.0), vec3(3, 2.0, 3.0, 4.0);
Vector5 vec4(5, 1.0, 2.0, 3.0, 4.0, 5.0);
CHECK(assert_equal(vec1, vec1, tol));
CHECK(assert_equal(vec1, vec2, tol));
CHECK(assert_equal(vec2, vec1, tol));
CHECK(!vec1.equals(vec3, tol));
CHECK(!vec3.equals(vec1, tol));
CHECK(!vec1.equals(vec4, tol));
CHECK(!vec4.equals(vec1, tol));
}
/* ************************************************************************* */
TEST( testFixedVector, static_constructors ) {
Vector3 actZero = Vector3::zero();
Vector3 expZero(3, 0.0, 0.0, 0.0);
CHECK(assert_equal(expZero, actZero, tol));
Vector3 actOnes = Vector3::ones();
Vector3 expOnes(3, 1.0, 1.0, 1.0);
CHECK(assert_equal(expOnes, actOnes, tol));
Vector3 actRepeat = Vector3::repeat(2.3);
Vector3 expRepeat(3, 2.3, 2.3, 2.3);
CHECK(assert_equal(expRepeat, actRepeat, tol));
Vector3 actBasis = Vector3::basis(1);
Vector3 expBasis(3, 0.0, 1.0, 0.0);
CHECK(assert_equal(expBasis, actBasis, tol));
Vector3 actDelta = Vector3::delta(1, 2.3);
Vector3 expDelta(3, 0.0, 2.3, 0.0);
CHECK(assert_equal(expDelta, actDelta, tol));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

85
gtsam/geometry/Tensor1.h
View File

@ -1,85 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Tensor1.h
* @brief Rank 1 tensors based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf
* @date Feb 10, 2010
* @author Frank Dellaert
*/
#pragma once
#include <gtsam/geometry/tensors.h>
namespace tensors {
/**
* A rank 1 tensor. Actually stores data.
* @ingroup tensors
* \nosubgrouping
*/
template<int N>
class Tensor1 {
double T[N]; ///< Storage
public:
/// @name Standard Constructors
/// @{
/** default constructor */
Tensor1() {
}
/** construct from data */
Tensor1(const double* data) {
for (int i = 0; i < N; i++)
T[i] = data[i];
}
/** construct from expression */
template<class A, char I>
Tensor1(const Tensor1Expression<A, Index<N, I> >& a) {
for (int i = 0; i < N; i++)
T[i] = a(i);
}
/// @}
/// @name Standard Interface
/// @{
/** return data */
inline int dim() const {
return N;
}
/** return data */
inline const double& operator()(int i) const {
return T[i];
}
/** return data */
inline double& operator()(int i) {
return T[i];
}
/// return an expression associated with an index
template<char I> Tensor1Expression<Tensor1, Index<N, I> > operator()(
Index<N, I> index) const {
return Tensor1Expression<Tensor1, Index<N, I> >(*this);
}
/// @}
};
// Tensor1
}// namespace tensors

181
gtsam/geometry/Tensor1Expression.h
View File

@ -1,181 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Tensor1Expression.h
* @brief Tensor expression templates based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf
* @date Feb 10, 2010
* @author Frank Dellaert
*/
#pragma once
#include <cmath>
#include <iostream>
#include <stdexcept>
#include <gtsam/geometry/tensors.h>
namespace tensors {
/**
* Templated class to provide a rank 1 tensor interface to a class.
* This class does not store any data but the result of an expression.
* It is associated with an index.
* @ingroup tensors
* \nosubgrouping
*/
template<class A, class I> class Tensor1Expression {
private:
A iter;
typedef Tensor1Expression<A, I> This;
/** Helper class for multiplying with a double */
class TimesDouble_ {
A iter;
const double s;
public:
/// Constructor
TimesDouble_(const A &a, double s_) :
iter(a), s(s_) {
}
/// Element access
inline double operator()(int i) const {
return iter(i) * s;
}
};
public:
/// @name Standard Constructors
/// @{
/** constructor */
Tensor1Expression(const A &a) :
iter(a) {
}
/// @}
/// @name Testable
/// @{
/** Print */
void print(const std::string s = "") const {
std::cout << s << "{";
std::cout << (*this)(0);
for (int i = 1; i < I::dim; i++)
std::cout << ", "<< (*this)(i);
std::cout << "}" << std::endl;
}
/// equality
template<class B>
bool equals(const Tensor1Expression<B, I> & q, double tol) const {
for (int i = 0; i < I::dim; i++)
if (fabs((*this)(i) - q(i)) > tol) return false;
return true;
}
/// @}
/// @name Standard Interface
/// @{
/** norm */
double norm() const {
double sumsqr = 0.0;
for (int i = 0; i < I::dim; i++)
sumsqr += iter(i) * iter(i);
return sqrt(sumsqr);
}
/// test equivalence
template<class B>
bool equivalent(const Tensor1Expression<B, I> & q, double tol = 1e-9) const {
return ((*this) * (1.0 / norm())).equals(q * (1.0 / q.norm()), tol)
|| ((*this) * (-1.0 / norm())).equals(q * (1.0 / q.norm()), tol);
}
/** Check if two expressions are equal */
template<class B>
bool operator==(const Tensor1Expression<B, I>& e) const {
for (int i = 0; i < I::dim; i++)
if (iter(i) != e(i)) return false;
return true;
}
/** element access */
double operator()(int i) const {
return iter(i);
}
/** mutliply with a double. */
inline Tensor1Expression<TimesDouble_, I> operator*(double s) const {
return TimesDouble_(iter, s);
}
/** Class for contracting two rank 1 tensor expressions, yielding a double. */
template<class B>
inline double operator*(const Tensor1Expression<B, I> &b) const {
double sum = 0.0;
for (int i = 0; i < I::dim; i++)
sum += (*this)(i) * b(i);
return sum;
}
}; // Tensor1Expression
/// @}
/// @name Advanced Interface
/// @{
/** Print a rank 1 expression */
template<class A, class I>
void print(const Tensor1Expression<A, I>& T, const std::string s = "") {
T.print(s);
}
/** norm */
template<class A, class I>
double norm(const Tensor1Expression<A, I>& T) {
return T.norm();
}
/**
* This template works for any two expressions
*/
template<class A, class B, class I>
bool assert_equality(const Tensor1Expression<A, I>& expected,
const Tensor1Expression<B, I>& actual, double tol = 1e-9) {
if (actual.equals(expected, tol)) return true;
std::cout << "Not equal:\n";
expected.print("expected:\n");
actual.print("actual:\n");
return false;
}
/**
* This template works for any two expressions
*/
template<class A, class B, class I>
bool assert_equivalent(const Tensor1Expression<A, I>& expected,
const Tensor1Expression<B, I>& actual, double tol = 1e-9) {
if (actual.equivalent(expected, tol)) return true;
std::cout << "Not equal:\n";
expected.print("expected:\n");
actual.print("actual:\n");
return false;
}
/// @}
} // namespace tensors

84
gtsam/geometry/Tensor2.h
View File

@ -1,84 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Tensor2.h
* @brief Rank 2 Tensor based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf
* @date Feb 10, 2010
* @author Frank Dellaert
*/
#pragma once
#include <gtsam/geometry/tensors.h>
namespace tensors {
/**
* Rank 2 Tensor
* @ingroup tensors
* \nosubgrouping
*/
template<int N1, int N2>
class Tensor2 {
protected:
Tensor1<N1> T[N2]; ///< Storage
public:
/// @name Standard Constructors
/// @{
/** default constructor */
Tensor2() {
}
/// construct from data - expressed in row major form
Tensor2(const double data[N2][N1]) {
for (int j = 0; j < N2; j++)
T[j] = Tensor1<N1> (data[j]);
}
/** construct from expression */
template<class A, char I, char J>
Tensor2(const Tensor2Expression<A, Index<N1, I> , Index<N2, J> >& a) {
for (int j = 0; j < N2; j++)
T[j] = a(j);
}
/// @}
/// @name Standard Interface
/// @{
/** dimension - TODO: is this right for anything other than 3x3? */
size_t dim() const {return N1 * N2;}
/// const element access
const double & operator()(int i, int j) const {
return T[j](i);
}
/// element access
double & operator()(int i, int j) {
return T[j](i);
}
/** convert to expression */
template<char I, char J> Tensor2Expression<Tensor2, Index<N1, I> , Index<
N2, J> > operator()(Index<N1, I> i, Index<N2, J> j) const {
return Tensor2Expression<Tensor2, Index<N1, I> , Index<N2, J> > (*this);
}
/// @}
};
} // namespace tensors

310
gtsam/geometry/Tensor2Expression.h
View File

@ -1,310 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Tensor2Expression.h
* @brief Tensor expression templates based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf
* @date Feb 10, 2010
* @author Frank Dellaert
*/
#pragma once
#include <stdexcept>
#include <iostream>
#include <gtsam/geometry/tensors.h>
namespace tensors {
/**
* Templated class to hold a rank 2 tensor expression.
* @ingroup 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 equal:\n";
expected.print("expected:\n");
actual.print("actual:\n");
return false;
}
/// @}
} // namespace tensors

106
gtsam/geometry/Tensor3.h
View File

@ -1,106 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Tensor3.h
* @brief Rank 3 tensors based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf
* @date Feb 10, 2010
* @author: Frank Dellaert
*/
#pragma once
#include <gtsam/geometry/tensors.h>
namespace tensors {
/**
* Rank 3 Tensor
* @ingroup tensors
* \nosubgrouping
*/
template<int N1, int N2, int N3>
class Tensor3 {
Tensor2<N1, N2> T[N3]; ///< Storage
public:
/// @name Standard Constructors
/// @{
/** default constructor */
Tensor3() {
}
/** construct from data */
Tensor3(const double data[N3][N2][N1]) {
for (int k = 0; k < N3; k++)
T[k] = data[k];
}
/// @}
/// @name Advanced Constructors
/// @{
/** construct from expression */
template<class A, char I, char J, char K>
Tensor3(const Tensor3Expression<A, Index<N1, I> , Index<N2, J> , Index<N3,
K> >& a) {
for (int k = 0; k < N3; k++)
T[k] = a(k);
}
/// @}
/// @name Standard Interface
/// @{
/// element access
double operator()(int i, int j, int k) const {
return T[k](i, j);
}
/** convert to expression */
template<char I, char J, char K> Tensor3Expression<Tensor3, Index<N1, I> ,
Index<N2, J> , Index<N3, K> > operator()(const Index<N1, I>& i,
const Index<N2, J>& j, const Index<N3, K>& k) {
return Tensor3Expression<Tensor3, Index<N1, I> , Index<N2, J> , Index<N3,
K> > (*this);
}
/** convert to expression */
template<char I, char J, char K> Tensor3Expression<const Tensor3, Index<N1, I> ,
Index<N2, J> , Index<N3, K> > operator()(const Index<N1, I>& i,
const Index<N2, J>& j, const Index<N3, K>& k) const {
return Tensor3Expression<const Tensor3, Index<N1, I> , Index<N2, J> , Index<N3,
K> > (*this);
}
}; // Tensor3
/** Rank 3 permutation tensor */
struct Eta3 {
/** calculate value. TODO: wasteful to actually use this */
double operator()(int i, int j, int k) const {
return ((j - i) * (k - i) * (k - j)) / 2;
}
/** create expression */
template<char I, char J, char K> Tensor3Expression<Eta3, Index<3, I> ,
Index<3, J> , Index<3, K> > operator()(const Index<3, I>& i,
const Index<3, J>& j, const Index<3, K>& k) const {
return Tensor3Expression<Eta3, Index<3, I> , Index<3, J> , Index<3, K> > (
*this);
}
}; // Eta
/// @}
} // namespace tensors

194
gtsam/geometry/Tensor3Expression.h
View File

@ -1,194 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Tensor3Expression.h
* @brief Tensor expression templates based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf
* @date Feb 10, 2010
* @author Frank Dellaert
*/
#pragma once
#include <iostream>
#include <gtsam/geometry/tensors.h>
namespace tensors {
/**
* templated class to interface to an object A as a rank 3 tensor
* @ingroup tensors
* \nosubgrouping
*/
template<class A, class I, class J, class K> class Tensor3Expression {
A iter;
typedef Tensor3Expression<A, I, J, K> This;
/** Helper class for instantiating one index */
class FixK_ {
const int k;
const A iter;
public:
FixK_(int k_, const A &a) :
k(k_), iter(a) {
}
double operator()(int i, int j) const {
return iter(i, j, k);
}
};
/** Helper class for contracting rank3 and rank1 tensor */
template<class B> class TimesRank1_ {
typedef Tensor1Expression<B, I> Rank1;
const This T;
const Rank1 t;
public:
TimesRank1_(const This &a, const Rank1 &b) :
T(a), t(b) {
}
double operator()(int j, int k) const {
double sum = 0.0;
for (int i = 0; i < I::dim; i++)
sum += T(i, j, k) * t(i);
return sum;
}
};
public:
/// @name Standard Constructors
/// @{
/** constructor */
Tensor3Expression(const A &a) :
iter(a) {
}
/// @}
/// @name Standard Interface
/// @{
/** Print */
void print(const std::string& s = "Tensor3:") const {
std::cout << s << "{";
(*this)(0).print("");
for (int k = 1; k < K::dim; k++) {
std::cout << ",";
(*this)(k).print("");
}
std::cout << "}" << std::endl;
}
/// test equality
template<class B>
bool equals(const Tensor3Expression<B, I, J, K> & q, double tol) const {
for (int k = 0; k < K::dim; k++)
if (!(*this)(k).equals(q(k), tol)) return false;
return true;
}
/** element access */
double operator()(int i, int j, int k) const {
return iter(i, j, k);
}
/** Fix a single index */
Tensor2Expression<FixK_, I, J> operator()(int k) const {
return FixK_(k, iter);
}
/** Contracting with rank1 tensor */
template<class B>
inline Tensor2Expression<TimesRank1_<B> , J, K> operator*(
const Tensor1Expression<B, I> &b) const {
return TimesRank1_<B> (*this, b);
}
}; // Tensor3Expression
/// @}
/// @name Advanced Interface
/// @{
/** Print */
template<class A, class I, class J, class K>
void print(const Tensor3Expression<A, I, J, K>& T, const std::string& s =
"Tensor3:") {
T.print(s);
}
/** Helper class for outer product of rank2 and rank1 tensor */
template<class A, class I, class J, class B, class K>
class Rank2Rank1_ {
typedef Tensor2Expression<A, I, J> Rank2;
typedef Tensor1Expression<B, K> Rank1;
const Rank2 iterA;
const Rank1 iterB;
public:
/// Constructor
Rank2Rank1_(const Rank2 &a, const Rank1 &b) :
iterA(a), iterB(b) {
}
/// Element access
double operator()(int i, int j, int k) const {
return iterA(i, j) * iterB(k);
}
};
/** outer product of rank2 and rank1 tensor */
template<class A, class I, class J, class B, class K>
inline Tensor3Expression<Rank2Rank1_<A, I, J, B, K> , I, J, K> operator*(
const Tensor2Expression<A, I, J>& a, const Tensor1Expression<B, K> &b) {
return Rank2Rank1_<A, I, J, B, K> (a, b);
}
/** Helper class for outer product of rank1 and rank2 tensor */
template<class A, class I, class B, class J, class K>
class Rank1Rank2_ {
typedef Tensor1Expression<A, I> Rank1;
typedef Tensor2Expression<B, J, K> Rank2;
const Rank1 iterA;
const Rank2 iterB;
public:
/// Constructor
Rank1Rank2_(const Rank1 &a, const Rank2 &b) :
iterA(a), iterB(b) {
}
/// Element access
double operator()(int i, int j, int k) const {
return iterA(i) * iterB(j, k);
}
};
/** outer product of rank2 and rank1 tensor */
template<class A, class I, class J, class B, class K>
inline Tensor3Expression<Rank1Rank2_<A, I, B, J, K> , I, J, K> operator*(
const Tensor1Expression<A, I>& a, const Tensor2Expression<B, J, K> &b) {
return Rank1Rank2_<A, I, B, J, K> (a, b);
}
/**
* This template works for any two expressions
*/
template<class A, class B, class I, class J, class K>
bool assert_equality(const Tensor3Expression<A, I, J, K>& expected,
const Tensor3Expression<B, I, J, K>& actual, double tol = 1e-9) {
if (actual.equals(expected, tol)) return true;
std::cout << "Not equal:\n";
expected.print("expected:\n");
actual.print("actual:\n");
return false;
}
/// @}
} // namespace tensors

58
gtsam/geometry/Tensor4.h
View File

@ -1,58 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Tensor4.h
* @brief Rank 4 tensors based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf
* @date Feb 12, 2010
* @author Frank Dellaert
*/
#pragma once
#include <gtsam/geometry/tensors.h>
namespace tensors {
/**
* Rank 4 Tensor
* @ingroup tensors
* \nosubgrouping
*/
template<int N1, int N2, int N3, int N4>
class Tensor4 {
private:
Tensor3<N1, N2, N3> T[N4]; ///< Storage
public:
/// @name Standard Constructors
/// @{
/** default constructor */
Tensor4() {
}
/// @}
/// @name Standard Interface
/// @{
/// element access
double operator()(int i, int j, int k, int l) const {
return T[l](i, j, k);
}
/// @}
}; // Tensor4
} // namespace tensors

75
gtsam/geometry/Tensor5.h
View File

@ -1,75 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Tensor5.h
* @brief Rank 5 tensors based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf
* @date Feb 12, 2010
* @author Frank Dellaert
*/
#pragma once
#include <gtsam/geometry/tensors.h>
namespace tensors {
/**
* Rank 5 Tensor
* @ingroup tensors
* \nosubgrouping
*/
template<int N1, int N2, int N3, int N4, int N5>
class Tensor5 {
private:
Tensor4<N1, N2, N3, N4> T[N5]; ///< Storage
public:
/// @name Standard Constructors
/// @{
/** default constructor */
Tensor5() {
}
/// @}
/// @name Standard Interface
/// @{
/** construct from expression */
template<class A, char I, char J, char K, char L, char M>
Tensor5(const Tensor5Expression<A, Index<N1, I> , Index<N2, J> , Index<N3,
K> , Index<N4, L> , Index<N5, M> >& a) {
for (int m = 0; m < N5; m++)
T[m] = a(m);
}
/// element access
double operator()(int i, int j, int k, int l, int m) const {
return T[m](i, j, k, l);
}
/** convert to expression */
template<char I, char J, char K, char L, char M> Tensor5Expression<Tensor5,
Index<N1, I> , Index<N2, J> , Index<N3, K> , Index<N4, L> ,
Index<N5, M> > operator()(Index<N1, I> i, Index<N2, J> j,
Index<N3, K> k, Index<N4, L> l, Index<N5, M> m) {
return Tensor5Expression<Tensor5, Index<N1, I> , Index<N2, J> , Index<N3,
K> , Index<N4, L> , Index<N5, M> > (*this);
}
/// @}
}; // Tensor5
} // namespace tensors

135
gtsam/geometry/Tensor5Expression.h
View File

@ -1,135 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Tensor5Expression.h
* @brief Tensor expression templates based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf
* @date Feb 10, 2010
* @author Frank Dellaert
*/
#pragma once
#include <iostream>
#include <gtsam/geometry/tensors.h>
namespace tensors {
/**
* templated class to interface to an object A as a rank 5 tensor
* @ingroup tensors
* \nosubgrouping
*/
template<class A, class I, class J, class K, class L, class M> class Tensor5Expression {
A iter;
typedef Tensor5Expression<A, I, J, K, L, M> This;
/** Helper class for swapping indices 3 and 4 :-) */
class Swap34_ {
const A iter;
public:
/// Constructor
Swap34_(const A &a) :
iter(a) {
}
/// swapping element access
double operator()(int i, int j, int k, int l, int m) const {
return iter(i, j, l, k, m);
}
};
public:
/// @name Standard Constructors
/// @{
/** constructor */
Tensor5Expression(const A &a) :
iter(a) {
}
/// @}
/// @name Standard Interface
/// @{
/** Print */
void print(const std::string& s = "Tensor5:") const {
std::cout << s << std::endl;
for (int m = 0; m < M::dim; m++)
for (int l = 0; l < L::dim; l++)
for (int k = 0; k < K::dim; k++) {
std::cout << "(m,l,k) = (" << m << "," << l << "," << k << ")"
<< std::endl;
for (int j = 0; j < J::dim; j++) {
for (int i = 0; i < I::dim; i++)
std::cout << " " << (*this)(i, j, k, l, m);
std::cout << std::endl;
}
}
std::cout << std::endl;
}
/** swap indices */
typedef Tensor5Expression<Swap34_, I, J, L, K, M> Swapped;
/// create Swap34_ helper class
Swapped swap34() {
return Swap34_(iter);
}
/** element access */
double operator()(int i, int j, int k, int l, int m) const {
return iter(i, j, k, l, m);
}
};
// Tensor5Expression
/// @}
/// @name Advanced Interface
/// @{
/** Print */
template<class A, class I, class J, class K, class L, class M>
void print(const Tensor5Expression<A, I, J, K, L, M>& T,
const std::string& s = "Tensor5:") {
T.print(s);
}
/** Helper class for outer product of rank3 and rank2 tensor */
template<class A, class I, class J, class K, class B, class L, class M>
class Rank3Rank2_ {
typedef Tensor3Expression<A, I, J, K> Rank3;
typedef Tensor2Expression<B, L, M> Rank2;
const Rank3 iterA;
const Rank2 iterB;
public:
/// Constructor
Rank3Rank2_(const Rank3 &a, const Rank2 &b) :
iterA(a), iterB(b) {
}
/// Element access
double operator()(int i, int j, int k, int l, int m) const {
return iterA(i, j, k) * iterB(l, m);
}
};
/** outer product of rank2 and rank1 tensor */
template<class A, class I, class J, class K, class B, class L, class M>
inline Tensor5Expression<Rank3Rank2_<A, I, J, K, B, L, M> , I, J, K, L, M> operator*(
const Tensor3Expression<A, I, J, K>& a,
const Tensor2Expression<B, L, M> &b) {
return Rank3Rank2_<A, I, J, K, B, L, M>(a, b);
}
/// @}
} // namespace tensors

71
gtsam/geometry/projectiveGeometry.cpp
View File

@ -1,71 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file projectiveGeometry.cpp
* @brief Projective geometry, implemented using tensor library
* @date Feb 12, 2010
* @author: Frank Dellaert
*/
#include <boost/foreach.hpp>
#include <gtsam/base/Matrix.h>
#include <gtsam/geometry/tensorInterface.h>
#include <gtsam/geometry/projectiveGeometry.h>
namespace gtsam {
using namespace std;
using namespace tensors;
/* ************************************************************************* */
Point2h point2h(double x, double y, double w) {
double data[3];
data[0] = x;
data[1] = y;
data[2] = w;
return data;
}
/* ************************************************************************* */
Line2h line2h(double a, double b, double c) {
double data[3];
data[0] = a;
data[1] = b;
data[2] = c;
return data;
}
/* ************************************************************************* */
Point3h point3h(double X, double Y, double Z, double W) {
double data[4];
data[0] = X;
data[1] = Y;
data[2] = Z;
data[3] = W;
return data;
}
/* ************************************************************************* */
Plane3h plane3h(double a, double b, double c, double d) {
double data[4];
data[0] = a;
data[1] = b;
data[2] = c;
data[3] = d;
return data;
}
/* ************************************************************************* */
} // namespace gtsam

124
gtsam/geometry/projectiveGeometry.h
View File

@ -1,124 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file projectiveGeometry.h
* @brief Projective geometry, implemented using tensor library
* @date Feb 12, 2010
* @author Frank Dellaert
*/
#pragma once
#include <list>
#include <gtsam/geometry/tensors.h>
namespace gtsam {
/**
* 2D Point in homogeneous coordinates
* @ingroup geometry
*/
typedef tensors::Tensor1<3> Point2h;
Point2h point2h(double x, double y, double w); ///< create Point2h
/**
* 2D Line in homogeneous coordinates
* @ingroup geometry
*/
typedef tensors::Tensor1<3> Line2h;
Line2h line2h(double a, double b, double c); ///< create Line2h
/**
* 2D (homegeneous) Point correspondence
* @ingroup 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
* @ingroup geometry
*/
typedef tensors::Tensor2<3, 3> Homography2;
/**
* Fundamental Matrix
* @ingroup geometry
*/
typedef tensors::Tensor2<3, 3> FundamentalMatrix;
/**
* Triplet of (homogeneous) 2D points
* @ingroup 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
* @ingroup geometry
*/
typedef tensors::Tensor3<3, 3, 3> TrifocalTensor;
/**
* 3D Point in homogeneous coordinates
* @ingroup geometry
*/
typedef tensors::Tensor1<4> Point3h;
Point3h point3h(double X, double Y, double Z, double W); ///< create Point3h
/**
* 3D Plane in homogeneous coordinates
* @ingroup geometry
*/
typedef tensors::Tensor1<4> Plane3h;
Plane3h plane3h(double a, double b, double c, double d); ///< create Plane3h
/**
* 3D to 2D projective camera
* @ingroup geometry
*/
typedef tensors::Tensor2<3, 4> ProjectiveCamera;
} // namespace gtsam

26
gtsam/geometry/tensorInterface.cpp
View File

@ -1,26 +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.cpp
* @brief Interfacing tensors template library and gtsam
* @date Feb 12, 2010
* @author Frank Dellaert
*/
#include <gtsam/geometry/tensorInterface.h>
using namespace std;
using namespace tensors;
namespace gtsam {
} // namespace gtsam

108
gtsam/geometry/tensorInterface.h
View File

@ -1,108 +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 rank 3 tensor into Matrix */
template<class A, class I, class J, class K>
Matrix reshape(const tensors::Tensor3Expression<A, I, J, K>& T, int m, int n) {
if (m * n != I::dim * J::dim * K::dim) throw std::invalid_argument(
"reshape: incompatible dimensions");
Matrix M(m, n);
int t = 0;
for (int k = 0; k < K::dim; k++)
for (int i = 0; i < I::dim; i++)
for (int j = 0; j < J::dim; j++)
M.data()[t++] = T(i, j, k);
return M;
}
/** Reshape Vector into rank 3 tensor */
template<int N1, int N2, int N3>
tensors::Tensor3<N1, N2, N3> reshape3(const Vector& v) {
if (v.size() != N1 * N2 * N3) throw std::invalid_argument(
"reshape3: incompatible dimensions");
double data[N3][N2][N1];
int t = 0;
for (int k = 0; k < N3; k++)
for (int j = 0; j < N2; j++)
for (int i = 0; i < N1; i++)
data[k][j][i] = v(t++);
return tensors::Tensor3<N1, N2, N3>(data);
}
/** Reshape rank 5 tensor into Matrix */
template<class A, class I, class J, class K, class L, class M>
Matrix reshape(const tensors::Tensor5Expression<A, I, J, K, L, M>& T, int m,
int n) {
if (m * n != I::dim * J::dim * K::dim * L::dim * M::dim) throw std::invalid_argument(
"reshape: incompatible dimensions");
Matrix R(m, n);
int t = 0;
for (int m = 0; m < M::dim; m++)
for (int l = 0; l < L::dim; l++)
for (int k = 0; k < K::dim; k++)
for (int i = 0; i < I::dim; i++)
for (int j = 0; j < J::dim; j++)
R.data()[t++] = T(i, j, k, l, m);
return R;
}
} // namespace gtsam

45
gtsam/geometry/tensors.h
View File

@ -1,45 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file tensors.h
* @brief Tensor expression templates based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf
* @date Feb 10, 2010
* @author Frank Dellaert
* @defgroup tensors
*/
#pragma once
namespace tensors {
/** index */
template<int Dim, char C> struct Index {
static const int dim = Dim; ///< dimension
};
} // namespace tensors
// Expression templates
#include <gtsam/geometry/Tensor1Expression.h>
#include <gtsam/geometry/Tensor2Expression.h>
#include <gtsam/geometry/Tensor3Expression.h>
// Tensor4 not needed so far
#include <gtsam/geometry/Tensor5Expression.h>
// Actual tensor classes
#include <gtsam/geometry/Tensor1.h>
#include <gtsam/geometry/Tensor2.h>
#include <gtsam/geometry/Tensor3.h>
#include <gtsam/geometry/Tensor4.h>
#include <gtsam/geometry/Tensor5.h>

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

@ -1,73 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testFundamental.cpp
* @brief try tensor expressions based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf
* @date Feb 13, 2010
* @author: Frank Dellaert
*/
#include <iostream>
#include <boost/foreach.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include <gtsam/geometry/tensors.h>
#include <gtsam/geometry/tensorInterface.h>
#include <gtsam/geometry/projectiveGeometry.h>
using namespace std;
using namespace gtsam;
using namespace tensors;
/* ************************************************************************* */
// Indices
Index<3, 'a'> a;
Index<3, 'b'> b;
Index<4, 'A'> A;
Index<4, 'B'> B;
/* ************************************************************************* */
TEST( Tensors, FundamentalMatrix)
{
double f[3][3] = { { 1, 0, 0 }, { 1, 2, 3 }, { 1, 2, 3 } };
FundamentalMatrix F(f);
Point2h p = point2h(1, 2, 3); // point p in view one
Point2h q = point2h(14, -1, 0); // point q in view two
// points p and q are in correspondence
CHECK(F(a,b)*p(a)*q(b) == 0)
// in detail, l1(b)*q(b)==0
Line2h l1 = line2h(1, 14, 14);
CHECK(F(a,b)*p(a) == l1(b))
CHECK(l1(b)*q(b) == 0); // q is on line l1
// and l2(a)*p(a)==0
Line2h l2 = line2h(13, -2, -3);
CHECK(F(a,b)*q(b) == l2(a))
CHECK(l2(a)*p(a) == 0); // p is on line l2
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

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

@ -1,188 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testHomography2.cpp
* @brief Test and estimate 2D homographies
* @date Feb 13, 2010
* @author Frank Dellaert
*/
#include <iostream>
#include <boost/foreach.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/geometry/tensors.h>
#include <gtsam/geometry/tensorInterface.h>
#include <gtsam/geometry/projectiveGeometry.h>
#include <gtsam/geometry/Pose3.h>
using namespace std;
using namespace gtsam;
using namespace tensors;
/* ************************************************************************* */
// Indices
Index<3, 'a'> a, _a;
Index<3, 'b'> b, _b;
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) {
Index<3, 'T'> _T; // covariant 2D template
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)
{
Index<3, 'T'> _T; // covariant 2D template
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)
{
Index<3, 'T'> _T; // covariant 2D template
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);
}
/* ************************************************************************* */

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

@ -1,240 +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
Index<3, 'a'> a, _a;
Index<3, 'b'> b, _b;
Index<3, 'c'> c, _c;
Index<4, 'A'> A;
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),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)));
}
/* ************************************************************************* */
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);
}
/* ************************************************************************* */

1
gtsam/inference/FactorGraph-inl.h
View File

@ -23,7 +23,6 @@
#pragma once
#include <gtsam/inference/BayesTree.h>
#include <gtsam/base/DSF.h>
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>

43
gtsam/slam/simulated3D.cpp
View File

@ -1,43 +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 Simulated3D.cpp
* @brief measurement functions and derivatives for simulated 3D robot
* @author Alex Cunningham
**/
#include <gtsam/slam/simulated3D.h>
namespace gtsam {
namespace simulated3D {
Point3 prior (const Point3& x, boost::optional<Matrix&> H) {
if (H) *H = eye(3);
return x;
}
Point3 odo(const Point3& x1, const Point3& x2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
if (H1) *H1 = -1 * eye(3);
if (H2) *H2 = eye(3);
return x2 - x1;
}
Point3 mea(const Point3& x, const Point3& l,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
if (H1) *H1 = -1 * eye(3);
if (H2) *H2 = eye(3);
return l - x;
}
}} // namespace gtsam::simulated3D

126
gtsam/slam/simulated3D.h
View File

@ -1,126 +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 Simulated3D.h
* @brief measurement functions and derivatives for simulated 3D robot
* @author Alex Cunningham
**/
// \callgraph
#pragma once
#include <gtsam/base/Matrix.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/Symbol.h>
// \namespace
namespace gtsam {
namespace simulated3D {
/**
* This is a linear SLAM domain where both poses and landmarks are
* 3D points, without rotation. The structure and use is based on
* the simulated2D domain.
*/
/// Convenience function for constructing a pose key
inline Symbol PoseKey(Index j) { return Symbol('x', j); }
/// Convenience function for constructing a pose key
inline Symbol PointKey(Index j) { return Symbol('l', j); }
/**
* Prior on a single pose
*/
Point3 prior(const Point3& x, boost::optional<Matrix&> H = boost::none);
/**
* odometry between two poses
*/
Point3 odo(const Point3& x1, const Point3& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none);
/**
* measurement between landmark and pose
*/
Point3 mea(const Point3& x, const Point3& l,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none);
/**
* A prior factor on a single linear robot pose
*/
struct PointPrior3D: public NoiseModelFactor1<Point3> {
Point3 measured_; ///< The prior pose value for the variable attached to this factor
/**
* Constructor for a prior factor
* @param measured is the measured/prior position for the pose
* @param model is the measurement model for the factor (Dimension: 3)
* @param key is the key for the pose
*/
PointPrior3D(const Point3& measured, const SharedNoiseModel& model, Key key) :
NoiseModelFactor1<Point3> (model, key), measured_(measured) {
}
/**
* Evaluates the error at a given value of x,
* with optional derivatives.
* @param x is the current value of the variable
* @param H is an optional Jacobian matrix (Dimension: 3x3)
* @return Vector error between prior value and x (Dimension: 3)
*/
Vector evaluateError(const Point3& x, boost::optional<Matrix&> H =
boost::none) const {
return (prior(x, H) - measured_).vector();
}
};
/**
* Models a linear 3D measurement between 3D points
*/
struct Simulated3DMeasurement: public NoiseModelFactor2<Point3, Point3> {
Point3 measured_; ///< Linear displacement between a pose and landmark
/**
* Creates a measurement factor with a given measurement
* @param measured is the measurement, a linear displacement between poses and landmarks
* @param model is a measurement model for the factor (Dimension: 3)
* @param poseKey is the pose key of the robot
* @param pointKey is the point key for the landmark
*/
Simulated3DMeasurement(const Point3& measured, const SharedNoiseModel& model,
Key poseKey, Key pointKey) :
NoiseModelFactor2<Point3, Point3>(model, poseKey, pointKey), measured_(measured) {}
/**
* Error function with optional derivatives
* @param x1 a robot pose value
* @param x2 a landmark point value
* @param H1 is an optional Jacobian matrix in terms of x1 (Dimension: 3x3)
* @param H2 is an optional Jacobian matrix in terms of x2 (Dimension: 3x3)
* @return vector error between measurement and prediction (Dimension: 3)
*/
Vector evaluateError(const Point3& x1, const Point3& x2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
return (mea(x1, x2, H1, H2) - measured_).vector();
}
};
}} // namespace simulated3D

63
gtsam/slam/tests/testSimulated3D.cpp
View File

@ -1,63 +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 testSimulated3D.cpp
* @brief Unit tests for simulated 3D measurement functions
* @author Alex Cunningham
**/
#include <iostream>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/simulated3D.h>
using namespace gtsam;
using namespace simulated3D;
/* ************************************************************************* */
TEST( simulated3D, Values )
{
Values actual;
actual.insert(simulated3D::PointKey(1),Point3(1,1,1));
actual.insert(simulated3D::PoseKey(2),Point3(2,2,2));
EXPECT(assert_equal(actual,actual,1e-9));
}
/* ************************************************************************* */
TEST( simulated3D, Dprior )
{
Point3 x(1,-9, 7);
Matrix numerical = numericalDerivative11<Point3, Point3>(boost::bind(simulated3D::prior, _1, boost::none),x);
Matrix computed;
simulated3D::prior(x,computed);
EXPECT(assert_equal(numerical,computed,1e-9));
}
/* ************************************************************************* */
TEST( simulated3D, DOdo )
{
Point3 x1(1,-9,7),x2(-5,6,7);
Matrix H1,H2;
simulated3D::odo(x1,x2,H1,H2);
Matrix A1 = numericalDerivative21<Point3, Point3, Point3>(boost::bind(simulated3D::odo, _1, _2, boost::none, boost::none),x1,x2);
EXPECT(assert_equal(A1,H1,1e-9));
Matrix A2 = numericalDerivative22<Point3, Point3, Point3>(boost::bind(simulated3D::odo, _1, _2, boost::none, boost::none),x1,x2);
EXPECT(assert_equal(A2,H2,1e-9));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */