diff --git a/gtsam/unstable/CMakeLists.txt b/gtsam/unstable/CMakeLists.txt new file mode 100644 index 000000000..7cc8bfa31 --- /dev/null +++ b/gtsam/unstable/CMakeLists.txt @@ -0,0 +1,62 @@ +# Build full gtsam2 library as a single library +# and also build tests +set (gtsam2_subdirs + base + dynamics + geometry + slam +) + +# assemble core libaries +foreach(subdir ${gtsam2_subdirs}) + # Build convenience libraries + file(GLOB subdir_srcs "${subdir}/*.cpp") + set(${subdir}_srcs ${subdir_srcs}) + if (GTSAM2_BUILD_CONVENIENCE_LIBRARIES) + message(STATUS "Building Convenience Library: ${subdir}") + add_library(${subdir} STATIC ${subdir_srcs}) + endif() + + # Build local library and tests + message(STATUS "Building ${subdir}") + add_subdirectory(${subdir}) +endforeach(subdir) + +# assemble gtsam2 components +set(gtsam2_srcs + ${base_srcs} + ${discrete_srcs} + ${dynamics_srcs} + ${geometry_srcs} + ${slam_srcs} +) + +option (GTSAM2_BUILD_SHARED_LIBRARY "Enable/Disable building of a shared version of gtsam2" ON) + +# Versions +set(gtsam2_version ${GTSAM2_VERSION_MAJOR}.${GTSAM2_VERSION_MINOR}.${GTSAM2_VERSION_PATCH}) +set(gtsam2_soversion ${GTSAM2_VERSION_MAJOR}) +message(STATUS "GTSAM2 Version: ${gtsam2_version}") +message(STATUS "Install prefix: ${CMAKE_INSTALL_PREFIX}") + +# build shared and static versions of the library +message(STATUS "Building GTSAM2 - static") +add_library(gtsam2-static STATIC ${gtsam2_srcs}) +set_target_properties(gtsam2-static PROPERTIES + OUTPUT_NAME gtsam2 + CLEAN_DIRECT_OUTPUT 1 + VERSION ${gtsam2_version} + SOVERSION ${gtsam2_soversion}) +install(TARGETS gtsam2-static ARCHIVE DESTINATION lib) + +if (GTSAM2_BUILD_SHARED_LIBRARY) + message(STATUS "Building GTSAM2 - shared") + add_library(gtsam2-shared SHARED ${gtsam2_srcs}) + set_target_properties(gtsam2-shared PROPERTIES + OUTPUT_NAME gtsam2 + CLEAN_DIRECT_OUTPUT 1 + VERSION ${gtsam2_version} + SOVERSION ${gtsam2_soversion}) + install(TARGETS gtsam2-shared LIBRARY DESTINATION lib ) +endif(GTSAM2_BUILD_SHARED_LIBRARY) + diff --git a/gtsam/unstable/base/BTree.h b/gtsam/unstable/base/BTree.h new file mode 100644 index 000000000..2a40465a3 --- /dev/null +++ b/gtsam/unstable/base/BTree.h @@ -0,0 +1,409 @@ +/* ---------------------------------------------------------------------------- + + * 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/unstable/base/CMakeLists.txt b/gtsam/unstable/base/CMakeLists.txt new file mode 100644 index 000000000..c7d4b7b8f --- /dev/null +++ b/gtsam/unstable/base/CMakeLists.txt @@ -0,0 +1,17 @@ +# Install headers +file(GLOB base_headers "*.h") +install(FILES ${base_headers} DESTINATION include/gtsam2/base) + +# Components to link tests in this subfolder against +set(base_local_libs + base) + +set (base_full_libs + gtsam2-static) + +# Exclude tests that don't work +set (base_excluded_tests "") + +# Add all tests +gtsam_add_subdir_tests(base "${base_local_libs}" "${base_full_libs}" "${base_excluded_tests}") + diff --git a/gtsam/unstable/base/DSF.h b/gtsam/unstable/base/DSF.h new file mode 100644 index 000000000..06d69e169 --- /dev/null +++ b/gtsam/unstable/base/DSF.h @@ -0,0 +1,174 @@ +/* ---------------------------------------------------------------------------- + + * 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/unstable/base/DSFVector.cpp b/gtsam/unstable/base/DSFVector.cpp new file mode 100644 index 000000000..2d5f2d55a --- /dev/null +++ b/gtsam/unstable/base/DSFVector.cpp @@ -0,0 +1,97 @@ +/* ---------------------------------------------------------------------------- + + * 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 gtsam + diff --git a/gtsam/unstable/base/DSFVector.h b/gtsam/unstable/base/DSFVector.h new file mode 100644 index 000000000..17e897032 --- /dev/null +++ b/gtsam/unstable/base/DSFVector.h @@ -0,0 +1,79 @@ +/* ---------------------------------------------------------------------------- + + * 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/unstable/base/FixedVector.h b/gtsam/unstable/base/FixedVector.h new file mode 100644 index 000000000..4faa670b6 --- /dev/null +++ b/gtsam/unstable/base/FixedVector.h @@ -0,0 +1,118 @@ +/* ---------------------------------------------------------------------------- + + * 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/unstable/base/tests/testBTree.cpp b/gtsam/unstable/base/tests/testBTree.cpp new file mode 100644 index 000000000..5da901cbd --- /dev/null +++ b/gtsam/unstable/base/tests/testBTree.cpp @@ -0,0 +1,210 @@ +/* ---------------------------------------------------------------------------- + + * 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/unstable/base/tests/testDSF.cpp b/gtsam/unstable/base/tests/testDSF.cpp new file mode 100644 index 000000000..a41ba30d3 --- /dev/null +++ b/gtsam/unstable/base/tests/testDSF.cpp @@ -0,0 +1,280 @@ +/* ---------------------------------------------------------------------------- + + * 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/unstable/base/tests/testDSFVector.cpp b/gtsam/unstable/base/tests/testDSFVector.cpp new file mode 100644 index 000000000..8ae9d077d --- /dev/null +++ b/gtsam/unstable/base/tests/testDSFVector.cpp @@ -0,0 +1,171 @@ +/* ---------------------------------------------------------------------------- + + * 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/unstable/base/tests/testFixedVector.cpp b/gtsam/unstable/base/tests/testFixedVector.cpp new file mode 100644 index 000000000..1d0840dca --- /dev/null +++ b/gtsam/unstable/base/tests/testFixedVector.cpp @@ -0,0 +1,87 @@ +/* ---------------------------------------------------------------------------- + + * 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> TestVector5; +typedef FixedVector<3> TestVector3; + +static const double tol = 1e-9; + +/* ************************************************************************* */ +TEST( testFixedVector, conversions ) { + double data1[] = {1.0, 2.0, 3.0}; + Vector v1 = Vector_(3, data1); + TestVector3 fv1(v1), fv2(data1); + + Vector actFv2(fv2); + EXPECT(assert_equal(v1, actFv2)); +} + +/* ************************************************************************* */ +TEST( testFixedVector, variable_constructor ) { + TestVector3 act(3, 1.0, 2.0, 3.0); + EXPECT_DOUBLES_EQUAL(1.0, act(0), tol); + EXPECT_DOUBLES_EQUAL(2.0, act(1), tol); + EXPECT_DOUBLES_EQUAL(3.0, act(2), tol); +} + +/* ************************************************************************* */ +TEST( testFixedVector, equals ) { + TestVector3 vec1(3, 1.0, 2.0, 3.0), vec2(3, 1.0, 2.0, 3.0), vec3(3, 2.0, 3.0, 4.0); + TestVector5 vec4(5, 1.0, 2.0, 3.0, 4.0, 5.0); + + EXPECT(assert_equal(vec1, vec1, tol)); + EXPECT(assert_equal(vec1, vec2, tol)); + EXPECT(assert_equal(vec2, vec1, tol)); + EXPECT(!vec1.equals(vec3, tol)); + EXPECT(!vec3.equals(vec1, tol)); + EXPECT(!vec1.equals(vec4, tol)); + EXPECT(!vec4.equals(vec1, tol)); +} + +/* ************************************************************************* */ +TEST( testFixedVector, static_constructors ) { + TestVector3 actZero = TestVector3::zero(); + TestVector3 expZero(3, 0.0, 0.0, 0.0); + EXPECT(assert_equal(expZero, actZero, tol)); + + TestVector3 actOnes = TestVector3::ones(); + TestVector3 expOnes(3, 1.0, 1.0, 1.0); + EXPECT(assert_equal(expOnes, actOnes, tol)); + + TestVector3 actRepeat = TestVector3::repeat(2.3); + TestVector3 expRepeat(3, 2.3, 2.3, 2.3); + EXPECT(assert_equal(expRepeat, actRepeat, tol)); + + TestVector3 actBasis = TestVector3::basis(1); + TestVector3 expBasis(3, 0.0, 1.0, 0.0); + EXPECT(assert_equal(expBasis, actBasis, tol)); + + TestVector3 actDelta = TestVector3::delta(1, 2.3); + TestVector3 expDelta(3, 0.0, 2.3, 0.0); + EXPECT(assert_equal(expDelta, actDelta, tol)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ + + diff --git a/gtsam/unstable/dynamics/CMakeLists.txt b/gtsam/unstable/dynamics/CMakeLists.txt new file mode 100644 index 000000000..6590832be --- /dev/null +++ b/gtsam/unstable/dynamics/CMakeLists.txt @@ -0,0 +1,17 @@ +# Install headers +file(GLOB dynamics_headers "*.h") +install(FILES ${dynamics_headers} DESTINATION include/gtsam2/dynamics) + +# Components to link tests in this subfolder against +set(dynamics_local_libs + dynamics) + +set (dynamics_full_libs + gtsam2-static) + +# Exclude tests that don't work +set (dynamics_excluded_tests "") + +# Add all tests +gtsam_add_subdir_tests(dynamics "${dynamics_local_libs}" "${dynamics_full_libs}" "${dynamics_excluded_tests}") + diff --git a/gtsam/unstable/dynamics/DynamicsPriors.h b/gtsam/unstable/dynamics/DynamicsPriors.h new file mode 100644 index 000000000..74908b209 --- /dev/null +++ b/gtsam/unstable/dynamics/DynamicsPriors.h @@ -0,0 +1,105 @@ +/** + * @file DynamicsPriors.h + * + * @brief Priors to be used with dynamic systems (Specifically PoseRTV) + * + * @date Nov 22, 2011 + * @author Alex Cunningham + */ + +#pragma once + +#include + +#include + +namespace gtsam { + +/** + * Forces the value of the height in a PoseRTV to a specific value + * Dim: 1 + */ +struct DHeightPrior : public gtsam::PartialPriorFactor { + typedef gtsam::PartialPriorFactor Base; + DHeightPrior(Key key, double height, const gtsam::SharedNoiseModel& model) + : Base(key, 5, height, model) { } +}; + +/** + * Forces the roll to a particular value - useful for flying robots + * Implied value is zero + * Dim: 1 + */ +struct DRollPrior : public gtsam::PartialPriorFactor { + typedef gtsam::PartialPriorFactor Base; + + /** allows for explicit roll parameterization - uses canonical coordinate */ + DRollPrior(Key key, double wx, const gtsam::SharedNoiseModel& model) + : Base(key, 0, wx, model) { } + + /** Forces roll to zero */ + DRollPrior(Key key, const gtsam::SharedNoiseModel& model) + : Base(key, 0, 0.0, model) { } +}; + +/** + * Constrains the full velocity of a state to a particular value + * Useful for enforcing a stationary state + * Dim: 3 + */ +struct VelocityPrior : public gtsam::PartialPriorFactor { + typedef gtsam::PartialPriorFactor Base; + VelocityPrior(Key key, const gtsam::Vector& vel, const gtsam::SharedNoiseModel& model) + : Base(key, model) { + this->prior_ = vel; + assert(vel.size() == 3); + this->mask_.resize(3); + this->mask_[0] = 6; + this->mask_[1] = 7; + this->mask_[2] = 8; + this->H_ = zeros(3, 9); + this->fillH(); + } +}; + +/** + * Ground constraint: forces the robot to be upright (no roll, pitch), a fixed height, and no + * velocity in z direction + * Dim: 4 + */ +struct DGroundConstraint : public gtsam::PartialPriorFactor { + typedef gtsam::PartialPriorFactor Base; + + /** + * Primary constructor allows for variable height of the "floor" + */ + DGroundConstraint(Key key, double height, const gtsam::SharedNoiseModel& model) + : Base(key, model) { + this->prior_ = delta(4, 0, height); // [z, vz, roll, pitch] + this->mask_.resize(4); + this->mask_[0] = 5; // z = height + this->mask_[1] = 8; // vz + this->mask_[2] = 0; // roll + this->mask_[3] = 1; // pitch + this->H_ = zeros(3, 9); + this->fillH(); + } + + /** + * Fully specify vector - use only for debugging + */ + DGroundConstraint(Key key, const Vector& constraint, const gtsam::SharedNoiseModel& model) + : Base(key, model) { + assert(constraint.size() == 4); + this->prior_ = constraint; // [z, vz, roll, pitch] + this->mask_.resize(4); + this->mask_[0] = 5; // z = height + this->mask_[1] = 8; // vz + this->mask_[2] = 0; // roll + this->mask_[3] = 1; // pitch + this->H_ = zeros(3, 9); + this->fillH(); + } +}; + +} // \namespace gtsam diff --git a/gtsam/unstable/dynamics/FullIMUFactor.h b/gtsam/unstable/dynamics/FullIMUFactor.h new file mode 100644 index 000000000..40ef80a15 --- /dev/null +++ b/gtsam/unstable/dynamics/FullIMUFactor.h @@ -0,0 +1,115 @@ +/** + * @file FullIMUFactor.h + * @brief Factor to express an IMU measurement between dynamic poses + * @author Alex Cunningham + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +/** + * Class that represents integrating IMU measurements over time for dynamic systems + * This factor has dimension 9, with a built-in constraint for velocity modeling + * + * Templated to allow for different key types, but variables all + * assumed to be PoseRTV + */ +template +class FullIMUFactor : public NoiseModelFactor2 { +public: + typedef NoiseModelFactor2 Base; + typedef FullIMUFactor This; + +protected: + + /** measurements from the IMU */ + Vector accel_, gyro_; + double dt_; /// time between measurements + +public: + + /** Standard constructor */ + FullIMUFactor(const Vector& accel, const Vector& gyro, + double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model) + : Base(model, key1, key2), accel_(accel), gyro_(gyro), dt_(dt) { + assert(model->dim() == 9); + } + + /** Single IMU vector - imu = [accel, gyro] */ + FullIMUFactor(const Vector& imu, + double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model) + : Base(model, key1, key2), accel_(imu.head(3)), gyro_(imu.tail(3)), dt_(dt) { + assert(imu.size() == 6); + assert(model->dim() == 9); + } + + virtual ~FullIMUFactor() {} + + /** Check if two factors are equal */ + virtual bool equals(const NonlinearFactor& e, double tol = 1e-9) const { + const This* const f = dynamic_cast(&e); + return f && Base::equals(e) && + equal_with_abs_tol(accel_, f->accel_, tol) && + equal_with_abs_tol(gyro_, f->gyro_, tol) && + fabs(dt_ - f->dt_) < tol; + } + + void print(const std::string& s="", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const { + std::string a = "FullIMUFactor: " + s; + Base::print(a, formatter); + gtsam::print(accel_, "accel"); + gtsam::print(gyro_, "gyro"); + std::cout << "dt: " << dt_ << std::endl; + } + + // access + const Vector& gyro() const { return gyro_; } + const Vector& accel() const { return accel_; } + Vector z() const { return concatVectors(2, &accel_, &gyro_); } + const Key& key1() const { return this->key1_; } + const Key& key2() const { return this->key2_; } + + /** + * Error evaluation with optional derivatives - calculates + * z - h(x1,x2) + */ + virtual Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const { + Vector z(9); + z.head(3).operator=(accel_); // Strange syntax to work around ambiguous operator error with clang + z.segment(3, 3).operator=(gyro_); // Strange syntax to work around ambiguous operator error with clang + z.tail(3).operator=(x2.t().vector()); // Strange syntax to work around ambiguous operator error with clang + if (H1) *H1 = numericalDerivative21( + boost::bind(This::predict_proxy, _1, _2, dt_), x1, x2, 1e-5); + if (H2) *H2 = numericalDerivative22( + boost::bind(This::predict_proxy, _1, _2, dt_), x1, x2, 1e-5); + return z - predict_proxy(x1, x2, dt_); + } + + /** dummy version that fails for non-dynamic poses */ + virtual Vector evaluateError(const Pose3& x1, const Pose3& x2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const { + assert(false); + return zero(x1.dim()); + } + +private: + + /** copy of the measurement function formulated for numerical derivatives */ + static LieVector predict_proxy(const PoseRTV& x1, const PoseRTV& x2, double dt) { + Vector hx(9); + hx.head(6).operator=(x1.imuPrediction(x2, dt)); // Strange syntax to work around ambiguous operator error with clang + hx.tail(3).operator=(x1.translationIntegration(x2, dt).vector()); // Strange syntax to work around ambiguous operator error with clang + return LieVector(hx); + } +}; + +} // \namespace gtsam diff --git a/gtsam/unstable/dynamics/IMUFactor.h b/gtsam/unstable/dynamics/IMUFactor.h new file mode 100644 index 000000000..1d83604df --- /dev/null +++ b/gtsam/unstable/dynamics/IMUFactor.h @@ -0,0 +1,103 @@ +/** + * @file IMUFactor.h + * @brief Factor to express an IMU measurement between dynamic poses + * @author Alex Cunningham + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +/** + * Class that represents integrating IMU measurements over time for dynamic systems + * Templated to allow for different key types, but variables all + * assumed to be PoseRTV + */ +template +class IMUFactor : public NoiseModelFactor2 { +public: + typedef NoiseModelFactor2 Base; + typedef IMUFactor This; + +protected: + + /** measurements from the IMU */ + Vector accel_, gyro_; + double dt_; /// time between measurements + +public: + + /** Standard constructor */ + IMUFactor(const Vector& accel, const Vector& gyro, + double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model) + : Base(model, key1, key2), accel_(accel), gyro_(gyro), dt_(dt) {} + + /** Full IMU vector specification */ + IMUFactor(const Vector& imu_vector, + double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model) + : Base(model, key1, key2), accel_(imu_vector.head(3)), gyro_(imu_vector.tail(3)), dt_(dt) {} + + virtual ~IMUFactor() {} + + /** Check if two factors are equal */ + virtual bool equals(const NonlinearFactor& e, double tol = 1e-9) const { + const This* const f = dynamic_cast(&e); + return f && Base::equals(e) && + equal_with_abs_tol(accel_, f->accel_, tol) && + equal_with_abs_tol(gyro_, f->gyro_, tol) && + fabs(dt_ - f->dt_) < tol; + } + + void print(const std::string& s="", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const { + std::string a = "IMUFactor: " + s; + Base::print(a, formatter); + gtsam::print(accel_, "accel"); + gtsam::print(gyro_, "gyro"); + std::cout << "dt: " << dt_ << std::endl; + } + + // access + const Vector& gyro() const { return gyro_; } + const Vector& accel() const { return accel_; } + Vector z() const { return concatVectors(2, &accel_, &gyro_); } + const Key& key1() const { return this->key1_; } + const Key& key2() const { return this->key2_; } + + /** + * Error evaluation with optional derivatives - calculates + * z - h(x1,x2) + */ + virtual Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const { + const Vector meas = z(); + if (H1) *H1 = numericalDerivative21( + boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5); + if (H2) *H2 = numericalDerivative22( + boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5); + return predict_proxy(x1, x2, dt_, meas); + } + + /** dummy version that fails for non-dynamic poses */ + virtual Vector evaluateError(const Pose3& x1, const Pose3& x2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const { + assert(false); // no corresponding factor here + return zero(x1.dim()); + } + +private: + /** copy of the measurement function formulated for numerical derivatives */ + static LieVector predict_proxy(const PoseRTV& x1, const PoseRTV& x2, + double dt, const Vector& meas) { + Vector hx = x1.imuPrediction(x2, dt); + return LieVector(meas - hx); + } +}; + +} // \namespace gtsam diff --git a/gtsam/unstable/dynamics/PoseRTV.cpp b/gtsam/unstable/dynamics/PoseRTV.cpp new file mode 100644 index 000000000..b3dcd2d12 --- /dev/null +++ b/gtsam/unstable/dynamics/PoseRTV.cpp @@ -0,0 +1,279 @@ +/** + * @file PoseRTV.cpp + * @author Alex Cunningham + */ + +#include + +#include +#include +#include +#include + +#include +#include + +namespace gtsam { + +using namespace std; + +static const Vector g = delta(3, 2, 9.81); +const double pi = M_PI; + +/* ************************************************************************* */ +double bound(double a, double min, double max) { + if (a < min) return min; + else if (a > max) return max; + else return a; +} + +/* ************************************************************************* */ +PoseRTV::PoseRTV(double roll, double pitch, double yaw, double x, double y, double z, + double vx, double vy, double vz) +: Rt_(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z)), v_(vx, vy, vz) {} + +/* ************************************************************************* */ +PoseRTV::PoseRTV(const Vector& rtv) +: Rt_(Rot3::RzRyRx(rtv.head(3)), Point3(rtv.segment(3, 3))), v_(rtv.tail(3)) +{ +} + +/* ************************************************************************* */ +Vector PoseRTV::vector() const { + Vector rtv(9); + rtv.head(3) = Rt_.rotation().xyz(); + rtv.segment(3,3) = Rt_.translation().vector(); + rtv.tail(3) = v_.vector(); + return rtv; +} + +/* ************************************************************************* */ +bool PoseRTV::equals(const PoseRTV& other, double tol) const { + return Rt_.equals(other.Rt_, tol) && v_.equals(other.v_, tol); +} + +/* ************************************************************************* */ +void PoseRTV::print(const string& s) const { + cout << s << ":" << endl; + gtsam::print((Vector)R().xyz(), " R:rpy"); + t().print(" T"); + v_.print(" V"); +} + +/* ************************************************************************* */ +PoseRTV PoseRTV::Expmap(const Vector& v) { + assert(v.size() == 9); + Pose3 newPose = Pose3::Expmap(sub(v, 0, 6)); + Velocity3 newVel = Velocity3::Expmap(sub(v, 6, 9)); + return PoseRTV(newPose, newVel); +} + +/* ************************************************************************* */ +Vector PoseRTV::Logmap(const PoseRTV& p) { + Vector Lx = Pose3::Logmap(p.Rt_); + Vector Lv = Velocity3::Logmap(p.v_); + return concatVectors(2, &Lx, &Lv); +} + +/* ************************************************************************* */ +PoseRTV PoseRTV::retract(const Vector& v) const { + assert(v.size() == 9); + // First order approximation + Pose3 newPose = Rt_.retract(sub(v, 0, 6)); + Velocity3 newVel = v_ + Rt_.rotation() * Point3(sub(v, 6, 9)); + return PoseRTV(newPose, newVel); +} + +/* ************************************************************************* */ +Vector PoseRTV::localCoordinates(const PoseRTV& p1) const { + const Pose3& x0 = pose(), &x1 = p1.pose(); + // First order approximation + Vector poseLogmap = x0.localCoordinates(x1); + Vector lv = rotation().unrotate(p1.velocity() - v_).vector(); + return concatVectors(2, &poseLogmap, &lv); +} + +/* ************************************************************************* */ +PoseRTV PoseRTV::inverse() const { + return PoseRTV(Rt_.inverse(), v_.inverse()); +} + +/* ************************************************************************* */ +PoseRTV PoseRTV::compose(const PoseRTV& p) const { + return PoseRTV(Rt_.compose(p.Rt_), v_.compose(p.v_)); +} + +/* ************************************************************************* */ +PoseRTV between_(const PoseRTV& p1, const PoseRTV& p2) { return p1.between(p2); } +PoseRTV PoseRTV::between(const PoseRTV& p, + boost::optional H1, + boost::optional H2) const { + if (H1) *H1 = numericalDerivative21(between_, *this, p, 1e-5); + if (H2) *H2 = numericalDerivative22(between_, *this, p, 1e-5); + return inverse().compose(p); +} + +/* ************************************************************************* */ +PoseRTV PoseRTV::planarDynamics(double vel_rate, double heading_rate, + double max_accel, double dt) const { + + // split out initial state + const Rot3& r1 = R(); + const Velocity3& v1 = v(); + + // Update vehicle heading + Rot3 r2 = r1.retract(Vector_(3, 0.0, 0.0, heading_rate * dt)); + const double yaw2 = r2.ypr()(0); + + // Update vehicle position + const double mag_v1 = v1.norm(); + + // FIXME: this doesn't account for direction in velocity bounds + double dv = bound(vel_rate - mag_v1, - (max_accel * dt), max_accel * dt); + double mag_v2 = mag_v1 + dv; + Velocity3 v2 = mag_v2 * Velocity3(cos(yaw2), sin(yaw2), 0.0); + + Point3 t2 = translationIntegration(r2, v2, dt); + + return PoseRTV(r2, t2, v2); +} + +/* ************************************************************************* */ +PoseRTV PoseRTV::flyingDynamics( + double pitch_rate, double heading_rate, double lift_control, double dt) const { + // split out initial state + const Rot3& r1 = R(); + const Velocity3& v1 = v(); + + // Update vehicle heading (and normalise yaw) + Vector rot_rates = Vector_(3, 0.0, pitch_rate, heading_rate); + Rot3 r2 = r1.retract(rot_rates*dt); + + // Work out dynamics on platform + const double thrust = 50.0; + const double lift = 50.0; + const double drag = 0.1; + double yaw2 = r2.yaw(); + double pitch2 = r2.pitch(); + double forward_accel = -thrust * sin(pitch2); // r2, pitch (in global frame?) controls forward force + double loss_lift = lift*fabs(sin(pitch2)); + Rot3 yaw_correction_bn = Rot3::yaw(yaw2); + Point3 forward(forward_accel, 0.0, 0.0); + Vector Acc_n = + yaw_correction_bn.rotate(forward).vector() // applies locally forward force in the global frame + - drag * Vector_(3, v1.x(), v1.y(), 0.0) // drag term dependent on v1 + + delta(3, 2, loss_lift - lift_control); // falling due to lift lost from pitch + + // Update Vehicle Position and Velocity + Velocity3 v2 = v1 + Velocity3(Acc_n * dt); + Point3 t2 = translationIntegration(r2, v2, dt); + + return PoseRTV(r2, t2, v2); +} + +/* ************************************************************************* */ +PoseRTV PoseRTV::generalDynamics( + const Vector& accel, const Vector& gyro, double dt) const { + // Integrate Attitude Equations + Rot3 r2 = rotation().retract(gyro * dt); + + // Integrate Velocity Equations + Velocity3 v2 = v_.compose(Velocity3(dt * (r2.matrix() * accel + g))); + + // Integrate Position Equations + Point3 t2 = translationIntegration(r2, v2, dt); + + return PoseRTV(t2, r2, v2); +} + +/* ************************************************************************* */ +Vector PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const { + // split out states + const Rot3 &r1 = R(), &r2 = x2.R(); + const Velocity3 &v1 = v(), &v2 = x2.v(); + + Vector imu(6); + + // acceleration + Vector accel = v1.localCoordinates(v2) / dt; + imu.head(3) = r2.transpose() * (accel - g); + + // rotation rates + // just using euler angles based on matlab code + // FIXME: this is silly - we shouldn't use differences in Euler angles + Matrix Enb = dynamics::RRTMnb(r1); + Vector euler1 = r1.xyz(), euler2 = r2.xyz(); + Vector dR = euler2 - euler1; + + // normalize yaw in difference (as per Mitch's code) + dR(2) = Rot2::fromAngle(dR(2)).theta(); + dR /= dt; + imu.tail(3) = Enb * dR; +// imu.tail(3) = r1.transpose() * dR; + + return imu; +} + +/* ************************************************************************* */ +Point3 PoseRTV::translationIntegration(const Rot3& r2, const Velocity3& v2, double dt) const { + // predict point for constraint + // NOTE: uses simple Euler approach for prediction + Point3 pred_t2 = t() + v2 * dt; + return pred_t2; +} + +/* ************************************************************************* */ +double range_(const PoseRTV& A, const PoseRTV& B) { return A.range(B); } +double PoseRTV::range(const PoseRTV& other, + boost::optional H1, boost::optional H2) const { + if (H1) *H1 = numericalDerivative21(range_, *this, other, 1e-5); + if (H2) *H2 = numericalDerivative22(range_, *this, other, 1e-5); + return t().dist(other.t()); +} + +/* ************************************************************************* */ +PoseRTV transformed_from_(const PoseRTV& global, const Pose3& transform) { + return global.transformed_from(transform); +} + +PoseRTV PoseRTV::transformed_from(const Pose3& trans, + boost::optional Dglobal, + boost::optional Dtrans) const { + // Note that we rotate the velocity + Matrix DVr, DTt; + Velocity3 newvel = trans.rotation().rotate(v_, DVr, DTt); + if (!Dglobal && !Dtrans) + return PoseRTV(trans.compose(pose()), newvel); + + // Pose3 transform is just compose + Matrix DTc, DGc; + Pose3 newpose = trans.compose(pose(), DTc, DGc); + + if (Dglobal) { + *Dglobal = zeros(9,9); + insertSub(*Dglobal, DGc, 0, 0); + + // Rotate velocity + insertSub(*Dglobal, eye(3,3), 6, 6); // FIXME: should this actually be an identity matrix? + } + + if (Dtrans) { + *Dtrans = numericalDerivative22(transformed_from_, *this, trans, 1e-6); + // + // *Dtrans = zeros(9,6); + // // directly affecting the pose + // insertSub(*Dtrans, DTc, 0, 0); // correct in tests + // + // // rotating the velocity + // Matrix vRhat = skewSymmetric(-v_.x(), -v_.y(), -v_.z()); + // trans.rotation().print("Transform rotation"); + // gtsam::print(vRhat, "vRhat"); + // gtsam::print(DVr, "DVr"); + // // FIXME: find analytic derivative + //// insertSub(*Dtrans, vRhat, 6, 0); // works if PoseRTV.rotation() = I + //// insertSub(*Dtrans, trans.rotation().matrix() * vRhat, 6, 0); // FAIL: both tests fail + } + return PoseRTV(newpose, newvel); +} + +} // \namespace gtsam diff --git a/gtsam/unstable/dynamics/PoseRTV.h b/gtsam/unstable/dynamics/PoseRTV.h new file mode 100644 index 000000000..b2e325f53 --- /dev/null +++ b/gtsam/unstable/dynamics/PoseRTV.h @@ -0,0 +1,166 @@ +/** + * @file PoseRTV.h + * @brief Pose3 with translational velocity + * @author Alex Cunningham + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/// Syntactic sugar to clarify components +typedef Point3 Velocity3; + +/** + * Robot state for use with IMU measurements + * - contains translation, translational velocity and rotation + */ +class PoseRTV : public DerivedValue { +protected: + + Pose3 Rt_; + Velocity3 v_; + +public: + // constructors - with partial versions + PoseRTV() {} + PoseRTV(const Point3& pt, const Rot3& rot, const Velocity3& vel) + : Rt_(rot, pt), v_(vel) {} + PoseRTV(const Rot3& rot, const Point3& pt, const Velocity3& vel) + : Rt_(rot, pt), v_(vel) {} + explicit PoseRTV(const Point3& pt) + : Rt_(Rot3::identity(), pt) {} + PoseRTV(const Pose3& pose, const Velocity3& vel) + : Rt_(pose), v_(vel) {} + explicit PoseRTV(const Pose3& pose) + : Rt_(pose) {} + + /** build from components - useful for data files */ + PoseRTV(double roll, double pitch, double yaw, double x, double y, double z, + double vx, double vy, double vz); + + /** build from single vector - useful for Matlab - in RtV format */ + explicit PoseRTV(const Vector& v); + + // access + const Point3& t() const { return Rt_.translation(); } + const Rot3& R() const { return Rt_.rotation(); } + const Velocity3& v() const { return v_; } + const Pose3& pose() const { return Rt_; } + + // longer function names + const Point3& translation() const { return Rt_.translation(); } + const Rot3& rotation() const { return Rt_.rotation(); } + const Velocity3& velocity() const { return v_; } + + // Access to vector for ease of use with Matlab + // and avoidance of Point3 + Vector vector() const; + Vector translationVec() const { return Rt_.translation().vector(); } + Vector velocityVec() const { return v_.vector(); } + + // testable + bool equals(const PoseRTV& other, double tol=1e-6) const; + void print(const std::string& s="") const; + + // Manifold + static size_t Dim() { return 9; } + size_t dim() const { return Dim(); } + + /** + * retract/unretract assume independence of components + * Tangent space parameterization: + * - v(0-2): Rot3 (roll, pitch, yaw) + * - v(3-5): Point3 + * - v(6-8): Translational velocity + */ + PoseRTV retract(const Vector& v) const; + Vector localCoordinates(const PoseRTV& p) const; + + // Lie + /** + * expmap/logmap are poor approximations that assume independence of components + * Currently implemented using the poor retract/unretract approximations + */ + static PoseRTV Expmap(const Vector& v); + static Vector Logmap(const PoseRTV& p); + + PoseRTV inverse() const; + + PoseRTV compose(const PoseRTV& p) const; + + static PoseRTV identity() { return PoseRTV(); } + + /** Derivatives calculated numerically */ + PoseRTV between(const PoseRTV& p, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const; + + // measurement functions + /** Derivatives calculated numerically */ + double range(const PoseRTV& other, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const; + + // IMU-specific + + /// Dynamics integrator for ground robots + /// Always move from time 1 to time 2 + PoseRTV planarDynamics(double vel_rate, double heading_rate, double max_accel, double dt) const; + + /// Simulates flying robot with simple flight model + /// Integrates state x1 -> x2 given controls + /// x1 = {p1, r1, v1}, x2 = {p2, r2, v2}, all in global coordinates + /// @return x2 + PoseRTV flyingDynamics(double pitch_rate, double heading_rate, double lift_control, double dt) const; + + /// General Dynamics update - supply control inputs in body frame + PoseRTV generalDynamics(const Vector& accel, const Vector& gyro, double dt) const; + + /// Dynamics predictor for both ground and flying robots, given states at 1 and 2 + /// Always move from time 1 to time 2 + /// @return imu measurement, as [accel, gyro] + Vector imuPrediction(const PoseRTV& x2, double dt) const; + + /// predict measurement and where Point3 for x2 should be, as a way + /// of enforcing a velocity constraint + /// This version splits out the rotation and velocity for x2 + Point3 translationIntegration(const Rot3& r2, const Velocity3& v2, double dt) const; + + /// predict measurement and where Point3 for x2 should be, as a way + /// of enforcing a velocity constraint + /// This version takes a full PoseRTV, but ignores the existing translation for x2 + inline Point3 translationIntegration(const PoseRTV& x2, double dt) const { + return translationIntegration(x2.rotation(), x2.velocity(), dt); + } + + /// @return a vector for Matlab compatibility + inline Vector translationIntegrationVec(const PoseRTV& x2, double dt) const { + return translationIntegration(x2, dt).vector(); + } + + /** + * Apply transform to this pose, with optional derivatives + * equivalent to: + * local = trans.transform_from(global, Dtrans, Dglobal) + * + * Note: the transform jacobian convention is flipped + */ + PoseRTV transformed_from(const Pose3& trans, + boost::optional Dglobal=boost::none, + boost::optional Dtrans=boost::none) const; + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(Rt_); + ar & BOOST_SERIALIZATION_NVP(v_); + } +}; + +} // \namespace gtsam diff --git a/gtsam/unstable/dynamics/VelocityConstraint.h b/gtsam/unstable/dynamics/VelocityConstraint.h new file mode 100644 index 000000000..836cdfcae --- /dev/null +++ b/gtsam/unstable/dynamics/VelocityConstraint.h @@ -0,0 +1,116 @@ +/** + * @file VelocityConstraint.h + * @brief Constraint enforcing the relationship between pose and velocity + * @author Alex Cunningham + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +namespace dynamics { + +/** controls which model to use for numerical integration to use for constraints */ +typedef enum { + TRAPEZOIDAL, // Constant acceleration + EULER_START, // Constant velocity, using starting velocity + EULER_END // Constant velocity, using ending velocity +} IntegrationMode; + +} + +/** + * Constraint to enforce dynamics between the velocities and poses, using + * a prediction based on a numerical integration flag. + * + * NOTE: this approximation is insufficient for large timesteps, but is accurate + * if timesteps are small. + */ +class VelocityConstraint : public gtsam::NoiseModelFactor2 { +public: + typedef gtsam::NoiseModelFactor2 Base; + +protected: + + double dt_; /// time difference between frames in seconds + dynamics::IntegrationMode integration_mode_; ///< Numerical integration control + +public: + + /** + * Creates a constraint relating the given variables with fully constrained noise model + */ + VelocityConstraint(Key key1, Key key2, const dynamics::IntegrationMode& mode, + double dt, double mu = 1000) + : Base(noiseModel::Constrained::All(3, mu), key1, key2), dt_(dt), integration_mode_(mode) {} + + /** + * Creates a constraint relating the given variables with fully constrained noise model + * Uses the default Trapezoidal integrator + */ + VelocityConstraint(Key key1, Key key2, double dt, double mu = 1000) + : Base(noiseModel::Constrained::All(3, mu), key1, key2), + dt_(dt), integration_mode_(dynamics::TRAPEZOIDAL) {} + + /** + * Creates a constraint relating the given variables with arbitrary noise model + */ + VelocityConstraint(Key key1, Key key2, const dynamics::IntegrationMode& mode, + double dt, const gtsam::SharedNoiseModel& model) + : Base(model, key1, key2), dt_(dt), integration_mode_(mode) {} + + /** + * Creates a constraint relating the given variables with arbitrary noise model + * Uses the default Trapezoidal integrator + */ + VelocityConstraint(Key key1, Key key2, double dt, const gtsam::SharedNoiseModel& model) + : Base(model, key1, key2), dt_(dt), integration_mode_(dynamics::TRAPEZOIDAL) {} + + virtual ~VelocityConstraint() {} + + /** + * Calculates the error for trapezoidal model given + */ + virtual gtsam::Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + if (H1) *H1 = gtsam::numericalDerivative21( + boost::bind(VelocityConstraint::evaluateError_, _1, _2, dt_, integration_mode_), x1, x2, 1e-5); + if (H2) *H2 = gtsam::numericalDerivative22( + boost::bind(VelocityConstraint::evaluateError_, _1, _2, dt_, integration_mode_), x1, x2, 1e-5); + return evaluateError_(x1, x2, dt_, integration_mode_); + } + + virtual void print(const std::string& s = "", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const { + std::string a = "VelocityConstraint: " + s; + Base::print(a, formatter); + switch(integration_mode_) { + case dynamics::TRAPEZOIDAL: std::cout << "Integration: Trapezoidal\n"; break; + case dynamics::EULER_START: std::cout << "Integration: Euler (start)\n"; break; + case dynamics::EULER_END: std::cout << "Integration: Euler (end)\n"; break; + default: std::cout << "Integration: Unknown\n" << std::endl; break; + } + std::cout << "dt: " << dt_ << std::endl; + } + +private: + static gtsam::LieVector evaluateError_(const PoseRTV& x1, const PoseRTV& x2, + double dt, const dynamics::IntegrationMode& mode) { + + const Velocity3& v1 = x1.v(), v2 = x2.v(), p1 = x1.t(), p2 = x2.t(); + Velocity3 hx; + switch(mode) { + case dynamics::TRAPEZOIDAL: hx = p1 + (v1 + v2) * dt *0.5; break; + case dynamics::EULER_START: hx = p1 + v1 * dt; break; + case dynamics::EULER_END : hx = p1 + v2 * dt; break; + default: assert(false); break; + } + return (p2 - hx).vector(); + } +}; + +} // \namespace gtsam diff --git a/gtsam/unstable/dynamics/imuSystem.cpp b/gtsam/unstable/dynamics/imuSystem.cpp new file mode 100644 index 000000000..11da98673 --- /dev/null +++ b/gtsam/unstable/dynamics/imuSystem.cpp @@ -0,0 +1,64 @@ +/** + * @file ilm3DSystem.cpp + * @brief Implementations of ilm 3D domain + * @author Alex Cunningham + */ + +#include + +#include + +namespace imu { + +using namespace gtsam; + +/* ************************************************************************* */ +void Graph::addPrior(Key key, const PoseRTV& pose, const SharedNoiseModel& noiseModel) { + add(Prior(PoseKey(key), pose, noiseModel)); +} + +/* ************************************************************************* */ +void Graph::addConstraint(Key key, const PoseRTV& pose) { + add(Constraint(PoseKey(key), pose)); +} + +/* ************************************************************************* */ +void Graph::addHeightPrior(Key key, double z, const SharedNoiseModel& noiseModel) { + add(DHeightPrior(PoseKey(key), z, noiseModel)); +} + +/* ************************************************************************* */ +void Graph::addFullIMUMeasurement(Key key1, Key key2, + const Vector& accel, const Vector& gyro, double dt, const SharedNoiseModel& noiseModel) { + add(FullIMUMeasurement(accel, gyro, dt, PoseKey(key1), PoseKey(key2), noiseModel)); +} + +/* ************************************************************************* */ +void Graph::addIMUMeasurement(Key key1, Key key2, + const Vector& accel, const Vector& gyro, double dt, const SharedNoiseModel& noiseModel) { + add(IMUMeasurement(accel, gyro, dt, PoseKey(key1), PoseKey(key2), noiseModel)); +} + +/* ************************************************************************* */ +void Graph::addVelocityConstraint(Key key1, Key key2, double dt, const SharedNoiseModel& noiseModel) { + add(VelocityConstraint(PoseKey(key1), PoseKey(key2), dt, noiseModel)); +} + +/* ************************************************************************* */ +void Graph::addBetween(Key key1, Key key2, const PoseRTV& z, const SharedNoiseModel& noiseModel) { + add(Between(PoseKey(key1), PoseKey(key2), z, noiseModel)); +} + +/* ************************************************************************* */ +void Graph::addRange(Key key1, Key key2, double z, const SharedNoiseModel& noiseModel) { + add(Range(PoseKey(key1), PoseKey(key2), z, noiseModel)); +} + +/* ************************************************************************* */ +Values Graph::optimize(const Values& init) const { + return gtsam::optimize(*this, init); +} +/* ************************************************************************* */ + +} // \namespace imu + diff --git a/gtsam/unstable/dynamics/imuSystem.h b/gtsam/unstable/dynamics/imuSystem.h new file mode 100644 index 000000000..1f518ef4a --- /dev/null +++ b/gtsam/unstable/dynamics/imuSystem.h @@ -0,0 +1,88 @@ +/** + * @file imuSystem.h + * @brief A 3D Dynamic system domain as a demonstration of IMU factors + * @author Alex Cunningham + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +/** + * This domain focuses on a single class of variables: PoseRTV, which + * models a dynamic pose operating with IMU measurements and assorted priors. + * + * There are also partial priors that constraint certain components of the + * poses, as well as between and range factors to model other between-pose + * information. + */ +namespace imu { + +using namespace gtsam; + +// Values: just poses +inline Symbol PoseKey(Index j) { return Symbol('x', j); } + +struct Values : public gtsam::Values { + typedef gtsam::Values Base; + + Values() {} + Values(const Values& values) : Base(values) {} + Values(const Base& values) : Base(values) {} + + void insertPose(Index key, const PoseRTV& pose) { insert(PoseKey(key), pose); } + PoseRTV pose(Index key) const { return at(PoseKey(key)); } +}; + +// factors +typedef IMUFactor IMUMeasurement; // IMU between measurements +typedef FullIMUFactor FullIMUMeasurement; // Full-state IMU between measurements +typedef BetweenFactor Between; // full odometry (including velocity) +typedef NonlinearEquality Constraint; +typedef PriorFactor Prior; +typedef RangeFactor Range; + +// graph components +struct Graph : public NonlinearFactorGraph { + typedef NonlinearFactorGraph Base; + + Graph() {} + Graph(const Base& graph) : Base(graph) {} + Graph(const Graph& graph) : Base(graph) {} + + // prior factors + void addPrior(size_t key, const PoseRTV& pose, const SharedNoiseModel& noiseModel); + void addConstraint(size_t key, const PoseRTV& pose); + void addHeightPrior(size_t key, double z, const SharedNoiseModel& noiseModel); + + // inertial factors + void addFullIMUMeasurement(size_t key1, size_t key2, const Vector& accel, const Vector& gyro, double dt, const SharedNoiseModel& noiseModel); + void addIMUMeasurement(size_t key1, size_t key2, const Vector& accel, const Vector& gyro, double dt, const SharedNoiseModel& noiseModel); + void addVelocityConstraint(size_t key1, size_t key2, double dt, const SharedNoiseModel& noiseModel); + + // other measurements + void addBetween(size_t key1, size_t key2, const PoseRTV& z, const SharedNoiseModel& noiseModel); + void addRange(size_t key1, size_t key2, double z, const SharedNoiseModel& noiseModel); + + // optimization + Values optimize(const Values& init) const; +}; + +} // \namespace imu + diff --git a/gtsam/unstable/dynamics/imu_examples.h b/gtsam/unstable/dynamics/imu_examples.h new file mode 100644 index 000000000..2eebb61a3 --- /dev/null +++ b/gtsam/unstable/dynamics/imu_examples.h @@ -0,0 +1,273 @@ +/** + * @file imu_examples.h + * @brief Example measurements from ACFR matlab simulation + * @author Alex Cunningham + */ + +#pragma once + +#include +#include + +namespace gtsam { +namespace examples { + +// examples pulled from simulated dataset + +// case pulled from dataset (frame 5000, no noise, flying robot) +namespace frame5000 { +double dt=0.010000; + +// previous frame +gtsam::Point3 posInit( + -34.959663877411039, + -36.312388041359441, + -9.929634578582256); +gtsam::Vector velInit = gtsam::Vector_(3, + -2.325950469365050, + -5.545469040035986, + 0.026939998635121); +gtsam::Vector attInit = gtsam::Vector_(3, + -0.005702574137157, + -0.029956547314287, + 1.625011216344428); + +// IMU measurement (accel (0-2), gyro (3-5)) - from current frame +gtsam::Vector accel = gtsam::Vector_(3, + 1.188806676070333, + -0.183885870345845, + -9.870747316422888); +gtsam::Vector gyro = gtsam::Vector_(3, + 0.0, + 0.026142019158168, + -2.617993877991494); + +// resulting frame +gtsam::Point3 posFinal( + -34.982904302954310, + -36.367693859767584, + -9.929367164045985); +gtsam::Vector velFinal = gtsam::Vector_(3, + -2.324042554327658, + -5.530581840815272, + 0.026741453627181); +gtsam::Vector attFinal = gtsam::Vector_(3, + -0.004918046965288, + -0.029844423605949, + 1.598818460739227); + +PoseRTV init (posInit, gtsam::Rot3::RzRyRx(attInit), velInit); +PoseRTV final(posFinal, gtsam::Rot3::RzRyRx(attFinal), velFinal); +} + +// case pulled from dataset (frame 10000, no noise, flying robot) +namespace frame10000 { +double dt=0.010000; + +// previous frame +gtsam::Point3 posInit( + -30.320731549352189, + -1.988742283760434, + -9.946212692656349); +gtsam::Vector velInit = gtsam::Vector_(3, + -0.094887047280235, + -5.200243574047883, + -0.006106911050672); +gtsam::Vector attInit = gtsam::Vector_(3, + -0.049884854704728, + -0.004630595995732, + -1.952691683647753); + +// IMU measurement (accel (0-2), gyro (3-5)) - from current frame +gtsam::Vector accel = gtsam::Vector_(3, + 0.127512027192321, + 0.508566822495083, + -9.987963604829643); +gtsam::Vector gyro = gtsam::Vector_(3, + 0.0, + 0.004040957322961, + 2.617993877991494); + +// resulting frame +gtsam::Point3 posFinal( + -30.321685191305157, + -2.040760054006146, + -9.946292804391417); +gtsam::Vector velFinal = gtsam::Vector_(3, + -0.095364195297074, + -5.201777024575911, + -0.008011173506779); +gtsam::Vector attFinal = gtsam::Vector_(3, + -0.050005924151669, + -0.003284795837998, + -1.926546047162884); + +PoseRTV init (posInit, gtsam::Rot3::RzRyRx(attInit), velInit); +PoseRTV final(posFinal, gtsam::Rot3::RzRyRx(attFinal), velFinal); +} + +// case pulled from dataset (frame at time 4.00, no noise, flying robot, poses from 3.99 to 4.0) +namespace flying400 { +double dt=0.010000; + +// start pose +// time 3.9900000e+00 +// pos (x,y,z) 1.8226188e+01 -6.7168156e+01 -9.8236334e+00 +// vel (dx,dy,dz) -5.2887267e+00 1.6117880e-01 -2.2665072e-02 +// att (r, p, y) 1.0928413e-02 -2.0811771e-02 2.7206011e+00 + +// previous frame +gtsam::Point3 posInit( + 1.8226188e+01, -6.7168156e+01, -9.8236334e+00); +gtsam::Vector velInit = gtsam::Vector_(3,-5.2887267e+00, 1.6117880e-01, -2.2665072e-02); +gtsam::Vector attInit = gtsam::Vector_(3, 1.0928413e-02, -2.0811771e-02, 2.7206011e+00); + +// time 4.0000000e+00 +// accel 3.4021509e-01 -3.4413998e-01 -9.8822495e+00 +// gyro 0.0000000e+00 1.8161697e-02 -2.6179939e+00 + +// IMU measurement (accel (0-2), gyro (3-5)) - ax, ay, az, r, p, y +gtsam::Vector accel = gtsam::Vector_(3, + 3.4021509e-01, -3.4413998e-01, -9.8822495e+00); +gtsam::Vector gyro = gtsam::Vector_(3, + 0.0000000e+00, 1.8161697e-02, -2.6179939e+00); // original version (possibly r, p, y) + +// final pose +// time 4.0000000e+00 +// pos (x,y,z) 1.8173262e+01 -6.7166500e+01 -9.8238667e+00 +// vel (vx,vy,vz) -5.2926092e+00 1.6559974e-01 -2.3330881e-02 +// att (r, p, y) 1.1473269e-02 -2.0344066e-02 2.6944191e+00 + +// resulting frame +gtsam::Point3 posFinal(1.8173262e+01, -6.7166500e+01, -9.8238667e+00); +gtsam::Vector velFinal = gtsam::Vector_(3,-5.2926092e+00, 1.6559974e-01, -2.3330881e-02); +gtsam::Vector attFinal = gtsam::Vector_(3, 1.1473269e-02, -2.0344066e-02, 2.6944191e+00); + +// full states +PoseRTV init (posInit, gtsam::Rot3::ypr( attInit(2), attInit(1), attInit(0)), velInit); +PoseRTV final(posFinal, gtsam::Rot3::ypr(attFinal(2), attFinal(1), attFinal(0)), velFinal); + +} // \namespace flying400 + +namespace flying650 { +double dt=0.010000; + +// from time 6.49 to 6.50 in robot F2 - + +// frame (6.49) +// 6.4900000e+00 -3.8065039e+01 -6.4788024e+01 -9.8947445e+00 -5.0099274e+00 1.5999675e-01 -1.7762191e-02 -5.7708025e-03 -1.0109000e-02 -3.0465662e+00 +gtsam::Point3 posInit( + -3.8065039e+01, -6.4788024e+01, -9.8947445e+00); +gtsam::Vector velInit = gtsam::Vector_(3,-5.0099274e+00, 1.5999675e-01, -1.7762191e-02); +gtsam::Vector attInit = gtsam::Vector_(3,-5.7708025e-03, -1.0109000e-02, -3.0465662e+00); + +// IMU measurement (accel (0-2), gyro (3-5)) - ax, ay, az, r, p, y +// 6.5000000e+00 -9.2017256e-02 8.6770069e-02 -9.8213017e+00 0.0000000e+00 1.0728860e-02 -2.6179939e+00 +gtsam::Vector accel = gtsam::Vector_(3, + -9.2017256e-02, 8.6770069e-02, -9.8213017e+00); +gtsam::Vector gyro = gtsam::Vector_(3, + 0.0000000e+00, 1.0728860e-02, -2.6179939e+00); // in r,p,y + +// resulting frame (6.50) +// 6.5000000e+00 -3.8115139e+01 -6.4786428e+01 -9.8949233e+00 -5.0099817e+00 1.5966531e-01 -1.7882777e-02 -5.5061386e-03 -1.0152792e-02 -3.0727477e+00 +gtsam::Point3 posFinal(-3.8115139e+01, -6.4786428e+01, -9.8949233e+00); +gtsam::Vector velFinal = gtsam::Vector_(3,-5.0099817e+00, 1.5966531e-01, -1.7882777e-02); +gtsam::Vector attFinal = gtsam::Vector_(3,-5.5061386e-03, -1.0152792e-02, -3.0727477e+00); + +// full states +PoseRTV init (posInit, gtsam::Rot3::ypr( attInit(2), attInit(1), attInit(0)), velInit); +PoseRTV final(posFinal, gtsam::Rot3::ypr(attFinal(2), attFinal(1), attFinal(0)), velFinal); + +} // \namespace flying650 + + +namespace flying39 { +double dt=0.010000; + +// from time 0.38 to 0.39 in robot F1 + +// frame (0.38) +// 3.8000000e-01 3.4204706e+01 -6.7413834e+01 -9.9996055e+00 -2.2484370e-02 -1.3603911e-03 1.5267496e-02 7.9033358e-04 -1.4946656e-02 -3.1174147e+00 +gtsam::Point3 posInit( 3.4204706e+01, -6.7413834e+01, -9.9996055e+00); +gtsam::Vector velInit = gtsam::Vector_(3,-2.2484370e-02, -1.3603911e-03, 1.5267496e-02); +gtsam::Vector attInit = gtsam::Vector_(3, 7.9033358e-04, -1.4946656e-02, -3.1174147e+00); + +// IMU measurement (accel (0-2), gyro (3-5)) - ax, ay, az, r, p, y +// 3.9000000e-01 7.2229967e-01 -9.5790214e-03 -9.3943087e+00 0.0000000e+00 -2.9157400e-01 -2.6179939e+00 +gtsam::Vector accel = gtsam::Vector_(3, 7.2229967e-01, -9.5790214e-03, -9.3943087e+00); +gtsam::Vector gyro = gtsam::Vector_(3, 0.0000000e+00, -2.9157400e-01, -2.6179939e+00); // in r,p,y + +// resulting frame (0.39) +// 3.9000000e-01 3.4204392e+01 -6.7413848e+01 -9.9994098e+00 -3.1382246e-02 -1.3577532e-03 1.9568177e-02 1.1816996e-03 -1.7841704e-02 3.1395854e+00 +gtsam::Point3 posFinal(3.4204392e+01, -6.7413848e+01, -9.9994098e+00); +gtsam::Vector velFinal = gtsam::Vector_(3,-3.1382246e-02, -1.3577532e-03, 1.9568177e-02); +gtsam::Vector attFinal = gtsam::Vector_(3, 1.1816996e-03, -1.7841704e-02, 3.1395854e+00); + +// full states +PoseRTV init (posInit, gtsam::Rot3::ypr( attInit(2), attInit(1), attInit(0)), velInit); +PoseRTV final(posFinal, gtsam::Rot3::ypr(attFinal(2), attFinal(1), attFinal(0)), velFinal); + +} // \namespace flying39 + +namespace ground200 { +double dt=0.010000; + +// from time 2.00 to 2.01 in robot G1 + +// frame (2.00) +// 2.0000000e+00 3.6863524e+01 -8.4874053e+01 0.0000000e+00 -7.9650687e-01 1.8345508e+00 0.0000000e+00 0.0000000e+00 0.0000000e+00 1.9804083e+00 +gtsam::Point3 posInit(3.6863524e+01, -8.4874053e+01, 0.0000000e+00); +gtsam::Vector velInit = gtsam::Vector_(3,-7.9650687e-01, 1.8345508e+00, 0.0000000e+00); +gtsam::Vector attInit = gtsam::Vector_(3, 0.0000000e+00, 0.0000000e+00, 1.9804083e+00); + +// IMU measurement (accel (0-2), gyro (3-5)) - ax, ay, az, r, p, y +// 2.0100000e+00 1.0000000e+00 8.2156504e-15 -9.8100000e+00 0.0000000e+00 0.0000000e+00 0.0000000e+00 +gtsam::Vector accel = gtsam::Vector_(3, + 1.0000000e+00, 8.2156504e-15, -9.8100000e+00); +gtsam::Vector gyro = gtsam::Vector_(3, + 0.0000000e+00, 0.0000000e+00, 0.0000000e+00); // in r,p,y + +// resulting frame (2.01) +// 2.0100000e+00 3.6855519e+01 -8.4855615e+01 0.0000000e+00 -8.0048940e-01 1.8437236e+00 0.0000000e+00 0.0000000e+00 0.0000000e+00 1.9804083e+00 +gtsam::Point3 posFinal(3.6855519e+01, -8.4855615e+01, 0.0000000e+00); +gtsam::Vector velFinal = gtsam::Vector_(3,-8.0048940e-01, 1.8437236e+00, 0.0000000e+00); +gtsam::Vector attFinal = gtsam::Vector_(3, 0.0000000e+00, 0.0000000e+00, 1.9804083e+00); + +// full states +PoseRTV init (posInit, gtsam::Rot3::ypr( attInit(2), attInit(1), attInit(0)), velInit); +PoseRTV final(posFinal, gtsam::Rot3::ypr(attFinal(2), attFinal(1), attFinal(0)), velFinal); + +} // \namespace ground200 + +namespace ground600 { +double dt=0.010000; + +// from time 6.00 to 6.01 in robot G1 + +// frame (6.00) +// 6.0000000e+00 3.0320057e+01 -7.0883904e+01 0.0000000e+00 -4.0020377e+00 2.9972811e+00 0.0000000e+00 0.0000000e+00 0.0000000e+00 2.4987711e+00 +gtsam::Point3 posInit(3.0320057e+01, -7.0883904e+01, 0.0000000e+00); +gtsam::Vector velInit = gtsam::Vector_(3,-4.0020377e+00, 2.9972811e+00, 0.0000000e+00); +gtsam::Vector attInit = gtsam::Vector_(3, 0.0000000e+00, 0.0000000e+00, 2.4987711e+00); + +// IMU measurement (accel (0-2), gyro (3-5)) - ax, ay, az, r, p, y +// 6.0100000e+00 6.1683759e-02 7.8536587e+00 -9.8100000e+00 0.0000000e+00 0.0000000e+00 1.5707963e+00 +gtsam::Vector accel = gtsam::Vector_(3, + 6.1683759e-02, 7.8536587e+00, -9.8100000e+00); +gtsam::Vector gyro = gtsam::Vector_(3, + 0.0000000e+00, 0.0000000e+00, 1.5707963e+00); // in r,p,y + +// resulting frame (6.01) +// 6.0100000e+00 3.0279571e+01 -7.0854564e+01 0.0000000e+00 -4.0486232e+00 2.9340501e+00 0.0000000e+00 0.0000000e+00 0.0000000e+00 2.5144791e+00 +gtsam::Point3 posFinal(3.0279571e+01, -7.0854564e+01, 0.0000000e+00); +gtsam::Vector velFinal = gtsam::Vector_(3,-4.0486232e+00, 2.9340501e+00, 0.0000000e+00); +gtsam::Vector attFinal = gtsam::Vector_(3, 0.0000000e+00, 0.0000000e+00, 2.5144791e+00); + +// full states +PoseRTV init (posInit, gtsam::Rot3::ypr( attInit(2), attInit(1), attInit(0)), velInit); +PoseRTV final(posFinal, gtsam::Rot3::ypr(attFinal(2), attFinal(1), attFinal(0)), velFinal); + +} // \namespace ground600 + +} // \namespace examples +} // \namespace gtsam diff --git a/gtsam/unstable/dynamics/inertialUtils.cpp b/gtsam/unstable/dynamics/inertialUtils.cpp new file mode 100644 index 000000000..473eff3ac --- /dev/null +++ b/gtsam/unstable/dynamics/inertialUtils.cpp @@ -0,0 +1,51 @@ +/** + * @file inertialUtils.cpp + * + * @date Nov 28, 2011 + * @author Alex Cunningham + */ + +#include + +namespace gtsam { +namespace dynamics { + +/* ************************************************************************* */ +Matrix RRTMbn(const Vector& euler) { + assert(euler.size() == 3); + const double s1 = sin(euler(1-1)), c1 = cos(euler(1-1)); + const double t2 = tan(euler(2-1)), c2 = cos(euler(2-1)); + Matrix Ebn(3,3); + Ebn << 1.0, s1 * t2, c1 * t2, + 0.0, c1, -s1, + 0.0, s1 / c2, c1 / c2; + return Ebn; +} + +/* ************************************************************************* */ +Matrix RRTMbn(const Rot3& att) { + return RRTMbn(att.rpy()); +} + +/* ************************************************************************* */ +Matrix RRTMnb(const Vector& euler) { + assert(euler.size() == 3); + Matrix Enb(3,3); + const double s1 = sin(euler(1-1)), c1 = cos(euler(1-1)); + const double s2 = sin(euler(2-1)), c2 = cos(euler(2-1)); + Enb << 1.0, 0.0, -s2, + 0.0, c1, s1*c2, + 0.0, -s1, c1*c2; + return Enb; +} + +/* ************************************************************************* */ +Matrix RRTMnb(const Rot3& att) { + return RRTMnb(att.rpy()); +} + +} // \namespace dynamics +} // \namespace gtsam + + + diff --git a/gtsam/unstable/dynamics/inertialUtils.h b/gtsam/unstable/dynamics/inertialUtils.h new file mode 100644 index 000000000..250ed3abe --- /dev/null +++ b/gtsam/unstable/dynamics/inertialUtils.h @@ -0,0 +1,32 @@ +/** + * @file inertialUtils.h + * + * @brief Utility functions for working with dynamic systems - derived from Mitch's matlab code + * This is mostly just syntactic sugar for working with dynamic systems that use + * Euler angles to specify the orientation of a robot. + * + * @date Nov 28, 2011 + * @author Alex Cunningham + */ + +#pragma once + +#include + +namespace gtsam { +namespace dynamics { + +/// RRTMbn - Function computes the rotation rate transformation matrix from +/// body axis rates to euler angle (global) rates +Matrix RRTMbn(const Vector& euler); + +Matrix RRTMbn(const Rot3& att); + +/// RRTMnb - Function computes the rotation rate transformation matrix from +/// euler angle rates to body axis rates +Matrix RRTMnb(const Vector& euler); + +Matrix RRTMnb(const Rot3& att); + +} // \namespace dynamics +} // \namespace gtsam diff --git a/gtsam/unstable/dynamics/tests/testIMUSystem.cpp b/gtsam/unstable/dynamics/tests/testIMUSystem.cpp new file mode 100644 index 000000000..c7baaf4a5 --- /dev/null +++ b/gtsam/unstable/dynamics/tests/testIMUSystem.cpp @@ -0,0 +1,203 @@ +/** + * @file testIMUSystem + * @author Alex Cunningham + */ + +#include + +#include +#include + +using namespace gtsam; +using namespace imu; + +const double tol=1e-5; + +static const Vector g = delta(3, 2, -9.81); + +Key x1 = PoseKey(1), x2 = PoseKey(2), x3 = PoseKey(3), x4 = PoseKey(4); + +/* ************************************************************************* */ +TEST(testIMUSystem, instantiations) { + // just checking for compilation + PoseRTV x1_v; + imu::Values local_values; + Graph graph; + + gtsam::SharedNoiseModel model1 = gtsam::noiseModel::Unit::Create(1); + gtsam::SharedNoiseModel model3 = gtsam::noiseModel::Unit::Create(3); + gtsam::SharedNoiseModel model6 = gtsam::noiseModel::Unit::Create(6); + gtsam::SharedNoiseModel model9 = gtsam::noiseModel::Unit::Create(9); + + Vector accel = ones(3), gyro = ones(3); + + IMUMeasurement imu(accel, gyro, 0.01, x1, x2, model6); + FullIMUMeasurement full_imu(accel, gyro, 0.01, x1, x2, model9); + Constraint poseHardPrior(x1, x1_v); + Between odom(x1, x2, x1_v, model9); + Range range(x1, x2, 1.0, model1); + VelocityConstraint constraint(x1, x2, 0.1, 10000); + Prior posePrior(x1, x1_v, model9); + DHeightPrior heightPrior(x1, 0.1, model1); + VelocityPrior velPrior(x1, ones(3), model3); +} + +/* ************************************************************************* */ +TEST( testIMUSystem, optimize_chain ) { + // create a simple chain of poses to generate IMU measurements + const double dt = 1.0; + PoseRTV pose1, + pose2(Point3(1.0, 1.0, 0.0), Rot3::ypr(0.1, 0.0, 0.0), Vector_(3, 2.0, 2.0, 0.0)), + pose3(Point3(2.0, 2.0, 0.0), Rot3::ypr(0.2, 0.0, 0.0), Vector_(3, 0.0, 0.0, 0.0)), + pose4(Point3(3.0, 3.0, 0.0), Rot3::ypr(0.3, 0.0, 0.0), Vector_(3, 2.0, 2.0, 0.0)); + + // create measurements + SharedDiagonal model = noiseModel::Unit::Create(6); + Vector imu12(6), imu23(6), imu34(6); + imu12 = pose1.imuPrediction(pose2, dt); + imu23 = pose2.imuPrediction(pose3, dt); + imu34 = pose3.imuPrediction(pose4, dt); + + // assemble simple graph with IMU measurements and velocity constraints + Graph graph; + graph.add(Constraint(x1, pose1)); + graph.add(IMUMeasurement(imu12, dt, x1, x2, model)); + graph.add(IMUMeasurement(imu23, dt, x2, x3, model)); + graph.add(IMUMeasurement(imu34, dt, x3, x4, model)); + graph.add(VelocityConstraint(x1, x2, dt)); + graph.add(VelocityConstraint(x2, x3, dt)); + graph.add(VelocityConstraint(x3, x4, dt)); + + // ground truth values + imu::Values true_values; + true_values.insert(x1, pose1); + true_values.insert(x2, pose2); + true_values.insert(x3, pose3); + true_values.insert(x4, pose4); + + // verify zero error + EXPECT_DOUBLES_EQUAL(0, graph.error(true_values), 1e-5); + + // initialize with zero values and optimize + imu::Values values; + values.insert(x1, PoseRTV()); + values.insert(x2, PoseRTV()); + values.insert(x3, PoseRTV()); + values.insert(x4, PoseRTV()); + + imu::Values actual = graph.optimize(values); + EXPECT(assert_equal(true_values, actual, tol)); +} + +/* ************************************************************************* */ +TEST( testIMUSystem, optimize_chain_fullfactor ) { + // create a simple chain of poses to generate IMU measurements + const double dt = 1.0; + PoseRTV pose1, + pose2(Point3(1.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Vector_(3, 1.0, 0.0, 0.0)), + pose3(Point3(2.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Vector_(3, 1.0, 0.0, 0.0)), + pose4(Point3(3.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Vector_(3, 1.0, 0.0, 0.0)); + + // create measurements + SharedDiagonal model = noiseModel::Isotropic::Sigma(9, 1.0); + Vector imu12(6), imu23(6), imu34(6); + imu12 = pose1.imuPrediction(pose2, dt); + imu23 = pose2.imuPrediction(pose3, dt); + imu34 = pose3.imuPrediction(pose4, dt); + + // assemble simple graph with IMU measurements and velocity constraints + Graph graph; + graph.add(Constraint(x1, pose1)); + graph.add(FullIMUMeasurement(imu12, dt, x1, x2, model)); + graph.add(FullIMUMeasurement(imu23, dt, x2, x3, model)); + graph.add(FullIMUMeasurement(imu34, dt, x3, x4, model)); + + // ground truth values + imu::Values true_values; + true_values.insert(x1, pose1); + true_values.insert(x2, pose2); + true_values.insert(x3, pose3); + true_values.insert(x4, pose4); + + // verify zero error + EXPECT_DOUBLES_EQUAL(0, graph.error(true_values), 1e-5); + + // initialize with zero values and optimize + imu::Values values; + values.insert(x1, PoseRTV()); + values.insert(x2, PoseRTV()); + values.insert(x3, PoseRTV()); + values.insert(x4, PoseRTV()); + + cout << "Initial Error: " << graph.error(values) << endl; // Initial error is 0.5 - need better prediction model + + imu::Values actual = graph.optimize(values); +// EXPECT(assert_equal(true_values, actual, tol)); // FAIL +} + +///* ************************************************************************* */ +//TEST( testIMUSystem, imu_factor_basics ) { +// using namespace examples; +// PoseKey x1(1), x2(2); +// SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(6, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6)); +// IMUMeasurement factor(frame5000::accel, frame5000::gyro, frame5000::dt, x1, x2, model); +// +// EXPECT(assert_equal(x1, factor.key1())); +// EXPECT(assert_equal(x2, factor.key2())); +// EXPECT(assert_equal(frame5000::accel, factor.accel(), tol)); +// EXPECT(assert_equal(frame5000::gyro, factor.gyro(), tol)); +// Vector full_meas = concatVectors(2, &frame5000::accel, &frame5000::gyro); +// EXPECT(assert_equal(full_meas, factor.z(), tol)); +//} +// +///* ************************************************************************* */ +//TEST( testIMUSystem, imu_factor_predict_function ) { +// using namespace examples; +// PoseKey x1(1), x2(2); +// SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(6, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6)); +// IMUMeasurement factor(frame5000::accel, frame5000::gyro, frame5000::dt, x1, x2, model); +// +// // verify zero error +// Vector actZeroError = factor.evaluateError(frame5000::init, frame5000::final); +// EXPECT(assert_equal(zero(6), actZeroError, tol)); +// +// // verify nonzero error - z-h(x) +// Vector actError = factor.evaluateError(frame10000::init, frame10000::final); +// Vector meas10k = concatVectors(2, &frame10000::accel, &frame10000::gyro); +// EXPECT(assert_equal(factor.z() - meas10k, actError, tol)); +//} + +/* ************************************************************************* */ +TEST( testIMUSystem, linear_trajectory) { + // create a linear trajectory of poses + // and verify simple solution + const double dt = 1.0; + + PoseRTV start; + Vector accel = delta(3, 0, 0.5); // forward force + Vector gyro = delta(3, 0, 0.1); // constant rotation + SharedDiagonal model = noiseModel::Unit::Create(9); + + imu::Values true_traj, init_traj; + Graph graph; + + graph.add(Constraint(PoseKey(0), start)); + true_traj.insert(PoseKey(0), start); + init_traj.insert(PoseKey(0), start); + + size_t nrPoses = 10; + PoseRTV cur_pose = start; + for (size_t i=1; i + +#include + +using namespace gtsam; + +static const double tol = 1e-5; + +/* ************************************************************************* */ +TEST(testInertialUtils, RRTMbn) { + EXPECT(assert_equal(Matrix::Identity(3,3), dynamics::RRTMbn(zero(3)), tol)); + EXPECT(assert_equal(Matrix::Identity(3,3), dynamics::RRTMbn(Rot3()), tol)); + EXPECT(assert_equal(dynamics::RRTMbn(Vector_(3, 0.3, 0.2, 0.1)), dynamics::RRTMbn(Rot3::ypr(0.1, 0.2, 0.3)), tol)); +} + +/* ************************************************************************* */ +TEST(testInertialUtils, RRTMnb) { + EXPECT(assert_equal(Matrix::Identity(3,3), dynamics::RRTMnb(zero(3)), tol)); + EXPECT(assert_equal(Matrix::Identity(3,3), dynamics::RRTMnb(Rot3()), tol)); + EXPECT(assert_equal(dynamics::RRTMnb(Vector_(3, 0.3, 0.2, 0.1)), dynamics::RRTMnb(Rot3::ypr(0.1, 0.2, 0.3)), tol)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/gtsam/unstable/dynamics/tests/testPoseRTV.cpp b/gtsam/unstable/dynamics/tests/testPoseRTV.cpp new file mode 100644 index 000000000..09afd24fe --- /dev/null +++ b/gtsam/unstable/dynamics/tests/testPoseRTV.cpp @@ -0,0 +1,390 @@ +/** + * @file testPoseRTV + * @author Alex Cunningham + */ + +#include +#include +#include +#include + +#include +#include + +using namespace gtsam; + +GTSAM_CONCEPT_TESTABLE_INST(PoseRTV) +GTSAM_CONCEPT_LIE_INST(PoseRTV) + +const double tol=1e-5; + +Rot3 rot = Rot3::RzRyRx(0.1, 0.2, 0.3); +Point3 pt(1.0, 2.0, 3.0); +Velocity3 vel(0.4, 0.5, 0.6); + +/* ************************************************************************* */ +TEST( testPoseRTV, constructors ) { + PoseRTV state1(pt, rot, vel); + EXPECT(assert_equal(pt, state1.t(), tol)); + EXPECT(assert_equal(rot, state1.R(), tol)); + EXPECT(assert_equal(vel, state1.v(), tol)); + EXPECT(assert_equal(Pose3(rot, pt), state1.pose(), tol)); + + PoseRTV state2; + EXPECT(assert_equal(Point3(), state2.t(), tol)); + EXPECT(assert_equal(Rot3(), state2.R(), tol)); + EXPECT(assert_equal(Velocity3(), state2.v(), tol)); + EXPECT(assert_equal(Pose3(), state2.pose(), tol)); + + PoseRTV state3(Pose3(rot, pt), vel); + EXPECT(assert_equal(pt, state3.t(), tol)); + EXPECT(assert_equal(rot, state3.R(), tol)); + EXPECT(assert_equal(vel, state3.v(), tol)); + EXPECT(assert_equal(Pose3(rot, pt), state3.pose(), tol)); + + PoseRTV state4(Pose3(rot, pt)); + EXPECT(assert_equal(pt, state4.t(), tol)); + EXPECT(assert_equal(rot, state4.R(), tol)); + EXPECT(assert_equal(Velocity3(), state4.v(), tol)); + EXPECT(assert_equal(Pose3(rot, pt), state4.pose(), tol)); + + Vector vec_init = Vector_(9, 0.1, 0.2, 0.3, 1.0, 2.0, 3.0, 0.4, 0.5, 0.6); + PoseRTV state5(vec_init); + EXPECT(assert_equal(pt, state5.t(), tol)); + EXPECT(assert_equal(rot, state5.R(), tol)); + EXPECT(assert_equal(vel, state5.v(), tol)); + EXPECT(assert_equal(vec_init, state5.vector(), tol)); +} + +/* ************************************************************************* */ +TEST( testPoseRTV, dim ) { + PoseRTV state1(pt, rot, vel); + EXPECT_LONGS_EQUAL(9, state1.dim()); + EXPECT_LONGS_EQUAL(9, PoseRTV::Dim()); +} + +/* ************************************************************************* */ +TEST( testPoseRTV, equals ) { + PoseRTV state1, state2(pt, rot, vel), state3(state2), state4(Pose3(rot, pt)); + EXPECT(assert_equal(state1, state1, tol)); + EXPECT(assert_equal(state2, state3, tol)); + EXPECT(assert_equal(state3, state2, tol)); + EXPECT(assert_inequal(state1, state2, tol)); + EXPECT(assert_inequal(state2, state1, tol)); + EXPECT(assert_inequal(state2, state4, tol)); +} + +/* ************************************************************************* */ +TEST( testPoseRTV, Lie ) { + // origin and zero deltas + EXPECT(assert_equal(PoseRTV(), PoseRTV().retract(zero(9)), tol)); + EXPECT(assert_equal(zero(9), PoseRTV().localCoordinates(PoseRTV()), tol)); + + PoseRTV state1(pt, rot, vel); + EXPECT(assert_equal(state1, state1.retract(zero(9)), tol)); + EXPECT(assert_equal(zero(9), state1.localCoordinates(state1), tol)); + + Vector delta = Vector_(9, 0.1, 0.1, 0.1, 0.2, 0.3, 0.4,-0.1,-0.2,-0.3); + Rot3 rot2 = rot.retract(repeat(3, 0.1)); + Point3 pt2 = pt + rot * Point3(0.2, 0.3, 0.4); + Velocity3 vel2 = vel + rot * Velocity3(-0.1,-0.2,-0.3); + PoseRTV state2(pt2, rot2, vel2); + EXPECT(assert_equal(state2, state1.retract(delta), tol)); + EXPECT(assert_equal(delta, state1.localCoordinates(state2), tol)); + EXPECT(assert_equal(-delta, state2.localCoordinates(state1), 1e-1)); // loose tolerance due to retract approximation +} + +/* ************************************************************************* */ +TEST( testPoseRTV, dynamics_identities ) { + // general dynamics should produce the same measurements as the imuPrediction function + PoseRTV x0, x1, x2, x3, x4; + + const double dt = 0.1; + Vector accel = Vector_(3, 0.2, 0.0, 0.0), gyro = Vector_(3, 0.0, 0.0, 0.2); + Vector imu01 = zero(6), imu12 = zero(6), imu23 = zero(6), imu34 = zero(6); + + x1 = x0.generalDynamics(accel, gyro, dt); + x2 = x1.generalDynamics(accel, gyro, dt); + x3 = x2.generalDynamics(accel, gyro, dt); + x4 = x3.generalDynamics(accel, gyro, dt); + +// EXPECT(assert_equal(imu01, x0.imuPrediction(x1, dt).first, tol)); +// EXPECT(assert_equal(imu12, x1.imuPrediction(x2, dt).first, tol)); +// EXPECT(assert_equal(imu23, x2.imuPrediction(x3, dt).first, tol)); +// EXPECT(assert_equal(imu34, x3.imuPrediction(x4, dt).first, tol)); +// +// EXPECT(assert_equal(x1.translation(), x0.imuPrediction(x1, dt).second, tol)); +// EXPECT(assert_equal(x2.translation(), x1.imuPrediction(x2, dt).second, tol)); +// EXPECT(assert_equal(x3.translation(), x2.imuPrediction(x3, dt).second, tol)); +// EXPECT(assert_equal(x4.translation(), x3.imuPrediction(x4, dt).second, tol)); +} + +///* ************************************************************************* */ +//TEST( testPoseRTV, constant_velocity ) { +// double dt = 1.0; +// PoseRTV init(Rot3(), Point3(1.0, 2.0, 3.0), Vector_(3, 0.5, 0.0, 0.0)); +// PoseRTV final(Rot3(), Point3(1.5, 2.0, 3.0), Vector_(3, 0.5, 0.0, 0.0)); +// +// // constant velocity, so gyro is zero, but accel includes gravity +// Vector accel = delta(3, 2, -9.81), gyro = zero(3); +// +// // perform integration +// PoseRTV actFinal = init.integrate(accel, gyro, dt); +// EXPECT(assert_equal(final, actFinal, tol)); +// +// // perform prediction +// Vector actAccel, actGyro; +// boost::tie(actAccel, actGyro) = init.predict(final, dt); +// EXPECT(assert_equal(accel, actAccel, tol)); +// EXPECT(assert_equal(gyro, actGyro, tol)); +//} +// +///* ************************************************************************* */ +//TEST( testPoseRTV, frame10000_imu ) { +// using namespace examples; +// +// // perform integration +// PoseRTV actFinal = frame10000::init.integrate(frame10000::accel, frame10000::gyro, frame10000::dt); +// EXPECT(assert_equal(frame10000::final, actFinal, tol)); +// +// // perform prediction +// Vector actAccel, actGyro; +// boost::tie(actAccel, actGyro) = frame10000::init.predict(frame10000::final, frame10000::dt); +// EXPECT(assert_equal(frame10000::accel, actAccel, tol)); +// EXPECT(assert_equal(frame10000::gyro, actGyro, tol)); +//} +// +///* ************************************************************************* */ +//TEST( testPoseRTV, frame5000_imu ) { +// using namespace examples; +// +// // perform integration +// PoseRTV actFinal = frame5000::init.integrate(frame5000::accel, frame5000::gyro, frame5000::dt); +// EXPECT(assert_equal(frame5000::final, actFinal, tol)); +// +// // perform prediction +// Vector actAccel, actGyro; +// boost::tie(actAccel, actGyro) = frame5000::init.predict(frame5000::final, frame5000::dt); +// EXPECT(assert_equal(frame5000::accel, actAccel, tol)); +// EXPECT(assert_equal(frame5000::gyro, actGyro, tol)); +//} +// +///* ************************************************************************* */ +//TEST( testPoseRTV, time4_imu ) { +// using namespace examples::flying400; +// +// // perform integration +// PoseRTV actFinal = init.integrate(accel, gyro, dt); +// EXPECT(assert_equal(final, actFinal, tol)); +// +// // perform prediction +// Vector actAccel, actGyro; +// boost::tie(actAccel, actGyro) = init.predict(final, dt); +// EXPECT(assert_equal(accel, actAccel, tol)); +// EXPECT(assert_equal(gyro, actGyro, tol)); +//} +// +///* ************************************************************************* */ +//TEST( testPoseRTV, time65_imu ) { +// using namespace examples::flying650; +// +// // perform integration +// PoseRTV actFinal = init.integrate(accel, gyro, dt); +// EXPECT(assert_equal(final, actFinal, tol)); +// +// // perform prediction +// Vector actAccel, actGyro; +// boost::tie(actAccel, actGyro) = init.predict(final, dt); +// EXPECT(assert_equal(accel, actAccel, tol)); +// EXPECT(assert_equal(gyro, actGyro, tol)); +//} + +/* ************************************************************************* */ +double range_proxy(const PoseRTV& A, const PoseRTV& B) { return A.range(B); } +TEST( testPoseRTV, range ) { + Point3 tA(1.0, 2.0, 3.0), tB(3.0, 2.0, 3.0); + PoseRTV rtvA(tA), rtvB(tB); + EXPECT_DOUBLES_EQUAL(0.0, rtvA.range(rtvA), tol); + EXPECT_DOUBLES_EQUAL(2.0, rtvA.range(rtvB), tol); + EXPECT_DOUBLES_EQUAL(2.0, rtvB.range(rtvA), tol); + + Matrix actH1, actH2; + rtvA.range(rtvB, actH1, actH2); + Matrix numericH1 = numericalDerivative21(range_proxy, rtvA, rtvB); + Matrix numericH2 = numericalDerivative22(range_proxy, rtvA, rtvB); + EXPECT(assert_equal(numericH1, actH1, tol)); + EXPECT(assert_equal(numericH2, actH2, tol)); +} + +/* ************************************************************************* */ +PoseRTV transformed_from_proxy(const PoseRTV& a, const Pose3& trans) { + return a.transformed_from(trans); +} +TEST( testPoseRTV, transformed_from_1 ) { + Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); + Point3 T(1.0, 2.0, 3.0); + Velocity3 V(2.0, 3.0, 4.0); + PoseRTV start(R, T, V); + Pose3 transform(Rot3::yaw(M_PI_2), Point3(1.0, 2.0, 3.0)); + + Matrix actDTrans, actDGlobal; + PoseRTV actual = start.transformed_from(transform, actDGlobal, actDTrans); + PoseRTV expected(transform.compose(start.pose()), transform.rotation().rotate(V)); + EXPECT(assert_equal(expected, actual, tol)); + + Matrix numDGlobal = numericalDerivative21(transformed_from_proxy, start, transform, 1e-8); + Matrix numDTrans = numericalDerivative22(transformed_from_proxy, start, transform, 1e-8); + EXPECT(assert_equal(numDGlobal, actDGlobal, tol)); + EXPECT(assert_equal(numDTrans, actDTrans, tol)); // FIXME: still needs analytic derivative +} + +/* ************************************************************************* */ +TEST( testPoseRTV, transformed_from_2 ) { + Rot3 R; + Point3 T(1.0, 2.0, 3.0); + Velocity3 V(2.0, 3.0, 4.0); + PoseRTV start(R, T, V); + Pose3 transform(Rot3::yaw(M_PI_2), Point3(1.0, 2.0, 3.0)); + + Matrix actDTrans, actDGlobal; + PoseRTV actual = start.transformed_from(transform, actDGlobal, actDTrans); + PoseRTV expected(transform.compose(start.pose()), transform.rotation().rotate(V)); + EXPECT(assert_equal(expected, actual, tol)); + + Matrix numDGlobal = numericalDerivative21(transformed_from_proxy, start, transform, 1e-8); + Matrix numDTrans = numericalDerivative22(transformed_from_proxy, start, transform, 1e-8); + EXPECT(assert_equal(numDGlobal, actDGlobal, tol)); + EXPECT(assert_equal(numDTrans, actDTrans, tol)); // FIXME: still needs analytic derivative +} + +/* ************************************************************************* */ +// ground robot maximums +//const static double ground_max_accel = 1.0; // m/s^2 +//const static double ground_mag_vel = 5.0; // m/s - fixed in simulator + +///* ************************************************************************* */ +//TEST(testPoseRTV, flying_integration650) { +// using namespace examples; +// const PoseRTV &x1 = flying650::init, &x2 = flying650::final; +// Vector accel = flying650::accel, gyro = flying650::gyro; +// double dt = flying650::dt; +// +// // control inputs +// double pitch_rate = gyro(1), +// heading_rate = gyro(2), +// lift_control = 0.0; /// FIXME: need to find this value +// +// PoseRTV actual_x2; +// actual_x2 = x1.flyingDynamics(pitch_rate, heading_rate, lift_control, dt); +// +// // FIXME: enable remaining components when there the lift control value is known +// EXPECT(assert_equal(x2.R(), actual_x2.R(), tol)); +//// EXPECT(assert_equal(x2.t(), actual_x2.t(), tol)); +//// EXPECT(assert_equal(x2.v(), actual_x2.v(), tol)); +//} + +///* ************************************************************************* */ +//TEST(testPoseRTV, imu_prediction650) { +// using namespace examples; +// const PoseRTV &x1 = flying650::init, &x2 = flying650::final; +// Vector accel = flying650::accel, gyro = flying650::gyro; +// double dt = flying650::dt; +// +// // given states, predict the imu measurement and t2 (velocity constraint) +// Vector actual_imu; +// Point3 actual_t2; +// boost::tie(actual_imu, actual_t2) = x1.imuPrediction(x2, dt); +// +// EXPECT(assert_equal(x2.t(), actual_t2, tol)); +// EXPECT(assert_equal(accel, actual_imu.head(3), tol)); +// EXPECT(assert_equal(gyro, actual_imu.tail(3), tol)); +//} +// +///* ************************************************************************* */ +//TEST(testPoseRTV, imu_prediction39) { +// // This case was a known failure case for gyro prediction, returning [9.39091; 0.204952; 625.63] using +// // the general approach for reverse-engineering the gyro updates +// using namespace examples; +// const PoseRTV &x1 = flying39::init, &x2 = flying39::final; +// Vector accel = flying39::accel, gyro = flying39::gyro; +// double dt = flying39::dt; +// +// // given states, predict the imu measurement and t2 (velocity constraint) +// Vector actual_imu; +// Point3 actual_t2; +// boost::tie(actual_imu, actual_t2) = x1.imuPrediction(x2, dt); +// +// EXPECT(assert_equal(x2.t(), actual_t2, tol)); +// EXPECT(assert_equal(accel, actual_imu.head(3), tol)); +// EXPECT(assert_equal(gyro, actual_imu.tail(3), tol)); +//} +// +///* ************************************************************************* */ +//TEST(testPoseRTV, ground_integration200) { +// using namespace examples; +// const PoseRTV &x1 = ground200::init, &x2 = ground200::final; +// Vector accel = ground200::accel, gyro = ground200::gyro; +// double dt = ground200::dt; +// +// // integrates from one pose to the next with known measurements +// // No heading change in this example +// // Hits maximum accel bound in this example +// +// PoseRTV actual_x2; +// actual_x2 = x1.planarDynamics(ground_mag_vel, gyro(2), ground_max_accel, dt); +// +// EXPECT(assert_equal(x2, actual_x2, tol)); +//} +// +///* ************************************************************************* */ +//TEST(testPoseRTV, ground_prediction200) { +// using namespace examples; +// const PoseRTV &x1 = ground200::init, &x2 = ground200::final; +// Vector accel = ground200::accel, gyro = ground200::gyro; +// double dt = ground200::dt; +// +// // given states, predict the imu measurement and t2 (velocity constraint) +// Vector actual_imu; +// Point3 actual_t2; +// boost::tie(actual_imu, actual_t2) = x1.imuPrediction(x2, dt); +// +// EXPECT(assert_equal(x2.t(), actual_t2, tol)); +// EXPECT(assert_equal(accel, actual_imu.head(3), tol)); +// EXPECT(assert_equal(gyro, actual_imu.tail(3), tol)); +//} +// +///* ************************************************************************* */ +//TEST(testPoseRTV, ground_integration600) { +// using namespace examples; +// const PoseRTV &x1 = ground600::init, &x2 = ground600::final; +// Vector accel = ground600::accel, gyro = ground600::gyro; +// double dt = ground600::dt; +// +// // integrates from one pose to the next with known measurements +// PoseRTV actual_x2; +// actual_x2 = x1.planarDynamics(ground_mag_vel, gyro(2), ground_max_accel, dt); +// +// EXPECT(assert_equal(x2, actual_x2, tol)); +//} +// +///* ************************************************************************* */ +//TEST(testPoseRTV, ground_prediction600) { +// using namespace examples; +// const PoseRTV &x1 = ground600::init, &x2 = ground600::final; +// Vector accel = ground600::accel, gyro = ground600::gyro; +// double dt = ground600::dt; +// +// // given states, predict the imu measurement and t2 (velocity constraint) +// Vector actual_imu; +// Point3 actual_t2; +// boost::tie(actual_imu, actual_t2) = x1.imuPrediction(x2, dt); +// +// EXPECT(assert_equal(x2.t(), actual_t2, tol)); +// EXPECT(assert_equal(accel, actual_imu.head(3), tol)); +// EXPECT(assert_equal(gyro, actual_imu.tail(3), tol)); +//} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ + diff --git a/gtsam/unstable/dynamics/tests/testVelocityConstraint.cpp b/gtsam/unstable/dynamics/tests/testVelocityConstraint.cpp new file mode 100644 index 000000000..99fb8e70e --- /dev/null +++ b/gtsam/unstable/dynamics/tests/testVelocityConstraint.cpp @@ -0,0 +1,60 @@ +/** + * @file testVelocityConstraint + * @author Alex Cunningham + */ + +#include +#include + +using namespace gtsam; +using namespace imu; + +const double tol=1e-5; + +Key x1 = PoseKey(1), x2 = PoseKey(2); +const double dt = 1.0; + +PoseRTV origin, + pose1(Point3(0.5, 0.0, 0.0), Rot3::identity(), Vector_(3, 1.0, 0.0, 0.0)), + pose1a(Point3(0.5, 0.0, 0.0)), + pose2(Point3(1.5, 0.0, 0.0), Rot3::identity(), Vector_(3, 1.0, 0.0, 0.0)); + +/* ************************************************************************* */ +TEST( testVelocityConstraint, trapezoidal ) { + // hard constraints don't need a noise model + VelocityConstraint constraint(x1, x2, dynamics::TRAPEZOIDAL, dt); + + // verify error function + EXPECT(assert_equal(zero(3), constraint.evaluateError(origin, pose1), tol)); + EXPECT(assert_equal(zero(3), constraint.evaluateError(origin, origin), tol)); + EXPECT(assert_equal(delta(3, 0,-1.0), constraint.evaluateError(pose1, pose1), tol)); + EXPECT(assert_equal(delta(3, 0, 0.5), constraint.evaluateError(origin, pose1a), tol)); +} + +/* ************************************************************************* */ +TEST( testEulerVelocityConstraint, euler_start ) { + // hard constraints create their own noise model + VelocityConstraint constraint(x1, x2, dynamics::EULER_START, dt); + + // verify error function + EXPECT(assert_equal(delta(3, 0, 0.5), constraint.evaluateError(origin, pose1), tol)); + EXPECT(assert_equal(zero(3), constraint.evaluateError(origin, origin), tol)); + EXPECT(assert_equal(zero(3), constraint.evaluateError(pose1, pose2), tol)); + EXPECT(assert_equal(delta(3, 0, 0.5), constraint.evaluateError(origin, pose1a), tol)); +} + +/* ************************************************************************* */ +TEST( testEulerVelocityConstraint, euler_end ) { + // hard constraints create their own noise model + VelocityConstraint constraint(x1, x2, dynamics::EULER_END, dt); + + // verify error function + EXPECT(assert_equal(delta(3, 0,-0.5), constraint.evaluateError(origin, pose1), tol)); + EXPECT(assert_equal(zero(3), constraint.evaluateError(origin, origin), tol)); + EXPECT(assert_equal(zero(3), constraint.evaluateError(pose1, pose2), tol)); + EXPECT(assert_equal(delta(3, 0, 0.5), constraint.evaluateError(origin, pose1a), tol)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/gtsam/unstable/geometry/CMakeLists.txt b/gtsam/unstable/geometry/CMakeLists.txt new file mode 100644 index 000000000..09710a005 --- /dev/null +++ b/gtsam/unstable/geometry/CMakeLists.txt @@ -0,0 +1,17 @@ +# Install headers +file(GLOB geometry_headers "*.h") +install(FILES ${geometry_headers} DESTINATION include/gtsam2/geometry) + +# Components to link tests in this subfolder against +set(geometry_local_libs + geometry) + +set (geometry_full_libs + gtsam2-static) + +# Exclude tests that don't work +set (geometry_excluded_tests "") + +# Add all tests +gtsam_add_subdir_tests(geometry "${geometry_local_libs}" "${geometry_full_libs}" "${geometry_excluded_tests}") + diff --git a/gtsam/unstable/geometry/Tensor1.h b/gtsam/unstable/geometry/Tensor1.h new file mode 100644 index 000000000..63e406ef6 --- /dev/null +++ b/gtsam/unstable/geometry/Tensor1.h @@ -0,0 +1,85 @@ +/* ---------------------------------------------------------------------------- + + * 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/unstable/geometry/Tensor1Expression.h b/gtsam/unstable/geometry/Tensor1Expression.h new file mode 100644 index 000000000..e06d9df06 --- /dev/null +++ b/gtsam/unstable/geometry/Tensor1Expression.h @@ -0,0 +1,181 @@ +/* ---------------------------------------------------------------------------- + + * 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/unstable/geometry/Tensor2.h b/gtsam/unstable/geometry/Tensor2.h new file mode 100644 index 000000000..76a0fb088 --- /dev/null +++ b/gtsam/unstable/geometry/Tensor2.h @@ -0,0 +1,84 @@ +/* ---------------------------------------------------------------------------- + + * 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/unstable/geometry/Tensor2Expression.h b/gtsam/unstable/geometry/Tensor2Expression.h new file mode 100644 index 000000000..8583c6396 --- /dev/null +++ b/gtsam/unstable/geometry/Tensor2Expression.h @@ -0,0 +1,310 @@ +/* ---------------------------------------------------------------------------- + + * 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/unstable/geometry/Tensor3.h b/gtsam/unstable/geometry/Tensor3.h new file mode 100644 index 000000000..660b30be9 --- /dev/null +++ b/gtsam/unstable/geometry/Tensor3.h @@ -0,0 +1,106 @@ +/* ---------------------------------------------------------------------------- + + * 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/unstable/geometry/Tensor3Expression.h b/gtsam/unstable/geometry/Tensor3Expression.h new file mode 100644 index 000000000..1e1c9990f --- /dev/null +++ b/gtsam/unstable/geometry/Tensor3Expression.h @@ -0,0 +1,194 @@ +/* ---------------------------------------------------------------------------- + + * 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/unstable/geometry/Tensor4.h b/gtsam/unstable/geometry/Tensor4.h new file mode 100644 index 000000000..6fde1b72c --- /dev/null +++ b/gtsam/unstable/geometry/Tensor4.h @@ -0,0 +1,58 @@ +/* ---------------------------------------------------------------------------- + + * 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/unstable/geometry/Tensor5.h b/gtsam/unstable/geometry/Tensor5.h new file mode 100644 index 000000000..6b11cb5ed --- /dev/null +++ b/gtsam/unstable/geometry/Tensor5.h @@ -0,0 +1,75 @@ +/* ---------------------------------------------------------------------------- + + * 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/unstable/geometry/Tensor5Expression.h b/gtsam/unstable/geometry/Tensor5Expression.h new file mode 100644 index 000000000..10c4b8213 --- /dev/null +++ b/gtsam/unstable/geometry/Tensor5Expression.h @@ -0,0 +1,135 @@ +/* ---------------------------------------------------------------------------- + + * 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/unstable/geometry/projectiveGeometry.cpp b/gtsam/unstable/geometry/projectiveGeometry.cpp new file mode 100644 index 000000000..94a1d86d0 --- /dev/null +++ b/gtsam/unstable/geometry/projectiveGeometry.cpp @@ -0,0 +1,71 @@ +/* ---------------------------------------------------------------------------- + + * 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/unstable/geometry/projectiveGeometry.h b/gtsam/unstable/geometry/projectiveGeometry.h new file mode 100644 index 000000000..3d8faa61d --- /dev/null +++ b/gtsam/unstable/geometry/projectiveGeometry.h @@ -0,0 +1,124 @@ +/* ---------------------------------------------------------------------------- + + * 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/unstable/geometry/tensorInterface.h b/gtsam/unstable/geometry/tensorInterface.h new file mode 100644 index 000000000..f7d5bc29f --- /dev/null +++ b/gtsam/unstable/geometry/tensorInterface.h @@ -0,0 +1,108 @@ +/* ---------------------------------------------------------------------------- + + * 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/unstable/geometry/tensors.h b/gtsam/unstable/geometry/tensors.h new file mode 100644 index 000000000..80c18dffa --- /dev/null +++ b/gtsam/unstable/geometry/tensors.h @@ -0,0 +1,45 @@ +/* ---------------------------------------------------------------------------- + + * 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/unstable/geometry/tests/testFundamental.cpp b/gtsam/unstable/geometry/tests/testFundamental.cpp new file mode 100644 index 000000000..93732e14c --- /dev/null +++ b/gtsam/unstable/geometry/tests/testFundamental.cpp @@ -0,0 +1,73 @@ +/* ---------------------------------------------------------------------------- + + * 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/unstable/geometry/tests/testHomography2.cpp b/gtsam/unstable/geometry/tests/testHomography2.cpp new file mode 100644 index 000000000..fa4845304 --- /dev/null +++ b/gtsam/unstable/geometry/tests/testHomography2.cpp @@ -0,0 +1,188 @@ +/* ---------------------------------------------------------------------------- + + * 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/unstable/geometry/tests/testTensors.cpp b/gtsam/unstable/geometry/tests/testTensors.cpp new file mode 100644 index 000000000..e2c40e7b1 --- /dev/null +++ b/gtsam/unstable/geometry/tests/testTensors.cpp @@ -0,0 +1,240 @@ +/* ---------------------------------------------------------------------------- + + * 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/unstable/mainpage.dox b/gtsam/unstable/mainpage.dox new file mode 100644 index 000000000..262dab1a6 --- /dev/null +++ b/gtsam/unstable/mainpage.dox @@ -0,0 +1,20 @@ +// This causes Doxygen to find classes inside the gtsam namespace without +// explicitly specifying it when writing class names. +namespace gtsam { + +/** + +\mainpage GTSAM2 + +\section gtsam2_overview GTSAM2 Overview + +GTSAM2 is a set of experimental additions to GTSAM that are not fully ready to be +integrated into the rest of the library. This library is actively maintained, +and should follow the same design procedures as GTSAM. + +Because parts of this library are supposed to be a part of GTSAM, all code should +be in the gtsam namespace, rather than a gtsam2 namespace. + +*/ + +} \ No newline at end of file diff --git a/gtsam/unstable/slam/CMakeLists.txt b/gtsam/unstable/slam/CMakeLists.txt new file mode 100644 index 000000000..fd25a9af8 --- /dev/null +++ b/gtsam/unstable/slam/CMakeLists.txt @@ -0,0 +1,17 @@ +# Install headers +file(GLOB slam_headers "*.h") +install(FILES ${slam_headers} DESTINATION include/gtsam2/slam) + +# Components to link tests in this subfolder against +set(slam_local_libs + slam) + +set (slam_full_libs + gtsam2-static) + +# Exclude tests that don't work +set (slam_excluded_tests "") + +# Add all tests +gtsam_add_subdir_tests(slam "${slam_local_libs}" "${slam_full_libs}" "${slam_excluded_tests}") + diff --git a/gtsam/unstable/slam/simulated3D.cpp b/gtsam/unstable/slam/simulated3D.cpp new file mode 100644 index 000000000..84b40780d --- /dev/null +++ b/gtsam/unstable/slam/simulated3D.cpp @@ -0,0 +1,43 @@ +/* ---------------------------------------------------------------------------- + + * 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/unstable/slam/simulated3D.h b/gtsam/unstable/slam/simulated3D.h new file mode 100644 index 000000000..7b4dfce37 --- /dev/null +++ b/gtsam/unstable/slam/simulated3D.h @@ -0,0 +1,126 @@ +/* ---------------------------------------------------------------------------- + + * 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/unstable/slam/tests/testSimulated3D.cpp b/gtsam/unstable/slam/tests/testSimulated3D.cpp new file mode 100644 index 000000000..799261451 --- /dev/null +++ b/gtsam/unstable/slam/tests/testSimulated3D.cpp @@ -0,0 +1,63 @@ +/* ---------------------------------------------------------------------------- + + * 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);} +/* ************************************************************************* */ diff --git a/gtsam/unstable/wrap/CMakeLists.txt b/gtsam/unstable/wrap/CMakeLists.txt new file mode 100644 index 000000000..92bb27c8e --- /dev/null +++ b/gtsam/unstable/wrap/CMakeLists.txt @@ -0,0 +1,52 @@ +# Find wrap +find_package(Wrap REQUIRED) + +# Set up codegen +include(GtsamMatlabWrap) + +# Wrap codegen +#usage: wrap mexExtension interfacePath moduleName toolboxPath +# mexExtension : OS/CPU-dependent extension for MEX binaries +# interfacePath : *absolute* path to directory of module interface file +# moduleName : the name of the module, interface file must be called moduleName.h +# toolboxPath : the directory in which to generate the wrappers +# [mexFlags] : extra flags for the mex command + +# TODO: generate these includes programmatically +set(mexFlags "-I${Boost_INCLUDE_DIR} -I${Wrap_INCLUDE_DIR} -I${CMAKE_INSTALL_PREFIX}/include/gtsam2 -I${CMAKE_INSTALL_PREFIX}/include/gtsam2/dynamics -I${CMAKE_INSTALL_PREFIX}/include/gtsam2/discrete -L${CMAKE_INSTALL_PREFIX}/lib -lgtsam -lgtsam2") +set(toolbox_path ${CMAKE_BINARY_DIR}/wrap/gtsam2) +set(moduleName gtsam2) + +find_mexextension() + +# Code generation command +add_custom_target(wrap_gtsam2 ALL COMMAND + ${Wrap_CMD} ${GTSAM_MEX_BIN_EXTENSION} ${CMAKE_CURRENT_SOURCE_DIR}/../ ${moduleName} ${toolbox_path} "${mexFlags}") + +option(GTSAM2_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" ON) +option(GTSAM2_INSTALL_MATLAB_EXAMPLES "Enable/Disable installation of matlab examples" ON) +option(GTSAM2_INSTALL_MATLAB_TESTS "Enable/Disable installation of matlab tests" ON) + +set(GTSAM2_TOOLBOX_INSTALL_PATH ${CMAKE_INSTALL_PREFIX}/borg/toolbox CACHE DOCSTRING "Path to install matlab toolbox") + +if (GTSAM2_INSTALL_MATLAB_TOOLBOX) + # Primary toolbox files + message(STATUS "Installing Matlab Toolbox to ${GTSAM2_TOOLBOX_INSTALL_PATH}") + install(DIRECTORY DESTINATION ${GTSAM2_TOOLBOX_INSTALL_PATH}) # make an empty folder + # exploit need for trailing slash to specify a full folder, rather than just its contents to copy + install(DIRECTORY ${toolbox_path} DESTINATION ${GTSAM2_TOOLBOX_INSTALL_PATH}) + + # Examples + if (GTSAM2_INSTALL_MATLAB_EXAMPLES) + message(STATUS "Installing Matlab Toolbox Examples") + file(GLOB matlab_examples "${CMAKE_SOURCE_DIR}/examples/matlab/*.m") + install(FILES ${matlab_examples} DESTINATION ${GTSAM2_TOOLBOX_INSTALL_PATH}/gtsam2/examples) + endif (GTSAM2_INSTALL_MATLAB_EXAMPLES) + + # Tests + if (GTSAM2_INSTALL_MATLAB_TESTS) + message(STATUS "Installing Matlab Toolbox Tests") + file(GLOB matlab_tests "${CMAKE_SOURCE_DIR}/tests/matlab/*.m") + install(FILES ${matlab_tests} DESTINATION ${GTSAM2_TOOLBOX_INSTALL_PATH}/gtsam2/tests) + endif (GTSAM2_INSTALL_MATLAB_TESTS) +endif (GTSAM2_INSTALL_MATLAB_TOOLBOX) diff --git a/gtsam/unstable/wrap/gtsam2.h b/gtsam/unstable/wrap/gtsam2.h new file mode 100644 index 000000000..257796b7b --- /dev/null +++ b/gtsam/unstable/wrap/gtsam2.h @@ -0,0 +1,96 @@ +/** + * Matlab toolbox interface definition for GTSAM2 + */ + +// Most things are in the gtsam namespace +using namespace gtsam; + +// specify the classes from gtsam we are using +class gtsam::Point3; +class gtsam::Rot3; +class gtsam::Pose3; +class gtsam::SharedNoiseModel; + +#include +class PoseRTV { + PoseRTV(); + PoseRTV(Vector rtv); + PoseRTV(const gtsam::Point3& pt, const gtsam::Rot3& rot, const gtsam::Point3& vel); + PoseRTV(const gtsam::Rot3& rot, const gtsam::Point3& pt, const gtsam::Point3& vel); + PoseRTV(const gtsam::Pose3& pose, const gtsam::Point3& vel); + PoseRTV(const gtsam::Pose3& pose); + PoseRTV(double roll, double pitch, double yaw, double x, double y, double z, double vx, double vy, double vz); + + // testable + bool equals(const PoseRTV& other, double tol) const; + void print(string s) const; + + // access + gtsam::Point3 translation() const; + gtsam::Rot3 rotation() const; + gtsam::Point3 velocity() const; + gtsam::Pose3 pose() const; + + // Vector interfaces + Vector vector() const; + Vector translationVec() const; + Vector velocityVec() const; + + // manifold/Lie + static size_t Dim(); + size_t dim() const; + PoseRTV retract(Vector v) const; + Vector localCoordinates(const PoseRTV& p) const; + static PoseRTV Expmap(Vector v); + static Vector Logmap(const PoseRTV& p); + PoseRTV inverse() const; + PoseRTV compose(const PoseRTV& p) const; + PoseRTV between(const PoseRTV& p) const; + + // measurement + double range(const PoseRTV& other) const; + PoseRTV transformed_from(const gtsam::Pose3& trans) const; + + // IMU/dynamics + PoseRTV planarDynamics(double vel_rate, double heading_rate, double max_accel, double dt) const; + PoseRTV flyingDynamics(double pitch_rate, double heading_rate, double lift_control, double dt) const; + PoseRTV generalDynamics(Vector accel, Vector gyro, double dt) const; + Vector imuPrediction(const PoseRTV& x2, double dt) const; + gtsam::Point3 translationIntegration(const PoseRTV& x2, double dt) const; + Vector translationIntegrationVec(const PoseRTV& x2, double dt) const; +}; + +#include +namespace imu { + +class Values { + Values(); + void print(string s) const; + + void insertPose(size_t key, const PoseRTV& pose); + PoseRTV pose(size_t key) const; +}; + +class Graph { + Graph(); + void print(string s) const; + + // prior factors + void addPrior(size_t key, const PoseRTV& pose, const gtsam::SharedNoiseModel& noiseModel); + void addConstraint(size_t key, const PoseRTV& pose); + void addHeightPrior(size_t key, double z, const gtsam::SharedNoiseModel& noiseModel); + + // inertial factors + void addFullIMUMeasurement(size_t key1, size_t key2, const Vector& accel, const Vector& gyro, double dt, const gtsam::SharedNoiseModel& noiseModel); + void addIMUMeasurement(size_t key1, size_t key2, const Vector& accel, const Vector& gyro, double dt, const gtsam::SharedNoiseModel& noiseModel); + void addVelocityConstraint(size_t key1, size_t key2, double dt, const gtsam::SharedNoiseModel& noiseModel); + + // other measurements + void addBetween(size_t key1, size_t key2, const PoseRTV& z, const gtsam::SharedNoiseModel& noiseModel); + void addRange(size_t key1, size_t key2, double z, const gtsam::SharedNoiseModel& noiseModel); + + // optimization + imu::Values optimize(const imu::Values& init) const; +}; + +}///\namespace imu