diff --git a/gtsam/base/BTree.h b/gtsam/base/BTree.h deleted file mode 100644 index 2a40465a3..000000000 --- a/gtsam/base/BTree.h +++ /dev/null @@ -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 -#include -#include -#include - -namespace gtsam { - - /** - * @brief Binary tree - * @ingroup base - */ - template - class BTree { - - public: - - typedef std::pair 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 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 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 - BTree map(boost::function f) const { - if (empty()) return BTree (); - std::pair xd(key(), f(key(), value())); - return BTree (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 - ACC fold(boost::function 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 flagged; - - /** path to the iterator, annotated with flag */ - std::stack 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 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 - diff --git a/gtsam/base/DSF.h b/gtsam/base/DSF.h deleted file mode 100644 index 9952f2e10..000000000 --- a/gtsam/base/DSF.h +++ /dev/null @@ -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 -#include -#include -#include -#include -#include - -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 DSF : protected BTree { - - public: - typedef KEY Label; // label can be different from key, but for now they are same - typedef DSF Self; - typedef std::set Set; - typedef BTree Tree; - typedef std::pair KeyLabel; - - // constructor - DSF() : Tree() { } - - // constructor - DSF(const Tree& tree) : Tree(tree) {} - - // constructor with a list of unconnected keys - DSF(const std::list& keys) : Tree() { BOOST_FOREACH(const KEY& key, keys) *this = this->add(key, key); } - - // constructor with a set of unconnected keys - DSF(const std::set& 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& 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 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 sets() const { - std::map 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 partition(const std::list& keys) const { - std::map 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 DSFInt; - -} // namespace gtsam diff --git a/gtsam/base/DSFVector.cpp b/gtsam/base/DSFVector.cpp deleted file mode 100644 index ceb33fc23..000000000 --- a/gtsam/base/DSFVector.cpp +++ /dev/null @@ -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 -#include -#include - -using namespace std; - -namespace gtsam { - - /* ************************************************************************* */ - DSFVector::DSFVector (const size_t numNodes) { - v_ = boost::make_shared(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_in, const std::vector& 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 DSFVector::set(const Label& label) const { - std::set set; - V::const_iterator it = keys_.begin(); - for (; it != keys_.end(); it++) { - if (findSet(*it) == label) - set.insert(*it); - } - return set; - } - - /* ************************************************************************* */ - std::map > DSFVector::sets() const { - std::map > sets; - V::const_iterator it = keys_.begin(); - for (; it != keys_.end(); it++) { - sets[findSet(*it)].insert(*it); - } - return sets; - } - - /* ************************************************************************* */ - std::map > DSFVector::arrays() const { - std::map > 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 - diff --git a/gtsam/base/DSFVector.h b/gtsam/base/DSFVector.h deleted file mode 100644 index 17e897032..000000000 --- a/gtsam/base/DSFVector.h +++ /dev/null @@ -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 -#include -#include -#include - -namespace gtsam { - - /** - * A fast implementation of disjoint set forests that uses vector as underly data structure. - * @ingroup base - */ - class DSFVector { - - public: - typedef std::vector 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_; - std::vector 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_in, const std::vector& 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 set(const Label& label) const; - - /// return all sets, i.e. a partition of all elements - std::map > sets() const; - - /// return all sets, i.e. a partition of all elements - std::map > arrays() const; - - /// the in-place version of makeUnion - void makeUnionInPlace(const size_t& i1, const size_t& i2); - - }; - -} diff --git a/gtsam/base/FixedVector.h b/gtsam/base/FixedVector.h deleted file mode 100644 index 4faa670b6..000000000 --- a/gtsam/base/FixedVector.h +++ /dev/null @@ -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 -#include - -namespace gtsam { - -/** - * Fixed size vectors - compatible with boost vectors, but with compile-type - * size checking. - */ -template -class FixedVector : public Eigen::Matrix { -public: - typedef Eigen::Matrix 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 - bool equals(const FixedVector& 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 diff --git a/gtsam/base/tests/testBTree.cpp b/gtsam/base/tests/testBTree.cpp deleted file mode 100644 index da3003bb5..000000000 --- a/gtsam/base/tests/testBTree.cpp +++ /dev/null @@ -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 -#include -#include // for += -using namespace boost::assign; - -#include -#include - -using namespace std; -using namespace gtsam; - -typedef pair Range; -typedef BTree RangeTree; -typedef BTree IntTree; - -static std::stringstream ss; -static string x1("x1"), x2("x2"), x3("x3"), x4("x4"), x5("x5"); -typedef pair 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 (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(add,10)) - - // test iterator - BTree::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 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 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 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); -} -/* ************************************************************************* */ diff --git a/gtsam/base/tests/testDSF.cpp b/gtsam/base/tests/testDSF.cpp deleted file mode 100644 index fd5ca6e84..000000000 --- a/gtsam/base/tests/testDSF.cpp +++ /dev/null @@ -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 -#include -#include -using namespace boost::assign; -#include - -#include - -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 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 > sets = dsf.sets(); - LONGS_EQUAL(1, sets.size()); - - set 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 > sets = dsf.sets(); - LONGS_EQUAL(1, sets.size()); - - set 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 > sets = dsf.sets(); - LONGS_EQUAL(2, sets.size()); - - set 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 keys; keys += 5; - map > partitions = dsf.partition(keys); - LONGS_EQUAL(1, partitions.size()); - - set 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 keys; keys += 7; - map > partitions = dsf.partition(keys); - LONGS_EQUAL(1, partitions.size()); - - set 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 keys; keys += 5, 7; - map > partitions = dsf.partition(keys); - LONGS_EQUAL(2, partitions.size()); - - set 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 set = dsf.set(5); - LONGS_EQUAL(2, set.size()); - - std::set 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 set = dsf.set(5); - LONGS_EQUAL(3, set.size()); - - std::set 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 keys; keys += x1,x2,x3,x4; - DSF dsf(keys); - dsf = dsf.makeUnion(x1,x2); - dsf = dsf.makeUnion(x3,x4); - dsf = dsf.makeUnion(x1,x3); - - CHECK(dsf != dsf.flatten()); - - DSF 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);} -/* ************************************************************************* */ - diff --git a/gtsam/base/tests/testDSFVector.cpp b/gtsam/base/tests/testDSFVector.cpp deleted file mode 100644 index c0b72f1a0..000000000 --- a/gtsam/base/tests/testDSFVector.cpp +++ /dev/null @@ -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 -#include -#include -#include -#include -using namespace boost::assign; -#include - -#include - -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 v = boost::make_shared(5); - std::vector 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 > sets = dsf.sets(); - LONGS_EQUAL(1, sets.size()); - - set expected; expected += 0, 1; - CHECK(expected == sets[dsf.findSet(0)]); -} - -/* ************************************************************************* */ -TEST(DSFVector, arrays) { - DSFVector dsf(2); - dsf.makeUnionInPlace(0,1); - map > arrays = dsf.arrays(); - LONGS_EQUAL(1, arrays.size()); - - vector 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 > sets = dsf.sets(); - LONGS_EQUAL(1, sets.size()); - - set 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 > arrays = dsf.arrays(); - LONGS_EQUAL(1, arrays.size()); - - vector expected; expected += 0, 1, 2; - CHECK(expected == arrays[dsf.findSet(0)]); -} - -/* ************************************************************************* */ -TEST(DSFVector, sets3) { - DSFVector dsf(3); - dsf.makeUnionInPlace(0,1); - map > sets = dsf.sets(); - LONGS_EQUAL(2, sets.size()); - - set expected; expected += 0, 1; - CHECK(expected == sets[dsf.findSet(0)]); -} - -/* ************************************************************************* */ -TEST(DSFVector, arrays3) { - DSFVector dsf(3); - dsf.makeUnionInPlace(0,1); - map > arrays = dsf.arrays(); - LONGS_EQUAL(2, arrays.size()); - - vector expected; expected += 0, 1; - CHECK(expected == arrays[dsf.findSet(0)]); -} - -/* ************************************************************************* */ -TEST(DSFVector, set) { - DSFVector dsf(3); - dsf.makeUnionInPlace(0,1); - set set = dsf.set(0); - LONGS_EQUAL(2, set.size()); - - std::set expected; expected += 0, 1; - CHECK(expected == set); -} - -/* ************************************************************************* */ -TEST(DSFVector, set2) { - DSFVector dsf(3); - dsf.makeUnionInPlace(0,1); - dsf.makeUnionInPlace(1,2); - set set = dsf.set(0); - LONGS_EQUAL(3, set.size()); - - std::set 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);} -/* ************************************************************************* */ - diff --git a/gtsam/base/tests/testFixedVector.cpp b/gtsam/base/tests/testFixedVector.cpp deleted file mode 100644 index 85a963c89..000000000 --- a/gtsam/base/tests/testFixedVector.cpp +++ /dev/null @@ -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 - -#include - -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); } -/* ************************************************************************* */ - - diff --git a/gtsam/geometry/Tensor1.h b/gtsam/geometry/Tensor1.h deleted file mode 100644 index 685166b53..000000000 --- a/gtsam/geometry/Tensor1.h +++ /dev/null @@ -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 - -namespace tensors { - - /** - * A rank 1 tensor. Actually stores data. - * @ingroup tensors - * \nosubgrouping - */ - template - 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 - Tensor1(const Tensor1Expression >& 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 Tensor1Expression > operator()( - Index index) const { - return Tensor1Expression >(*this); - } - - /// @} - - }; -// Tensor1 - -}// namespace tensors diff --git a/gtsam/geometry/Tensor1Expression.h b/gtsam/geometry/Tensor1Expression.h deleted file mode 100644 index 4e43fff58..000000000 --- a/gtsam/geometry/Tensor1Expression.h +++ /dev/null @@ -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 -#include -#include -#include - -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 Tensor1Expression { - - private: - - A iter; - - typedef Tensor1Expression 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 - bool equals(const Tensor1Expression & 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 - bool equivalent(const Tensor1Expression & 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 - bool operator==(const Tensor1Expression& 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 operator*(double s) const { - return TimesDouble_(iter, s); - } - - /** Class for contracting two rank 1 tensor expressions, yielding a double. */ - template - inline double operator*(const Tensor1Expression &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 - void print(const Tensor1Expression& T, const std::string s = "") { - T.print(s); - } - - /** norm */ - template - double norm(const Tensor1Expression& T) { - return T.norm(); - } - - /** - * This template works for any two expressions - */ - template - bool assert_equality(const Tensor1Expression& expected, - const Tensor1Expression& 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 - bool assert_equivalent(const Tensor1Expression& expected, - const Tensor1Expression& 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 diff --git a/gtsam/geometry/Tensor2.h b/gtsam/geometry/Tensor2.h deleted file mode 100644 index 12fd1509f..000000000 --- a/gtsam/geometry/Tensor2.h +++ /dev/null @@ -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 - -namespace tensors { - -/** - * Rank 2 Tensor - * @ingroup tensors - * \nosubgrouping - */ -template -class Tensor2 { -protected: - Tensor1 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 (data[j]); - } - - /** construct from expression */ - template - Tensor2(const Tensor2Expression , Index >& 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 Tensor2Expression , Index< - N2, J> > operator()(Index i, Index j) const { - return Tensor2Expression , Index > (*this); - } - - /// @} - -}; - -} // namespace tensors - diff --git a/gtsam/geometry/Tensor2Expression.h b/gtsam/geometry/Tensor2Expression.h deleted file mode 100644 index 6c953a5aa..000000000 --- a/gtsam/geometry/Tensor2Expression.h +++ /dev/null @@ -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 -#include -#include - -namespace tensors { - - /** - * Templated class to hold a rank 2 tensor expression. - * @ingroup tensors - * \nosubgrouping - */ - template class Tensor2Expression { - - private: - - A iter; - - typedef Tensor2Expression 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 ITimesRank1_ { - const This a; - const Tensor1Expression b; - public: - /// Constructor - ITimesRank1_(const This &a_, const Tensor1Expression &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 JTimesRank1_ { - const This a; - const Tensor1Expression b; - public: - /// Constructor - JTimesRank1_(const This &a_, const Tensor1Expression &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 ITimesRank2_ { - const This a; - const Tensor2Expression b; - public: - /// Constructor - ITimesRank2_(const This &a_, const Tensor2Expression &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 - bool equals(const Tensor2Expression & 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 - bool equivalent(const Tensor2Expression & 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 Swapped; - /// Return Swap_ helper class - Swapped swap() { - return Swap_(iter); - } - - /** mutliply with a double. */ - inline Tensor2Expression operator*(double s) const { - return TimesDouble_(iter, s); - } - - /** Fix a single index */ - Tensor1Expression operator()(int j) const { - return FixJ_(j, iter); - } - - /** Check if two expressions are equal */ - template - bool operator==(const Tensor2Expression& 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 - inline Tensor1Expression, J> operator*( - const Tensor1Expression& p) { - return ITimesRank1_(*this, p); - } - - /** c(i) = a(i,j)*b(j) */ - template - inline Tensor1Expression, I> operator*( - const Tensor1Expression &p) { - return JTimesRank1_(*this, p); - } - - /** c(j,k) = a(i,j)*T(i,k) */ - template - inline Tensor2Expression , J, K> operator*( - const Tensor2Expression& p) { - return ITimesRank2_(*this, p); - } - - }; - // Tensor2Expression - - /** Print */ - template - void print(const Tensor2Expression& T, const std::string& s = - "Tensor2:") { - T.print(s); - } - - /** Helper class for multiplying two covariant tensors */ - template class Rank1Rank1_ { - const Tensor1Expression iterA; - const Tensor1Expression iterB; - public: - /// Constructor - Rank1Rank1_(const Tensor1Expression &a, - const Tensor1Expression &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 - inline Tensor2Expression , I, J> operator*( - const Tensor1Expression &a, const Tensor1Expression &b) { - return Rank1Rank1_(a, b); - } - - /** - * This template works for any two expressions - */ - template - bool assert_equality(const Tensor2Expression& expected, - const Tensor2Expression& 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 - bool assert_equivalent(const Tensor2Expression& expected, - const Tensor2Expression& 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 diff --git a/gtsam/geometry/Tensor3.h b/gtsam/geometry/Tensor3.h deleted file mode 100644 index 78cdcb9f2..000000000 --- a/gtsam/geometry/Tensor3.h +++ /dev/null @@ -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 - -namespace tensors { - - /** - * Rank 3 Tensor - * @ingroup tensors - * \nosubgrouping - */ - template - class Tensor3 { - Tensor2 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 - Tensor3(const Tensor3Expression , Index , Index >& 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 Tensor3Expression , - Index , Index > operator()(const Index& i, - const Index& j, const Index& k) { - return Tensor3Expression , Index , Index > (*this); - } - - /** convert to expression */ - template Tensor3Expression , - Index , Index > operator()(const Index& i, - const Index& j, const Index& k) const { - return Tensor3Expression , Index , Index > (*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 Tensor3Expression , - 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 , Index<3, J> , Index<3, K> > ( - *this); - } - - }; // Eta - - /// @} - -} // namespace tensors diff --git a/gtsam/geometry/Tensor3Expression.h b/gtsam/geometry/Tensor3Expression.h deleted file mode 100644 index 0356caa6f..000000000 --- a/gtsam/geometry/Tensor3Expression.h +++ /dev/null @@ -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 -#include - -namespace tensors { - - /** - * templated class to interface to an object A as a rank 3 tensor - * @ingroup tensors - * \nosubgrouping - */ - template class Tensor3Expression { - A iter; - - typedef Tensor3Expression 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 TimesRank1_ { - typedef Tensor1Expression 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 - bool equals(const Tensor3Expression & 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 operator()(int k) const { - return FixK_(k, iter); - } - - /** Contracting with rank1 tensor */ - template - inline Tensor2Expression , J, K> operator*( - const Tensor1Expression &b) const { - return TimesRank1_ (*this, b); - } - - }; // Tensor3Expression - - /// @} - /// @name Advanced Interface - /// @{ - - /** Print */ - template - void print(const Tensor3Expression& T, const std::string& s = - "Tensor3:") { - T.print(s); - } - - /** Helper class for outer product of rank2 and rank1 tensor */ - template - class Rank2Rank1_ { - typedef Tensor2Expression Rank2; - typedef Tensor1Expression 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 - inline Tensor3Expression , I, J, K> operator*( - const Tensor2Expression& a, const Tensor1Expression &b) { - return Rank2Rank1_ (a, b); - } - - /** Helper class for outer product of rank1 and rank2 tensor */ - template - class Rank1Rank2_ { - typedef Tensor1Expression Rank1; - typedef Tensor2Expression 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 - inline Tensor3Expression , I, J, K> operator*( - const Tensor1Expression& a, const Tensor2Expression &b) { - return Rank1Rank2_ (a, b); - } - - /** - * This template works for any two expressions - */ - template - bool assert_equality(const Tensor3Expression& expected, - const Tensor3Expression& 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 diff --git a/gtsam/geometry/Tensor4.h b/gtsam/geometry/Tensor4.h deleted file mode 100644 index 26a52750c..000000000 --- a/gtsam/geometry/Tensor4.h +++ /dev/null @@ -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 - -namespace tensors { - - /** - * Rank 4 Tensor - * @ingroup tensors - * \nosubgrouping - */ - template - class Tensor4 { - - private: - - Tensor3 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 diff --git a/gtsam/geometry/Tensor5.h b/gtsam/geometry/Tensor5.h deleted file mode 100644 index e12f7d2fb..000000000 --- a/gtsam/geometry/Tensor5.h +++ /dev/null @@ -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 - -namespace tensors { - - /** - * Rank 5 Tensor - * @ingroup tensors - * \nosubgrouping - */ - template - class Tensor5 { - - private: - - Tensor4 T[N5]; ///< Storage - - public: - - /// @name Standard Constructors - /// @{ - - /** default constructor */ - Tensor5() { - } - - /// @} - /// @name Standard Interface - /// @{ - - /** construct from expression */ - template - Tensor5(const Tensor5Expression , Index , Index , Index , Index >& 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 Tensor5Expression , Index , Index , Index , - Index > operator()(Index i, Index j, - Index k, Index l, Index m) { - return Tensor5Expression , Index , Index , Index , Index > (*this); - } - - /// @} - - }; // Tensor5 - -} // namespace tensors diff --git a/gtsam/geometry/Tensor5Expression.h b/gtsam/geometry/Tensor5Expression.h deleted file mode 100644 index e9f0e852a..000000000 --- a/gtsam/geometry/Tensor5Expression.h +++ /dev/null @@ -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 -#include - -namespace tensors { - - /** - * templated class to interface to an object A as a rank 5 tensor - * @ingroup tensors - * \nosubgrouping - */ - template class Tensor5Expression { - A iter; - - typedef Tensor5Expression 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 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 - void print(const Tensor5Expression& T, - const std::string& s = "Tensor5:") { - T.print(s); - } - - /** Helper class for outer product of rank3 and rank2 tensor */ - template - class Rank3Rank2_ { - typedef Tensor3Expression Rank3; - typedef Tensor2Expression 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 - inline Tensor5Expression , I, J, K, L, M> operator*( - const Tensor3Expression& a, - const Tensor2Expression &b) { - return Rank3Rank2_(a, b); - } - - /// @} - -} // namespace tensors diff --git a/gtsam/geometry/projectiveGeometry.cpp b/gtsam/geometry/projectiveGeometry.cpp deleted file mode 100644 index 31c975c43..000000000 --- a/gtsam/geometry/projectiveGeometry.cpp +++ /dev/null @@ -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 -#include - -#include -#include - -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 diff --git a/gtsam/geometry/projectiveGeometry.h b/gtsam/geometry/projectiveGeometry.h deleted file mode 100644 index cfad515ac..000000000 --- a/gtsam/geometry/projectiveGeometry.h +++ /dev/null @@ -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 -#include - -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 diff --git a/gtsam/geometry/tensorInterface.cpp b/gtsam/geometry/tensorInterface.cpp deleted file mode 100644 index 683543e19..000000000 --- a/gtsam/geometry/tensorInterface.cpp +++ /dev/null @@ -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 - -using namespace std; -using namespace tensors; - -namespace gtsam { - -} // namespace gtsam diff --git a/gtsam/geometry/tensorInterface.h b/gtsam/geometry/tensorInterface.h deleted file mode 100644 index 61212e2c9..000000000 --- a/gtsam/geometry/tensorInterface.h +++ /dev/null @@ -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 -#include - -namespace gtsam { - - /** Reshape rank 2 tensor into Matrix */ - template - Matrix reshape(const tensors::Tensor2Expression& 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 - Vector toVector(const tensors::Tensor2Expression& 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 - tensors::Tensor2 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(data); - } - - /** Reshape rank 3 tensor into Matrix */ - template - Matrix reshape(const tensors::Tensor3Expression& 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 - tensors::Tensor3 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(data); - } - - /** Reshape rank 5 tensor into Matrix */ - template - Matrix reshape(const tensors::Tensor5Expression& 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 diff --git a/gtsam/geometry/tensors.h b/gtsam/geometry/tensors.h deleted file mode 100644 index 1441a7823..000000000 --- a/gtsam/geometry/tensors.h +++ /dev/null @@ -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 struct Index { - static const int dim = Dim; ///< dimension - }; - -} // namespace tensors - -// Expression templates -#include -#include -#include -// Tensor4 not needed so far -#include - -// Actual tensor classes -#include -#include -#include -#include -#include - - diff --git a/gtsam/geometry/tests/testFundamental.cpp b/gtsam/geometry/tests/testFundamental.cpp deleted file mode 100644 index 297b36036..000000000 --- a/gtsam/geometry/tests/testFundamental.cpp +++ /dev/null @@ -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 -#include -#include // for operator += -using namespace boost::assign; - -#include - -#include -#include -#include - -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); -} -/* ************************************************************************* */ - diff --git a/gtsam/geometry/tests/testHomography2.cpp b/gtsam/geometry/tests/testHomography2.cpp deleted file mode 100644 index 417d41977..000000000 --- a/gtsam/geometry/tests/testHomography2.cpp +++ /dev/null @@ -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 -#include -#include // for operator += -using namespace boost::assign; - -#include - -#include -#include -#include -#include -#include - -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 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 - -/* ************************************************************************* */ -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(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); -} -/* ************************************************************************* */ - diff --git a/gtsam/geometry/tests/testTensors.cpp b/gtsam/geometry/tests/testTensors.cpp deleted file mode 100644 index 755b274a6..000000000 --- a/gtsam/geometry/tests/testTensors.cpp +++ /dev/null @@ -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 -#include -#include // for operator += -using namespace boost::assign; - -#include - -#include -#include -#include - -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); -} -/* ************************************************************************* */ - diff --git a/gtsam/inference/FactorGraph-inl.h b/gtsam/inference/FactorGraph-inl.h index 337b571c6..ea92cb791 100644 --- a/gtsam/inference/FactorGraph-inl.h +++ b/gtsam/inference/FactorGraph-inl.h @@ -23,7 +23,6 @@ #pragma once #include -#include #include #include diff --git a/gtsam/slam/simulated3D.cpp b/gtsam/slam/simulated3D.cpp deleted file mode 100644 index e2f6211a5..000000000 --- a/gtsam/slam/simulated3D.cpp +++ /dev/null @@ -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 - -namespace gtsam { - -namespace simulated3D { - -Point3 prior (const Point3& x, boost::optional H) { - if (H) *H = eye(3); - return x; -} - -Point3 odo(const Point3& x1, const Point3& x2, - boost::optional H1, boost::optional 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 H1, boost::optional H2) { - if (H1) *H1 = -1 * eye(3); - if (H2) *H2 = eye(3); - return l - x; -} - -}} // namespace gtsam::simulated3D diff --git a/gtsam/slam/simulated3D.h b/gtsam/slam/simulated3D.h deleted file mode 100644 index 7b4dfce37..000000000 --- a/gtsam/slam/simulated3D.h +++ /dev/null @@ -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 -#include -#include -#include -#include - -// \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 H = boost::none); - -/** - * odometry between two poses - */ -Point3 odo(const Point3& x1, const Point3& x2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none); - -/** - * measurement between landmark and pose - */ -Point3 mea(const Point3& x, const Point3& l, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none); - -/** - * A prior factor on a single linear robot pose - */ -struct PointPrior3D: public NoiseModelFactor1 { - - 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 (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 H = - boost::none) const { - return (prior(x, H) - measured_).vector(); - } -}; - -/** - * Models a linear 3D measurement between 3D points - */ -struct Simulated3DMeasurement: public NoiseModelFactor2 { - - 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(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 H1 = boost::none, boost::optional H2 = boost::none) const { - return (mea(x1, x2, H1, H2) - measured_).vector(); - } -}; - -}} // namespace simulated3D diff --git a/gtsam/slam/tests/testSimulated3D.cpp b/gtsam/slam/tests/testSimulated3D.cpp deleted file mode 100644 index a48e797e5..000000000 --- a/gtsam/slam/tests/testSimulated3D.cpp +++ /dev/null @@ -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 -#include - -#include -#include -#include -#include - -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(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(boost::bind(simulated3D::odo, _1, _2, boost::none, boost::none),x1,x2); - EXPECT(assert_equal(A1,H1,1e-9)); - Matrix A2 = numericalDerivative22(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);} -/* ************************************************************************* */