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

Added code from gtsam2 to "unstable" section

release/4.3a0
Alex Cunningham 2012-05-03 17:03:16 +00:00
parent 29ea1450eb
commit b0d71aa58c
51 changed files with 6311 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
gtsam/unstable/CMakeLists.txt Normal file
View File

@ -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)

409
gtsam/unstable/base/BTree.h Normal file
View File

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

17
gtsam/unstable/base/CMakeLists.txt Normal file
View File

@ -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}")

174
gtsam/unstable/base/DSF.h Normal file
View File

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

97
gtsam/unstable/base/DSFVector.cpp Normal file
View File

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

79
gtsam/unstable/base/DSFVector.h Normal file
View File

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

118
gtsam/unstable/base/FixedVector.h Normal file
View File

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

210
gtsam/unstable/base/tests/testBTree.cpp Normal file
View File

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

280
gtsam/unstable/base/tests/testDSF.cpp Normal file
View File

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

171
gtsam/unstable/base/tests/testDSFVector.cpp Normal file
View File

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

87
gtsam/unstable/base/tests/testFixedVector.cpp Normal file
View File

@ -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 <CppUnitLite/TestHarness.h>
#include <gtsam2/base/FixedVector.h>
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); }
/* ************************************************************************* */

17
gtsam/unstable/dynamics/CMakeLists.txt Normal file
View File

@ -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}")

105
gtsam/unstable/dynamics/DynamicsPriors.h Normal file
View File

@ -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 <gtsam/slam/PartialPriorFactor.h>
#include <gtsam2/dynamics/PoseRTV.h>
namespace gtsam {
/**
* Forces the value of the height in a PoseRTV to a specific value
* Dim: 1
*/
struct DHeightPrior : public gtsam::PartialPriorFactor<PoseRTV> {
typedef gtsam::PartialPriorFactor<PoseRTV> 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<PoseRTV> {
typedef gtsam::PartialPriorFactor<PoseRTV> 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<PoseRTV> {
typedef gtsam::PartialPriorFactor<PoseRTV> 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<PoseRTV> {
typedef gtsam::PartialPriorFactor<PoseRTV> 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

115
gtsam/unstable/dynamics/FullIMUFactor.h Normal file
View File

@ -0,0 +1,115 @@
/**
* @file FullIMUFactor.h
* @brief Factor to express an IMU measurement between dynamic poses
* @author Alex Cunningham
*/
#pragma once
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam2/dynamics/PoseRTV.h>
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 POSE>
class FullIMUFactor : public NoiseModelFactor2<POSE, POSE> {
public:
typedef NoiseModelFactor2<POSE, POSE> Base;
typedef FullIMUFactor<POSE> 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<const This*>(&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<Matrix&> H1 = boost::none,
boost::optional<Matrix&> 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<LieVector, PoseRTV, PoseRTV>(
boost::bind(This::predict_proxy, _1, _2, dt_), x1, x2, 1e-5);
if (H2) *H2 = numericalDerivative22<LieVector, PoseRTV, PoseRTV>(
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<Matrix&> H1 = boost::none,
boost::optional<Matrix&> 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

103
gtsam/unstable/dynamics/IMUFactor.h Normal file
View File

@ -0,0 +1,103 @@
/**
* @file IMUFactor.h
* @brief Factor to express an IMU measurement between dynamic poses
* @author Alex Cunningham
*/
#pragma once
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam2/dynamics/PoseRTV.h>
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 POSE>
class IMUFactor : public NoiseModelFactor2<POSE, POSE> {
public:
typedef NoiseModelFactor2<POSE, POSE> Base;
typedef IMUFactor<POSE> 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<const This*>(&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<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
const Vector meas = z();
if (H1) *H1 = numericalDerivative21<LieVector, PoseRTV, PoseRTV>(
boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5);
if (H2) *H2 = numericalDerivative22<LieVector, PoseRTV, PoseRTV>(
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<Matrix&> H1 = boost::none,
boost::optional<Matrix&> 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

279
gtsam/unstable/dynamics/PoseRTV.cpp Normal file
View File

@ -0,0 +1,279 @@
/**
* @file PoseRTV.cpp
* @author Alex Cunningham
*/
#include <gtsam/3rdparty/Eigen/Eigen/LU>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/Lie-inl.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam2/dynamics/inertialUtils.h>
#include <gtsam2/dynamics/PoseRTV.h>
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<Matrix&> H1,
boost::optional<Matrix&> 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<Matrix&> H1, boost::optional<Matrix&> 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<Matrix&> Dglobal,
boost::optional<Matrix&> 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

166
gtsam/unstable/dynamics/PoseRTV.h Normal file
View File

@ -0,0 +1,166 @@
/**
* @file PoseRTV.h
* @brief Pose3 with translational velocity
* @author Alex Cunningham
*/
#pragma once
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Pose3.h>
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<PoseRTV> {
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<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
// measurement functions
/** Derivatives calculated numerically */
double range(const PoseRTV& other,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<Matrix&> Dglobal=boost::none,
boost::optional<Matrix&> Dtrans=boost::none) const;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(Rt_);
ar & BOOST_SERIALIZATION_NVP(v_);
}
};
} // \namespace gtsam

116
gtsam/unstable/dynamics/VelocityConstraint.h Normal file
View File

@ -0,0 +1,116 @@
/**
* @file VelocityConstraint.h
* @brief Constraint enforcing the relationship between pose and velocity
* @author Alex Cunningham
*/
#pragma once
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam2/dynamics/PoseRTV.h>
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<PoseRTV,PoseRTV> {
public:
typedef gtsam::NoiseModelFactor2<PoseRTV,PoseRTV> 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<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none) const {
if (H1) *H1 = gtsam::numericalDerivative21<gtsam::LieVector,PoseRTV,PoseRTV>(
boost::bind(VelocityConstraint::evaluateError_, _1, _2, dt_, integration_mode_), x1, x2, 1e-5);
if (H2) *H2 = gtsam::numericalDerivative22<gtsam::LieVector,PoseRTV,PoseRTV>(
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

64
gtsam/unstable/dynamics/imuSystem.cpp Normal file
View File

@ -0,0 +1,64 @@
/**
* @file ilm3DSystem.cpp
* @brief Implementations of ilm 3D domain
* @author Alex Cunningham
*/
#include <gtsam2/dynamics/imuSystem.h>
#include <gtsam/nonlinear/NonlinearOptimization.h>
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<Graph>(*this, init);
}
/* ************************************************************************* */
} // \namespace imu

88
gtsam/unstable/dynamics/imuSystem.h Normal file
View File

@ -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 <boost/shared_ptr.hpp>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/slam/PartialPriorFactor.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam2/dynamics/PoseRTV.h>
#include <gtsam2/dynamics/IMUFactor.h>
#include <gtsam2/dynamics/FullIMUFactor.h>
#include <gtsam2/dynamics/VelocityConstraint.h>
#include <gtsam2/dynamics/DynamicsPriors.h>
/**
* 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<PoseRTV>(PoseKey(key)); }
};
// factors
typedef IMUFactor<PoseRTV> IMUMeasurement; // IMU between measurements
typedef FullIMUFactor<PoseRTV> FullIMUMeasurement; // Full-state IMU between measurements
typedef BetweenFactor<PoseRTV> Between; // full odometry (including velocity)
typedef NonlinearEquality<PoseRTV> Constraint;
typedef PriorFactor<PoseRTV> Prior;
typedef RangeFactor<PoseRTV, PoseRTV> 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

273
gtsam/unstable/dynamics/imu_examples.h Normal file
View File

@ -0,0 +1,273 @@
/**
* @file imu_examples.h
* @brief Example measurements from ACFR matlab simulation
* @author Alex Cunningham
*/
#pragma once
#include <gtsam/base/Vector.h>
#include <gtsam2/dynamics/PoseRTV.h>
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

51
gtsam/unstable/dynamics/inertialUtils.cpp Normal file
View File

@ -0,0 +1,51 @@
/**
* @file inertialUtils.cpp
*
* @date Nov 28, 2011
* @author Alex Cunningham
*/
#include <gtsam2/dynamics/inertialUtils.h>
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

32
gtsam/unstable/dynamics/inertialUtils.h Normal file
View File

@ -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 <gtsam/geometry/Rot3.h>
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

203
gtsam/unstable/dynamics/tests/testIMUSystem.cpp Normal file
View File

@ -0,0 +1,203 @@
/**
* @file testIMUSystem
* @author Alex Cunningham
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam2/dynamics/imuSystem.h>
#include <gtsam2/dynamics/imu_examples.h>
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<nrPoses; ++i) {
Key xA = PoseKey(i-1), xB = PoseKey(i);
cur_pose = cur_pose.generalDynamics(accel, gyro, dt);
graph.add(FullIMUMeasurement(accel - g, gyro, dt, xA, xB, model));
true_traj.insert(xB, cur_pose);
init_traj.insert(xB, PoseRTV());
}
// EXPECT_DOUBLES_EQUAL(0, graph.error(true_traj), 1e-5); // FAIL
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

34
gtsam/unstable/dynamics/tests/testInertialUtils.cpp Normal file
View File

@ -0,0 +1,34 @@
/**
* @file testInertialUtils.cpp
*
* @brief Conversions for the utility functions from the matlab implementation
*
* @date Nov 28, 2011
* @author Alex Cunningham
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam2/dynamics/inertialUtils.h>
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); }
/* ************************************************************************* */

390
gtsam/unstable/dynamics/tests/testPoseRTV.cpp Normal file
View File

@ -0,0 +1,390 @@
/**
* @file testPoseRTV
* @author Alex Cunningham
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam2/dynamics/PoseRTV.h>
#include <gtsam2/dynamics/imu_examples.h>
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); }
/* ************************************************************************* */

60
gtsam/unstable/dynamics/tests/testVelocityConstraint.cpp Normal file
View File

@ -0,0 +1,60 @@
/**
* @file testVelocityConstraint
* @author Alex Cunningham
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam2/dynamics/imuSystem.h>
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); }
/* ************************************************************************* */

17
gtsam/unstable/geometry/CMakeLists.txt Normal file
View File

@ -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}")

85
gtsam/unstable/geometry/Tensor1.h Normal file
View File

@ -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 <gtsam2/geometry/tensors.h>
namespace tensors {
/**
* A rank 1 tensor. Actually stores data.
* @ingroup tensors
* \nosubgrouping
*/
template<int N>
class Tensor1 {
double T[N]; ///< Storage
public:
/// @name Standard Constructors
/// @{
/** default constructor */
Tensor1() {
}
/** construct from data */
Tensor1(const double* data) {
for (int i = 0; i < N; i++)
T[i] = data[i];
}
/** construct from expression */
template<class A, char I>
Tensor1(const Tensor1Expression<A, Index<N, I> >& a) {
for (int i = 0; i < N; i++)
T[i] = a(i);
}
/// @}
/// @name Standard Interface
/// @{
/** return data */
inline int dim() const {
return N;
}
/** return data */
inline const double& operator()(int i) const {
return T[i];
}
/** return data */
inline double& operator()(int i) {
return T[i];
}
/// return an expression associated with an index
template<char I> Tensor1Expression<Tensor1, Index<N, I> > operator()(
Index<N, I> index) const {
return Tensor1Expression<Tensor1, Index<N, I> >(*this);
}
/// @}
};
// Tensor1
}// namespace tensors

181
gtsam/unstable/geometry/Tensor1Expression.h Normal file
View File

@ -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 <cmath>
#include <iostream>
#include <stdexcept>
#include <gtsam2/geometry/tensors.h>
namespace tensors {
/**
* Templated class to provide a rank 1 tensor interface to a class.
* This class does not store any data but the result of an expression.
* It is associated with an index.
* @ingroup tensors
* \nosubgrouping
*/
template<class A, class I> class Tensor1Expression {
private:
A iter;
typedef Tensor1Expression<A, I> This;
/** Helper class for multiplying with a double */
class TimesDouble_ {
A iter;
const double s;
public:
/// Constructor
TimesDouble_(const A &a, double s_) :
iter(a), s(s_) {
}
/// Element access
inline double operator()(int i) const {
return iter(i) * s;
}
};
public:
/// @name Standard Constructors
/// @{
/** constructor */
Tensor1Expression(const A &a) :
iter(a) {
}
/// @}
/// @name Testable
/// @{
/** Print */
void print(const std::string s = "") const {
std::cout << s << "{";
std::cout << (*this)(0);
for (int i = 1; i < I::dim; i++)
std::cout << ", "<< (*this)(i);
std::cout << "}" << std::endl;
}
/// equality
template<class B>
bool equals(const Tensor1Expression<B, I> & q, double tol) const {
for (int i = 0; i < I::dim; i++)
if (fabs((*this)(i) - q(i)) > tol) return false;
return true;
}
/// @}
/// @name Standard Interface
/// @{
/** norm */
double norm() const {
double sumsqr = 0.0;
for (int i = 0; i < I::dim; i++)
sumsqr += iter(i) * iter(i);
return sqrt(sumsqr);
}
/// test equivalence
template<class B>
bool equivalent(const Tensor1Expression<B, I> & q, double tol = 1e-9) const {
return ((*this) * (1.0 / norm())).equals(q * (1.0 / q.norm()), tol)
|| ((*this) * (-1.0 / norm())).equals(q * (1.0 / q.norm()), tol);
}
/** Check if two expressions are equal */
template<class B>
bool operator==(const Tensor1Expression<B, I>& e) const {
for (int i = 0; i < I::dim; i++)
if (iter(i) != e(i)) return false;
return true;
}
/** element access */
double operator()(int i) const {
return iter(i);
}
/** mutliply with a double. */
inline Tensor1Expression<TimesDouble_, I> operator*(double s) const {
return TimesDouble_(iter, s);
}
/** Class for contracting two rank 1 tensor expressions, yielding a double. */
template<class B>
inline double operator*(const Tensor1Expression<B, I> &b) const {
double sum = 0.0;
for (int i = 0; i < I::dim; i++)
sum += (*this)(i) * b(i);
return sum;
}
}; // Tensor1Expression
/// @}
/// @name Advanced Interface
/// @{
/** Print a rank 1 expression */
template<class A, class I>
void print(const Tensor1Expression<A, I>& T, const std::string s = "") {
T.print(s);
}
/** norm */
template<class A, class I>
double norm(const Tensor1Expression<A, I>& T) {
return T.norm();
}
/**
* This template works for any two expressions
*/
template<class A, class B, class I>
bool assert_equality(const Tensor1Expression<A, I>& expected,
const Tensor1Expression<B, I>& actual, double tol = 1e-9) {
if (actual.equals(expected, tol)) return true;
std::cout << "Not equal:\n";
expected.print("expected:\n");
actual.print("actual:\n");
return false;
}
/**
* This template works for any two expressions
*/
template<class A, class B, class I>
bool assert_equivalent(const Tensor1Expression<A, I>& expected,
const Tensor1Expression<B, I>& actual, double tol = 1e-9) {
if (actual.equivalent(expected, tol)) return true;
std::cout << "Not equal:\n";
expected.print("expected:\n");
actual.print("actual:\n");
return false;
}
/// @}
} // namespace tensors

84
gtsam/unstable/geometry/Tensor2.h Normal file
View File

@ -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 <gtsam2/geometry/tensors.h>
namespace tensors {
/**
* Rank 2 Tensor
* @ingroup tensors
* \nosubgrouping
*/
template<int N1, int N2>
class Tensor2 {
protected:
Tensor1<N1> T[N2]; ///< Storage
public:
/// @name Standard Constructors
/// @{
/** default constructor */
Tensor2() {
}
/// construct from data - expressed in row major form
Tensor2(const double data[N2][N1]) {
for (int j = 0; j < N2; j++)
T[j] = Tensor1<N1> (data[j]);
}
/** construct from expression */
template<class A, char I, char J>
Tensor2(const Tensor2Expression<A, Index<N1, I> , Index<N2, J> >& a) {
for (int j = 0; j < N2; j++)
T[j] = a(j);
}
/// @}
/// @name Standard Interface
/// @{
/** dimension - TODO: is this right for anything other than 3x3? */
size_t dim() const {return N1 * N2;}
/// const element access
const double & operator()(int i, int j) const {
return T[j](i);
}
/// element access
double & operator()(int i, int j) {
return T[j](i);
}
/** convert to expression */
template<char I, char J> Tensor2Expression<Tensor2, Index<N1, I> , Index<
N2, J> > operator()(Index<N1, I> i, Index<N2, J> j) const {
return Tensor2Expression<Tensor2, Index<N1, I> , Index<N2, J> > (*this);
}
/// @}
};
} // namespace tensors

310
gtsam/unstable/geometry/Tensor2Expression.h Normal file
View File

@ -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 <stdexcept>
#include <iostream>
#include <gtsam2/geometry/tensors.h>
namespace tensors {
/**
* Templated class to hold a rank 2 tensor expression.
* @ingroup tensors
* \nosubgrouping
*/
template<class A, class I, class J> class Tensor2Expression {
private:
A iter;
typedef Tensor2Expression<A, I, J> This;
/** Helper class for instantiating one index */
class FixJ_ {
const int j;
const A iter;
public:
FixJ_(int j_, const A &a) :
j(j_), iter(a) {
}
double operator()(int i) const {
return iter(i, j);
}
};
/** Helper class for swapping indices */
class Swap_ {
const A iter;
public:
/// Constructor
Swap_(const A &a) :
iter(a) {
}
/// Element access
double operator()(int j, int i) const {
return iter(i, j);
}
};
/** Helper class for multiplying with a double */
class TimesDouble_ {
A iter;
const double s;
public:
/// Constructor
TimesDouble_(const A &a, double s_) :
iter(a), s(s_) {
}
/// Element access
inline double operator()(int i, int j) const {
return iter(i, j) * s;
}
};
/** Helper class for contracting index I with rank 1 tensor */
template<class B> class ITimesRank1_ {
const This a;
const Tensor1Expression<B, I> b;
public:
/// Constructor
ITimesRank1_(const This &a_, const Tensor1Expression<B, I> &b_) :
a(a_), b(b_) {
}
/// Element access
double operator()(int j) const {
double sum = 0.0;
for (int i = 0; i < I::dim; i++)
sum += a(i, j) * b(i);
return sum;
}
};
/** Helper class for contracting index J with rank 1 tensor */
template<class B> class JTimesRank1_ {
const This a;
const Tensor1Expression<B, J> b;
public:
/// Constructor
JTimesRank1_(const This &a_, const Tensor1Expression<B, J> &b_) :
a(a_), b(b_) {
}
/// Element access
double operator()(int i) const {
double sum = 0.0;
for (int j = 0; j < J::dim; j++)
sum += a(i, j) * b(j);
return sum;
}
};
/** Helper class for contracting index I with rank 2 tensor */
template<class B, class K> class ITimesRank2_ {
const This a;
const Tensor2Expression<B, I, K> b;
public:
/// Constructor
ITimesRank2_(const This &a_, const Tensor2Expression<B, I, K> &b_) :
a(a_), b(b_) {
}
/// Element access
double operator()(int j, int k) const {
double sum = 0.0;
for (int i = 0; i < I::dim; i++)
sum += a(i, j) * b(i, k);
return sum;
}
};
public:
/// @name Standard Constructors
/// @{
/** constructor */
Tensor2Expression(const A &a) :
iter(a) {
}
/// @}
/// @name Testable
/// @{
/** Print */
void print(const std::string& s = "Tensor2:") const {
std::cout << s << "{";
(*this)(0).print();
for (int j = 1; j < J::dim; j++) {
std::cout << ",";
(*this)(j).print("");
}
std::cout << "}" << std::endl;
}
/// test equality
template<class B>
bool equals(const Tensor2Expression<B, I, J> & q, double tol) const {
for (int j = 0; j < J::dim; j++)
if (!(*this)(j).equals(q(j), tol))
return false;
return true;
}
/// @}
/// @name Standard Interface
/// @{
/** norm */
double norm() const {
double sumsqr = 0.0;
for (int i = 0; i < I::dim; i++)
for (int j = 0; j < J::dim; j++)
sumsqr += iter(i, j) * iter(i, j);
return sqrt(sumsqr);
}
/// test equivalence
template<class B>
bool equivalent(const Tensor2Expression<B, I, J> & q, double tol) const {
return ((*this) * (1.0 / norm())).equals(q * (1.0 / q.norm()), tol)
|| ((*this) * (-1.0 / norm())).equals(q * (1.0 / q.norm()), tol);
}
/** element access */
double operator()(int i, int j) const {
return iter(i, j);
}
/** swap indices */
typedef Tensor2Expression<Swap_, J, I> Swapped;
/// Return Swap_ helper class
Swapped swap() {
return Swap_(iter);
}
/** mutliply with a double. */
inline Tensor2Expression<TimesDouble_, I, J> operator*(double s) const {
return TimesDouble_(iter, s);
}
/** Fix a single index */
Tensor1Expression<FixJ_, I> operator()(int j) const {
return FixJ_(j, iter);
}
/** Check if two expressions are equal */
template<class B>
bool operator==(const Tensor2Expression<B, I, J>& T) const {
for (int i = 0; i < I::dim; i++)
for (int j = 0; j < J::dim; j++)
if (iter(i, j) != T(i, j))
return false;
return true;
}
/// @}
/// @name Advanced Interface
/// @{
/** c(j) = a(i,j)*b(i) */
template<class B>
inline Tensor1Expression<ITimesRank1_<B>, J> operator*(
const Tensor1Expression<B, I>& p) {
return ITimesRank1_<B>(*this, p);
}
/** c(i) = a(i,j)*b(j) */
template<class B>
inline Tensor1Expression<JTimesRank1_<B>, I> operator*(
const Tensor1Expression<B, J> &p) {
return JTimesRank1_<B>(*this, p);
}
/** c(j,k) = a(i,j)*T(i,k) */
template<class B, class K>
inline Tensor2Expression<ITimesRank2_<B, K> , J, K> operator*(
const Tensor2Expression<B, I, K>& p) {
return ITimesRank2_<B, K>(*this, p);
}
};
// Tensor2Expression
/** Print */
template<class A, class I, class J>
void print(const Tensor2Expression<A, I, J>& T, const std::string& s =
"Tensor2:") {
T.print(s);
}
/** Helper class for multiplying two covariant tensors */
template<class A, class I, class B, class J> class Rank1Rank1_ {
const Tensor1Expression<A, I> iterA;
const Tensor1Expression<B, J> iterB;
public:
/// Constructor
Rank1Rank1_(const Tensor1Expression<A, I> &a,
const Tensor1Expression<B, J> &b) :
iterA(a), iterB(b) {
}
/// element access
double operator()(int i, int j) const {
return iterA(i) * iterB(j);
}
};
/** Multiplying two different indices yields an outer product */
template<class A, class I, class B, class J>
inline Tensor2Expression<Rank1Rank1_<A, I, B, J> , I, J> operator*(
const Tensor1Expression<A, I> &a, const Tensor1Expression<B, J> &b) {
return Rank1Rank1_<A, I, B, J>(a, b);
}
/**
* This template works for any two expressions
*/
template<class A, class B, class I, class J>
bool assert_equality(const Tensor2Expression<A, I, J>& expected,
const Tensor2Expression<B, I, J>& actual, double tol = 1e-9) {
if (actual.equals(expected, tol))
return true;
std::cout << "Not equal:\n";
expected.print("expected:\n");
actual.print("actual:\n");
return false;
}
/**
* This template works for any two expressions
*/
template<class A, class B, class I, class J>
bool assert_equivalent(const Tensor2Expression<A, I, J>& expected,
const Tensor2Expression<B, I, J>& actual, double tol = 1e-9) {
if (actual.equivalent(expected, tol))
return true;
std::cout << "Not equal:\n";
expected.print("expected:\n");
actual.print("actual:\n");
return false;
}
/// @}
} // namespace tensors

106
gtsam/unstable/geometry/Tensor3.h Normal file
View File

@ -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 <gtsam2/geometry/tensors.h>
namespace tensors {
/**
* Rank 3 Tensor
* @ingroup tensors
* \nosubgrouping
*/
template<int N1, int N2, int N3>
class Tensor3 {
Tensor2<N1, N2> T[N3]; ///< Storage
public:
/// @name Standard Constructors
/// @{
/** default constructor */
Tensor3() {
}
/** construct from data */
Tensor3(const double data[N3][N2][N1]) {
for (int k = 0; k < N3; k++)
T[k] = data[k];
}
/// @}
/// @name Advanced Constructors
/// @{
/** construct from expression */
template<class A, char I, char J, char K>
Tensor3(const Tensor3Expression<A, Index<N1, I> , Index<N2, J> , Index<N3,
K> >& a) {
for (int k = 0; k < N3; k++)
T[k] = a(k);
}
/// @}
/// @name Standard Interface
/// @{
/// element access
double operator()(int i, int j, int k) const {
return T[k](i, j);
}
/** convert to expression */
template<char I, char J, char K> Tensor3Expression<Tensor3, Index<N1, I> ,
Index<N2, J> , Index<N3, K> > operator()(const Index<N1, I>& i,
const Index<N2, J>& j, const Index<N3, K>& k) {
return Tensor3Expression<Tensor3, Index<N1, I> , Index<N2, J> , Index<N3,
K> > (*this);
}
/** convert to expression */
template<char I, char J, char K> Tensor3Expression<const Tensor3, Index<N1, I> ,
Index<N2, J> , Index<N3, K> > operator()(const Index<N1, I>& i,
const Index<N2, J>& j, const Index<N3, K>& k) const {
return Tensor3Expression<const Tensor3, Index<N1, I> , Index<N2, J> , Index<N3,
K> > (*this);
}
}; // Tensor3
/** Rank 3 permutation tensor */
struct Eta3 {
/** calculate value. TODO: wasteful to actually use this */
double operator()(int i, int j, int k) const {
return ((j - i) * (k - i) * (k - j)) / 2;
}
/** create expression */
template<char I, char J, char K> Tensor3Expression<Eta3, Index<3, I> ,
Index<3, J> , Index<3, K> > operator()(const Index<3, I>& i,
const Index<3, J>& j, const Index<3, K>& k) const {
return Tensor3Expression<Eta3, Index<3, I> , Index<3, J> , Index<3, K> > (
*this);
}
}; // Eta
/// @}
} // namespace tensors

194
gtsam/unstable/geometry/Tensor3Expression.h Normal file
View File

@ -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 <iostream>
#include <gtsam2/geometry/tensors.h>
namespace tensors {
/**
* templated class to interface to an object A as a rank 3 tensor
* @ingroup tensors
* \nosubgrouping
*/
template<class A, class I, class J, class K> class Tensor3Expression {
A iter;
typedef Tensor3Expression<A, I, J, K> This;
/** Helper class for instantiating one index */
class FixK_ {
const int k;
const A iter;
public:
FixK_(int k_, const A &a) :
k(k_), iter(a) {
}
double operator()(int i, int j) const {
return iter(i, j, k);
}
};
/** Helper class for contracting rank3 and rank1 tensor */
template<class B> class TimesRank1_ {
typedef Tensor1Expression<B, I> Rank1;
const This T;
const Rank1 t;
public:
TimesRank1_(const This &a, const Rank1 &b) :
T(a), t(b) {
}
double operator()(int j, int k) const {
double sum = 0.0;
for (int i = 0; i < I::dim; i++)
sum += T(i, j, k) * t(i);
return sum;
}
};
public:
/// @name Standard Constructors
/// @{
/** constructor */
Tensor3Expression(const A &a) :
iter(a) {
}
/// @}
/// @name Standard Interface
/// @{
/** Print */
void print(const std::string& s = "Tensor3:") const {
std::cout << s << "{";
(*this)(0).print("");
for (int k = 1; k < K::dim; k++) {
std::cout << ",";
(*this)(k).print("");
}
std::cout << "}" << std::endl;
}
/// test equality
template<class B>
bool equals(const Tensor3Expression<B, I, J, K> & q, double tol) const {
for (int k = 0; k < K::dim; k++)
if (!(*this)(k).equals(q(k), tol)) return false;
return true;
}
/** element access */
double operator()(int i, int j, int k) const {
return iter(i, j, k);
}
/** Fix a single index */
Tensor2Expression<FixK_, I, J> operator()(int k) const {
return FixK_(k, iter);
}
/** Contracting with rank1 tensor */
template<class B>
inline Tensor2Expression<TimesRank1_<B> , J, K> operator*(
const Tensor1Expression<B, I> &b) const {
return TimesRank1_<B> (*this, b);
}
}; // Tensor3Expression
/// @}
/// @name Advanced Interface
/// @{
/** Print */
template<class A, class I, class J, class K>
void print(const Tensor3Expression<A, I, J, K>& T, const std::string& s =
"Tensor3:") {
T.print(s);
}
/** Helper class for outer product of rank2 and rank1 tensor */
template<class A, class I, class J, class B, class K>
class Rank2Rank1_ {
typedef Tensor2Expression<A, I, J> Rank2;
typedef Tensor1Expression<B, K> Rank1;
const Rank2 iterA;
const Rank1 iterB;
public:
/// Constructor
Rank2Rank1_(const Rank2 &a, const Rank1 &b) :
iterA(a), iterB(b) {
}
/// Element access
double operator()(int i, int j, int k) const {
return iterA(i, j) * iterB(k);
}
};
/** outer product of rank2 and rank1 tensor */
template<class A, class I, class J, class B, class K>
inline Tensor3Expression<Rank2Rank1_<A, I, J, B, K> , I, J, K> operator*(
const Tensor2Expression<A, I, J>& a, const Tensor1Expression<B, K> &b) {
return Rank2Rank1_<A, I, J, B, K> (a, b);
}
/** Helper class for outer product of rank1 and rank2 tensor */
template<class A, class I, class B, class J, class K>
class Rank1Rank2_ {
typedef Tensor1Expression<A, I> Rank1;
typedef Tensor2Expression<B, J, K> Rank2;
const Rank1 iterA;
const Rank2 iterB;
public:
/// Constructor
Rank1Rank2_(const Rank1 &a, const Rank2 &b) :
iterA(a), iterB(b) {
}
/// Element access
double operator()(int i, int j, int k) const {
return iterA(i) * iterB(j, k);
}
};
/** outer product of rank2 and rank1 tensor */
template<class A, class I, class J, class B, class K>
inline Tensor3Expression<Rank1Rank2_<A, I, B, J, K> , I, J, K> operator*(
const Tensor1Expression<A, I>& a, const Tensor2Expression<B, J, K> &b) {
return Rank1Rank2_<A, I, B, J, K> (a, b);
}
/**
* This template works for any two expressions
*/
template<class A, class B, class I, class J, class K>
bool assert_equality(const Tensor3Expression<A, I, J, K>& expected,
const Tensor3Expression<B, I, J, K>& actual, double tol = 1e-9) {
if (actual.equals(expected, tol)) return true;
std::cout << "Not equal:\n";
expected.print("expected:\n");
actual.print("actual:\n");
return false;
}
/// @}
} // namespace tensors

58
gtsam/unstable/geometry/Tensor4.h Normal file
View File

@ -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 <gtsam2/geometry/tensors.h>
namespace tensors {
/**
* Rank 4 Tensor
* @ingroup tensors
* \nosubgrouping
*/
template<int N1, int N2, int N3, int N4>
class Tensor4 {
private:
Tensor3<N1, N2, N3> T[N4]; ///< Storage
public:
/// @name Standard Constructors
/// @{
/** default constructor */
Tensor4() {
}
/// @}
/// @name Standard Interface
/// @{
/// element access
double operator()(int i, int j, int k, int l) const {
return T[l](i, j, k);
}
/// @}
}; // Tensor4
} // namespace tensors

75
gtsam/unstable/geometry/Tensor5.h Normal file
View File

@ -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 <gtsam2/geometry/tensors.h>
namespace tensors {
/**
* Rank 5 Tensor
* @ingroup tensors
* \nosubgrouping
*/
template<int N1, int N2, int N3, int N4, int N5>
class Tensor5 {
private:
Tensor4<N1, N2, N3, N4> T[N5]; ///< Storage
public:
/// @name Standard Constructors
/// @{
/** default constructor */
Tensor5() {
}
/// @}
/// @name Standard Interface
/// @{
/** construct from expression */
template<class A, char I, char J, char K, char L, char M>
Tensor5(const Tensor5Expression<A, Index<N1, I> , Index<N2, J> , Index<N3,
K> , Index<N4, L> , Index<N5, M> >& a) {
for (int m = 0; m < N5; m++)
T[m] = a(m);
}
/// element access
double operator()(int i, int j, int k, int l, int m) const {
return T[m](i, j, k, l);
}
/** convert to expression */
template<char I, char J, char K, char L, char M> Tensor5Expression<Tensor5,
Index<N1, I> , Index<N2, J> , Index<N3, K> , Index<N4, L> ,
Index<N5, M> > operator()(Index<N1, I> i, Index<N2, J> j,
Index<N3, K> k, Index<N4, L> l, Index<N5, M> m) {
return Tensor5Expression<Tensor5, Index<N1, I> , Index<N2, J> , Index<N3,
K> , Index<N4, L> , Index<N5, M> > (*this);
}
/// @}
}; // Tensor5
} // namespace tensors

135
gtsam/unstable/geometry/Tensor5Expression.h Normal file
View File

@ -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 <iostream>
#include <gtsam2/geometry/tensors.h>
namespace tensors {
/**
* templated class to interface to an object A as a rank 5 tensor
* @ingroup tensors
* \nosubgrouping
*/
template<class A, class I, class J, class K, class L, class M> class Tensor5Expression {
A iter;
typedef Tensor5Expression<A, I, J, K, L, M> This;
/** Helper class for swapping indices 3 and 4 :-) */
class Swap34_ {
const A iter;
public:
/// Constructor
Swap34_(const A &a) :
iter(a) {
}
/// swapping element access
double operator()(int i, int j, int k, int l, int m) const {
return iter(i, j, l, k, m);
}
};
public:
/// @name Standard Constructors
/// @{
/** constructor */
Tensor5Expression(const A &a) :
iter(a) {
}
/// @}
/// @name Standard Interface
/// @{
/** Print */
void print(const std::string& s = "Tensor5:") const {
std::cout << s << std::endl;
for (int m = 0; m < M::dim; m++)
for (int l = 0; l < L::dim; l++)
for (int k = 0; k < K::dim; k++) {
std::cout << "(m,l,k) = (" << m << "," << l << "," << k << ")"
<< std::endl;
for (int j = 0; j < J::dim; j++) {
for (int i = 0; i < I::dim; i++)
std::cout << " " << (*this)(i, j, k, l, m);
std::cout << std::endl;
}
}
std::cout << std::endl;
}
/** swap indices */
typedef Tensor5Expression<Swap34_, I, J, L, K, M> Swapped;
/// create Swap34_ helper class
Swapped swap34() {
return Swap34_(iter);
}
/** element access */
double operator()(int i, int j, int k, int l, int m) const {
return iter(i, j, k, l, m);
}
};
// Tensor5Expression
/// @}
/// @name Advanced Interface
/// @{
/** Print */
template<class A, class I, class J, class K, class L, class M>
void print(const Tensor5Expression<A, I, J, K, L, M>& T,
const std::string& s = "Tensor5:") {
T.print(s);
}
/** Helper class for outer product of rank3 and rank2 tensor */
template<class A, class I, class J, class K, class B, class L, class M>
class Rank3Rank2_ {
typedef Tensor3Expression<A, I, J, K> Rank3;
typedef Tensor2Expression<B, L, M> Rank2;
const Rank3 iterA;
const Rank2 iterB;
public:
/// Constructor
Rank3Rank2_(const Rank3 &a, const Rank2 &b) :
iterA(a), iterB(b) {
}
/// Element access
double operator()(int i, int j, int k, int l, int m) const {
return iterA(i, j, k) * iterB(l, m);
}
};
/** outer product of rank2 and rank1 tensor */
template<class A, class I, class J, class K, class B, class L, class M>
inline Tensor5Expression<Rank3Rank2_<A, I, J, K, B, L, M> , I, J, K, L, M> operator*(
const Tensor3Expression<A, I, J, K>& a,
const Tensor2Expression<B, L, M> &b) {
return Rank3Rank2_<A, I, J, K, B, L, M>(a, b);
}
/// @}
} // namespace tensors

71
gtsam/unstable/geometry/projectiveGeometry.cpp Normal file
View File

@ -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 <boost/foreach.hpp>
#include <gtsam/base/Matrix.h>
#include <gtsam2/geometry/tensorInterface.h>
#include <gtsam2/geometry/projectiveGeometry.h>
namespace gtsam {
using namespace std;
using namespace tensors;
/* ************************************************************************* */
Point2h point2h(double x, double y, double w) {
double data[3];
data[0] = x;
data[1] = y;
data[2] = w;
return data;
}
/* ************************************************************************* */
Line2h line2h(double a, double b, double c) {
double data[3];
data[0] = a;
data[1] = b;
data[2] = c;
return data;
}
/* ************************************************************************* */
Point3h point3h(double X, double Y, double Z, double W) {
double data[4];
data[0] = X;
data[1] = Y;
data[2] = Z;
data[3] = W;
return data;
}
/* ************************************************************************* */
Plane3h plane3h(double a, double b, double c, double d) {
double data[4];
data[0] = a;
data[1] = b;
data[2] = c;
data[3] = d;
return data;
}
/* ************************************************************************* */
} // namespace gtsam

124
gtsam/unstable/geometry/projectiveGeometry.h Normal file
View File

@ -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 <list>
#include <gtsam2/geometry/tensors.h>
namespace gtsam {
/**
* 2D Point in homogeneous coordinates
* @ingroup geometry
*/
typedef tensors::Tensor1<3> Point2h;
Point2h point2h(double x, double y, double w); ///< create Point2h
/**
* 2D Line in homogeneous coordinates
* @ingroup geometry
*/
typedef tensors::Tensor1<3> Line2h;
Line2h line2h(double a, double b, double c); ///< create Line2h
/**
* 2D (homegeneous) Point correspondence
* @ingroup geometry
*/
struct Correspondence {
Point2h first; ///< First point
Point2h second; ///< Second point
/// Create a correspondence pair
Correspondence(const Point2h &p1, const Point2h &p2) :
first(p1), second(p2) {
}
/// Swap points
Correspondence swap() const {
return Correspondence(second, first);
}
/// print
void print() {
tensors::Index<3, 'i'> i;
tensors::print(first(i), "first :");
tensors::print(second(i), "second:");
}
};
/**
* 2D-2D Homography
* @ingroup geometry
*/
typedef tensors::Tensor2<3, 3> Homography2;
/**
* Fundamental Matrix
* @ingroup geometry
*/
typedef tensors::Tensor2<3, 3> FundamentalMatrix;
/**
* Triplet of (homogeneous) 2D points
* @ingroup geometry
*/
struct Triplet {
Point2h first; ///< First point
Point2h second; ///< Second point
Point2h third; ///< Third point
/// Create a Triplet correspondence
Triplet(const Point2h &p1, const Point2h &p2, const Point2h &p3) :
first(p1), second(p2), third(p3) {
}
/// print
void print() {
tensors::Index<3, 'i'> i;
tensors::print(first(i), "first :");
tensors::print(second(i), "second:");
tensors::print(third(i), "third :");
}
};
/**
* Trifocal Tensor
* @ingroup geometry
*/
typedef tensors::Tensor3<3, 3, 3> TrifocalTensor;
/**
* 3D Point in homogeneous coordinates
* @ingroup geometry
*/
typedef tensors::Tensor1<4> Point3h;
Point3h point3h(double X, double Y, double Z, double W); ///< create Point3h
/**
* 3D Plane in homogeneous coordinates
* @ingroup geometry
*/
typedef tensors::Tensor1<4> Plane3h;
Plane3h plane3h(double a, double b, double c, double d); ///< create Plane3h
/**
* 3D to 2D projective camera
* @ingroup geometry
*/
typedef tensors::Tensor2<3, 4> ProjectiveCamera;
} // namespace gtsam

108
gtsam/unstable/geometry/tensorInterface.h Normal file
View File

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

45
gtsam/unstable/geometry/tensors.h Normal file
View File

@ -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<int Dim, char C> struct Index {
static const int dim = Dim; ///< dimension
};
} // namespace tensors
// Expression templates
#include <gtsam2/geometry/Tensor1Expression.h>
#include <gtsam2/geometry/Tensor2Expression.h>
#include <gtsam2/geometry/Tensor3Expression.h>
// Tensor4 not needed so far
#include <gtsam2/geometry/Tensor5Expression.h>
// Actual tensor classes
#include <gtsam2/geometry/Tensor1.h>
#include <gtsam2/geometry/Tensor2.h>
#include <gtsam2/geometry/Tensor3.h>
#include <gtsam2/geometry/Tensor4.h>
#include <gtsam2/geometry/Tensor5.h>

73
gtsam/unstable/geometry/tests/testFundamental.cpp Normal file
View File

@ -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 <iostream>
#include <boost/foreach.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include <gtsam2/geometry/tensors.h>
#include <gtsam2/geometry/tensorInterface.h>
#include <gtsam2/geometry/projectiveGeometry.h>
using namespace std;
using namespace gtsam;
using namespace tensors;
/* ************************************************************************* */
// Indices
Index<3, 'a'> a;
Index<3, 'b'> b;
Index<4, 'A'> A;
Index<4, 'B'> B;
/* ************************************************************************* */
TEST( Tensors, FundamentalMatrix)
{
double f[3][3] = { { 1, 0, 0 }, { 1, 2, 3 }, { 1, 2, 3 } };
FundamentalMatrix F(f);
Point2h p = point2h(1, 2, 3); // point p in view one
Point2h q = point2h(14, -1, 0); // point q in view two
// points p and q are in correspondence
CHECK(F(a,b)*p(a)*q(b) == 0)
// in detail, l1(b)*q(b)==0
Line2h l1 = line2h(1, 14, 14);
CHECK(F(a,b)*p(a) == l1(b))
CHECK(l1(b)*q(b) == 0); // q is on line l1
// and l2(a)*p(a)==0
Line2h l2 = line2h(13, -2, -3);
CHECK(F(a,b)*q(b) == l2(a))
CHECK(l2(a)*p(a) == 0); // p is on line l2
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

188
gtsam/unstable/geometry/tests/testHomography2.cpp Normal file
View File

@ -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 <iostream>
#include <boost/foreach.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam2/geometry/tensors.h>
#include <gtsam2/geometry/tensorInterface.h>
#include <gtsam2/geometry/projectiveGeometry.h>
#include <gtsam/geometry/Pose3.h>
using namespace std;
using namespace gtsam;
using namespace tensors;
/* ************************************************************************* */
// Indices
Index<3, 'a'> a, _a;
Index<3, 'b'> b, _b;
Index<3, 'c'> c, _c;
/* ************************************************************************* */
TEST( Homography2, RealImages)
{
// 4 point correspondences MATLAB from the floor of bt001.png and bt002.png
Correspondence p1(point2h(216.841, 443.220, 1), point2h(213.528, 414.671, 1));
Correspondence p2(point2h(252.119, 363.481, 1), point2h(244.614, 348.842, 1));
Correspondence p3(point2h(316.614, 414.768, 1), point2h(303.128, 390.000, 1));
Correspondence p4(point2h(324.165, 465.463, 1), point2h(308.614, 431.129, 1));
// Homography obtained using MATLAB code
double h[3][3] = { { 0.9075, 0.0031, -0 }, { -0.1165, 0.8288, -0.0004 }, {
30.8472, 16.0449, 1 } };
Homography2 H(h);
// CHECK whether they are equivalent
CHECK(assert_equivalent(p1.second(b),H(b,a)*p1.first(a),0.05))
CHECK(assert_equivalent(p2.second(b),H(b,a)*p2.first(a),0.05))
CHECK(assert_equivalent(p3.second(b),H(b,a)*p3.first(a),0.05))
CHECK(assert_equivalent(p4.second(b),H(b,a)*p4.first(a),0.05))
}
/* ************************************************************************* */
// Homography test case
// 4 trivial correspondences of a translating square
Correspondence p1(point2h(0, 0, 1), point2h(4, 5, 1));
Correspondence p2(point2h(1, 0, 1), point2h(5, 5, 1));
Correspondence p3(point2h(1, 1, 1), point2h(5, 6, 1));
Correspondence p4(point2h(0, 1, 1), point2h(4, 6, 1));
double h[3][3] = { { 1, 0, 4 }, { 0, 1, 5 }, { 0, 0, 1 } };
Homography2 H(h);
/* ************************************************************************* */
TEST( Homography2, TestCase)
{
// Check homography
list<Correspondence> correspondences;
correspondences += p1, p2, p3, p4;
BOOST_FOREACH(const Correspondence& p, correspondences)
CHECK(assert_equality(p.second(b),H(_a,b) * p.first(a)))
// Check a line
Line2h l1 = line2h(1, 0, -1); // in a
Line2h l2 = line2h(1, 0, -5); // x==5 in b
CHECK(assert_equality(l1(a), H(a,b)*l2(b)))
}
/* ************************************************************************* */
/**
* Computes the homography H(I,_T) from template to image
* given the pose tEc of the camera in the template coordinate frame.
* Assumption is Z is normal to the template, template texture in X-Y plane.
*/
Homography2 patchH(const Pose3& tEc) {
Pose3 cEt = tEc.inverse();
Rot3 cRt = cEt.rotation();
Point3 r1 = cRt.r1(), r2 = cRt.r2(), t = cEt.translation();
// TODO cleanup !!!!
// column 1
double H11 = r1.x();
double H21 = r1.y();
double H31 = r1.z();
// column 2
double H12 = r2.x();
double H22 = r2.y();
double H32 = r2.z();
// column 3
double H13 = t.x();
double H23 = t.y();
double H33 = t.z();
double data2[3][3] = { { H11, H21, H31 }, { H12, H22, H32 },
{ H13, H23, H33 } };
return Homography2(data2);
}
/* ************************************************************************* */
namespace gtsam {
// size_t dim(const tensors::Tensor2<3, 3>& H) {return 9;}
Vector toVector(const tensors::Tensor2<3, 3>& H) {
Index<3, 'T'> _T; // covariant 2D template
Index<3, 'C'> I; // contravariant 2D camera
return toVector(H(I,_T));
}
Vector localCoordinates(const tensors::Tensor2<3, 3>& A, const tensors::Tensor2<3, 3>& B) {
return toVector(A)-toVector(B); // TODO correct order ?
}
}
#include <gtsam/base/numericalDerivative.h>
/* ************************************************************************* */
TEST( Homography2, patchH)
{
Index<3, 'T'> _T; // covariant 2D template
Index<3, 'C'> I; // contravariant 2D camera
// data[_T][I]
double data1[3][3] = {{1,0,0},{0,-1,0},{0,0,10}};
Homography2 expected(data1);
// camera rotation, looking in negative Z
Rot3 gRc(Point3(1,0,0),Point3(0,-1,0),Point3(0,0,-1));
Point3 gTc(0,0,10); // Camera location, out on the Z axis
Pose3 gEc(gRc,gTc); // Camera pose
Homography2 actual = patchH(gEc);
// GTSAM_PRINT(expected(I,_T));
// GTSAM_PRINT(actual(I,_T));
CHECK(assert_equality(expected(I,_T),actual(I,_T)));
// FIXME: this doesn't appear to be tested, and requires that Tensor2 be a Lie object
// Matrix D = numericalDerivative11<Homography2,Pose3>(patchH, gEc);
// print(D,"D");
}
/* ************************************************************************* */
TEST( Homography2, patchH2)
{
Index<3, 'T'> _T; // covariant 2D template
Index<3, 'C'> I; // contravariant 2D camera
// data[_T][I]
double data1[3][3] = {{1,0,0},{0,-1,0},{0,0,10}};
Homography2 expected(data1);
// camera rotation, looking in negative Z
Rot3 gRc(Point3(1,0,0),Point3(0,-1,0),Point3(0,0,-1));
Point3 gTc(0,0,10); // Camera location, out on the Z axis
Pose3 gEc(gRc,gTc); // Camera pose
Homography2 actual = patchH(gEc);
// GTSAM_PRINT(expected(I,_T));
// GTSAM_PRINT(actual(I,_T));
CHECK(assert_equality(expected(I,_T),actual(I,_T)));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

240
gtsam/unstable/geometry/tests/testTensors.cpp Normal file
View File

@ -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 <iostream>
#include <boost/foreach.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include <gtsam2/geometry/tensors.h>
#include <gtsam2/geometry/tensorInterface.h>
#include <gtsam2/geometry/projectiveGeometry.h>
using namespace std;
using namespace gtsam;
using namespace tensors;
/* ************************************************************************* */
// Indices
Index<3, 'a'> a, _a;
Index<3, 'b'> b, _b;
Index<3, 'c'> c, _c;
Index<4, 'A'> A;
Index<4, 'B'> B;
/* ************************************************************************* */
// Tensor1
/* ************************************************************************* */
TEST(Tensor1, Basics)
{
// you can create 1-tensors corresponding to 2D homogeneous points
// using the function point2h in projectiveGeometry.*
Point2h p = point2h(1, 2, 3), q = point2h(2, 4, 6);
// equality tests always take tensor expressions, not tensors themselves
// the difference is that a tensor expression has indices
CHECK(p(a)==p(a))
CHECK(assert_equality(p(a),p(a)))
CHECK(assert_equality(p(a)*2,q(a)))
CHECK(assert_equivalent(p(a),q(a))) // projectively equivalent
// and you can take a norm, typically for normalization to the sphere
DOUBLES_EQUAL(sqrt(14),norm(p(a)),1e-9)
}
/* ************************************************************************* */
TEST( Tensor1, Incidence2D)
{
// 2D lines are created with line2h
Line2h l = line2h(-13, 5, 1);
Point2h p = point2h(1, 2, 3), q = point2h(2, 5, 1);
// Incidence between a line and a point is checked with simple contraction
// It does not matter which index you use, but it has to be of dimension 3
DOUBLES_EQUAL(l(a)*p(a),0,1e-9)
DOUBLES_EQUAL(l(b)*q(b),0,1e-9)
DOUBLES_EQUAL(p(a)*l(a),0,1e-9)
DOUBLES_EQUAL(q(a)*l(a),0,1e-9)
}
/* ************************************************************************* */
TEST( Tensor1, Incidence3D)
{
// similar constructs exist for 3D points and planes
Plane3h pi = plane3h(0, 1, 0, -2);
Point3h P = point3h(0, 2, 0, 1), Q = point3h(1, 2, 0, 1);
// Incidence is checked similarly
DOUBLES_EQUAL(pi(A)*P(A),0,1e-9)
DOUBLES_EQUAL(pi(A)*Q(A),0,1e-9)
DOUBLES_EQUAL(P(A)*pi(A),0,1e-9)
DOUBLES_EQUAL(Q(A)*pi(A),0,1e-9)
}
/* ************************************************************************* */
// Tensor2
/* ************************************************************************* */
TEST( Tensor2, Outer33)
{
Line2h l1 = line2h(1, 2, 3), l2 = line2h(1, 3, 5);
// We can also create tensors directly from data
double data[3][3] = { { 1, 2, 3 }, { 3, 6, 9 }, {5, 10, 15} };
Tensor2<3, 3> expected(data);
// in this case expected(0) == {1,2,3}
Line2h l0 = expected(a,b)(0);
CHECK(l0(a) == l1(a))
// And we create rank 2 tensors from the outer product of two rank 1 tensors
CHECK(expected(a,b) == l1(a) * l2(b))
// swap just swaps how you access a tensor, but note the data is the same
CHECK(assert_equality(expected(a,b).swap(), l2(b) * l1(a)));
}
/* ************************************************************************* */
TEST( Tensor2, AnotherOuter33)
{
// first cube point from testFundamental, projected in left and right
// Point2h p = point2h(0, -1, 2), q = point2h(-2, -1, 2);
// print(p(a)*q(b));
// print(p(b)*q(a));
// print(q(a)*p(b));
// print(q(b)*p(a));
}
/* ************************************************************************* */
TEST( Tensor2, Outer34)
{
Line2h l = line2h(1, 2, 3);
Plane3h pi = plane3h(1, 3, 5, 7);
double
data[4][3] = { { 1, 2, 3 }, { 3, 6, 9 }, { 5, 10, 15 }, { 7, 14, 21 } };
Tensor2<3, 4> expected(data);
CHECK(assert_equality(expected(a,B),l(a) * pi(B)))
CHECK(assert_equality(expected(a,B).swap(),pi(B) * l(a)))
}
/* ************************************************************************* */
TEST( Tensor2, SpecialContract)
{
double data[3][3] = { { 1, 2, 3 }, { 2, 4, 6 }, { 3, 6, 9 } };
Tensor2<3, 3> S(data), T(data);
//print(S(a, b) * T(a, c)); // contract a -> b,c
// S(a,0)*T(a,0) = [1 2 3] . [1 2 3] = 14
// S(a,0)*T(a,2) = [1 2 3] . [3 6 9] = 3+12+27 = 42
double data2[3][3] = { { 14, 28, 42 }, { 28, 56, 84 }, { 42, 84, 126 } };
Tensor2<3, 3> expected(data2);
CHECK(assert_equality(expected(b,c), S(a, b) * T(a, c)));
}
/* ************************************************************************* */
TEST( Tensor2, ProjectiveCamera)
{
Point2h p = point2h(1 + 2, 2, 5);
Point3h P = point3h(1, 2, 5, 1);
double data[4][3] = { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }, { 2, 0, 0 } };
ProjectiveCamera M(data);
CHECK(assert_equality(p(a),M(a,A)*P(A)))
}
/* ************************************************************************* */
namespace camera {
// to specify the tensor M(a,A), we need to give four 2D points
double data[4][3] = { { 1, 2, 3 }, { 4, 5, 6 }, { 7, 8, 9 }, { 10, 11, 12 } };
ProjectiveCamera M(data);
Matrix matrix = Matrix_(4,3,1.,2.,3.,4.,5.,6.,7.,8.,9.,10.,11.,12.);
Vector vector = Vector_( 12,1.,2.,3.,4.,5.,6.,7.,8.,9.,10.,11.,12.);
}
/* ************************************************************************* */
TEST( Tensor2, reshape )
{
// it is annoying that a camera can only be reshaped to a 4*3
// print(camera::M(a,A));
Matrix actual = reshape(camera::M(a,A),4,3);
EQUALITY(camera::matrix,actual);
}
/* ************************************************************************* */
TEST( Tensor2, toVector )
{
// Vectors are created with the leftmost indices iterating the fastest
Vector actual = toVector(camera::M(a,A));
CHECK(assert_equal(camera::vector,actual));
}
/* ************************************************************************* */
TEST( Tensor2, reshape2 )
{
Tensor2<3,4> actual = reshape2<3,4>(camera::vector);
CHECK(assert_equality(camera::M(a,A),actual(a,A)));
}
/* ************************************************************************* */
TEST( Tensor2, reshape_33_to_9 )
{
double data[3][3] = { { 1, 2, 3 }, { 4, 5, 6 }, { 7, 8, 9 } };
FundamentalMatrix F(data);
Matrix matrix = Matrix_(1,9,1.,2.,3.,4.,5.,6.,7.,8.,9.);
Matrix actual = reshape(F(a,b),1,9);
EQUALITY(matrix,actual);
Vector v = Vector_( 9,1.,2.,3.,4.,5.,6.,7.,8.,9.);
CHECK(assert_equality(F(a,b),reshape2<3, 3> (v)(a,b)));
}
/* ************************************************************************* */
// Tensor3
/* ************************************************************************* */
TEST( Tensor3, Join)
{
Line2h l = line2h(-13, 5, 1);
Point2h p = point2h(1, 2, 3), q = point2h(2, 5, 1);
// join points into line
Eta3 e;
CHECK(assert_equality(e(a, b, c) * p(a) * q(b), l(c)))
}
/* ************************************************************************* */
TEST( Tensor5, Outer32)
{
double t[3][3][3] = { { { 0, 0, 3 }, { 0, 8, -125 }, { -3, 125, 1 } }, { { 0,
0, 3 }, { 0, 8, -125 }, { -3, 125, 1 } }, { { 0, 0, 3 }, { 0, 8, -125 },
{ -3, 125, 1 } } };
TrifocalTensor T(t);
double data[3][3] = { { 0, 0, 3 }, { 0, 8, -125 }, { -3, 125, 1 } };
FundamentalMatrix F(data);
//Index<3, 'd'> d, _d;
//Index<3, 'e'> e, _e;
//print(T(_a,b,c)*F(_d,_e));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

20
gtsam/unstable/mainpage.dox Normal file
View File

@ -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.
*/
}

17
gtsam/unstable/slam/CMakeLists.txt Normal file
View File

@ -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}")

43
gtsam/unstable/slam/simulated3D.cpp Normal file
View File

@ -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 <gtsam2/slam/simulated3D.h>
namespace gtsam {
namespace simulated3D {
Point3 prior (const Point3& x, boost::optional<Matrix&> H) {
if (H) *H = eye(3);
return x;
}
Point3 odo(const Point3& x1, const Point3& x2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
if (H1) *H1 = -1 * eye(3);
if (H2) *H2 = eye(3);
return x2 - x1;
}
Point3 mea(const Point3& x, const Point3& l,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
if (H1) *H1 = -1 * eye(3);
if (H2) *H2 = eye(3);
return l - x;
}
}} // namespace gtsam::simulated3D

126
gtsam/unstable/slam/simulated3D.h Normal file
View File

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

63
gtsam/unstable/slam/tests/testSimulated3D.cpp Normal file
View File

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

52
gtsam/unstable/wrap/CMakeLists.txt Normal file
View File

@ -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)

96
gtsam/unstable/wrap/gtsam2.h Normal file
View File

@ -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 <gtsam2/dynamics/PoseRTV.h>
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 <gtsam2/dynamics/imuSystem.h>
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