Added code from gtsam2 to "unstable" section

gtsam/unstable/CMakeLists.txt

@@ -0,0 +1,62 @@
+# Build full gtsam2 library as a single library
+# and also build tests 
+set (gtsam2_subdirs
+    base 
+    dynamics
+    geometry
+    slam 
+)
+
+# assemble core libaries
+foreach(subdir ${gtsam2_subdirs})
+    # Build convenience libraries
+    file(GLOB subdir_srcs "${subdir}/*.cpp")
+    set(${subdir}_srcs ${subdir_srcs})
+    if (GTSAM2_BUILD_CONVENIENCE_LIBRARIES)
+        message(STATUS "Building Convenience Library: ${subdir}")
+        add_library(${subdir} STATIC ${subdir_srcs})
+    endif()
+
+    # Build local library and tests
+    message(STATUS "Building ${subdir}") 
+    add_subdirectory(${subdir})
+endforeach(subdir)
+ 
+# assemble gtsam2 components
+set(gtsam2_srcs
+    ${base_srcs} 
+    ${discrete_srcs}
+    ${dynamics_srcs}
+    ${geometry_srcs}
+    ${slam_srcs} 
+)
+
+option (GTSAM2_BUILD_SHARED_LIBRARY "Enable/Disable building of a shared version of gtsam2" ON)
+ 
+# Versions
+set(gtsam2_version   ${GTSAM2_VERSION_MAJOR}.${GTSAM2_VERSION_MINOR}.${GTSAM2_VERSION_PATCH})
+set(gtsam2_soversion ${GTSAM2_VERSION_MAJOR})
+message(STATUS "GTSAM2 Version: ${gtsam2_version}")
+message(STATUS "Install prefix: ${CMAKE_INSTALL_PREFIX}")
+ 
+# build shared and static versions of the library
+message(STATUS "Building GTSAM2 - static")
+add_library(gtsam2-static STATIC ${gtsam2_srcs})
+set_target_properties(gtsam2-static PROPERTIES 
+    OUTPUT_NAME         gtsam2
+    CLEAN_DIRECT_OUTPUT 1
+	VERSION             ${gtsam2_version}
+ 	SOVERSION           ${gtsam2_soversion})
+install(TARGETS gtsam2-static ARCHIVE DESTINATION lib)
+
+if (GTSAM2_BUILD_SHARED_LIBRARY)
+    message(STATUS "Building GTSAM2 - shared")
+    add_library(gtsam2-shared SHARED ${gtsam2_srcs})
+    set_target_properties(gtsam2-shared PROPERTIES 
+        OUTPUT_NAME         gtsam2
+        CLEAN_DIRECT_OUTPUT 1
+    	VERSION             ${gtsam2_version}
+     	SOVERSION           ${gtsam2_soversion})
+    install(TARGETS gtsam2-shared LIBRARY DESTINATION lib )
+endif(GTSAM2_BUILD_SHARED_LIBRARY)
+

gtsam/unstable/base/BTree.h

@@ -0,0 +1,409 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file BTree.h
+ * @brief purely functional binary tree
+ * @author Chris Beall
+ * @author Frank Dellaert
+ * @date Feb 3, 2010
+ */
+
+#include <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
+

gtsam/unstable/base/CMakeLists.txt

@@ -0,0 +1,17 @@
+# Install headers
+file(GLOB base_headers "*.h")
+install(FILES ${base_headers} DESTINATION include/gtsam2/base)
+
+# Components to link tests in this subfolder against
+set(base_local_libs 
+    base)
+
+set (base_full_libs
+    gtsam2-static)
+
+# Exclude tests that don't work
+set (base_excluded_tests "")
+
+# Add all tests
+gtsam_add_subdir_tests(base "${base_local_libs}" "${base_full_libs}" "${base_excluded_tests}") 
+

gtsam/unstable/base/DSF.h

@@ -0,0 +1,174 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file DSF.h
+ * @date Mar 26, 2010
+ * @author Kai Ni
+ * @brief An implementation of Disjoint set forests (see CLR page 446 and up)
+ */
+
+#pragma once
+
+#include <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

gtsam/unstable/base/DSFVector.cpp

@@ -0,0 +1,97 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file DSFVector.cpp
+ * @date Jun 25, 2010
+ * @author Kai Ni
+ * @brief a faster implementation for DSF, which uses vector rather than btree.
+ */
+
+#include <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
+

gtsam/unstable/base/DSFVector.h

@@ -0,0 +1,79 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file DSFVector.h
+ * @date Jun 25, 2010
+ * @author Kai Ni
+ * @brief A faster implementation for DSF, which uses vector rather than btree. As a result, the size of the forest is prefixed.
+ */
+
+#pragma once
+
+#include <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);
+
+	};
+
+}

gtsam/unstable/base/FixedVector.h

@@ -0,0 +1,118 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file FixedVector.h
+ * @brief Extension of boost's bounded_vector to allow for fixed size operation
+ * @author Alex Cunningham
+ */
+
+#pragma once
+
+#include <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

gtsam/unstable/base/tests/testBTree.cpp

@@ -0,0 +1,210 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file testBTree.cpp
+ * @date Feb 3, 2010
+ * @author Chris Beall
+ * @author Frank Dellaert
+ */
+
+#include <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);
+}
+/* ************************************************************************* */

gtsam/unstable/base/tests/testDSF.cpp

@@ -0,0 +1,280 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file testDSF.cpp
+ * @date Mar 26, 2010
+ * @author nikai
+ * @brief unit tests for DSF
+ */
+
+#include <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);}
+/* ************************************************************************* */
+

gtsam/unstable/base/tests/testDSFVector.cpp

@@ -0,0 +1,171 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file testDSF.cpp
+ * @date June 25, 2010
+ * @author nikai
+ * @brief unit tests for DSF
+ */
+
+#include <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);}
+/* ************************************************************************* */
+

gtsam/unstable/base/tests/testFixedVector.cpp

@@ -0,0 +1,87 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file testFixedVector.cpp
+ * @author Alex Cunningham
+ */
+
+#include <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); }
+/* ************************************************************************* */
+
+

gtsam/unstable/dynamics/CMakeLists.txt

@@ -0,0 +1,17 @@
+# Install headers
+file(GLOB dynamics_headers "*.h")
+install(FILES ${dynamics_headers} DESTINATION include/gtsam2/dynamics)
+
+# Components to link tests in this subfolder against
+set(dynamics_local_libs 
+    dynamics)
+
+set (dynamics_full_libs
+    gtsam2-static)
+
+# Exclude tests that don't work
+set (dynamics_excluded_tests "")
+
+# Add all tests
+gtsam_add_subdir_tests(dynamics "${dynamics_local_libs}" "${dynamics_full_libs}" "${dynamics_excluded_tests}") 
+

gtsam/unstable/dynamics/DynamicsPriors.h

@@ -0,0 +1,105 @@
+/**
+ * @file DynamicsPriors.h
+ *
+ * @brief Priors to be used with dynamic systems (Specifically PoseRTV)
+ *
+ * @date Nov 22, 2011
+ * @author Alex Cunningham
+ */
+
+#pragma once
+
+#include <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

gtsam/unstable/dynamics/FullIMUFactor.h

@@ -0,0 +1,115 @@
+/**
+ * @file FullIMUFactor.h
+ * @brief Factor to express an IMU measurement between dynamic poses
+ * @author Alex Cunningham
+ */
+
+#pragma once
+
+#include <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

gtsam/unstable/dynamics/IMUFactor.h

@@ -0,0 +1,103 @@
+/**
+ * @file IMUFactor.h
+ * @brief Factor to express an IMU measurement between dynamic poses
+ * @author Alex Cunningham
+ */
+
+#pragma once
+
+#include <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

gtsam/unstable/dynamics/PoseRTV.cpp

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

gtsam/unstable/dynamics/PoseRTV.h

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

gtsam/unstable/dynamics/VelocityConstraint.h

@@ -0,0 +1,116 @@
+/**
+ * @file VelocityConstraint.h
+ * @brief Constraint enforcing the relationship between pose and velocity
+ * @author Alex Cunningham
+ */
+
+#pragma once
+
+#include <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

gtsam/unstable/dynamics/imuSystem.cpp

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

gtsam/unstable/dynamics/imuSystem.h

@@ -0,0 +1,88 @@
+/**
+ * @file imuSystem.h
+ * @brief A 3D Dynamic system domain as a demonstration of IMU factors
+ * @author Alex Cunningham
+ */
+
+#pragma once
+
+#include <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
+

gtsam/unstable/dynamics/imu_examples.h

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

gtsam/unstable/dynamics/inertialUtils.cpp

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

gtsam/unstable/dynamics/inertialUtils.h

@@ -0,0 +1,32 @@
+/**
+ * @file inertialUtils.h
+ *
+ * @brief Utility functions for working with dynamic systems - derived from Mitch's matlab code
+ * This is mostly just syntactic sugar for working with dynamic systems that use
+ * Euler angles to specify the orientation of a robot.
+ *
+ * @date Nov 28, 2011
+ * @author Alex Cunningham
+ */
+
+#pragma once
+
+#include <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

gtsam/unstable/dynamics/tests/testIMUSystem.cpp

@@ -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); }
+/* ************************************************************************* */

gtsam/unstable/dynamics/tests/testInertialUtils.cpp

@@ -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); }
+/* ************************************************************************* */

gtsam/unstable/dynamics/tests/testPoseRTV.cpp

@@ -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); }
+/* ************************************************************************* */
+

gtsam/unstable/dynamics/tests/testVelocityConstraint.cpp

@@ -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); }
+/* ************************************************************************* */

gtsam/unstable/geometry/CMakeLists.txt

@@ -0,0 +1,17 @@
+# Install headers
+file(GLOB geometry_headers "*.h")
+install(FILES ${geometry_headers} DESTINATION include/gtsam2/geometry)
+
+# Components to link tests in this subfolder against
+set(geometry_local_libs 
+    geometry)
+
+set (geometry_full_libs
+    gtsam2-static)
+
+# Exclude tests that don't work
+set (geometry_excluded_tests "")
+
+# Add all tests
+gtsam_add_subdir_tests(geometry "${geometry_local_libs}" "${geometry_full_libs}" "${geometry_excluded_tests}") 
+

gtsam/unstable/geometry/Tensor1.h

@@ -0,0 +1,85 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file Tensor1.h
+ * @brief Rank 1 tensors based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf
+ * @date Feb 10, 2010
+ * @author Frank Dellaert
+ */
+
+#pragma once
+#include <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

gtsam/unstable/geometry/Tensor1Expression.h

@@ -0,0 +1,181 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file Tensor1Expression.h
+ * @brief Tensor expression templates based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf
+ * @date Feb 10, 2010
+ * @author Frank Dellaert
+ */
+
+#pragma once
+
+#include <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

gtsam/unstable/geometry/Tensor2.h

@@ -0,0 +1,84 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file Tensor2.h
+ * @brief Rank 2 Tensor based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf
+ * @date Feb 10, 2010
+ * @author Frank Dellaert
+ */
+
+#pragma once
+#include <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
+

gtsam/unstable/geometry/Tensor2Expression.h

@@ -0,0 +1,310 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file Tensor2Expression.h
+ * @brief Tensor expression templates based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf
+ * @date Feb 10, 2010
+ * @author Frank Dellaert
+ */
+
+#pragma once
+
+#include <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

gtsam/unstable/geometry/Tensor3.h

@@ -0,0 +1,106 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file Tensor3.h
+ * @brief Rank 3 tensors based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf
+ * @date Feb 10, 2010
+ * @author: Frank Dellaert
+ */
+
+#pragma once
+#include <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

gtsam/unstable/geometry/Tensor3Expression.h

@@ -0,0 +1,194 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file Tensor3Expression.h
+ * @brief Tensor expression templates based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf
+ * @date Feb 10, 2010
+ * @author Frank Dellaert
+ */
+
+#pragma once
+
+#include <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

gtsam/unstable/geometry/Tensor4.h

@@ -0,0 +1,58 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file Tensor4.h
+ * @brief Rank 4 tensors based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf
+ * @date Feb 12, 2010
+ * @author Frank Dellaert
+ */
+
+#pragma once
+#include <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

gtsam/unstable/geometry/Tensor5.h

@@ -0,0 +1,75 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file Tensor5.h
+ * @brief Rank 5 tensors based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf
+ * @date Feb 12, 2010
+ * @author Frank Dellaert
+ */
+
+#pragma once
+#include <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

gtsam/unstable/geometry/Tensor5Expression.h

@@ -0,0 +1,135 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file Tensor5Expression.h
+ * @brief Tensor expression templates based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf
+ * @date Feb 10, 2010
+ * @author Frank Dellaert
+ */
+
+#pragma once
+
+#include <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

gtsam/unstable/geometry/projectiveGeometry.cpp

@@ -0,0 +1,71 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file projectiveGeometry.cpp
+ * @brief Projective geometry, implemented using tensor library
+ * @date Feb 12, 2010
+ * @author: Frank Dellaert
+ */
+
+#include <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

gtsam/unstable/geometry/projectiveGeometry.h

@@ -0,0 +1,124 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file projectiveGeometry.h
+ * @brief Projective geometry, implemented using tensor library
+ * @date Feb 12, 2010
+ * @author Frank Dellaert
+ */
+
+#pragma once
+
+#include <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

gtsam/unstable/geometry/tensorInterface.h

@@ -0,0 +1,108 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file tensorInterface.h
+ * @brief Interfacing tensors template library and gtsam
+ * @date Feb 12, 2010
+ * @author Frank Dellaert
+ */
+
+#pragma once
+
+#include <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

gtsam/unstable/geometry/tensors.h

@@ -0,0 +1,45 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file tensors.h
+ * @brief Tensor expression templates based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf
+ * @date Feb 10, 2010
+ * @author Frank Dellaert
+ * @defgroup tensors
+ */
+
+#pragma once
+
+namespace tensors {
+
+	/** index */
+	template<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>
+
+

gtsam/unstable/geometry/tests/testFundamental.cpp

@@ -0,0 +1,73 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file testFundamental.cpp
+ * @brief try tensor expressions based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf
+ * @date Feb 13, 2010
+ * @author: Frank Dellaert
+ */
+
+#include <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);
+}
+/* ************************************************************************* */
+

gtsam/unstable/geometry/tests/testHomography2.cpp

@@ -0,0 +1,188 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file testHomography2.cpp
+ * @brief Test and estimate 2D homographies
+ * @date Feb 13, 2010
+ * @author Frank Dellaert
+ */
+
+#include <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);
+}
+/* ************************************************************************* */
+

gtsam/unstable/geometry/tests/testTensors.cpp

@@ -0,0 +1,240 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file testTensors.cpp
+ * @brief try tensor expressions based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf
+ * @date Feb 9, 2010
+ * @author Frank Dellaert
+ */
+
+#include <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);
+}
+/* ************************************************************************* */
+

gtsam/unstable/mainpage.dox

@@ -0,0 +1,20 @@
+// This causes Doxygen to find classes inside the gtsam namespace without
+// explicitly specifying it when writing class names.
+namespace gtsam {
+
+/**
+
+\mainpage GTSAM2
+
+\section  gtsam2_overview GTSAM2 Overview
+
+GTSAM2 is a set of experimental additions to GTSAM that are not fully ready to be 
+integrated into the rest of the library.  This library is actively maintained,
+and should follow the same design procedures as GTSAM.
+
+Because parts of this library are supposed to be a part of GTSAM, all code should
+be in the gtsam namespace, rather than a gtsam2 namespace.    
+
+*/
+
+}

gtsam/unstable/slam/CMakeLists.txt

@@ -0,0 +1,17 @@
+# Install headers
+file(GLOB slam_headers "*.h")
+install(FILES ${slam_headers} DESTINATION include/gtsam2/slam)
+
+# Components to link tests in this subfolder against
+set(slam_local_libs 
+    slam)
+
+set (slam_full_libs
+    gtsam2-static)
+
+# Exclude tests that don't work
+set (slam_excluded_tests "")
+
+# Add all tests
+gtsam_add_subdir_tests(slam "${slam_local_libs}" "${slam_full_libs}" "${slam_excluded_tests}") 
+

gtsam/unstable/slam/simulated3D.cpp

@@ -0,0 +1,43 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+* @file   Simulated3D.cpp
+* @brief  measurement functions and derivatives for simulated 3D robot
+* @author Alex Cunningham
+**/
+
+#include <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

gtsam/unstable/slam/simulated3D.h

@@ -0,0 +1,126 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file   Simulated3D.h
+ * @brief  measurement functions and derivatives for simulated 3D robot
+ * @author Alex Cunningham
+ **/
+
+// \callgraph
+
+#pragma once
+
+#include <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

gtsam/unstable/slam/tests/testSimulated3D.cpp

@@ -0,0 +1,63 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file   testSimulated3D.cpp
+ * @brief  Unit tests for simulated 3D measurement functions
+ * @author Alex Cunningham
+ **/
+
+#include <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);}
+/* ************************************************************************* */

gtsam/unstable/wrap/CMakeLists.txt

@@ -0,0 +1,52 @@
+# Find wrap
+find_package(Wrap REQUIRED)
+
+# Set up codegen
+include(GtsamMatlabWrap)
+
+# Wrap codegen
+#usage: wrap mexExtension interfacePath moduleName toolboxPath
+#  mexExtension  : OS/CPU-dependent extension for MEX binaries
+#  interfacePath : *absolute* path to directory of module interface file
+#  moduleName    : the name of the module, interface file must be called moduleName.h
+#  toolboxPath   : the directory in which to generate the wrappers
+#  [mexFlags]    : extra flags for the mex command
+
+# TODO: generate these includes programmatically
+set(mexFlags "-I${Boost_INCLUDE_DIR} -I${Wrap_INCLUDE_DIR} -I${CMAKE_INSTALL_PREFIX}/include/gtsam2 -I${CMAKE_INSTALL_PREFIX}/include/gtsam2/dynamics -I${CMAKE_INSTALL_PREFIX}/include/gtsam2/discrete -L${CMAKE_INSTALL_PREFIX}/lib -lgtsam -lgtsam2")
+set(toolbox_path ${CMAKE_BINARY_DIR}/wrap/gtsam2)
+set(moduleName gtsam2)
+
+find_mexextension()
+
+# Code generation command
+add_custom_target(wrap_gtsam2 ALL COMMAND 
+    ${Wrap_CMD} ${GTSAM_MEX_BIN_EXTENSION} ${CMAKE_CURRENT_SOURCE_DIR}/../ ${moduleName} ${toolbox_path} "${mexFlags}")
+
+option(GTSAM2_INSTALL_MATLAB_TOOLBOX  "Enable/Disable installation of matlab toolbox"  ON)
+option(GTSAM2_INSTALL_MATLAB_EXAMPLES "Enable/Disable installation of matlab examples" ON)
+option(GTSAM2_INSTALL_MATLAB_TESTS    "Enable/Disable installation of matlab tests"    ON)
+
+set(GTSAM2_TOOLBOX_INSTALL_PATH ${CMAKE_INSTALL_PREFIX}/borg/toolbox CACHE DOCSTRING "Path to install matlab toolbox")
+
+if (GTSAM2_INSTALL_MATLAB_TOOLBOX)
+    # Primary toolbox files
+    message(STATUS "Installing Matlab Toolbox to ${GTSAM2_TOOLBOX_INSTALL_PATH}")
+    install(DIRECTORY DESTINATION ${GTSAM2_TOOLBOX_INSTALL_PATH}) # make an empty folder
+    # exploit need for trailing slash to specify a full folder, rather than just its contents to copy
+    install(DIRECTORY ${toolbox_path} DESTINATION ${GTSAM2_TOOLBOX_INSTALL_PATH})
+    
+    # Examples
+    if (GTSAM2_INSTALL_MATLAB_EXAMPLES)
+        message(STATUS "Installing Matlab Toolbox Examples")
+        file(GLOB matlab_examples "${CMAKE_SOURCE_DIR}/examples/matlab/*.m")
+        install(FILES ${matlab_examples} DESTINATION ${GTSAM2_TOOLBOX_INSTALL_PATH}/gtsam2/examples)
+    endif (GTSAM2_INSTALL_MATLAB_EXAMPLES)
+    
+    # Tests
+    if (GTSAM2_INSTALL_MATLAB_TESTS)
+        message(STATUS "Installing Matlab Toolbox Tests")
+        file(GLOB matlab_tests "${CMAKE_SOURCE_DIR}/tests/matlab/*.m")
+        install(FILES ${matlab_tests} DESTINATION ${GTSAM2_TOOLBOX_INSTALL_PATH}/gtsam2/tests)
+    endif (GTSAM2_INSTALL_MATLAB_TESTS)
+endif (GTSAM2_INSTALL_MATLAB_TOOLBOX)

gtsam/unstable/wrap/gtsam2.h

@@ -0,0 +1,96 @@
+/**
+ * Matlab toolbox interface definition for GTSAM2
+ */
+
+// Most things are in the gtsam namespace
+using namespace gtsam;
+
+// specify the classes from gtsam we are using
+class gtsam::Point3;
+class gtsam::Rot3;
+class gtsam::Pose3;
+class gtsam::SharedNoiseModel;
+
+#include <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