Fixing directory structure

AUTHORS

@@ -0,0 +1,4 @@
+Frank Dellaert
+Christian Potthast
+Alireza Fathi
+Carlos Nieto

COPYING

@@ -0,0 +1,3 @@
+The GTSAM library is not (yet) open source: the software was created
+by Frank Dellaert and his students, but the copyright is legally owned
+by the Georgia Board Of Regents.

CppUnitLite/Failure.cpp

@@ -0,0 +1,50 @@
+
+
+#include "Failure.h"
+
+#include <stdio.h>
+#include <string.h> 
+
+
+Failure::Failure (const SimpleString&	theTestName, 
+				  const SimpleString&	theFileName, 
+		          long	 				theLineNumber,
+		          const SimpleString&	theCondition) 
+: message (theCondition), 
+  testName (theTestName), 
+  fileName (theFileName), 
+  lineNumber (theLineNumber)
+{
+}
+
+
+Failure::Failure (const SimpleString&	theTestName, 
+			 	  const SimpleString&	theFileName, 
+				  long					theLineNumber,
+				  const SimpleString&	expected,
+				  const SimpleString&	actual) 
+: testName (theTestName), 
+  fileName (theFileName), 
+  lineNumber (theLineNumber)
+{
+	char *part1 = "expected ";
+	char *part3 = " but was: ";
+
+	char *stage = new char [strlen (part1) 
+					+ expected.size () 
+					+ strlen (part3)
+					+ actual.size ()
+					+ 1];
+
+	sprintf(stage, "%s%s%s%s", 
+		part1, 
+		expected.asCharString(), 
+		part3, 
+		actual.asCharString());
+
+	message = SimpleString(stage);
+
+	delete stage;
+}
+
+

CppUnitLite/Failure.h

@@ -0,0 +1,41 @@
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// FAILURE.H
+//
+// Failure is a class which holds information pertaining to a specific
+// test failure.  The stream insertion operator is overloaded to allow easy
+// display.
+//
+///////////////////////////////////////////////////////////////////////////////
+
+
+#ifndef FAILURE_H
+#define FAILURE_H
+
+#include "SimpleString.h"
+
+
+class Failure
+{
+
+public:
+	Failure (const SimpleString&		theTestName, 
+			 const SimpleString&		theFileName, 
+			 long	  					theLineNumber,
+			 const SimpleString&		theCondition);
+
+	Failure (const SimpleString&		theTestName, 
+			 const SimpleString&		theFileName, 
+			 long						theLineNumber,
+			 const SimpleString&		expected,
+			 const SimpleString&		actual);
+
+	SimpleString		message;
+	SimpleString		testName;
+	SimpleString		fileName;
+	long				lineNumber;
+};
+
+
+#endif

CppUnitLite/Makefile

@@ -0,0 +1,29 @@
+# Makefile to compile the unit test library, will end up in $(LIBDIR)
+
+.PHONY: all install clean
+
+all: libCppUnitLite.a
+
+CC=gcc
+CXX=gcc
+CPP=gcc
+CXXFLAGS += -O2
+
+sources = $(shell ls *.cpp)
+objects = $(sources:.cpp=.o)
+
+library = libCppUnitLite.a
+
+$(library): $(objects) 
+	ar crsv $@ $(objects) 
+	ranlib $(library)
+
+clean:
+	rm -f *.o *.*~ $(library)
+
+check:
+	echo 'no check for CppUnitLite'
+
+distdir:
+
+install:

CppUnitLite/SimpleString.cpp

@@ -0,0 +1,95 @@
+
+
+#include "SimpleString.h"
+#include <string.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+
+static const int DEFAULT_SIZE = 20;
+
+SimpleString::SimpleString ()
+: buffer(new char [1])
+{
+	buffer [0] = '\0';
+}
+
+
+SimpleString::SimpleString (const char *otherBuffer)
+: buffer (new char [strlen (otherBuffer) + 1])
+{
+	strcpy (buffer, otherBuffer);
+}
+
+SimpleString::SimpleString (const SimpleString& other)
+{
+	buffer = new char [other.size() + 1];
+	strcpy(buffer, other.buffer);
+}
+
+
+SimpleString SimpleString::operator= (const SimpleString& other)
+{
+	delete buffer;
+	buffer = new char [other.size() + 1];
+	strcpy(buffer, other.buffer);	
+	return *this;
+}
+
+
+char *SimpleString::asCharString () const
+{
+	return buffer;
+}
+
+int SimpleString::size() const
+{
+	return strlen (buffer);
+}
+
+SimpleString::~SimpleString ()
+{
+	delete [] buffer;
+}
+
+
+bool operator== (const SimpleString& left, const SimpleString& right)
+{
+	return !strcmp (left.asCharString (), right.asCharString ());
+}
+
+
+SimpleString StringFrom (bool value)
+{
+	char buffer [sizeof ("false") + 1];
+	sprintf (buffer, "%s", value ? "true" : "false");
+	return SimpleString(buffer);
+}
+
+SimpleString StringFrom (const char *value)
+{
+	return SimpleString(value);
+}
+
+SimpleString StringFrom (long value)
+{
+	char buffer [DEFAULT_SIZE];
+	sprintf (buffer, "%ld", value);
+
+	return SimpleString(buffer);
+}
+
+SimpleString StringFrom (double value)
+{
+	char buffer [DEFAULT_SIZE];
+	sprintf (buffer, "%lf", value);
+
+	return SimpleString(buffer);
+}
+
+SimpleString StringFrom (const SimpleString& value)
+{
+	return SimpleString(value);
+}
+
+

CppUnitLite/SimpleString.h

@@ -0,0 +1,45 @@
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// SIMPLESTRING.H
+//
+// One of the design goals of CppUnitLite is to compilation with very old C++
+// compilers.  For that reason, I've added a simple string class that provides
+// only the operations needed in CppUnitLite.
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef SIMPLE_STRING
+#define SIMPLE_STRING
+
+
+
+class SimpleString
+{
+	friend bool	operator== (const SimpleString& left, const SimpleString& right);
+
+public:
+						SimpleString ();
+						SimpleString (const char *value);
+						SimpleString (const SimpleString& other);
+						~SimpleString ();
+
+	SimpleString		operator= (const SimpleString& other);
+
+	char				*asCharString () const;
+	int					size() const;
+
+private:
+	char				*buffer;
+};
+
+
+
+SimpleString StringFrom (bool value);
+SimpleString StringFrom (const char *value);
+SimpleString StringFrom (long value);
+SimpleString StringFrom (double value);
+SimpleString StringFrom (const SimpleString& other);
+
+
+#endif

CppUnitLite/Test.cpp

@@ -0,0 +1,59 @@
+
+
+#include "Test.h"
+#include "TestRegistry.h"
+#include "TestResult.h"
+#include "Failure.h"
+
+
+Test::Test (const SimpleString& testName) 
+	: name_ (testName) 
+{
+	TestRegistry::addTest (this);
+}
+
+
+
+Test *Test::getNext() const
+{
+	return next_;
+}
+
+void Test::setNext(Test *test)
+{	
+	next_ = test;
+}
+
+bool Test::check(long expected, long actual, TestResult& result, const SimpleString& fileName, long lineNumber)
+{
+	if (expected == actual)
+		return true;
+	result.addFailure (
+		Failure (
+			name_, 
+			StringFrom (__FILE__), 
+			__LINE__, 
+			StringFrom (expected), 
+			StringFrom (actual)));
+
+	return false;
+
+}
+
+
+bool Test::check(const SimpleString& expected, const SimpleString& actual, TestResult& result, const SimpleString& fileName, long lineNumber)
+{
+	if (expected == actual)
+		return true;
+	result.addFailure (
+		Failure (
+			name_, 
+			StringFrom (__FILE__), 
+			__LINE__, 
+			expected, 
+			actual));
+
+	return false;
+
+}
+

CppUnitLite/Test.h

@@ -0,0 +1,86 @@
+///////////////////////////////////////////////////////////////////////////////
+//
+// TEST.H
+// 
+// This file contains the Test class along with the macros which make effective
+// in the harness.
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef TEST_H
+#define TEST_H
+
+
+#include <cmath>
+#include "SimpleString.h"
+
+class TestResult;
+
+
+
+class Test
+{
+public:
+	Test (const SimpleString& testName);
+  virtual ~Test() {};
+
+	virtual void	run (TestResult& result) = 0;
+
+
+	void			setNext(Test *test);
+	Test			*getNext () const;
+
+protected:
+
+	bool check (long expected, long actual, TestResult& result, const SimpleString& fileName, long lineNumber);
+	bool check (const SimpleString& expected, const SimpleString& actual, TestResult& result, const SimpleString& fileName, long lineNumber);
+
+	SimpleString	name_;
+	Test			*next_;
+
+};
+
+
+#define TEST(testName, testGroup)\
+  class testGroup##testName##Test : public Test \
+	{ public: testGroup##testName##Test () : Test (#testName "Test") {} \
+            void run (TestResult& result_);} \
+    testGroup##testName##Instance; \
+	void testGroup##testName##Test::run (TestResult& result_) 
+
+
+
+#define CHECK(condition)\
+{ if (!(condition)) \
+{ result_.addFailure (Failure (name_, __FILE__,__LINE__, #condition)); return; } }
+
+
+
+#define CHECK_EQUAL(expected,actual)\
+{ if ((expected) == (actual)) return; result_.addFailure(Failure(name_, __FILE__, __LINE__, StringFrom(expected), StringFrom(actual))); }
+
+
+#define LONGS_EQUAL(expected,actual)\
+{ long actualTemp = actual; \
+  long expectedTemp = expected; \
+  if ((expectedTemp) != (actualTemp)) \
+{ result_.addFailure (Failure (name_, __FILE__, __LINE__, StringFrom(expectedTemp), \
+StringFrom(actualTemp))); return; } }
+
+
+
+#define DOUBLES_EQUAL(expected,actual,threshold)\
+{ double actualTemp = actual; \
+  double expectedTemp = expected; \
+  if (fabs ((expectedTemp)-(actualTemp)) > threshold) \
+{ result_.addFailure (Failure (name_, __FILE__, __LINE__, \
+StringFrom((double)expectedTemp), StringFrom((double)actualTemp))); return; } }
+
+
+
+#define FAIL(text) \
+{ result_.addFailure (Failure (name_, __FILE__, __LINE__,(text))); return; }
+
+
+
+#endif

CppUnitLite/TestHarness.h

@@ -0,0 +1,18 @@
+///////////////////////////////////////////////////////////////////////////////
+//
+// TESTHARNESS.H
+// 
+// The primary include file for the framework.
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef TESTHARNESS_H
+#define TESTHARNESS_H
+
+#include "Test.h"
+#include "TestResult.h"
+#include "Failure.h"
+#include "TestRegistry.h"
+
+#endif
+

CppUnitLite/TestRegistry.cpp

@@ -0,0 +1,50 @@
+
+
+#include "Test.h"
+#include "TestResult.h"
+#include "TestRegistry.h"
+
+
+void TestRegistry::addTest (Test *test) 
+{
+	instance ().add (test);
+}
+
+
+int TestRegistry::runAllTests (TestResult& result) 
+{
+	instance ().run (result);
+}
+
+
+TestRegistry& TestRegistry::instance () 
+{
+	static TestRegistry registry;
+	return registry;
+}
+
+
+void TestRegistry::add (Test *test) 
+{
+	if (tests == 0) {
+		tests = test;
+		return;
+	}
+	
+	test->setNext (tests);
+	tests = test;
+}
+
+
+int TestRegistry::run (TestResult& result) 
+{
+	result.testsStarted ();
+
+	for (Test *test = tests; test != 0; test = test->getNext ())
+		test->run (result);
+	result.testsEnded ();
+  return result.getFailureCount();
+}
+
+
+

CppUnitLite/TestRegistry.h

@@ -0,0 +1,38 @@
+///////////////////////////////////////////////////////////////////////////////
+//
+// TESTREGISTRY.H
+// 
+// TestRegistry is a singleton collection of all the tests to run in a system.  
+// 
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef TESTREGISTRY_H
+#define TESTREGISTRY_H
+
+
+class Test;
+class TestResult;
+
+
+
+class TestRegistry
+{
+public:
+	static void addTest (Test *test);
+	static int runAllTests (TestResult& result);
+
+private:
+
+	static TestRegistry&	instance ();
+	void					add (Test *test);
+	int 					run (TestResult& result);
+
+	
+	Test					*tests;
+
+};
+
+
+
+
+#endif

CppUnitLite/TestResult.cpp

@@ -0,0 +1,40 @@
+
+#include "TestResult.h"
+#include "Failure.h"
+
+#include <stdio.h>
+
+
+TestResult::TestResult ()
+	: failureCount (0)
+{
+}
+
+
+void TestResult::testsStarted () 
+{
+}
+
+
+void TestResult::addFailure (const Failure& failure) 
+{
+	fprintf (stdout, "%s%s%s%s%ld%s%s\n",
+		"Failure: \"",
+		failure.message.asCharString (),
+		"\" " ,
+		"line ",
+		failure.lineNumber,
+		" in ",
+		failure.fileName.asCharString ());
+		
+	failureCount++;
+}
+
+
+void TestResult::testsEnded () 
+{
+	if (failureCount > 0)
+		fprintf (stdout, "There were %ld failures\n", failureCount);
+	else
+		fprintf (stdout, "There were no test failures\n");
+}

CppUnitLite/TestResult.h

@@ -0,0 +1,32 @@
+///////////////////////////////////////////////////////////////////////////////
+//
+// TESTRESULT.H
+// 
+// A TestResult is a collection of the history of some test runs.  Right now
+// it just collects failures.
+// 
+///////////////////////////////////////////////////////////////////////////////
+
+
+
+#ifndef TESTRESULT_H
+#define TESTRESULT_H
+
+class Failure;
+
+class TestResult
+{
+public:
+					TestResult (); 
+  virtual ~TestResult() {};
+	virtual void	testsStarted ();
+	virtual void	addFailure (const Failure& failure);
+	virtual void	testsEnded ();
+  
+  int getFailureCount() {return failureCount;}
+
+private:
+	int				failureCount;
+};
+
+#endif

INSTALL

@@ -0,0 +1,234 @@
+Installation Instructions
+*************************
+
+Copyright (C) 1994, 1995, 1996, 1999, 2000, 2001, 2002, 2004, 2005,
+2006 Free Software Foundation, Inc.
+
+This file is free documentation; the Free Software Foundation gives
+unlimited permission to copy, distribute and modify it.
+
+Basic Installation
+==================
+
+Briefly, the shell commands `./configure; make; make install' should
+configure, build, and install this package.  The following
+more-detailed instructions are generic; see the `README' file for
+instructions specific to this package.
+
+   The `configure' shell script attempts to guess correct values for
+various system-dependent variables used during compilation.  It uses
+those values to create a `Makefile' in each directory of the package.
+It may also create one or more `.h' files containing system-dependent
+definitions.  Finally, it creates a shell script `config.status' that
+you can run in the future to recreate the current configuration, and a
+file `config.log' containing compiler output (useful mainly for
+debugging `configure').
+
+   It can also use an optional file (typically called `config.cache'
+and enabled with `--cache-file=config.cache' or simply `-C') that saves
+the results of its tests to speed up reconfiguring.  Caching is
+disabled by default to prevent problems with accidental use of stale
+cache files.
+
+   If you need to do unusual things to compile the package, please try
+to figure out how `configure' could check whether to do them, and mail
+diffs or instructions to the address given in the `README' so they can
+be considered for the next release.  If you are using the cache, and at
+some point `config.cache' contains results you don't want to keep, you
+may remove or edit it.
+
+   The file `configure.ac' (or `configure.in') is used to create
+`configure' by a program called `autoconf'.  You need `configure.ac' if
+you want to change it or regenerate `configure' using a newer version
+of `autoconf'.
+
+The simplest way to compile this package is:
+
+  1. `cd' to the directory containing the package's source code and type
+     `./configure' to configure the package for your system.
+
+     Running `configure' might take a while.  While running, it prints
+     some messages telling which features it is checking for.
+
+  2. Type `make' to compile the package.
+
+  3. Optionally, type `make check' to run any self-tests that come with
+     the package.
+
+  4. Type `make install' to install the programs and any data files and
+     documentation.
+
+  5. You can remove the program binaries and object files from the
+     source code directory by typing `make clean'.  To also remove the
+     files that `configure' created (so you can compile the package for
+     a different kind of computer), type `make distclean'.  There is
+     also a `make maintainer-clean' target, but that is intended mainly
+     for the package's developers.  If you use it, you may have to get
+     all sorts of other programs in order to regenerate files that came
+     with the distribution.
+
+Compilers and Options
+=====================
+
+Some systems require unusual options for compilation or linking that the
+`configure' script does not know about.  Run `./configure --help' for
+details on some of the pertinent environment variables.
+
+   You can give `configure' initial values for configuration parameters
+by setting variables in the command line or in the environment.  Here
+is an example:
+
+     ./configure CC=c99 CFLAGS=-g LIBS=-lposix
+
+   *Note Defining Variables::, for more details.
+
+Compiling For Multiple Architectures
+====================================
+
+You can compile the package for more than one kind of computer at the
+same time, by placing the object files for each architecture in their
+own directory.  To do this, you can use GNU `make'.  `cd' to the
+directory where you want the object files and executables to go and run
+the `configure' script.  `configure' automatically checks for the
+source code in the directory that `configure' is in and in `..'.
+
+   With a non-GNU `make', it is safer to compile the package for one
+architecture at a time in the source code directory.  After you have
+installed the package for one architecture, use `make distclean' before
+reconfiguring for another architecture.
+
+Installation Names
+==================
+
+By default, `make install' installs the package's commands under
+`/usr/local/bin', include files under `/usr/local/include', etc.  You
+can specify an installation prefix other than `/usr/local' by giving
+`configure' the option `--prefix=PREFIX'.
+
+   You can specify separate installation prefixes for
+architecture-specific files and architecture-independent files.  If you
+pass the option `--exec-prefix=PREFIX' to `configure', the package uses
+PREFIX as the prefix for installing programs and libraries.
+Documentation and other data files still use the regular prefix.
+
+   In addition, if you use an unusual directory layout you can give
+options like `--bindir=DIR' to specify different values for particular
+kinds of files.  Run `configure --help' for a list of the directories
+you can set and what kinds of files go in them.
+
+   If the package supports it, you can cause programs to be installed
+with an extra prefix or suffix on their names by giving `configure' the
+option `--program-prefix=PREFIX' or `--program-suffix=SUFFIX'.
+
+Optional Features
+=================
+
+Some packages pay attention to `--enable-FEATURE' options to
+`configure', where FEATURE indicates an optional part of the package.
+They may also pay attention to `--with-PACKAGE' options, where PACKAGE
+is something like `gnu-as' or `x' (for the X Window System).  The
+`README' should mention any `--enable-' and `--with-' options that the
+package recognizes.
+
+   For packages that use the X Window System, `configure' can usually
+find the X include and library files automatically, but if it doesn't,
+you can use the `configure' options `--x-includes=DIR' and
+`--x-libraries=DIR' to specify their locations.
+
+Specifying the System Type
+==========================
+
+There may be some features `configure' cannot figure out automatically,
+but needs to determine by the type of machine the package will run on.
+Usually, assuming the package is built to be run on the _same_
+architectures, `configure' can figure that out, but if it prints a
+message saying it cannot guess the machine type, give it the
+`--build=TYPE' option.  TYPE can either be a short name for the system
+type, such as `sun4', or a canonical name which has the form:
+
+     CPU-COMPANY-SYSTEM
+
+where SYSTEM can have one of these forms:
+
+     OS KERNEL-OS
+
+   See the file `config.sub' for the possible values of each field.  If
+`config.sub' isn't included in this package, then this package doesn't
+need to know the machine type.
+
+   If you are _building_ compiler tools for cross-compiling, you should
+use the option `--target=TYPE' to select the type of system they will
+produce code for.
+
+   If you want to _use_ a cross compiler, that generates code for a
+platform different from the build platform, you should specify the
+"host" platform (i.e., that on which the generated programs will
+eventually be run) with `--host=TYPE'.
+
+Sharing Defaults
+================
+
+If you want to set default values for `configure' scripts to share, you
+can create a site shell script called `config.site' that gives default
+values for variables like `CC', `cache_file', and `prefix'.
+`configure' looks for `PREFIX/share/config.site' if it exists, then
+`PREFIX/etc/config.site' if it exists.  Or, you can set the
+`CONFIG_SITE' environment variable to the location of the site script.
+A warning: not all `configure' scripts look for a site script.
+
+Defining Variables
+==================
+
+Variables not defined in a site shell script can be set in the
+environment passed to `configure'.  However, some packages may run
+configure again during the build, and the customized values of these
+variables may be lost.  In order to avoid this problem, you should set
+them in the `configure' command line, using `VAR=value'.  For example:
+
+     ./configure CC=/usr/local2/bin/gcc
+
+causes the specified `gcc' to be used as the C compiler (unless it is
+overridden in the site shell script).
+
+Unfortunately, this technique does not work for `CONFIG_SHELL' due to
+an Autoconf bug.  Until the bug is fixed you can use this workaround:
+
+     CONFIG_SHELL=/bin/bash /bin/bash ./configure CONFIG_SHELL=/bin/bash
+
+`configure' Invocation
+======================
+
+`configure' recognizes the following options to control how it operates.
+
+`--help'
+`-h'
+     Print a summary of the options to `configure', and exit.
+
+`--version'
+`-V'
+     Print the version of Autoconf used to generate the `configure'
+     script, and exit.
+
+`--cache-file=FILE'
+     Enable the cache: use and save the results of the tests in FILE,
+     traditionally `config.cache'.  FILE defaults to `/dev/null' to
+     disable caching.
+
+`--config-cache'
+`-C'
+     Alias for `--cache-file=config.cache'.
+
+`--quiet'
+`--silent'
+`-q'
+     Do not print messages saying which checks are being made.  To
+     suppress all normal output, redirect it to `/dev/null' (any error
+     messages will still be shown).
+
+`--srcdir=DIR'
+     Look for the package's source code in directory DIR.  Usually
+     `configure' can determine that directory automatically.
+
+`configure' also accepts some other, not widely useful, options.  Run
+`configure --help' for more details.
+

LICENSE

@@ -0,0 +1 @@
+The GTSAM library has not been licensed yet. See also COPYING

Makefile.am

@@ -0,0 +1,30 @@
+#The option -I m4 tells Autoconf to look for additional Autoconf macros in the m4 subdirectory.
+ACLOCAL_AMFLAGS = -I m4
+
+# make automake install some standard but missing files
+AUTOMAKE_OPTIONS = foreign
+SUBDIRS = cpp wrap
+
+# install the matlab toolbox
+install-exec-hook:
+	install -d ${toolbox}/gtsam && \
+	cp -rf toolbox/* ${toolbox}/gtsam && \
+	install -d ${prefix}/include/wrap && \
+	cp -f wrap/wrap-matlab.h  ${prefix}/include/wrap/matlab.h && \
+	install -d $(prefix)/include && \
+	install -d $(prefix)/include/colamd && \
+	cp -f colamd/colamd.h $(prefix)/include/colamd/ && \
+	cp -f colamd/UFconfig.h $(prefix)/include/colamd/
+
+dist-hook:
+	mkdir $(distdir)/CppUnitLite
+	cp -p $(srcdir)/CppUnitLite/Makefile $(distdir)/CppUnitLite
+	cp -p $(srcdir)/CppUnitLite/*.h $(distdir)/CppUnitLite
+	cp -p $(srcdir)/CppUnitLite/*.cpp $(distdir)/CppUnitLite
+	mkdir $(distdir)/colamd
+	cp -p $(srcdir)/colamd/Makefile $(distdir)/colamd
+	cp -p $(srcdir)/colamd/*.h $(distdir)/colamd
+	cp -p $(srcdir)/colamd/*.c $(distdir)/colamd
+	cp -p $(srcdir)/colamd/*.cpp $(distdir)/colamd
+	mkdir $(distdir)/matlab
+	cp -p $(srcdir)/matlab/*.m $(distdir)/matlab

README

@@ -0,0 +1,88 @@
+README - Georgia Tech Smoothing and Mapping library
+---------------------------------------------------
+
+What is GTSAM ?
+
+  GTSAM is a library of C++ classes that implement smoothing and
+  mapping (SAM) in robotics and vision, using factor graphs and Bayes
+  networks as the underlying computing paradigm rather than sparse
+  matrices. A set of MATLAB mex wrappers is included.
+
+GTSAM is not (yet) open source: See COPYING & LICENSE
+
+Directory structure:
+
+  cpp	  C++ source 
+  matlab  MATLAB proxy classes and wrappers
+
+
+Boost Depedencies:
+------------------
+The GTSAM library is based on the 'Boost C++ Libraries' which can be
+found here: http://www.boost.org/. 
+Donwload the lates version and extract Boost in any place. To this
+place the compiler will be linking.
+
+- On Linux BOOST can also be installed with a packaged manager.
+- On Mac OS Mac Port can be used.
+
+For example the Boost path could be '/opt/local/include/' on a typical
+Mac system, where you should be able to find one of the header files:
+/opt/local/include/boost/config.hpp 
+
+If your boost files are on a different place change the path according
+to your path.
+
+The path to the Boost files can be set as an environmental variable in
+the startup scrip. For a Bash shell the startup file is ~/.bashrc
+put the following command in this file:
+export BOOST_DIR=/opt/local/include/ 
+
+Installation:
+-------------
+To finally install the library go into the directory where you unpacked the 
+GTSAM library, run the command below for example:
+$]./configure --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/
+
+where the path after --with-toolbox should point to the directory you want to have the gtsam
+matlab scripts installed in.
+
+This command will configure the makefile for compiling the GTSAM library.
+
+The 'toolbox' flag sets the path where you want to install the GTSAM Matlab Toolbox.
+You have to set it to an existing directory. After successful installation there
+will be a gtsam directory with all Matlab GTSAM Toolbox files.
+
+The 'boost' flag sets the path where you installed or copied the BOOST C++ Library.
+The path has to be set to the top boost directory. In this directory there are a bunch
+of folders (e.g. boost, doc, libs ....).
+Set the path to this folder.
+  
+
+After configure you makefile you have to compile the library
+Type:
+$] make
+$] make install
+
+Built-in Unit Tests:
+----------------
+There is one more optional step in which you can invoke the unit tests included in the gtsam libraries. 
+$] make check
+By verifying all the test results are positive, you can make sure that the functionalities of the gtsam
+libraries are correct.
+
+The toolbox directory flag is where you want to compile the GTSAM Matlab toolbox.
+
+Compile Matlab Toolbox:
+-----------------------
+1) Start Matlab
+2) Go to File->Set Path and add the toolbox directory where you installed the
+   GTSAM Matlab Toolbox
+3) Change your current directory to the GTSAM Matlab Toolbox
+4) Type 'make_gtsam' at the Command Window
+
+Run Matlab Unit Tests:
+-----------------------
+In the matlab command window, change directory to $gtsam/matlab and then type 'run_tests', which will 
+invoke the matlab unit tests.
+

autogen.sh

@@ -0,0 +1,2 @@
+#!/bin/sh
+autoreconf --force --install -I config -I m4

colamd/Makefile

@@ -0,0 +1,37 @@
+# Makefile to compile the unit test library, will end up in $(LIBDIR)
+
+.PHONY: all install clean
+
+all: libcolamd.a
+
+CC=gcc
+CXX=gcc
+CPP=gcc
+CXXFLAGS += -O2
+CXXFLAGS += -fPIC
+
+
+sources = $(shell ls *.c)
+objects = $(sources:.c=.o)
+
+library = libcolamd.a
+
+#Note: hack was added to ensure that flags are acutally used for compilation 
+# This should probably be fixed, but will work for 64 bit machines now
+
+$(library): $(objects) 
+	$(CPP) $(CXXFLAGS) -c -o colamd.o colamd.c
+	$(CPP) $(CXXFLAGS) -c -o colamd_global.o colamd_global.c
+	ar crsv $@ $(objects) 
+	ranlib $(library)
+
+clean:
+	rm -f *.o *.*~ $(library)
+
+check:
+	echo 'no check for colamd'
+
+distdir:
+
+install:
+

colamd/UFconfig.h

@@ -0,0 +1,118 @@
+/* ========================================================================== */
+/* === UFconfig.h =========================================================== */
+/* ========================================================================== */
+
+/* Configuration file for SuiteSparse: a Suite of Sparse matrix packages
+ * (AMD, COLAMD, CCOLAMD, CAMD, CHOLMOD, UMFPACK, CXSparse, and others).
+ *
+ * UFconfig.h provides the definition of the long integer.  On most systems,
+ * a C program can be compiled in LP64 mode, in which long's and pointers are
+ * both 64-bits, and int's are 32-bits.  Windows 64, however, uses the LLP64
+ * model, in which int's and long's are 32-bits, and long long's and pointers
+ * are 64-bits.
+ *
+ * SuiteSparse packages that include long integer versions are
+ * intended for the LP64 mode.  However, as a workaround for Windows 64
+ * (and perhaps other systems), the long integer can be redefined.
+ *
+ * If _WIN64 is defined, then the __int64 type is used instead of long.
+ *
+ * The long integer can also be defined at compile time.  For example, this
+ * could be added to UFconfig.mk:
+ *
+ * CFLAGS = -O -D'UF_long=long long' -D'UF_long_max=9223372036854775801' \
+ *   -D'UF_long_id="%lld"'
+ *
+ * This file defines UF_long as either long (on all but _WIN64) or
+ * __int64 on Windows 64.  The intent is that a UF_long is always a 64-bit
+ * integer in a 64-bit code.  ptrdiff_t might be a better choice than long;
+ * it is always the same size as a pointer.
+ *
+ * This file also defines the SUITESPARSE_VERSION and related definitions.
+ *
+ * Copyright (c) 2007, University of Florida.  No licensing restrictions
+ * apply to this file or to the UFconfig directory.  Author: Timothy A. Davis.
+ */
+
+#ifndef _UFCONFIG_H
+#define _UFCONFIG_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <limits.h>
+
+/* ========================================================================== */
+/* === UF_long ============================================================== */
+/* ========================================================================== */
+
+#ifndef UF_long
+
+#ifdef _WIN64
+
+#define UF_long __int64
+#define UF_long_max _I64_MAX
+#define UF_long_id "%I64d"
+
+#else
+
+#define UF_long long
+#define UF_long_max LONG_MAX
+#define UF_long_id "%ld"
+
+#endif
+#endif
+
+/* ========================================================================== */
+/* === SuiteSparse version ================================================== */
+/* ========================================================================== */
+
+/* SuiteSparse is not a package itself, but a collection of packages, some of
+ * which must be used together (UMFPACK requires AMD, CHOLMOD requires AMD,
+ * COLAMD, CAMD, and CCOLAMD, etc).  A version number is provided here for the
+ * collection itself.  The versions of packages within each version of
+ * SuiteSparse are meant to work together.  Combining one packge from one
+ * version of SuiteSparse, with another package from another version of
+ * SuiteSparse, may or may not work.
+ *
+ * SuiteSparse Version 3.2.0 contains the following packages:
+ *
+ *  AMD		    version 2.2.0
+ *  CAMD	    version 2.2.0
+ *  COLAMD	    version 2.7.1
+ *  CCOLAMD	    version 2.7.1
+ *  CHOLMOD	    version 1.7.0
+ *  CSparse	    version 2.2.1
+ *  CXSparse	    version 2.2.1
+ *  KLU		    version 1.0.1
+ *  BTF		    version 1.0.1
+ *  LDL		    version 2.0.1
+ *  UFconfig	    version number is the same as SuiteSparse
+ *  UMFPACK	    version 5.2.0
+ *  RBio	    version 1.1.1
+ *  UFcollection    version 1.1.1
+ *  LINFACTOR       version 1.1.0
+ *  MESHND          version 1.1.0
+ *  SSMULT          version 1.1.0
+ *  MATLAB_Tools    no specific version number
+ *  SuiteSparseQR   version 1.1.0
+ *
+ * Other package dependencies:
+ *  BLAS	    required by CHOLMOD and UMFPACK
+ *  LAPACK	    required by CHOLMOD
+ *  METIS 4.0.1	    required by CHOLMOD (optional) and KLU (optional)
+ */
+
+#define SUITESPARSE_DATE "Sept 20, 2008"
+#define SUITESPARSE_VER_CODE(main,sub) ((main) * 1000 + (sub))
+#define SUITESPARSE_MAIN_VERSION 3
+#define SUITESPARSE_SUB_VERSION 2
+#define SUITESPARSE_SUBSUB_VERSION 0
+#define SUITESPARSE_VERSION \
+    SUITESPARSE_VER_CODE(SUITESPARSE_MAIN_VERSION,SUITESPARSE_SUB_VERSION)
+
+#ifdef __cplusplus
+}
+#endif
+#endif

colamd/colamd.h

@@ -0,0 +1,255 @@
+/* ========================================================================== */
+/* === colamd/symamd prototypes and definitions ============================= */
+/* ========================================================================== */
+
+/* COLAMD / SYMAMD include file
+
+    You must include this file (colamd.h) in any routine that uses colamd,
+    symamd, or the related macros and definitions.
+
+    Authors:
+
+	The authors of the code itself are Stefan I. Larimore and Timothy A.
+	Davis (davis at cise.ufl.edu), University of Florida.  The algorithm was
+	developed in collaboration with John Gilbert, Xerox PARC, and Esmond
+	Ng, Oak Ridge National Laboratory.
+
+    Acknowledgements:
+
+	This work was supported by the National Science Foundation, under
+	grants DMS-9504974 and DMS-9803599.
+
+    Notice:
+
+	Copyright (c) 1998-2007, Timothy A. Davis, All Rights Reserved.
+
+	THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY
+	EXPRESSED OR IMPLIED.  ANY USE IS AT YOUR OWN RISK.
+
+	Permission is hereby granted to use, copy, modify, and/or distribute
+	this program, provided that the Copyright, this License, and the
+	Availability of the original version is retained on all copies and made
+	accessible to the end-user of any code or package that includes COLAMD
+	or any modified version of COLAMD. 
+
+    Availability:
+
+	The colamd/symamd library is available at
+
+	    http://www.cise.ufl.edu/research/sparse/colamd/
+
+	This is the http://www.cise.ufl.edu/research/sparse/colamd/colamd.h
+	file.  It is required by the colamd.c, colamdmex.c, and symamdmex.c
+	files, and by any C code that calls the routines whose prototypes are
+	listed below, or that uses the colamd/symamd definitions listed below.
+
+*/
+
+#ifndef COLAMD_H
+#define COLAMD_H
+
+/* make it easy for C++ programs to include COLAMD */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* ========================================================================== */
+/* === Include files ======================================================== */
+/* ========================================================================== */
+
+#include <stdlib.h>
+
+/* ========================================================================== */
+/* === COLAMD version ======================================================= */
+/* ========================================================================== */
+
+/* COLAMD Version 2.4 and later will include the following definitions.
+ * As an example, to test if the version you are using is 2.4 or later:
+ *
+ * #ifdef COLAMD_VERSION
+ *	if (COLAMD_VERSION >= COLAMD_VERSION_CODE (2,4)) ...
+ * #endif
+ *
+ * This also works during compile-time:
+ *
+ *  #if defined(COLAMD_VERSION) && (COLAMD_VERSION >= COLAMD_VERSION_CODE (2,4))
+ *    printf ("This is version 2.4 or later\n") ;
+ *  #else
+ *    printf ("This is an early version\n") ;
+ *  #endif
+ *
+ * Versions 2.3 and earlier of COLAMD do not include a #define'd version number.
+ */
+
+#define COLAMD_DATE "Nov 1, 2007"
+#define COLAMD_VERSION_CODE(main,sub) ((main) * 1000 + (sub))
+#define COLAMD_MAIN_VERSION 2
+#define COLAMD_SUB_VERSION 7
+#define COLAMD_SUBSUB_VERSION 1
+#define COLAMD_VERSION \
+	COLAMD_VERSION_CODE(COLAMD_MAIN_VERSION,COLAMD_SUB_VERSION)
+
+/* ========================================================================== */
+/* === Knob and statistics definitions ====================================== */
+/* ========================================================================== */
+
+/* size of the knobs [ ] array.  Only knobs [0..1] are currently used. */
+#define COLAMD_KNOBS 20
+
+/* number of output statistics.  Only stats [0..6] are currently used. */
+#define COLAMD_STATS 20
+
+/* knobs [0] and stats [0]: dense row knob and output statistic. */
+#define COLAMD_DENSE_ROW 0
+
+/* knobs [1] and stats [1]: dense column knob and output statistic. */
+#define COLAMD_DENSE_COL 1
+
+/* knobs [2]: aggressive absorption */
+#define COLAMD_AGGRESSIVE 2
+
+/* stats [2]: memory defragmentation count output statistic */
+#define COLAMD_DEFRAG_COUNT 2
+
+/* stats [3]: colamd status:  zero OK, > 0 warning or notice, < 0 error */
+#define COLAMD_STATUS 3
+
+/* stats [4..6]: error info, or info on jumbled columns */ 
+#define COLAMD_INFO1 4
+#define COLAMD_INFO2 5
+#define COLAMD_INFO3 6
+
+/* error codes returned in stats [3]: */
+#define COLAMD_OK				(0)
+#define COLAMD_OK_BUT_JUMBLED			(1)
+#define COLAMD_ERROR_A_not_present		(-1)
+#define COLAMD_ERROR_p_not_present		(-2)
+#define COLAMD_ERROR_nrow_negative		(-3)
+#define COLAMD_ERROR_ncol_negative		(-4)
+#define COLAMD_ERROR_nnz_negative		(-5)
+#define COLAMD_ERROR_p0_nonzero			(-6)
+#define COLAMD_ERROR_A_too_small		(-7)
+#define COLAMD_ERROR_col_length_negative	(-8)
+#define COLAMD_ERROR_row_index_out_of_bounds	(-9)
+#define COLAMD_ERROR_out_of_memory		(-10)
+#define COLAMD_ERROR_internal_error		(-999)
+
+
+/* ========================================================================== */
+/* === Prototypes of user-callable routines ================================= */
+/* ========================================================================== */
+
+/* define UF_long */
+#include "UFconfig.h"
+
+size_t colamd_recommended	/* returns recommended value of Alen, */
+				/* or 0 if input arguments are erroneous */
+(
+    int nnz,			/* nonzeros in A */
+    int n_row,			/* number of rows in A */
+    int n_col			/* number of columns in A */
+) ;
+
+size_t colamd_l_recommended	/* returns recommended value of Alen, */
+				/* or 0 if input arguments are erroneous */
+(
+    UF_long nnz,		/* nonzeros in A */
+    UF_long n_row,		/* number of rows in A */
+    UF_long n_col		/* number of columns in A */
+) ;
+
+void colamd_set_defaults	/* sets default parameters */
+(				/* knobs argument is modified on output */
+    double knobs [COLAMD_KNOBS]	/* parameter settings for colamd */
+) ;
+
+void colamd_l_set_defaults	/* sets default parameters */
+(				/* knobs argument is modified on output */
+    double knobs [COLAMD_KNOBS]	/* parameter settings for colamd */
+) ;
+
+int colamd			/* returns (1) if successful, (0) otherwise*/
+(				/* A and p arguments are modified on output */
+    int n_row,			/* number of rows in A */
+    int n_col,			/* number of columns in A */
+    int Alen,			/* size of the array A */
+    int A [],			/* row indices of A, of size Alen */
+    int p [],			/* column pointers of A, of size n_col+1 */
+    double knobs [COLAMD_KNOBS],/* parameter settings for colamd */
+    int stats [COLAMD_STATS]	/* colamd output statistics and error codes */
+) ;
+
+UF_long colamd_l		/* returns (1) if successful, (0) otherwise*/
+(				/* A and p arguments are modified on output */
+    UF_long n_row,		/* number of rows in A */
+    UF_long n_col,		/* number of columns in A */
+    UF_long Alen,		/* size of the array A */
+    UF_long A [],		/* row indices of A, of size Alen */
+    UF_long p [],		/* column pointers of A, of size n_col+1 */
+    double knobs [COLAMD_KNOBS],/* parameter settings for colamd */
+    UF_long stats [COLAMD_STATS]/* colamd output statistics and error codes */
+) ;
+
+int symamd				/* return (1) if OK, (0) otherwise */
+(
+    int n,				/* number of rows and columns of A */
+    int A [],				/* row indices of A */
+    int p [],				/* column pointers of A */
+    int perm [],			/* output permutation, size n_col+1 */
+    double knobs [COLAMD_KNOBS],	/* parameters (uses defaults if NULL) */
+    int stats [COLAMD_STATS],		/* output statistics and error codes */
+    void * (*allocate) (size_t, size_t),
+    					/* pointer to calloc (ANSI C) or */
+					/* mxCalloc (for MATLAB mexFunction) */
+    void (*release) (void *)
+    					/* pointer to free (ANSI C) or */
+    					/* mxFree (for MATLAB mexFunction) */
+) ;
+
+UF_long symamd_l			/* return (1) if OK, (0) otherwise */
+(
+    UF_long n,				/* number of rows and columns of A */
+    UF_long A [],			/* row indices of A */
+    UF_long p [],			/* column pointers of A */
+    UF_long perm [],			/* output permutation, size n_col+1 */
+    double knobs [COLAMD_KNOBS],	/* parameters (uses defaults if NULL) */
+    UF_long stats [COLAMD_STATS],	/* output statistics and error codes */
+    void * (*allocate) (size_t, size_t),
+    					/* pointer to calloc (ANSI C) or */
+					/* mxCalloc (for MATLAB mexFunction) */
+    void (*release) (void *)
+    					/* pointer to free (ANSI C) or */
+    					/* mxFree (for MATLAB mexFunction) */
+) ;
+
+void colamd_report
+(
+    int stats [COLAMD_STATS]
+) ;
+
+void colamd_l_report
+(
+    UF_long stats [COLAMD_STATS]
+) ;
+
+void symamd_report
+(
+    int stats [COLAMD_STATS]
+) ;
+
+void symamd_l_report
+(
+    UF_long stats [COLAMD_STATS]
+) ;
+
+#ifndef EXTERN
+#define EXTERN extern
+#endif
+
+EXTERN int (*colamd_printf) (const char *, ...) ;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* COLAMD_H */

colamd/colamd_global.c

@@ -0,0 +1,24 @@
+/* ========================================================================== */
+/* === colamd_global.c ====================================================== */
+/* ========================================================================== */
+
+/* ----------------------------------------------------------------------------
+ * COLAMD, Copyright (C) 2007, Timothy A. Davis.
+ * See License.txt for the Version 2.1 of the GNU Lesser General Public License
+ * http://www.cise.ufl.edu/research/sparse
+ * -------------------------------------------------------------------------- */
+
+/* Global variables for COLAMD */
+
+#ifndef NPRINT
+#ifdef MATLAB_MEX_FILE
+#include "mex.h"
+int (*colamd_printf) (const char *, ...) = mexPrintf ;
+#else
+#include <stdio.h>
+int (*colamd_printf) (const char *, ...) = printf ;
+#endif
+#else
+int (*colamd_printf) (const char *, ...) = ((void *) 0) ;
+#endif
+

colamd/main.cpp

@@ -0,0 +1,38 @@
+#include "iostream"
+#include "colamd.h"
+
+using namespace std;
+
+#define ALEN 100
+
+void use_colamd()
+{
+  int A [ALEN] = {0, 1, 4, 2, 4, 0, 1, 2, 3, 1, 3} ;
+  int p [ ] = {0, 3, 5, 9, 11} ;
+  int stats [COLAMD_STATS] ;
+  colamd (5, 4, ALEN, A, p, (double *) NULL, stats) ;
+  for(int i = 0; i < 5; i++)
+    printf("%d ", p[i]);
+  printf("\n");
+  for(int i = 0; i < COLAMD_STATS; i++)
+    printf("%d ", stats[i]);
+  printf("\n");
+}
+
+int main()
+{
+  /*
+    A:
+      [0 x x 0 x
+       x x 0 0 x
+       x 0 0 x x
+       0 0 x 0 0
+       x x x 0 x
+                      ]
+   */
+  //int A [ALEN] = {0, 3, 2, 3, 1, 2, 0, 1, 3, 4, 3} ;
+  //int p [ ] = {0, 2, 4, 6, 10, 11} ;
+  
+  use_colamd();
+  return 0;
+}

configure.ac

@@ -0,0 +1,84 @@
+#                                               -*- Autoconf -*-
+# Process this file with autoconf to produce a configure script.
+
+AC_PREREQ(2.59)
+AC_INIT(gtsam, 0.0.0, dellaert@cc.gatech.edu)
+AM_INIT_AUTOMAKE(gtsam, 0.0.0)
+AC_OUTPUT(Makefile cpp/Makefile wrap/Makefile)
+AC_CONFIG_SRCDIR([cpp/cal3_S2.cpp])
+AC_CONFIG_HEADER([cpp/config.h])
+AC_CONFIG_SRCDIR([wrap/wrap.cpp])
+
+# Check for OS
+AC_CANONICAL_HOST    # needs to be called at some point earlier
+AM_CONDITIONAL([DARWIN], [case $host_os in darwin*) true;; *) false;; esac]) 
+
+# Checks for programs.
+AC_PROG_CXX
+AC_PROG_CC
+# FIXME: Need to use boost macros to get serialization library linked
+#AX_BOOST_BASE([1.37.0])
+#AX_BOOST_SERIALIZATION 
+#AX_BOOST_BASE([1.33]) # does not work on windows, even after compiling & installing boost manually
+
+# Checks for libraries.
+AC_PROG_RANLIB
+
+# Checks for header files.
+AC_HEADER_STDC
+AC_CHECK_HEADERS([string.h])
+
+# Checks for typedefs, structures, and compiler characteristics.
+AC_HEADER_STDBOOL
+AC_C_CONST
+AC_C_INLINE
+AC_TYPE_SIZE_T
+
+# Checks for library functions.
+AC_FUNC_ERROR_AT_LINE
+AC_CHECK_FUNCS([pow sqrt])
+
+# ask for CppUnitLite directory
+#AC_ARG_WITH([CppUnitLite],
+#            [AS_HELP_STRING([--with-CppUnitLite],
+#              [specify the directory of CppUnitLite library (mandatory)])],
+#            [CppUnitLite=$withval],
+#            [AC_MSG_FAILURE(
+#                   [--with-CppUnitLite has to be specified])
+#            ])
+#AC_SUBST([CppUnitLite])
+
+# ask for toolbox directory
+AC_ARG_WITH([toolbox],
+            [AS_HELP_STRING([--with-toolbox],
+              [specify the matlab toolbox directory for installation (mandatory)])],
+            [toolbox=$withval],
+            [AC_MSG_FAILURE(
+                   [--with-toolbox has to be specified])
+            ])
+AC_SUBST([toolbox])
+
+# ask for boost directory
+AC_ARG_WITH([boost],
+            [AS_HELP_STRING([--with-boost],
+              [specify the boost directory for installation (mandatory)])],
+            [boost=$withval],
+            [AC_MSG_FAILURE(
+                   [--with-boost has to be specified])
+            ])
+AC_SUBST([boost])
+
+# ask for boost serialization
+AC_ARG_WITH([boost_serialization],
+	     [AS_HELP_STRING([--with-boost-serialization],
+			      [(optional) use the Serialization library from boost - specify the library linking command with the full name of the library
+			        e.g. --with-boost-serialization=-lboost_serialization-gcc-mt-d-1_33_1])],
+		[AC_DEFINE([HAVE_BOOST_SERIALIZATION], ["<boost/foreach.hpp>"], [boost serialization flag]) 
+                
+		 boost_serialization=$withval ])
+AC_SUBST([boost_serialization])
+
+#AC_DEFINE([TEST_AC_DEFINE], ["<boost/foreach.hpp>"], [testing of ac_define])
+	     
+
+AC_OUTPUT

cpp/.cvsignore

@@ -0,0 +1,43 @@
+*.lo
+testPoint2
+testSimulated2D
+testCal3_S2
+testChordalBayesNet
+testNonlinearFactorGraph
+testLinearFactor
+testConditionalGaussian
+testPose3
+testFactorgraph
+testSimulated3D
+testFGConfig
+testNonlinearFactor
+testPoint3
+testMatrix
+testVector
+testRot3
+testLinearFactorGraph
+testVSLAMFactor
+config.h
+html
+config.h.in
+stamp-h1
+Makefile
+Makefile.in
+libgtsam.la
+simple_svd_test
+simple_ublas_resize_test
+simple_ublas_resize_test.cpp
+test_svdcmp.cpp
+.deps
+.libs
+resize_test
+testConstrainedChordalBayesNet
+testConstrainedLinearFactorGraph
+testConstrainedNonlinearFactorGraph
+testDeltaFunction
+testUnconstrainedLinearFactorGraph
+testUnconstrainedNonlinearFactorGraph
+testEqualityFactor
+.DS_Store
+testSimpleCamera
+testCalibratedCamera

cpp/Cal3_S2.cpp

@@ -0,0 +1,76 @@
+/**
+ * @file   Cal3_S2.cpp
+ * @brief  The most common 5DOF 3D->2D calibration
+ * @author Frank Dellaert
+ */
+
+#include <fstream>
+#include <iostream>
+
+#include "Cal3_S2.h"
+
+namespace gtsam {
+using namespace std;
+
+/* ************************************************************************* */
+void Cal3_S2::load(std::string path) {
+	char buffer[200];
+	buffer[0] = 0;
+	sprintf(buffer, "%s/calibration_info.txt", path.c_str());
+	std::ifstream infile(buffer, std::ios::in);
+	if (infile)
+		infile >> fx_ >> fy_ >> s_ >> u0_ >> v0_;
+	else {
+		printf("Unable to load the calibration\n");
+		exit(0);
+	}
+
+	infile.close();
+}
+
+/* ************************************************************************* */
+
+bool Cal3_S2::equals(const Cal3_S2& K, double tol) const {
+	if (fabs(fx_ - K.fx_) > tol) return false;
+	if (fabs(fy_ - K.fy_) > tol) return false;
+	if (fabs(s_ - K.s_) > tol) return false;
+	if (fabs(u0_ - K.u0_) > tol) return false;
+	if (fabs(v0_ - K.v0_) > tol) return false;
+	return true;
+}
+
+/* ************************************************************************* */
+
+bool assert_equal(const Cal3_S2& actual, const Cal3_S2& expected, double tol) {
+	bool ret = actual.equals(expected, tol);
+	if (!ret) {
+		cout << "Not Equal:" << endl;
+		actual.print("Actual");
+		expected.print("Expected");
+	}
+	return ret;
+}
+
+/* ************************************************************************* */
+
+Point2 uncalibrate(const Cal3_S2& K, const Point2& p) {
+	return K.uncalibrate(p);
+}
+
+/* ************************************************************************* */
+
+Matrix Duncalibrate1(const Cal3_S2& K, const Point2& p) {
+	return Matrix_(2, 5, p.x(), 000.0, p.y(), 1.0, 0.0, 0.000, p.y(), 0.000, 0.0,
+			1.0);
+}
+
+/* ************************************************************************* */
+
+Matrix Duncalibrate2(const Cal3_S2& K, const Point2& p) {
+
+	return Matrix_(2, 2, K.fx_, K.s_, 0.000, K.fy_);
+}
+
+/* ************************************************************************* */
+
+} // namespace gtsam

cpp/Cal3_S2.h

@@ -0,0 +1,109 @@
+/**
+ * @file   Cal3_S2.h
+ * @brief  The most common 5DOF 3D->2D calibration
+ * @author Frank Dellaert
+ */
+
+#pragma once
+
+#include "Matrix.h"
+#include "Point2.h"
+
+namespace gtsam {
+
+	/** The most common 5DOF 3D->2D calibration */
+	class Cal3_S2 {
+	private:
+		double fx_, fy_, s_, u0_, v0_;
+
+	public:
+		/**
+		 * default calibration leaves coordinates unchanged
+		 */
+		Cal3_S2() :
+			fx_(1), fy_(1), s_(0), u0_(0), v0_(0) {
+		}
+
+		/**
+		 * constructor from doubles
+		 */
+		Cal3_S2(double fx, double fy, double s, double u0, double v0) :
+			fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) {
+		}
+
+		/**
+		 * return DOF, dimensionality of tangent space
+		 */
+		size_t dim() const {
+			return 5;
+		}
+
+		/**
+		 * load calibration from location (default name is calibration_info.txt)
+		 */
+		void load(std::string path);
+
+		/**
+		 * Given 5-dim tangent vector, create new calibration
+		 */
+		Cal3_S2 exmap(const Vector& d) const {
+			return Cal3_S2(fx_ + d(0), fy_ + d(1), s_ + d(2), u0_ + d(3), v0_ + d(4));
+		}
+
+		/**
+		 * return vectorized form (column-wise)
+		 */
+		Vector vector() const {
+			double r[] = { fx_, fy_, s_, u0_, v0_ };
+			Vector v(5);
+			copy(r, r + 5, v.begin());
+			return v;
+		}
+
+		/**
+		 * return calibration matrix K
+		 */
+		Matrix matrix() const {
+			return Matrix_(3, 3, fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0);
+		}
+
+		/**
+		 * convert intrinsic coordinates xy to image coordinates uv
+		 */
+		Point2 uncalibrate(const Point2& p) const {
+			const double x = p.x(), y = p.y();
+			return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
+		}
+
+		void print(const std::string& s = "") const {
+			gtsam::print(matrix(), s);
+		}
+
+		std::string dump() const {
+			char buffer[100];
+			buffer[0] = 0;
+			sprintf(buffer, "%f %f %f %f %f", fx_, fy_, s_, u0_, v0_);
+			return std::string(buffer);
+		}
+
+		/**
+		 * Check if equal up to specified tolerance
+		 */
+		bool equals(const Cal3_S2& K, double tol) const;
+
+		/** friends */
+		friend Matrix Duncalibrate2(const Cal3_S2& K, const Point2& p);
+	};
+
+	/**
+	 * assert for unit tests
+	 */
+	bool assert_equal(const Cal3_S2& actual, const Cal3_S2& expected, double tol = 1e-8);
+
+	/**
+	 * convert intrinsic coordinates xy to image coordinates uv
+	 */
+	Point2 uncalibrate(const Cal3_S2& K, const Point2& p);
+	Matrix Duncalibrate1(const Cal3_S2& K, const Point2& p);
+	Matrix Duncalibrate2(const Cal3_S2& K, const Point2& p);
+}

cpp/CalibratedCamera.cpp

@@ -0,0 +1,85 @@
+/*
+ * CalibratedCamera.cpp
+ *
+ *  Created on: Aug 17, 2009
+ *      Author: dellaert
+ */
+
+#include "CalibratedCamera.h"
+
+namespace gtsam {
+
+	/* ************************************************************************* */
+	// Auxiliary functions
+	/* ************************************************************************* */
+
+	Point2 project_to_camera(const Point3& P) {
+		return Point2(P.x() / P.z(), P.y() / P.z());
+	}
+
+	Matrix Dproject_to_camera1(const Point3& P) {
+		double d = 1.0 / P.z(), d2 = d * d;
+		return Matrix_(2, 3, d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2);
+	}
+
+	/* ************************************************************************* */
+
+	CalibratedCamera::CalibratedCamera(const Pose3& pose) :
+		pose_(pose) {
+	}
+
+	CalibratedCamera::~CalibratedCamera() {
+	}
+
+	Point2 CalibratedCamera::project(const Point3 & P) const {
+		Point3 cameraPoint = transform_to(pose_, P);
+		Point2 intrinsic = project_to_camera(cameraPoint);
+		return intrinsic;
+	}
+
+	/* ************************************************************************* */
+	Point2 project(const CalibratedCamera& camera, const Point3& point) {
+		return camera.project(point);
+	}
+
+	/* ************************************************************************* */
+	Matrix Dproject_pose(const CalibratedCamera& camera, const Point3& point) {
+		Point3 cameraPoint = transform_to(camera.pose(), point);
+		Matrix D_cameraPoint_pose = Dtransform_to1(camera.pose(), point);
+
+		Point2 intrinsic = project_to_camera(cameraPoint);
+		Matrix D_intrinsic_cameraPoint = Dproject_to_camera1(cameraPoint);
+
+		Matrix D_intrinsic_pose = D_intrinsic_cameraPoint * D_cameraPoint_pose;
+		return D_intrinsic_pose;
+	}
+
+	/* ************************************************************************* */
+	Matrix Dproject_point(const CalibratedCamera& camera, const Point3& point) {
+		Point3 cameraPoint = transform_to(camera.pose(), point);
+		Matrix D_cameraPoint_point = Dtransform_to2(camera.pose());
+
+		Point2 intrinsic = project_to_camera(cameraPoint);
+		Matrix D_intrinsic_cameraPoint = Dproject_to_camera1(cameraPoint);
+
+		Matrix D_intrinsic_point = D_intrinsic_cameraPoint * D_cameraPoint_point;
+		return D_intrinsic_point;
+	}
+
+	/* ************************************************************************* */
+	void Dproject_pose_point(const CalibratedCamera& camera, const Point3& point,
+			Point2& intrinsic, Matrix& D_intrinsic_pose, Matrix& D_intrinsic_point) {
+
+		Point3 cameraPoint = transform_to(camera.pose(), point);
+		Matrix D_cameraPoint_pose = Dtransform_to1(camera.pose(), point); // 3*6
+		Matrix D_cameraPoint_point = Dtransform_to2(camera.pose()); // 3*3
+
+		intrinsic = project_to_camera(cameraPoint);
+		Matrix D_intrinsic_cameraPoint = Dproject_to_camera1(cameraPoint);
+
+		D_intrinsic_pose = D_intrinsic_cameraPoint * D_cameraPoint_pose; // 2*6
+		D_intrinsic_point = D_intrinsic_cameraPoint * D_cameraPoint_point; // 2*3
+	}
+
+/* ************************************************************************* */
+}

cpp/CalibratedCamera.h

@@ -0,0 +1,72 @@
+/*
+ * CalibratedCamera.h
+ *
+ *  Created on: Aug 17, 2009
+ *      Author: dellaert
+ */
+
+#ifndef CalibratedCAMERA_H_
+#define CalibratedCAMERA_H_
+
+#include "Point2.h"
+#include "Pose3.h"
+
+namespace gtsam {
+
+	/**
+	 * projects a 3-dimensional point in camera coordinates into the
+	 * camera and returns a 2-dimensional point, no calibration applied
+	 */
+	Point2 project_to_camera(const Point3& cameraPoint);
+
+	/**
+	 * Derivative of project_to_camera
+	 */
+	Matrix Dproject_to_camera1(const Point3& cameraPoint); /*2by3 <--*/
+
+	/**
+	 * A Calibrated camera class [R|-R't], calibration K=I.
+	 * If calibration is known, it is more computationally efficient
+	 * to calibrate the measurements rather than try to predict in pixels.
+	 */
+	class CalibratedCamera {
+	private:
+		Pose3 pose_; // 6DOF pose
+
+	public:
+		CalibratedCamera(const Pose3& pose);
+		virtual ~CalibratedCamera();
+
+		const Pose3& pose() const {
+			return pose_;
+		}
+
+		Point2 project(const Point3& P) const;
+	};
+
+	/* ************************************************************************* */
+	// measurement functions and derivatives
+	/* ************************************************************************* */
+
+	/**
+	 * This function receives the camera pose and the landmark location and
+	 returns the location the point is supposed to appear in the image
+	 */
+	Point2 project(const CalibratedCamera& camera, const Point3& point);
+
+	/**
+	 * Derivatives of project.
+	 */
+	Matrix Dproject_pose(const CalibratedCamera& camera, const Point3& point);
+	Matrix Dproject_point(const CalibratedCamera& camera, const Point3& point);
+
+	/**
+	 * super-duper combined evaluation + derivatives
+	 * saves a lot of time because a lot of computation is shared
+	 */
+	void
+	Dproject_pose_point(const CalibratedCamera& camera, const Point3& point,
+			Point2& intrinsic, Matrix& D_intrinsic_pose, Matrix& D_intrinsic_point);
+}
+
+#endif /* CalibratedCAMERA_H_ */

cpp/ChordalBayesNet.cpp

@@ -0,0 +1,144 @@
+/**
+ * @file   ChordalBayesNet.cpp
+ * @brief  Chordal Bayes Net, the result of eliminating a factor graph
+ * @author Frank Dellaert
+ */
+
+#include <stdarg.h>
+#include <boost/foreach.hpp>
+#include <boost/tuple/tuple.hpp>
+#include "ChordalBayesNet.h"
+
+using namespace std;
+using namespace gtsam;
+
+// trick from some reading group
+#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) 
+
+/* ************************************************************************* */
+void ChordalBayesNet::insert(const string& key, ConditionalGaussian::shared_ptr node)
+{
+  keys.push_front(key);
+  nodes.insert(make_pair(key,node));
+}
+
+/* ************************************************************************* */
+void ChordalBayesNet::erase(const string& key)
+{
+	list<string>::iterator it;
+	for (it=keys.begin(); it != keys.end(); ++it){
+	  if( strcmp(key.c_str(), (*it).c_str()) == 0 )
+			break;
+	}
+	keys.erase(it);	
+	nodes.erase(key);
+}
+
+/* ************************************************************************* */
+// optimize, i.e. return x = inv(R)*d
+/* ************************************************************************* */
+boost::shared_ptr<FGConfig> ChordalBayesNet::optimize()
+{
+  boost::shared_ptr<FGConfig> result(new FGConfig);
+	result = optimize(result);
+  return result;
+}
+
+/* ************************************************************************* */
+boost::shared_ptr<FGConfig> ChordalBayesNet::optimize(const boost::shared_ptr<FGConfig> &c)
+{
+  boost::shared_ptr<FGConfig> result(new FGConfig);
+	result = c;
+	
+  /** solve each node in turn in topological sort order (parents first)*/
+  BOOST_FOREACH(string key, keys) {
+    ConditionalGaussian::shared_ptr cg = nodes[key]; // get node
+    Vector x = cg->solve(*result);                   // Solve it
+    result->insert(key,x);   // store result in partial solution
+  }
+  return result;
+}
+
+/* ************************************************************************* */
+void ChordalBayesNet::print() const {
+  BOOST_FOREACH(string key, keys) {
+    const_iterator it = nodes.find(key);
+    it->second->print("\nNode[" + key + "]");
+  }
+}
+
+/* ************************************************************************* */
+bool ChordalBayesNet::equals(const ChordalBayesNet& cbn) const
+{
+  const_iterator it1 = nodes.begin(), it2 = cbn.nodes.begin();
+ 
+  if(nodes.size() != cbn.nodes.size()) goto fail;
+  for(; it1 != nodes.end(); it1++, it2++){
+    const string& j1 = it1->first, j2 = it2->first;
+    ConditionalGaussian::shared_ptr node1 = it1->second, node2 = it2->second;
+    if (j1 != j2) goto fail;
+    if (!node1->equals(*node2)) {
+      cout << "node1[" << j1 << "] != node2[" << j2 << "]" << endl;
+      goto fail;
+    }
+  }
+  return true;
+
+ fail:
+  // they don't match, print out and fail
+  print();
+  cbn.print();
+  return false;
+}
+
+/* ************************************************************************* */  
+pair<Matrix,Vector> ChordalBayesNet::matrix() const {
+
+  // add the dimensions of all variables to get matrix dimension
+  // and at the same time create a mapping from keys to indices
+  size_t N=0; map<string,size_t> indices;
+  BOOST_REVERSE_FOREACH(string key, keys) {
+    // find corresponding node
+    const_iterator it = nodes.find(key);
+    indices.insert(make_pair(key,N));
+    N += it->second->dim();
+  }
+
+  // create matrix and copy in values
+  Matrix R = zeros(N,N);
+  Vector d(N);
+  string key; size_t I;
+  FOREACH_PAIR(key,I,indices) {
+    // find corresponding node
+    const_iterator it = nodes.find(key);
+    ConditionalGaussian::shared_ptr cg = it->second;
+    
+    // get RHS and copy to d
+    const Vector& d_ = cg->get_d();
+    const size_t n = d_.size();
+    for (size_t i=0;i<n;i++)
+      d(I+i) = d_(i);
+
+    // get leading R matrix and copy to R
+    const Matrix& R_ = cg->get_R();
+    for (size_t i=0;i<n;i++)
+      for(size_t j=0;j<n;j++)
+	R(I+i,I+j) = R_(i,j);
+
+    // loop over S matrices and copy them into R
+    ConditionalGaussian::const_iterator keyS = cg->parentsBegin();
+    for (; keyS!=cg->parentsEnd(); keyS++) {
+      Matrix S = keyS->second;                   // get S matrix      
+      const size_t m = S.size1(), n = S.size2(); // find S size
+      const size_t J = indices[keyS->first];     // find column index
+      for (size_t i=0;i<m;i++)
+	for(size_t j=0;j<n;j++)
+	  R(I+i,J+j) = S(i,j);
+    } // keyS
+
+  } // keyI
+
+  return make_pair(R,d);
+}
+
+/* ************************************************************************* */

cpp/ChordalBayesNet.h

@@ -0,0 +1,90 @@
+/**
+ * @file    ChordalBayesNet.h
+ * @brief   Chordal Bayes Net, the result of eliminating a factor graph
+ * @brief   ChordalBayesNet
+ * @author  Frank Dellaert
+ */
+
+// \callgraph
+
+#pragma once
+
+#include <list>
+#include <boost/serialization/map.hpp>
+#include <boost/serialization/list.hpp>
+
+#include "ConditionalGaussian.h"
+#include "FGConfig.h"
+
+namespace gtsam {
+
+/** Chordal Bayes Net, the result of eliminating a factor graph */
+class ChordalBayesNet
+{
+public:
+	typedef boost::shared_ptr<ChordalBayesNet> shared_ptr;
+
+protected:
+
+	/** nodes keys stored in topological sort order, i.e. from parents to children */
+	std::list<std::string> keys;
+
+	/** nodes stored on key */
+	std::map<std::string,ConditionalGaussian::shared_ptr> nodes;
+
+	typedef std::map<std::string, ConditionalGaussian::shared_ptr>::iterator iterator;
+
+public:
+
+	/** Construct an empty net */
+	ChordalBayesNet() {}
+
+	/** Copy Constructor */
+	ChordalBayesNet(const ChordalBayesNet& cbn_in) : keys(cbn_in.keys), nodes(cbn_in.nodes) {}
+
+	/** Destructor */
+	virtual ~ChordalBayesNet() {}
+
+	/** insert: use reverse topological sort (i.e. parents last) */
+	void insert(const std::string& key, ConditionalGaussian::shared_ptr node);
+
+	/** delete */
+	void erase(const std::string& key);
+
+	/** return node with given key */
+	inline ConditionalGaussian::shared_ptr get       (const std::string& key) { return nodes[key];}
+	inline ConditionalGaussian::shared_ptr operator[](const std::string& key) { return nodes[key];}
+
+	/** return begin and end of the nodes. FD: breaks encapsulation? */
+	typedef std::map<std::string, ConditionalGaussian::shared_ptr>::const_iterator const_iterator;
+	const_iterator const begin() {return nodes.begin();}
+	const_iterator const end()   {return nodes.end();}
+
+	/** optimize */
+	boost::shared_ptr<FGConfig> optimize();
+	boost::shared_ptr<FGConfig> optimize(const boost::shared_ptr<FGConfig> &c);
+
+	/** print */
+	void print() const;
+
+	/** check equality */
+	bool equals(const ChordalBayesNet& cbn) const;
+
+	/**
+	 * Return (dense) upper-triangular matrix representation
+	 */
+	std::pair<Matrix,Vector> matrix() const;
+
+private:
+	/** Serialization function */
+	friend class boost::serialization::access;
+	template<class Archive>
+	void serialize(Archive & ar, const unsigned int version)
+	{
+		ar & BOOST_SERIALIZATION_NVP(keys);
+		ar & BOOST_SERIALIZATION_NVP(nodes);
+	}
+
+};
+
+} /// namespace gtsam

cpp/ConditionalGaussian.cpp

@@ -0,0 +1,104 @@
+/**
+ * @file   ConditionalGaussian.cpp
+ * @brief  Conditional Gaussian Base class
+ * @author Christian Potthast
+ */
+
+
+#include <string.h>
+#include <boost/numeric/ublas/vector.hpp>
+#include "ConditionalGaussian.h"
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+ConditionalGaussian::ConditionalGaussian(Vector d,Matrix R) : R_(R),d_(d)
+{
+}
+
+/* ************************************************************************* */
+ConditionalGaussian::ConditionalGaussian(Vector d,
+					 Matrix R,
+					 const string& name1,
+					 Matrix S)
+  : R_(R),d_(d)
+{
+  parents_.insert(make_pair(name1, S));
+}
+
+/* ************************************************************************* */
+ConditionalGaussian::ConditionalGaussian(Vector d,
+					 Matrix R,
+					 const string& name1,
+					 Matrix S,
+					 const string& name2,
+					 Matrix T)
+  : R_(R),d_(d)
+{
+  parents_.insert(make_pair(name1, S));
+  parents_.insert(make_pair(name2, T));
+}
+
+/* ************************************************************************* */
+void ConditionalGaussian::print(const string &s) const
+{
+  cout << s << ":" << endl;
+  gtsam::print(R_,"R");
+  for(map<string, Matrix>::const_iterator it = parents_.begin() ; it != parents_.end() ; it++ ) {
+    const string&   j = it->first;
+    const Matrix& Aj = it->second;
+    gtsam::print(Aj, "A["+j+"]");
+  }
+  gtsam::print(d_,"d");
+}    
+
+/* ************************************************************************* */
+Vector ConditionalGaussian::solve(const FGConfig& x)
+{
+  Vector rhs = d_;
+  for(map<string, Matrix>::const_iterator it = parents_.begin() ; it != parents_.end() ; it++ ) {
+    const string&   j = it->first;
+    const Matrix& Aj = it->second;
+    rhs -= Aj*x[j];
+    
+  }
+  Vector result = backsubstitution(R_, rhs);
+  return result;
+  
+}    
+
+/* ************************************************************************* */
+bool ConditionalGaussian::equals(const ConditionalGaussian &cg) const
+{
+  map<string, Matrix>::const_iterator it = parents_.begin();
+
+  // check if the size of the parents_ map is the same
+  if( parents_.size() != cg.parents_.size() ) goto fail;
+
+  // check if R_ is equal
+  if( !(equal_with_abs_tol(R_, cg.R_, 0.0001) ) ) goto fail;
+
+  // check if d_ is equal
+  if( !(::equal_with_abs_tol(d_, cg.d_, 0.0001) ) ) goto fail;
+
+  // check if the matrices are the same
+  // iterate over the parents_ map
+  for(it = parents_.begin(); it != parents_.end(); it++){
+    map<string, Matrix>::const_iterator it2 = cg.parents_.find(it->first.c_str());
+    if( it2 != cg.parents_.end() ){
+      if( !(equal_with_abs_tol(it->second, it2->second, 0.0001)) ) goto fail;
+    }else{
+      goto fail;
+    }
+  } 
+  return true;
+
+ fail:
+  (*this).print();
+  cg.print();
+  return false;
+
+}
+
+/* ************************************************************************* */

cpp/ConditionalGaussian.h

@@ -0,0 +1,130 @@
+/**
+ * @file    ConditionalGaussian.h
+ * @brief   Conditional Gaussian Base class
+ * @author  Christian Potthast
+ */
+
+
+// \callgraph
+
+#pragma once
+
+#include <map>
+#include <boost/utility.hpp>
+#include <boost/shared_ptr.hpp>
+#include <boost/serialization/map.hpp>
+#include <boost/serialization/shared_ptr.hpp>
+#include "Matrix.h"
+#include "FGConfig.h"
+#include "Ordering.h"
+
+namespace gtsam {
+	
+  /**
+   * A conditional Gaussian functions as the node in a Bayes network
+   * It has a set of parents y,z, etc. and implements a probability density on x.
+   * The negative log-probability is given by || Rx - (d - Sy - Tz - ...)||^2
+   */
+  class ConditionalGaussian : boost::noncopyable
+  {
+  public:
+    typedef std::map<std::string, Matrix>::const_iterator const_iterator;
+		
+  protected:
+
+    /** the triangular matrix (square root information matrix) */
+    Matrix R_; 
+
+    /** the names and the matrices connecting to parent nodes */
+    std::map<std::string, Matrix> parents_; 
+
+    /** the RHS vector */
+    Vector d_;
+
+  public:
+    typedef boost::shared_ptr<ConditionalGaussian> shared_ptr;
+
+    /** constructor */
+    ConditionalGaussian() {};
+    
+    /** Copy Constructor */
+    ConditionalGaussian(const ConditionalGaussian &cg) :
+      boost::noncopyable(), R_(cg.R_), parents_(cg.parents_), d_(cg.d_){}
+		
+    /** constructor with no parents
+     * |Rx-d|
+     */
+    ConditionalGaussian(Vector d,
+			Matrix R);
+			
+    /** constructor with only one parent
+     * |Rx+Sy-d|
+     */
+    ConditionalGaussian(Vector d,
+			Matrix R,
+			const std::string& name1,
+			Matrix S
+      );
+			
+    /** constructor with two parents
+     * |Rx+Sy+Tz-d|
+     */
+    ConditionalGaussian(Vector d,
+			Matrix R,
+			const std::string& name1,
+			Matrix S,
+			const std::string& name2,
+			Matrix T
+      );
+
+    /** deconstructor */
+    virtual ~ConditionalGaussian() {};
+
+    /** print */
+    void print(const std::string& = "ConditionalGaussian") const;
+
+    /** dimension of multivariate variable */
+    size_t dim() const {return R_.size2();}
+
+    /** return stuff contained in ConditionalGaussian */
+    const Vector& get_d() const {return d_;}
+    const Matrix& get_R() const {return R_;}
+
+    /** STL like, return the iterator pointing to the first node */
+    const_iterator const parentsBegin(){ return parents_.begin(); }
+
+    /** STL like, return the iterator pointing to the last node */
+    const_iterator const parentsEnd(){ return parents_.end(); }
+
+    /** find the number of parents */
+    size_t size() const {return parents_.size();}
+
+    /** determine whether a key is among the parents */
+    size_t contains(const std::string& key) const {return parents_.count(key);}
+
+    /**
+     * solve a conditional Gaussian
+     * @param x configuration in which the parents values (y,z,...) are known
+     * @return solution x = R \ (d - Sy - Tz - ...)
+     */
+    Vector solve(const FGConfig& x);
+
+    /**
+     * adds a parent
+     */    
+    void add(std::string key, Matrix S){ parents_.insert(make_pair(key, S)); }
+			
+    /** equals function */
+    bool equals(const ConditionalGaussian &cg) const;
+
+  private:
+		/** Serialization function */
+		friend class boost::serialization::access;
+		template<class Archive>
+		void serialize(Archive & ar, const unsigned int version) {
+			ar & BOOST_SERIALIZATION_NVP(R_);
+			ar & BOOST_SERIALIZATION_NVP(d_);
+			ar & BOOST_SERIALIZATION_NVP(parents_);
+		}
+	};
+}

cpp/ConstrainedChordalBayesNet.cpp

@@ -0,0 +1,134 @@
+/*
+ * ConstrainedChordalBayesNet.cpp
+ *
+ *  Created on: Aug 11, 2009
+ *      Author: alexgc
+ */
+
+#include <iostream>
+#include <boost/foreach.hpp>
+#include "ConstrainedChordalBayesNet.h"
+
+// trick from some reading group
+#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
+
+namespace gtsam {
+
+using namespace std;
+
+ConstrainedChordalBayesNet::ConstrainedChordalBayesNet()
+: ChordalBayesNet()
+{
+}
+
+ConstrainedChordalBayesNet::ConstrainedChordalBayesNet(const ChordalBayesNet& cbn)
+: ChordalBayesNet(cbn)
+{
+}
+
+ConstrainedChordalBayesNet::~ConstrainedChordalBayesNet() {
+	// TODO Auto-generated destructor stub
+}
+
+void ConstrainedChordalBayesNet::insert_df(const string& key, DeltaFunction::shared_ptr node)
+{
+	keys.push_front(key);
+	delta_nodes.insert(make_pair(key,node));
+}
+
+void ConstrainedChordalBayesNet::insert(const string& key, ConditionalGaussian::shared_ptr node)
+{
+	keys.push_front(key);
+	nodes.insert(make_pair(key,node));
+}
+
+bool ConstrainedChordalBayesNet::equals(const ConstrainedChordalBayesNet& cbn) const
+{
+	// check delta function nodes
+	if (delta_nodes.size() != cbn.delta_nodes.size()) return false;
+	const_delta_iterator it1 = delta_nodes.begin(), it2 = cbn.delta_nodes.begin();
+	for(; it1 != delta_nodes.end(); it1++, it2++){
+		const string& j1 = it1->first, j2 = it2->first;
+		DeltaFunction::shared_ptr node1 = it1->second, node2 = it2->second;
+		if (j1 != j2) return false;
+		if (!node1->equals(*node2)) return false;
+	}
+
+	// use default equals
+	return convert().equals(cbn.convert());
+}
+
+bool assert_equal(const ConstrainedChordalBayesNet& expected, const ConstrainedChordalBayesNet& actual, double tol)
+{
+	bool ret = expected.equals(actual);
+	if (!ret)
+	{
+		cout << "Not Equal!" << endl;
+		expected.print("Expected");
+		actual.print("Actual");
+	}
+	return ret;
+}
+
+void ConstrainedChordalBayesNet::print(const std::string& s) const
+{
+	cout << s << ":" << endl;
+	pair<string, DeltaFunction::shared_ptr> p1;
+	BOOST_FOREACH(p1, delta_nodes)
+	{
+		cout << "  " << p1.first << endl;
+		p1.second->print();
+	}
+	pair<string, ConditionalGaussian::shared_ptr> p2;
+	BOOST_FOREACH(p2, nodes)
+	{
+		cout << "  " << p2.first << endl;
+		p2.second->print();
+	}
+}
+
+boost::shared_ptr<FGConfig> ConstrainedChordalBayesNet::optimize()
+{
+	boost::shared_ptr<FGConfig> empty(new FGConfig);
+	return optimize(empty);
+}
+
+boost::shared_ptr<FGConfig> ConstrainedChordalBayesNet::optimize(const boost::shared_ptr<FGConfig> &c)
+{
+	// check if it is necessary to handle delta functions
+	if (delta_nodes.size() == 0)
+	{
+		return ChordalBayesNet::optimize(c);
+	}
+
+	// verifying that there are no incorrect values for variables with delta functions
+	vector<string> keys = c->get_names();
+	BOOST_FOREACH(string k, keys)
+	{
+		if (delta_nodes.count(k) && !(delta_nodes[k]->get_value() == c->get(k)))
+			throw(std::invalid_argument("ConstrainedChordalBayesNet:: Passed incorrect value for " + k + " to optimize()"));
+	}
+
+	// create a config with the delta functions
+	pair<string, DeltaFunction::shared_ptr> p;
+	BOOST_FOREACH(p, delta_nodes)
+	{
+		Vector v = p.second->get_value();
+		string key = p.first;
+		c->insert(key, v);
+	}
+	return convert().optimize(c);
+}
+
+ChordalBayesNet ConstrainedChordalBayesNet::convert() const
+{
+	ChordalBayesNet ret;
+	pair<string, ConditionalGaussian::shared_ptr> p;
+	BOOST_FOREACH(p, nodes)
+	{
+		ret.insert(p.first, p.second);
+	}
+	return ret;
+}
+
+}

cpp/ConstrainedChordalBayesNet.h

@@ -0,0 +1,69 @@
+/*
+ * ConstrainedChordalBayesNet.h
+ *
+ *  Created on: Aug 11, 2009
+ *      Author: alexgc
+ */
+
+#ifndef CONSTRAINEDCHORDALBAYESNET_H_
+#define CONSTRAINEDCHORDALBAYESNET_H_
+
+#include "ChordalBayesNet.h"
+#include "DeltaFunction.h"
+
+namespace gtsam {
+
+class ConstrainedChordalBayesNet : public ChordalBayesNet{
+
+protected:
+	std::map<std::string, DeltaFunction::shared_ptr> delta_nodes;
+
+public:
+	typedef std::map<std::string, DeltaFunction::shared_ptr>::const_iterator const_delta_iterator;
+	typedef std::map<std::string, DeltaFunction::shared_ptr>::iterator delta_iterator;
+
+public:
+	typedef boost::shared_ptr<ConstrainedChordalBayesNet> shared_ptr;
+
+	/**
+	 * Default Constructor
+	 */
+	ConstrainedChordalBayesNet();
+
+	/**
+	 * Copies an existing ChordalBayesNet
+	 */
+	ConstrainedChordalBayesNet(const ChordalBayesNet& cbn);
+
+	virtual ~ConstrainedChordalBayesNet();
+
+	/** insert: use reverse topological sort (i.e. parents last) */
+	void insert_df(const std::string& key, DeltaFunction::shared_ptr node);
+	void insert(const std::string& key, ConditionalGaussian::shared_ptr node);
+
+	/** optimize the solution - just a wrapper on the existing optimize implementation */
+	boost::shared_ptr<FGConfig> optimize();
+	boost::shared_ptr<FGConfig> optimize(const boost::shared_ptr<FGConfig> &c);
+
+	/** convert to a regular cbn - strips out the delta functions
+	 * TODO: make this check whether this is a safe conversion
+	 */
+	ChordalBayesNet convert() const;
+
+	/** get delta functions by key */
+	DeltaFunction::shared_ptr get_delta(const std::string& key) {return delta_nodes[key];}
+
+	/** check equality */
+	bool equals(const ConstrainedChordalBayesNet& cbn) const;
+
+	/** prints the contents */
+	void print(const std::string& s="") const;
+};
+
+
+/** check equality for testing */
+bool assert_equal(const ConstrainedChordalBayesNet& expected, const ConstrainedChordalBayesNet& actual, double tol=1e-9);
+
+}
+
+#endif /* CONSTRAINEDCHORDALBAYESNET_H_ */

cpp/ConstrainedLinearFactorGraph.cpp

@@ -0,0 +1,220 @@
+/*
+ * ConstrainedLinearFactorGraph.cpp
+ *
+ *  Created on: Aug 10, 2009
+ *      Author: alexgc
+ */
+
+#include <iostream>
+#include "ConstrainedLinearFactorGraph.h"
+
+using namespace std;
+
+// trick from some reading group
+#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
+
+namespace gtsam {
+
+ConstrainedLinearFactorGraph::ConstrainedLinearFactorGraph() {
+
+}
+
+ConstrainedLinearFactorGraph::ConstrainedLinearFactorGraph(const LinearFactorGraph& lfg)
+{
+	BOOST_FOREACH(LinearFactor::shared_ptr s, lfg)
+	{
+		push_back(s);
+	}
+}
+
+ConstrainedLinearFactorGraph::~ConstrainedLinearFactorGraph() {
+}
+
+void ConstrainedLinearFactorGraph::push_back_eq(EqualityFactor::shared_ptr factor)
+{
+	eq_factors.push_back(factor);
+}
+
+bool ConstrainedLinearFactorGraph::involves_equality(const std::string& key) const
+{
+	BOOST_FOREACH(EqualityFactor::shared_ptr e, eq_factors)
+	{
+		if (e->get_key() == key) return true;
+	}
+	return false;
+}
+
+bool ConstrainedLinearFactorGraph::equals(const LinearFactorGraph& fg, double tol) const
+{
+	const ConstrainedLinearFactorGraph* p = (const ConstrainedLinearFactorGraph *) &fg;
+	if (p == NULL) return false;
+
+	/** check equality factors */
+	if (eq_factors.size() != p->eq_factors.size()) return false;
+	BOOST_FOREACH(EqualityFactor::shared_ptr ef1, eq_factors)
+	{
+		bool contains = false;
+		BOOST_FOREACH(EqualityFactor::shared_ptr ef2, p->eq_factors)
+		if (ef1->equals(*ef2))
+			contains = true;
+		if (!contains) return false;
+	}
+
+	/** default equality check */
+	return LinearFactorGraph::equals(fg, tol);
+}
+
+ConstrainedChordalBayesNet::shared_ptr ConstrainedLinearFactorGraph::eliminate(const Ordering& ordering){
+	ConstrainedChordalBayesNet::shared_ptr cbn (new ConstrainedChordalBayesNet());
+
+	BOOST_FOREACH(string key, ordering) {
+		if (involves_equality(key)) // check whether this is an existing equality factor
+		{
+			// check if eliminating an equality factor
+			DeltaFunction::shared_ptr d = eliminate_one_eq(key);
+			cbn->insert_df(key,d);
+		}
+		else
+		{
+			ConditionalGaussian::shared_ptr cg = eliminate_one(key);
+			cbn->insert(key,cg);
+		}
+	}
+
+	return cbn;
+}
+
+DeltaFunction::shared_ptr ConstrainedLinearFactorGraph::eliminate_one_eq(const string& key)
+{
+	// extract the equality factor - also removes from graph
+	EqualityFactor::shared_ptr eqf = extract_eq(key);
+
+	// remove all unary linear factors on this node
+	vector<LinearFactor::shared_ptr> newfactors;
+	BOOST_FOREACH(LinearFactor::shared_ptr f, factors)
+	{
+		if (f->size() != 1 || !f->involves(key))
+		{
+			newfactors.push_back(f);
+		}
+	}
+	factors = newfactors;
+
+	// combine the linear factors connected to equality node
+	boost::shared_ptr<MutableLinearFactor> joint_factor = combine_factors(key);
+
+	// combine the joint factor with the equality factor and add factors to graph
+	if (joint_factor->size() > 0)
+		eq_combine_and_eliminate(*eqf, *joint_factor);
+
+	// create the delta function - all delta function information contained in the equality factor
+	DeltaFunction::shared_ptr d = eqf->getDeltaFunction();
+
+	return d;
+}
+
+EqualityFactor::shared_ptr ConstrainedLinearFactorGraph::extract_eq(const string& key)
+{
+	EqualityFactor::shared_ptr ret;
+	vector<EqualityFactor::shared_ptr> new_vec;
+	BOOST_FOREACH(EqualityFactor::shared_ptr eq, eq_factors)
+	{
+		if (eq->get_key() == key)
+			ret = eq;
+		else
+			new_vec.push_back(eq);
+	}
+	eq_factors = new_vec;
+	return ret;
+}
+
+FGConfig ConstrainedLinearFactorGraph::optimize(const Ordering& ordering){
+	if (eq_factors.size() == 0)
+	{
+		// use default optimization
+		return LinearFactorGraph::optimize(ordering);
+	}
+
+	// eliminate all nodes in the given ordering -> chordal Bayes net
+	ConstrainedChordalBayesNet::shared_ptr cbn = eliminate(ordering);
+
+	// calculate new configuration (using backsubstitution)
+	boost::shared_ptr<FGConfig> newConfig = cbn->optimize();
+	return *newConfig;
+}
+
+void ConstrainedLinearFactorGraph::print(const std::string& s) const
+{
+	cout << "ConstrainedFactorGraph: " << s << endl;
+	BOOST_FOREACH(LinearFactor::shared_ptr f, factors)
+	{
+		f->print();
+	}
+	BOOST_FOREACH(EqualityFactor::shared_ptr f, eq_factors)
+	{
+		f->print();
+	}
+}
+
+void ConstrainedLinearFactorGraph::eq_combine_and_eliminate(
+		const EqualityFactor& eqf, MutableLinearFactor& joint_factor) // joint factor should be const
+{
+	// start empty remaining factor to be returned
+	boost::shared_ptr<MutableLinearFactor> lf(new MutableLinearFactor);
+
+	// get the value of the target variable (x)
+	Vector x = eqf.get_value();
+
+	// get the RHS vector
+	Vector b = joint_factor.get_b();
+
+	// get key
+	string key = eqf.get_key();
+
+	// get the Ax matrix
+	Matrix Ax = joint_factor.get_A(key);
+
+	// calculate new b
+	b -= Ax * x;
+
+	// reassemble new factor
+	lf->set_b(b);
+	string j; Matrix A;
+	LinearFactor::const_iterator it = joint_factor.begin();
+	for (; it != joint_factor.end(); it++)
+	{
+		j = it->first;
+		A = it->second;
+		if (j != key)
+		{
+			lf->insert(j, A);
+		}
+	}
+
+	// insert factor
+	push_back(lf);
+}
+
+Ordering ConstrainedLinearFactorGraph::getOrdering() const
+{
+	Ordering ord = LinearFactorGraph::getOrdering();
+	BOOST_FOREACH(EqualityFactor::shared_ptr e, eq_factors)
+	{
+		ord.push_back(e->get_key());
+	}
+	return ord;
+}
+
+LinearFactorGraph ConstrainedLinearFactorGraph::convert() const
+{
+	LinearFactorGraph ret;
+	BOOST_FOREACH(LinearFactor::shared_ptr f, factors)
+	{
+		ret.push_back(f);
+	}
+	return ret;
+}
+
+
+
+}

cpp/ConstrainedLinearFactorGraph.h

@@ -0,0 +1,132 @@
+/*
+ * ConstrainedLinearFactorGraph.h
+ *
+ *  Created on: Aug 10, 2009
+ *      Author: alexgc
+ */
+
+#ifndef CONSTRAINEDLINEARFACTORGRAPH_H_
+#define CONSTRAINEDLINEARFACTORGRAPH_H_
+
+#include <boost/shared_ptr.hpp>
+#include <boost/foreach.hpp>
+
+#include "LinearFactorGraph.h"
+#include "EqualityFactor.h"
+#include "ConstrainedChordalBayesNet.h"
+
+namespace gtsam {
+
+class ConstrainedLinearFactorGraph: public LinearFactorGraph {
+
+protected:
+    std::vector<EqualityFactor::shared_ptr> eq_factors; /// collection of equality factors
+
+public:
+    // iterators for equality constraints - same interface as linear factors
+    typedef std::vector<EqualityFactor::shared_ptr>::const_iterator eq_const_iterator;
+    typedef std::vector<EqualityFactor::shared_ptr>::iterator eq_iterator;
+
+public:
+    /**
+     * Default constructor
+     */
+	ConstrainedLinearFactorGraph();
+
+	/**
+	 * Copy from linear factor graph
+	 */
+	ConstrainedLinearFactorGraph(const LinearFactorGraph& lfg);
+
+	virtual ~ConstrainedLinearFactorGraph();
+
+	void push_back_eq(EqualityFactor::shared_ptr factor);
+
+	// Additional STL-like functions for Equality Factors
+
+	EqualityFactor::shared_ptr eq_at(const size_t i) const {return eq_factors.at(i);}
+
+    /** return the iterator pointing to the first equality factor */
+    eq_const_iterator eq_begin() const {
+      return eq_factors.begin();
+    }
+
+    /** return the iterator pointing to the last factor */
+    eq_const_iterator eq_end() const {
+      return eq_factors.end();
+    }
+
+    /** clear the factor graph - reimplemented to include equality factors */
+    void clear(){
+      factors.clear();
+      node_to_factors_.clear();
+      eq_factors.clear();
+    }
+
+    /** size - reimplemented to include the equality factors */
+    inline size_t size() const { return factors.size() + eq_factors.size(); }
+
+    /** Check equality - checks equality constraints as well*/
+    bool equals(const LinearFactorGraph& fg, double tol=1e-9) const;
+
+    /**
+     * eliminate factor graph in place(!) in the given order, yielding
+     * a chordal Bayes net
+     */
+    ConstrainedChordalBayesNet::shared_ptr eliminate(const Ordering& ordering);
+
+    /**
+     * optimize a linear factor graph
+     * @param ordering fg in order
+     */
+    FGConfig optimize(const Ordering& ordering);
+
+    /**
+     * eliminate one node yielding a DeltaFunction
+     * Eliminates the factors from the factor graph through find_factors_and_remove
+     * and adds a new factor to the factor graph
+     *
+     * Only eliminates nodes *with* equality factors
+     */
+    DeltaFunction::shared_ptr eliminate_one_eq(const std::string& key);
+
+    /**
+     * Determines if a node has any equality factors connected to it
+     */
+    bool involves_equality(const std::string& key) const;
+
+    /**
+     * Prints the contents of the factor graph with optional name string
+     */
+    void print(const std::string& s="") const;
+
+    /**
+     * Finds a matching equality constraint by key - assumed present
+     * Performs in-place removal of the equality constraint
+     */
+    EqualityFactor::shared_ptr extract_eq(const std::string& key);
+
+    /**
+     * Combines an equality factor with a joined linear factor
+     * Executes in place, and will add new factors back to the graph
+     */
+    void eq_combine_and_eliminate(const EqualityFactor& eqf, MutableLinearFactor& joint_factor);
+
+    /**
+     * This function returns the best ordering for this linear factor
+     * graph, computed using colamd for the linear factors with all
+     * of the equality factors eliminated first
+     */
+    Ordering getOrdering() const;
+
+    /**
+     * Converts the graph into a linear factor graph
+     * Removes all equality constraints
+     */
+    LinearFactorGraph convert() const;
+
+};
+
+}
+
+#endif /* CONSTRAINEDLINEARFACTORGRAPH_H_ */

cpp/ConstrainedNonlinearFactorGraph.cpp

@@ -0,0 +1,55 @@
+/*
+ * ConstrainedNonlinearFactorGraph.cpp
+ *
+ *  Created on: Aug 10, 2009
+ *      Author: alexgc
+ */
+
+#include "ConstrainedNonlinearFactorGraph.h"
+
+namespace gtsam {
+
+ConstrainedNonlinearFactorGraph::ConstrainedNonlinearFactorGraph()
+{
+}
+
+ConstrainedNonlinearFactorGraph::ConstrainedNonlinearFactorGraph(
+		const NonlinearFactorGraph& nfg)
+: NonlinearFactorGraph(nfg)
+{
+}
+
+ConstrainedNonlinearFactorGraph::~ConstrainedNonlinearFactorGraph()
+{
+}
+
+ConstrainedLinearFactorGraph ConstrainedNonlinearFactorGraph::linearize(const FGConfig& config) const
+{
+	ConstrainedLinearFactorGraph ret;
+
+	// linearize all nonlinear factors
+	for(const_iterator factor=factors.begin(); factor<factors.end(); factor++){
+		LinearFactor::shared_ptr lf = (*factor)->linearize(config);
+		ret.push_back(lf);
+	}
+
+	// linearize the equality factors (set to zero because they are now in delta space)
+	for(eq_const_iterator e_factor=eq_factors.begin(); e_factor<eq_factors.end(); e_factor++){
+		EqualityFactor::shared_ptr eq = (*e_factor)->linearize();
+		ret.push_back_eq(eq);
+	}
+
+	return ret;
+}
+
+NonlinearFactorGraph ConstrainedNonlinearFactorGraph::convert() const
+{
+	NonlinearFactorGraph ret;
+	BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> f, factors)
+	{
+		ret.push_back(f);
+	}
+	return ret;
+}
+
+}

cpp/ConstrainedNonlinearFactorGraph.h

@@ -0,0 +1,60 @@
+/*
+ * ConstrainedNonlinearFactorGraph.h
+ *
+ *  Created on: Aug 10, 2009
+ *      Author: alexgc
+ */
+
+#ifndef CONSTRAINEDNONLINEARFACTORGRAPH_H_
+#define CONSTRAINEDNONLINEARFACTORGRAPH_H_
+
+#include "NonlinearFactorGraph.h"
+#include "EqualityFactor.h"
+#include "ConstrainedLinearFactorGraph.h"
+
+namespace gtsam {
+
+class ConstrainedNonlinearFactorGraph: public NonlinearFactorGraph {
+protected:
+	/** collection of equality factors */
+	std::vector<EqualityFactor::shared_ptr> eq_factors;
+
+public:
+	// iterators over equality factors
+	typedef std::vector<EqualityFactor::shared_ptr>::const_iterator eq_const_iterator;
+	typedef std::vector<EqualityFactor::shared_ptr>::iterator eq_iterator;
+
+	/**
+	 * Default constructor
+	 */
+	ConstrainedNonlinearFactorGraph();
+
+	/**
+	 * Copy constructor from regular NLFGs
+	 */
+	ConstrainedNonlinearFactorGraph(const NonlinearFactorGraph& nfg);
+
+	virtual ~ConstrainedNonlinearFactorGraph();
+
+	/**
+	 * Linearize a nonlinear graph to produce a linear graph
+	 * Note that equality constraints will just pass through
+	 */
+	ConstrainedLinearFactorGraph linearize(const FGConfig& initial) const;
+
+	/**
+	 * Insert a equality factor into the graph
+	 */
+	void push_back_eq(const EqualityFactor::shared_ptr& eq) {
+		eq_factors.push_back(eq);
+	}
+
+	/**
+	 * converts the graph to a regular nonlinear graph - removes equality constraints
+	 */
+	NonlinearFactorGraph convert() const;
+};
+
+}
+
+#endif /* CONSTRAINEDNONLINEARFACTORGRAPH_H_ */

cpp/DeltaFunction.cpp

@@ -0,0 +1,60 @@
+/*
+ * DeltaFunction.cpp
+ *
+ *  Created on: Aug 11, 2009
+ *      Author: alexgc
+ */
+
+#include <iostream>
+#include "DeltaFunction.h"
+
+namespace gtsam {
+
+using namespace std;
+
+DeltaFunction::DeltaFunction() {
+	// TODO Auto-generated constructor stub
+
+}
+
+DeltaFunction::DeltaFunction(const Vector& v, const std::string& id)
+: value(v), key(id)
+{
+}
+
+DeltaFunction::DeltaFunction(const DeltaFunction& df)
+: boost::noncopyable(), value(df.value), key(df.key)
+{
+}
+
+DeltaFunction::~DeltaFunction() {
+	// TODO Auto-generated destructor stub
+}
+
+bool DeltaFunction::equals(const DeltaFunction &df) const
+{
+	return equal_with_abs_tol(value, df.value) && key == df.key;
+}
+
+void DeltaFunction::print() const
+{
+	cout << "DeltaFunction: [" << key << "]";
+	gtsam::print(value);
+	cout << endl;
+}
+
+bool assert_equal(const DeltaFunction& actual, const DeltaFunction& expected, double tol)
+{
+	bool ret = actual.equals(expected);
+	if (!ret)
+	{
+		cout << "Not Equal!" << endl;
+		cout << "  Actual:" << endl;
+		actual.print();
+		cout << "  Expected:" << endl;
+		expected.print();
+	}
+	return ret;
+}
+
+}

cpp/DeltaFunction.h

@@ -0,0 +1,63 @@
+/*
+ * DeltaFunction.h
+ *
+ *  Created on: Aug 11, 2009
+ *      Author: alexgc
+ */
+
+#ifndef DELTAFUNCTION_H_
+#define DELTAFUNCTION_H_
+
+#include <string>
+#include <boost/utility.hpp>
+#include <boost/shared_ptr.hpp>
+#include "Vector.h"
+
+namespace gtsam {
+
+class DeltaFunction : boost::noncopyable {
+protected:
+	Vector value; /// location of the delta function
+	std::string key; /// id of node with delta function
+
+public:
+	typedef boost::shared_ptr<DeltaFunction> shared_ptr;
+
+	/**
+	 * Default Constructor
+	 */
+	DeltaFunction();
+
+	/**
+	 * Constructor with initialization
+	 */
+	DeltaFunction(const Vector& value, const std::string& key);
+
+	/**
+	 * Copy constructor
+	 */
+	DeltaFunction(const DeltaFunction& df);
+
+	virtual ~DeltaFunction();
+
+	/**
+	 * basic get functions
+	 */
+	Vector get_value() const {return value;}
+	std::string get_key() const {return key;}
+
+
+	/** equals function */
+	bool equals(const DeltaFunction &cg) const;
+
+	/** basic print */
+	void print() const;
+
+};
+
+/** equals function for testing - prints if not equal */
+bool assert_equal(const DeltaFunction& actual, const DeltaFunction& expected, double tol=1e-9);
+
+}
+
+#endif /* DELTAFUNCTION_H_ */

cpp/EqualityFactor.cpp

@@ -0,0 +1,76 @@
+/*
+ * EqualityFactor.cpp
+ *
+ *  Created on: Aug 10, 2009
+ *      Author: alexgc
+ */
+
+#include "EqualityFactor.h"
+#include <iostream>
+
+namespace gtsam {
+using namespace std;
+
+EqualityFactor::EqualityFactor()
+: key(""), value(Vector(0))
+{
+}
+
+EqualityFactor::EqualityFactor(const Vector& constraint, const std::string& id)
+: key(id), value(constraint)
+{
+}
+
+double EqualityFactor::error(const FGConfig& c) const
+{
+	return 0.0;
+}
+
+void EqualityFactor::print(const string& s) const
+{
+	cout << s << ": " << dump() << endl;
+}
+
+bool EqualityFactor::equals(const EqualityFactor& f, double tol) const
+{
+	return equal_with_abs_tol(value, f.get_value(), tol) && key == f.get_key();
+}
+
+bool EqualityFactor::equals(const Factor& f, double tol) const
+{
+	const EqualityFactor* p = dynamic_cast<const EqualityFactor*>(&f);
+	  if (p == NULL) return false;
+	  return equals(f, tol);
+}
+
+string EqualityFactor::dump() const
+{
+	string ret = "[" + key + "] " + gtsam::dump(value);
+	return ret;
+}
+
+DeltaFunction::shared_ptr EqualityFactor::getDeltaFunction() const
+{
+	DeltaFunction::shared_ptr ret(new DeltaFunction(value, key));
+	return ret;
+}
+
+EqualityFactor::shared_ptr EqualityFactor::linearize() const
+{
+	EqualityFactor::shared_ptr ret(new EqualityFactor(zero(value.size()), key));
+	return ret;
+}
+
+bool assert_equal(const EqualityFactor& actual, const EqualityFactor& expected, double tol)
+{
+	bool ret = actual.equals(expected, tol);
+	if (!ret)
+	{
+		cout << "Not Equal:" << endl;
+		actual.print("Actual");
+		expected.print("Expected");
+	}
+	return ret;
+}
+
+}

cpp/EqualityFactor.h

@@ -0,0 +1,90 @@
+/*
+ * EqualityFactor.h
+ *
+ *  Created on: Aug 10, 2009
+ *      Author: alexgc
+ */
+
+#ifndef EQUALITYFACTOR_H_
+#define EQUALITYFACTOR_H_
+
+#include "Factor.h"
+#include "DeltaFunction.h"
+
+namespace gtsam {
+
+class EqualityFactor: public Factor {
+public:
+	typedef boost::shared_ptr<EqualityFactor> shared_ptr;
+
+
+protected:
+	Vector value; /// forces a variable be equal to this value
+	std::string key; /// name of variable factor is attached to
+
+public:
+	/**
+	 * Default constructor
+	 */
+	EqualityFactor();
+
+	/**
+	 * Constructor with initializiation
+	 * @param constraint the value that the variable node is defined as equal to
+	 * @param key identifier for the variable node
+	 */
+	EqualityFactor(const Vector& constraint, const std::string& key);
+
+	/**
+	 * Default Destructor
+	 */
+	~EqualityFactor() {}
+
+	/**
+	 * negative log probability
+	 */
+    double error(const FGConfig& c) const;
+
+    /**
+     * print
+     * @param s optional string naming the factor
+     */
+    void print(const std::string& s="") const;
+
+    /**
+     * equality up to tolerance
+     */
+    bool equals(const Factor& f, double tol=1e-9) const;
+    bool equals(const EqualityFactor& f, double tol=1e-9) const;
+
+    /**
+     * linearize
+     */
+    EqualityFactor::shared_ptr linearize() const;
+
+    /**
+     * returns a version of the factor as a string
+     */
+    std::string dump() const;
+
+    // get functions
+    std::string get_key() const {return key;}
+    Vector get_value() const {return value;}
+
+    /**
+     * @return the number of nodes the factor connects
+     */
+    std::size_t size() const {return 1;}
+
+    /**
+     * Returns the corresponding delta function for elimination
+     */
+    DeltaFunction::shared_ptr getDeltaFunction() const;
+};
+
+/** assert equals for testing - prints when not equal */
+bool assert_equal(const EqualityFactor& actual, const EqualityFactor& expected, double tol=1e-9);
+
+}
+
+#endif /* EQUALITYFACTOR_H_ */

cpp/FGConfig.cpp

@@ -0,0 +1,95 @@
+/**
+ * @file    FGConfig.cpp
+ * @brief   Factor Graph Configuration
+ * @brief   fgConfig
+ * @author  Carlos Nieto
+ * @author  Christian Potthast
+ */
+
+#include <boost/foreach.hpp>
+#include <boost/tuple/tuple.hpp>
+#include "FGConfig.h"
+#include "Value.h"
+
+using namespace std;
+
+// trick from some reading group
+#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) 
+
+using namespace gtsam;
+
+/* ************************************************************************* */
+void check_size(const string& key, const Vector & vj, const Vector & dj) {
+  if (dj.size()!=vj.size()) {
+    cout << "For key \"" << key << "\"" << endl;
+    cout << "vj.size = " << vj.size() << endl;
+    cout << "dj.size = " << dj.size() << endl;
+    throw(std::invalid_argument("FGConfig::+ mismatched dimensions"));
+  }
+}
+
+/* ************************************************************************* */
+void FGConfig::operator+=(const FGConfig & delta)
+{
+  for (iterator it = values.begin(); it!=values.end(); it++) {
+    string j = it->first;
+    Vector &vj = it->second;
+    const Vector& dj = delta[j];
+    check_size(j,vj,dj);
+    vj += dj;
+  }
+}
+
+/* ************************************************************************* */
+FGConfig FGConfig::operator+(const FGConfig & delta) const
+{
+  FGConfig result = *this;
+  result += delta;
+  return result;
+}
+
+/* ************************************************************************* */
+Vector FGConfig::get(const std::string& name) const {
+  const_iterator it = values.find(name);
+  if (it==values.end()) {
+    print();
+    cout << "asked for key " << name << endl;
+    throw(std::invalid_argument("FGConfig::[] invalid key"));
+  }
+  return it->second;
+}
+
+/* ************************************************************************* */
+void FGConfig::print(const std::string& name) const {
+  odprintf("FGConfig %s\n", name.c_str());
+  odprintf("size: %d\n", values.size());
+  string j; Vector v;
+  FOREACH_PAIR(j, v, values) {
+    odprintf("%s:", j.c_str());
+    gtsam::print(v);
+  }
+}
+
+/* ************************************************************************* */
+bool FGConfig::equals(const FGConfig& expected, double tol) const {
+  string j; Vector vActual;
+  if( values.size() != expected.size() ) goto fail;
+
+  // iterate over all nodes
+  FOREACH_PAIR(j, vActual, values) {
+    Vector vExpected = expected[j];
+    if(!equal_with_abs_tol(vExpected,vActual,tol)) 
+      goto fail;
+  }
+
+  return true;
+
+fail: 
+  // print and return false
+  print();
+  expected.print();
+  return false;
+}
+
+/* ************************************************************************* */
+

cpp/FGConfig.h

@@ -0,0 +1,94 @@
+/**
+ * @file    FGConfig.h
+ * @brief   Factor Graph Configuration
+ * @author  Carlos Nieto
+ * @author  Christian Potthast
+ */
+
+// \callgraph
+
+#pragma once
+
+#include <map>
+#include <boost/serialization/map.hpp>
+
+#include "Value.h"
+
+namespace gtsam {
+	
+  /** Factor Graph Configuration */
+  class FGConfig {
+
+  protected:
+    /** Map from string indices to values */
+    std::map<std::string, Vector> values;
+
+  public:
+    typedef std::map<std::string, Vector>::iterator iterator;
+    typedef std::map<std::string, Vector>::const_iterator const_iterator;
+
+    FGConfig() {};
+  FGConfig(const FGConfig& cfg_in) : values(cfg_in.values){};
+    
+    virtual ~FGConfig() {};
+
+    /** return all the nodes in the graph **/
+    std::vector<std::string> get_names(){
+      std::vector<std::string> names;
+      for(iterator it=values.begin(); it!=values.end(); it++)
+        names.push_back(it->first);
+      return names;
+    }
+
+    /** Insert a value into the configuration with a given index */
+    FGConfig& insert(const std::string& name, const Vector& val) {
+      values.insert(std::make_pair(name,val));
+      return *this;
+    }
+
+    /**
+     * add a delta config, should apply exponential map more generally
+     */
+    virtual void operator+=(const FGConfig & delta);
+    virtual FGConfig operator+(const FGConfig & delta) const;
+ 
+    iterator begin() {return values.begin();}
+    iterator end()   {return values.end();}
+
+    /** get a vector in the configuration by name */
+    Vector get(const std::string& name) const;
+
+    /** operator[] syntax for get */
+    inline Vector operator[](const std::string& name) const { return get(name); }
+
+    bool contains(const std::string& name) const {
+      const_iterator it = values.find(name);
+      if (it==values.end())
+        return false;
+      return true;
+    }
+
+    /** size of the configurations */
+    size_t size() const {
+      return values.size();
+    }
+
+    /** print */
+    void print(const std::string& name = "") const;
+
+    /** equals, for unit testing */
+    bool equals(const FGConfig& expected, double tol=1e-6) const;
+
+    void clear() {values.clear();}
+    
+    
+  private:
+    /** Serialization function */
+    friend class boost::serialization::access;
+    template<class Archive>
+      void serialize(Archive & ar, const unsigned int version)
+    {
+      ar & BOOST_SERIALIZATION_NVP(values);
+    }
+  };
+}

cpp/Factor.h

@@ -0,0 +1,70 @@
+/**
+ * @file    Factor.h
+ * @brief   A simple factor class to use in a factor graph
+ * @brief   factor
+ * @author  Kai Ni
+ */
+
+// \callgraph
+
+#pragma once
+
+#include <set>
+#include "FGConfig.h"
+
+namespace gtsam {
+
+  struct Variable {
+  private:
+    std::string key_;
+    std::size_t dim_;
+  public:
+  Variable(const std::string& key, std::size_t dim) : key_(key), dim_(dim) {}
+    bool operator< (const Variable& other) const {return key_<other.key_; }
+    const std::string& key() const { return key_;}
+    std::size_t        dim() const { return dim_;}
+  };
+
+  class VariableSet : public std::set<Variable> {
+  };
+	
+  /** 
+   * A simple factor class to use in a factor graph.
+   *
+   * We make it noncopyable so we enforce the fact that factors are
+   * kept in pointer containers. To be safe, you should make them
+   * immutable, i.e., practicing functional programming. However, this
+   * conflicts with efficiency as well, esp. in the case of incomplete
+   * QR factorization. A solution is still being sought.
+   */ 
+  class Factor : boost::noncopyable
+  {
+  public:
+
+    virtual ~Factor() {};
+		
+    /**
+     * negative log probability 
+     */
+    virtual double error(const FGConfig& c) const = 0;
+
+    /**
+     * print
+     * @param s optional string naming the factor
+     */
+    virtual void print(const std::string& s="") const = 0;
+
+    /**
+     * equality up to tolerance
+     * tricky to implement, see NonLinearFactor1 for an example
+     */
+    virtual bool equals(const Factor& f, double tol=1e-9) const = 0;
+
+    virtual std::string dump() const = 0;
+		
+    /** 
+     * @return the number of nodes the factor connects
+     */
+    virtual std::size_t size() const = 0;
+  };
+}

cpp/FactorGraph.cpp

@@ -0,0 +1,9 @@
+/**
+ * @file   FactorGraph.cpp
+ * @brief  Factor Graph Base Class
+ * @author Carlos Nieto
+ */
+
+#include "FactorGraph.h"
+
+

cpp/FactorGraph.h

@@ -0,0 +1,112 @@
+/**
+ * @file    FactorGraph.h
+ * @brief   Factor Graph Base Class
+ * @author  Carlos Nieto
+ * @author  Christian Potthast
+ */
+
+// \callgraph
+
+#pragma once
+
+#include <list>
+#include <map>
+#include <vector>
+#include <boost/shared_ptr.hpp>
+#include "Factor.h"
+#include "FGConfig.h"
+
+namespace gtsam {
+	
+  /**
+   * A factor graph is a bipartite graph with factor nodes connected to variable nodes.
+   * In this class, however, only factor nodes are kept around.
+   */
+  template<class T> class FactorGraph
+  {
+  public:
+    typedef typename boost::shared_ptr<T> shared_factor;
+    typedef typename std::vector<shared_factor>::iterator iterator;
+    typedef typename std::vector<shared_factor>::const_iterator const_iterator;
+
+  protected:
+    /** Collection of factors */
+    std::vector<shared_factor> factors;
+    std::map<std::string, std::list<int> > node_to_factors_;
+
+  public:
+
+    /** get the factors to a specific node */
+      const std::list<int>& get_factors_to_node(const std::string& key) {
+      return node_to_factors_[key];
+    }
+
+    /** STL like, return the iterator pointing to the first factor */
+    const_iterator begin() const {
+      return factors.begin();
+    }
+
+    /** STL like, return the iterator pointing to the last factor */
+    const_iterator end() const {
+      return factors.end();
+    }
+
+    /** clear the factor graph */
+    void clear(){
+      factors.clear();
+      node_to_factors_.clear();
+    }
+
+    /** Get a specific factor by index */
+    shared_factor operator[](size_t i) const {
+      return factors[i];
+    }
+
+    /** return the numbers of the factors in the factor graph */
+    inline size_t size() const { return factors.size(); }
+
+    /** Add a factor */
+    void push_back(shared_factor ptr_f) {factors.push_back(ptr_f);}
+		
+    /** unnormalized error */
+    double error(const FGConfig& c) const {
+      double total_error = 0.;
+      /** iterate over all the factors to accumulate the log probabilities */
+      for(const_iterator factor=factors.begin(); factor!=factors.end(); factor++)
+        total_error += (*factor)->error(c);
+
+      return total_error;
+    }
+
+    /** Unnormalized probability. O(n) */
+    double probPrime(const FGConfig& c) const {
+      return exp(-0.5*error(c));
+    }  
+
+    /** print out graph */
+    void print(const std::string& s = "FactorGraph") const{
+      std::cout << s << std::endl;
+      printf("size: %d\n", (int)size());
+      for(const_iterator factor=factors.begin(); factor!=factors.end(); factor++)
+        (*factor)->print();
+    }
+
+    /** Check equality */
+    bool equals(const FactorGraph& fg, double tol=1e-9) const
+    {
+      /** check whether the two factor graphs have the same number of factors */
+      if( factors.size() != fg.size() ) goto fail;
+		
+      /** check whether the factors are the same */
+      for(size_t i=0;i<factors.size();i++)
+        if ( ! factors[i]->equals(*fg.factors[i], tol) ) goto fail; //TODO: Doesn't this force order of factor insertion?
+      return true;
+
+    fail:
+      print();
+      fg.print();
+      return false;
+    }
+  };
+
+} // namespace gtsam

cpp/LinearFactor.cpp

@@ -0,0 +1,245 @@
+/**
+ * @file    LinearFactor.cpp
+ * @brief   Linear Factor....A Gaussian
+ * @brief   linearFactor
+ * @author  Christian Potthast
+ */
+
+#include <boost/foreach.hpp>
+#include <boost/tuple/tuple.hpp>
+#include "LinearFactor.h"
+
+using namespace std;
+namespace ublas = boost::numeric::ublas;
+
+// trick from some reading group
+#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) 
+
+using namespace gtsam;
+
+typedef pair<const string, Matrix>& mypair;
+
+/* ************************************************************************* */
+// we might have multiple As, so iterate and subtract from b
+double LinearFactor::error(const FGConfig& c) const {
+  if (empty()) return 0;
+  Vector e = b;
+  string j; Matrix Aj;
+  FOREACH_PAIR(j, Aj, As)
+    e -= Vector(Aj * c[j]);
+  return 0.5 * inner_prod(trans(e),e);
+}
+
+/* ************************************************************************* */
+void LinearFactor::print(const string& s) const {
+  cout << s << endl;
+  if (empty()) cout << " empty" << endl; 
+  else {
+    string j; Matrix A;
+    FOREACH_PAIR(j,A,As) gtsam::print(A, "A["+j+"]=\n");
+    gtsam::print(b,"b=");
+  }
+}
+
+/* ************************************************************************* */
+// Check if two linear factors are equal
+bool LinearFactor::equals(const Factor& f, double tol) const {
+    
+  const LinearFactor* lf = dynamic_cast<const LinearFactor*>(&f);
+  if (lf == NULL) return false;
+
+  if (empty()) return (lf->empty());
+
+  const_iterator it1 = As.begin(), it2 = lf->As.begin();
+  if(As.size() != lf->As.size()) goto fail;
+
+  for(; it1 != As.end(); it1++, it2++){
+    const string& j1 = it1->first, j2 = it2->first;
+    const Matrix A1 = it1->second, A2 = it2->second;
+    if (j1 != j2) goto fail;
+    if (!equal_with_abs_tol(A1,A2,tol)) {
+      cout << "A1[" << j1 << "] != A2[" << j2 << "]" << endl;
+      goto fail;
+    }
+  }
+  if( !(::equal_with_abs_tol(b, (lf->b),tol)) ) {
+    cout << "RHS disagree" << endl;
+    goto fail;
+  }
+  return true;
+
+ fail:
+  // they don't match, print out and fail
+  print();
+  lf->print();
+  return false;
+}
+
+/* ************************************************************************* */
+VariableSet LinearFactor::variables() const {
+  VariableSet result;
+  string j; Matrix A;
+  FOREACH_PAIR(j,A,As) {
+    Variable v(j,A.size2());
+    result.insert(v);
+  }
+  return result;
+}
+
+/* ************************************************************************* */
+void LinearFactor::tally_separator(const string& key, set<string>& separator) const {
+  if(involves(key)) {
+    string j; Matrix A;
+    FOREACH_PAIR(j,A,As)
+      if(j != key) separator.insert(j);
+  }
+}
+
+/* ************************************************************************* */  
+pair<Matrix,Vector> LinearFactor::matrix(const Ordering& ordering) const {
+
+  // get pointers to the matrices
+  vector<const Matrix *> matrices;
+  BOOST_FOREACH(string j, ordering) {
+    const Matrix& Aj = get_A(j);
+    matrices.push_back(&Aj);
+  }
+
+  return make_pair( collect(matrices), b);
+}
+
+/* ************************************************************************* */
+void MutableLinearFactor::append_factor(LinearFactor::shared_ptr f, const size_t m, const size_t pos)
+{
+  // iterate over all matrices from the factor f
+  LinearFactor::const_iterator it = f->begin();
+  for(; it != f->end(); it++) {
+    string j = it->first;
+    Matrix A = it->second;
+
+    // find rows and columns
+    const size_t mrhs = A.size1(), n = A.size2();
+
+    // find the corresponding matrix among As
+    const_iterator mine = As.find(j);
+    const bool exists = mine!=As.end();
+
+    // create the matrix or use existing
+    Matrix Anew = exists ? mine->second : zeros(m,n);
+
+    // copy the values in the existing matrix
+    for (size_t i=0;i<mrhs;i++)
+      for(size_t j=0;j<n;j++)
+	Anew(pos+i,j)=A(i,j);
+
+    // insert the matrix into the factor
+    if(exists) As.erase(j);
+    insert(j,Anew);
+  }
+}
+
+/* ************************************************************************* */
+MutableLinearFactor::MutableLinearFactor(const set<shared_ptr> & factors)
+{
+  // Create RHS vector of the right size by adding together row counts
+  size_t m = 0;
+  BOOST_FOREACH(shared_ptr factor, factors) m += factor->numberOfRows();
+  b = Vector(m);
+
+  size_t pos = 0; // save last position inserted into the new rhs vector
+
+  // iterate over all factors
+  BOOST_FOREACH(shared_ptr factor, factors){
+    // number of rows for factor f
+    const size_t mf = factor->numberOfRows();
+
+    // copy the rhs vector from factor to b
+    const Vector bf = factor->get_b();
+    for (size_t i=0; i<mf; i++) b(pos+i) = bf(i);
+
+    // update the matrices
+    append_factor(factor,m,pos);
+
+    pos += mf;
+  }
+}
+
+/* ************************************************************************* */
+/* Note, in place !!!!
+ * Do incomplete QR factorization for the first n columns
+ * We will do QR on all matrices and on RHS
+ * Then take first n rows and make a ConditionalGaussian,
+ * and last rows to make a new joint linear factor on separator
+ */
+/* ************************************************************************* */
+pair<ConditionalGaussian::shared_ptr, LinearFactor::shared_ptr>
+MutableLinearFactor::eliminate(const string& key)
+{
+  // start empty remaining factor to be returned
+  boost::shared_ptr<MutableLinearFactor> lf(new MutableLinearFactor);
+
+  // find the matrix associated with key
+  iterator it = As.find(key);
+
+  // if this factor does not involve key, we exit with empty CG and LF
+  if (it==As.end()) {
+    // Conditional Gaussian is just a parent-less node with P(x)=1
+    ConditionalGaussian::shared_ptr cg(new ConditionalGaussian);
+    return make_pair(cg,lf);
+  }
+
+  // get the matrix reference associated with key
+  const Matrix& R = it->second;
+  const size_t m = R.size1(), n = R.size2();
+
+  // if m<n, this factor cannot be eliminated
+  if (m<n)
+    throw(domain_error("LinearFactor::eliminate: fewer constraints than unknowns"));
+  
+  // we will apply n Householder reflections to zero out R below diagonal
+  for(size_t j=0; j < n; j++){
+    // below, the indices r,c always refer to original A
+
+    // copy column from matrix to xjm, i.e. x(j:m) = R(j:m,j)
+    Vector xjm(m-j);
+    for(size_t r = j ; r < m; r++)
+      xjm(r-j) = R(r,j);
+        
+    // calculate the Householder vector v
+    double beta; Vector vjm;
+    boost::tie(beta,vjm) = house(xjm);
+
+    // update all matrices
+    BOOST_FOREACH(mypair jA,As) {
+      // update A matrix using reflection as in householder_
+      Matrix& A = jA.second;
+      householder_update(A, j, beta, vjm);
+    }
+
+    // update RHS, b -= (beta * inner_prod(v,b)) * v;
+    double inner = 0;
+    for(size_t r = j ; r < m; r++) 
+      inner += vjm(r-j) * (b)(r);
+    for(size_t r = j ; r < m; r++) 
+      (b)(r) -= beta*inner*vjm(r-j);
+  } // column j
+  
+  // create ConditionalGaussian with first n rows
+  ConditionalGaussian::shared_ptr cg (new ConditionalGaussian(::sub(b,0,n), sub(R,0,n,0,n)) );
+  
+  // create linear factor with remaining rows
+  lf->set_b(::sub(b,n,m));
+
+  // for every separator variable
+  string j; Matrix A;
+  FOREACH_PAIR(j,A,As) {
+    if (j != key) {
+      const size_t nj = A.size2();   // get dimension of variable
+      cg->add(j, sub(A,0,n,0,nj));   // add a parent to conditional gaussian
+      lf->insert(j,sub(A,n,m,0,nj)); // insert into linear factor
+    }
+  }
+  return make_pair(cg,lf);
+}
+
+/* ************************************************************************* */

cpp/LinearFactor.h

@@ -0,0 +1,245 @@
+/**
+ * @file    LinearFactor.h
+ * @brief   Linear Factor....A Gaussian
+ * @brief   linearFactor
+ * @author  Christian Potthast
+ */
+
+// \callgraph
+
+#pragma once
+
+#include <list>
+#include <map>
+#include <set>
+#include <ostream>
+#include <boost/shared_ptr.hpp>
+
+#include "Matrix.h"
+#include "Factor.h"
+#include "LinearFactorSet.h"
+#include "ConditionalGaussian.h"
+#include "Ordering.h"
+
+#define CONSTRUCTOR 
+
+namespace gtsam {
+
+class MutableLinearFactor;
+
+/**
+ * Base Class for a linear factor.
+ * LinearFactor is non-mutable (all methods const!).
+ * The factor value is exp(-0.5*||Ax-b||^2)
+ */
+class LinearFactor: public Factor {
+public:
+
+	typedef boost::shared_ptr<LinearFactor> shared_ptr;
+	typedef std::map<std::string, Matrix>::iterator iterator;
+	typedef std::map<std::string, Matrix>::const_iterator const_iterator;
+
+protected:
+
+	std::map<std::string, Matrix> As; // linear matrices
+	Vector b; // right-hand-side
+
+	LinearFactor()  {
+	}
+
+public:
+
+	/** Construct Null factor */
+	CONSTRUCTOR
+	LinearFactor(const Vector& b_in) :
+		b(b_in) { //TODO: add a way to initializing base class meaningfully
+	}
+
+	/** Construct unary factor */
+	CONSTRUCTOR
+	LinearFactor(const std::string& key1, const Matrix& A1, const Vector& b_in) :
+		b(b_in) {
+		As.insert(make_pair(key1, A1));
+	}
+
+	/** Construct binary factor */
+	CONSTRUCTOR
+	LinearFactor(const std::string& key1, const Matrix& A1,
+			const std::string& key2, const Matrix& A2, const Vector& b_in) :
+		b(b_in) {
+		As.insert(make_pair(key1, A1));
+		As.insert(make_pair(key2, A2));
+	}
+
+	/** Construct ternary factor */
+	CONSTRUCTOR
+	LinearFactor(const std::string& key1, const Matrix& A1,
+			const std::string& key2, const Matrix& A2, const std::string& key3,
+			const Matrix& A3, const Vector& b_in) :
+		b(b_in) {
+		As.insert(make_pair(key1, A1));
+		As.insert(make_pair(key2, A2));
+		As.insert(make_pair(key3, A3));
+	}
+
+	/** Construct from Conditional Gaussian */
+	CONSTRUCTOR
+	LinearFactor(const std::string& key, const boost::shared_ptr<
+			ConditionalGaussian> cg) :
+		b(cg->get_d()) {
+		As.insert(make_pair(key, cg->get_R()));
+		std::map<std::string, Matrix>::const_iterator it = cg->parentsBegin();
+		for (; it != cg->parentsEnd(); it++) {
+			const std::string& j = it->first;
+			const Matrix& Aj = it->second;
+			As.insert(make_pair(j, Aj));
+		}
+	}
+
+	// Implementing Factor virtual functions
+
+	double error(const FGConfig& c) const; /**  0.5*(A*x-b)'*(A*x-b) */
+	void print(const std::string& s = "") const;
+	bool equals(const Factor& lf, double tol = 1e-9) const;
+	std::string dump() const { return "";}
+	std::size_t size() const { return As.size();}
+
+	/** STL like, return the iterator pointing to the first node */
+	const_iterator const begin() { return As.begin();}
+
+	/** STL like, return the iterator pointing to the last node */
+	const_iterator const end() { return As.end();	}
+
+	/** check if empty */
+	bool empty() const { return b.size() == 0;}
+
+	/** get a copy of b */
+	const Vector& get_b() const {	return b;	}
+
+	/**
+	 * get a copy of the A matrix from a specific node
+	 * O(log n)
+	 */
+	const Matrix& get_A(const std::string& key) const {
+		const_iterator it = As.find(key);
+		if (it == As.end())
+			throw(std::invalid_argument("LinearFactor::[] invalid key: " + key));
+		return it->second;
+	}
+
+	/** operator[] syntax for get */
+	inline const Matrix& operator[](const std::string& name) const {
+		return get_A(name);
+	}
+
+	/** Check if factor involves variable with key */
+	bool involves(const std::string& key) const {
+		const_iterator it = As.find(key);
+		return (it != As.end());
+	}
+
+	/**
+	 * return the number of rows from the b vector
+	 * @return a integer with the number of rows from the b vector
+	 */
+	int numberOfRows() { return b.size();}
+
+	/**
+	 * Find all variables and their dimensions
+	 * @return The set of all variable/dimension pairs
+	 */
+	VariableSet variables() const;
+
+	/**
+	 * Add to separator set if this factor involves key, but don't add key itself
+	 * @param key
+	 * @param separator set to add to
+	 */
+	void tally_separator(const std::string& key,
+			std::set<std::string>& separator) const;
+
+	/**
+	 * Return (dense) matrix associated with factor
+	 * @param ordering of variables needed for matrix column order
+	 */
+	std::pair<Matrix, Vector> matrix(const Ordering& ordering) const;
+
+}; // LinearFactor
+
+/* ************************************************************************* */
+
+/**
+ * Mutable subclass of LinearFactor
+ * To isolate bugs
+ */
+class MutableLinearFactor: public LinearFactor {
+public:
+
+	CONSTRUCTOR
+	MutableLinearFactor() {
+	}
+
+	/**
+	 * Constructor that combines a set of factors
+	 * @param factors Set of factors to combine
+	 */
+	CONSTRUCTOR
+	MutableLinearFactor(const std::set<shared_ptr> & factors);
+
+	/** Construct unary mutable factor */
+	CONSTRUCTOR
+	MutableLinearFactor(const std::string& key1, const Matrix& A1,
+			const Vector& b_in) :
+				LinearFactor(key1, A1, b_in) {
+	}
+
+	/** Construct binary mutable factor */
+	CONSTRUCTOR
+	MutableLinearFactor(const std::string& key1, const Matrix& A1,
+			const std::string& key2, const Matrix& A2, const Vector& b_in) :
+				LinearFactor(key1, A1, key2, A2, b_in) {
+	}
+
+	/** Construct ternary mutable factor */
+	CONSTRUCTOR
+	MutableLinearFactor(const std::string& key1, const Matrix& A1,
+			const std::string& key2, const Matrix& A2, const std::string& key3,
+			const Matrix& A3, const Vector& b_in) :
+				LinearFactor(key1, A1, key2, A2, key3, A3, b_in) {
+	}
+
+	/** insert, copies A */
+	void insert(const std::string& key, const Matrix& A) {
+		As.insert(std::make_pair(key, A));
+	}
+
+	/** set RHS, copies b */
+	void set_b(const Vector& b) {
+		this->b = b;
+	}
+
+	// set A matrices for the linear factor, same as insert ?
+	inline void set_A(const std::string& key, const Matrix &A) {
+		insert(key, A);
+	}
+
+	/**
+	 * eliminate (in place!) one of the variables connected to this factor
+	 * @param key the key of the node to be eliminated
+	 * @return a new factor and a conditional gaussian on the eliminated variable
+	 */
+	std::pair<ConditionalGaussian::shared_ptr, shared_ptr> eliminate(
+			const std::string& key);
+
+	/**
+	 * Take the factor f, and append to current matrices. Not very general.
+	 * @param f linear factor graph
+	 * @param m final number of rows of f, needs to be known in advance
+	 * @param pos where to insert in the m-sized matrices
+	 */
+	void append_factor(LinearFactor::shared_ptr f, const size_t m,
+			const size_t pos);
+
+};
+
+} // namespace gtsam

cpp/LinearFactorGraph.cpp

@@ -0,0 +1,308 @@
+/**
+ * @file    LinearFactorGraph.cpp
+ * @brief   Linear Factor Graph where all factors are Gaussians
+ * @author  Kai Ni
+ * @author  Christian Potthast
+ */
+
+#include <boost/foreach.hpp>
+#include <boost/tuple/tuple.hpp>
+#include <boost/numeric/ublas/lu.hpp>
+#include <boost/numeric/ublas/io.hpp>
+
+#include <colamd/colamd.h>
+
+#include "LinearFactorGraph.h"
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+LinearFactorGraph::LinearFactorGraph(ChordalBayesNet::shared_ptr CBN)
+{
+	setCBN(CBN);
+}
+
+/* ************************************************************************* */
+void LinearFactorGraph::setCBN(ChordalBayesNet::shared_ptr CBN)
+{
+	clear();
+	ChordalBayesNet::const_iterator it = CBN->begin();
+	for(; it != CBN->end(); it++) {
+		LinearFactor::shared_ptr lf(new LinearFactor(it->first, it->second));
+		push_back(lf);
+	}
+}
+
+/* ************************************************************************* */
+/* find the separators                                                       */
+/* ************************************************************************* */
+set<string> LinearFactorGraph::find_separator(const string& key) const
+{
+	set<string> separator;
+	BOOST_FOREACH(shared_factor factor,factors)
+	factor->tally_separator(key,separator);
+
+	return separator;
+}
+
+/* ************************************************************************* */
+/** O(n)                                                                     */
+/* ************************************************************************* */ 
+LinearFactorSet
+LinearFactorGraph::find_factors_and_remove(const string& key)
+{
+	LinearFactorSet found;
+
+	for(iterator factor=factors.begin(); factor!=factors.end(); )
+		if ((*factor)->involves(key)) {
+			found.insert(*factor);
+			factor = factors.erase(factor);
+		} else {
+			factor++; // important, erase will have effect of ++
+		}
+
+	return found;
+}
+
+/* ************************************************************************* */
+/* find factors and remove them from the factor graph: O(n)                  */
+/* ************************************************************************* */ 
+boost::shared_ptr<MutableLinearFactor> 
+LinearFactorGraph::combine_factors(const string& key)
+{
+	LinearFactorSet found = find_factors_and_remove(key);
+	boost::shared_ptr<MutableLinearFactor> lf(new MutableLinearFactor(found));
+	return lf;
+}
+
+/* ************************************************************************* */
+/* eliminate one node from the linear factor graph                           */ 
+/* ************************************************************************* */
+ConditionalGaussian::shared_ptr LinearFactorGraph::eliminate_one(const string& key)
+{
+	// combine the factors of all nodes connected to the variable to be eliminated
+	// if no factors are connected to key, returns an empty factor
+	boost::shared_ptr<MutableLinearFactor> joint_factor = combine_factors(key);
+
+	// eliminate that joint factor
+	try {
+		ConditionalGaussian::shared_ptr conditional;
+		LinearFactor::shared_ptr factor;
+		boost::tie(conditional,factor) = joint_factor->eliminate(key);
+
+		if (!factor->empty())
+			push_back(factor);
+
+		// return the conditional Gaussian
+		return conditional;
+	}
+	catch (domain_error&) {
+		throw(domain_error("LinearFactorGraph::eliminate: singular graph"));
+	}
+}
+
+/* ************************************************************************* */
+// eliminate factor graph using the given (not necessarily complete)
+// ordering, yielding a chordal Bayes net and partially eliminated FG
+/* ************************************************************************* */
+ChordalBayesNet::shared_ptr
+LinearFactorGraph::eliminate_partially(const Ordering& ordering)
+{
+	ChordalBayesNet::shared_ptr chordalBayesNet (new ChordalBayesNet()); // empty
+
+	BOOST_FOREACH(string key, ordering) {
+		ConditionalGaussian::shared_ptr cg = eliminate_one(key);
+		chordalBayesNet->insert(key,cg);
+	}
+
+	return chordalBayesNet;
+}
+
+/* ************************************************************************* */
+/** eliminate factor graph in the given order, yielding a chordal Bayes net  */ 
+/* ************************************************************************* */
+ChordalBayesNet::shared_ptr
+LinearFactorGraph::eliminate(const Ordering& ordering)
+{
+	ChordalBayesNet::shared_ptr chordalBayesNet = eliminate_partially(ordering);
+
+	// after eliminate, only one zero indegree factor should remain
+	// TODO: this check needs to exist - verify that unit tests work when this check is in place
+	/*
+  if (factors.size() != 1) {
+    print();
+    throw(invalid_argument("LinearFactorGraph::eliminate: graph not empty after eliminate, ordering incomplete?"));
+  }
+	 */
+	return chordalBayesNet;
+}
+
+/* ************************************************************************* */
+/** optimize the linear factor graph                                          */ 
+/* ************************************************************************* */
+FGConfig LinearFactorGraph::optimize(const Ordering& ordering)
+{
+	// eliminate all nodes in the given ordering -> chordal Bayes net
+	ChordalBayesNet::shared_ptr chordalBayesNet = eliminate(ordering);
+
+	// calculate new configuration (using backsubstitution)
+	boost::shared_ptr<FGConfig> newConfig = chordalBayesNet->optimize();
+
+	return *newConfig;
+}
+
+/* ************************************************************************* */
+/** combine two factor graphs                                                 */ 
+/* ************************************************************************* */
+void LinearFactorGraph::combine(LinearFactorGraph &lfg){
+	for(const_iterator factor=lfg.factors.begin(); factor!=lfg.factors.end(); factor++){
+		push_back(*factor);
+	}
+}
+
+/* ************************************************************************* */
+/** combine two factor graphs                                                */ 
+/* ************************************************************************* */
+
+const LinearFactorGraph LinearFactorGraph::combine2(const LinearFactorGraph& lfg1,
+		const LinearFactorGraph& lfg2)  {
+	// create new linear factor graph
+	LinearFactorGraph fg;
+	// set the first linear factor graph
+	fg = lfg1;
+
+	// add the second factors in the graph
+	for(const_iterator factor=lfg2.factors.begin(); factor!=lfg2.factors.end(); factor++){
+		fg.push_back(*factor);
+	}
+
+	return fg;
+}
+
+
+/* ************************************************************************* */  
+// find all variables and their dimensions
+VariableSet LinearFactorGraph::variables() const {
+	VariableSet result;
+	BOOST_FOREACH(shared_factor factor,factors) {
+		VariableSet vs = factor->variables();
+		BOOST_FOREACH(Variable v,vs) result.insert(v);
+	}
+	return result;
+}
+
+/* ************************************************************************* */  
+LinearFactorGraph LinearFactorGraph::add_priors(double sigma) const {
+
+	// start with this factor graph
+	LinearFactorGraph result = *this;
+
+	// find all variables and their dimensions
+	VariableSet vs = variables();
+
+	// for each of the variables, add a prior
+	BOOST_FOREACH(Variable v,vs) {
+		size_t n = v.dim();
+		const string& key = v.key();
+		Matrix A = sigma*eye(n);
+		Vector b = zero(n);
+		shared_factor prior(new LinearFactor(key,A,b));
+		result.push_back(prior);
+	}
+	return result;
+}
+
+/* ************************************************************************* */  
+pair<Matrix,Vector> LinearFactorGraph::matrix(const Ordering& ordering) const {
+
+	// get all factors
+	LinearFactorSet found;
+	BOOST_FOREACH(shared_factor factor,factors)
+	found.insert(factor);
+
+	// combine them
+	MutableLinearFactor lf(found);
+
+	// Return Matrix and Vector
+	return lf.matrix(ordering);
+}
+
+/* ************************************************************************* */  
+Ordering LinearFactorGraph::getOrdering() const {
+	int * _symbolicMatrix;
+	int * _symbolicColumns;
+	int _symbolicLength;
+	int _symbolicColLength;
+	std::vector<int> _symbolicElimintationOrder;
+
+	Ordering result;
+
+	map<string, vector<int> > symbolToMatrixElement;
+	_symbolicLength = 0;
+	_symbolicColLength = 0;
+	int matrixRow = 0;
+	int symNRows = 0;
+	int symNCols = 0;
+	for(vector<LinearFactor::shared_ptr>::const_iterator it = begin(); it != end(); it++)
+	{
+		symNRows++;
+		for(map<string, Matrix>::const_iterator lit = (*it)->begin(); lit != (*it)->end(); lit++)
+		{
+			_symbolicLength++;
+			symbolToMatrixElement[lit->first].push_back(matrixRow);
+		}
+		matrixRow++;
+	}
+
+
+	symNCols =  (int)(symbolToMatrixElement.size());
+	_symbolicColLength = symNCols + 1;
+
+	if(symNCols == 0) {result.clear(); return result;}
+	//printf("%d %d\n", _symbolicLength, _symbolicColLength);
+
+	_symbolicMatrix = new int[_symbolicLength];
+	_symbolicColumns = new int[_symbolicColLength];
+
+	int count = 0;
+	_symbolicColumns[0] = 0;
+	int colCount = 1;
+	for(map<string, vector<int> >::iterator it = symbolToMatrixElement.begin(); it != symbolToMatrixElement.end(); it++)
+	{
+		for(vector<int>::iterator rit = it->second.begin(); rit != it->second.end(); rit++)
+			_symbolicMatrix[count++] = (*rit);
+		_symbolicColumns[colCount++] = count;
+	}
+
+	vector<string> initialOrder;
+	for(map<string, vector<int> >::iterator it = symbolToMatrixElement.begin(); it != symbolToMatrixElement.end(); it++)
+		initialOrder.push_back(it->first);
+
+	int * tempColumnOrdering = new int[_symbolicColLength];
+	for(int i = 0; i < _symbolicColLength; i++) tempColumnOrdering[i] = _symbolicColumns[i];
+	int * tempSymbolicMatrix = new int[_symbolicLength*30];
+	for(int i = 0; i < _symbolicLength; i++) tempSymbolicMatrix[i] = _symbolicMatrix[i];
+	int stats [COLAMD_STATS] ;
+	//for(int i = 0; i < _symbolicColLength; i++) printf("!%d\n", tempColumnOrdering[i]);
+	colamd(symNRows, symNCols, _symbolicLength*30, tempSymbolicMatrix, tempColumnOrdering, (double *) NULL, stats) ;
+	_symbolicElimintationOrder.clear();
+	for(int i = 0; i < _symbolicColLength; i++)
+		_symbolicElimintationOrder.push_back(tempColumnOrdering[i]);
+	//for(int i = 0; i < _symbolicColLength; i++) printf("!%d\n", tempColumnOrdering[i]);
+	delete [] tempColumnOrdering;
+	delete [] tempSymbolicMatrix;
+
+	result.clear();
+	for(vector<int>::const_iterator it = _symbolicElimintationOrder.begin(); it != _symbolicElimintationOrder.end(); it++)
+	{
+		//printf("%d\n", (*it));
+		if((*it) == -1) continue;
+		result.push_back(initialOrder[(*it)]);
+	}
+	delete [] _symbolicMatrix;
+	delete [] _symbolicColumns;
+
+	return result;
+}
+

cpp/LinearFactorGraph.h

@@ -0,0 +1,129 @@
+/**
+ * @file    LinearFactorGraph.h
+ * @brief   Linear Factor Graph where all factors are Gaussians
+ * @author  Kai Ni
+ * @author  Christian Potthast
+ * @author  Alireza Fathi
+ */ 
+
+// $Id: LinearFactorGraph.h,v 1.24 2009/08/14 20:48:51 acunning Exp $
+
+// \callgraph
+ 
+#pragma once
+
+#include <boost/shared_ptr.hpp>
+
+#include "LinearFactor.h"
+#include "FactorGraph.h"
+#include "ChordalBayesNet.h"
+
+namespace gtsam {
+
+  /** Linear Factor Graph where all factors are Gaussians */
+  class LinearFactorGraph : public FactorGraph<LinearFactor> {
+  public:
+
+    /**
+     * Default constructor 
+     */
+    LinearFactorGraph() {}
+
+    /**
+     * Constructor that receives a Chordal Bayes Net and returns a LinearFactorGraph
+     */
+    LinearFactorGraph(ChordalBayesNet::shared_ptr CBN);
+
+    /**
+     * given a chordal bayes net, sets the linear factor graph identical to that CBN
+     * FD: imperative !!
+     */
+    void setCBN(ChordalBayesNet::shared_ptr CBN);
+
+    /**
+     * This function returns the best ordering for this linear factor
+     * graph, computed using colamd
+     */
+    Ordering getOrdering() const;
+
+    /**
+     * find the separator, i.e. all the nodes that have at least one
+     * common factor with the given node. FD: not used AFAIK.
+     */
+    std::set<std::string> find_separator(const std::string& key) const;
+
+    /**
+     * find all the factors that involve the given node and remove them
+     * from the factor graph
+     * @param key the key for the given node
+     */
+    LinearFactorSet find_factors_and_remove(const std::string& key);
+
+    /**
+     * extract and combine all the factors that involve a given node
+     * @param key the key for the given node
+     * @return the combined linear factor
+     */
+    boost::shared_ptr<MutableLinearFactor> 
+      combine_factors(const std::string& key);
+
+    /**
+     * eliminate one node yielding a ConditionalGaussian
+     * Eliminates the factors from the factor graph through find_factors_and_remove
+     * and adds a new factor to the factor graph
+     */
+    ConditionalGaussian::shared_ptr eliminate_one(const std::string& key);
+	 
+    /**
+     * eliminate factor graph in place(!) in the given order, yielding
+     * a chordal Bayes net
+     */
+    ChordalBayesNet::shared_ptr eliminate(const Ordering& ordering);
+		
+    /**
+     * Same as eliminate but allows for passing an incomplete ordering
+     * that does not completely eliminate the graph
+     */
+    ChordalBayesNet::shared_ptr eliminate_partially(const Ordering& ordering);
+		
+    /**
+     * optimize a linear factor graph
+     * @param ordering fg in order
+     */
+    FGConfig optimize(const Ordering& ordering);
+
+    /**
+     * combine two factor graphs
+     * @param const &lfg1 Linear factor graph
+     * @param const &lfg2 Linear factor graph
+     * @return a new combined factor graph
+     */
+    static const LinearFactorGraph combine2(const LinearFactorGraph& lfg1,
+				     const LinearFactorGraph& lfg2 ) ;
+		
+    /**
+     * combine two factor graphs
+     * @param *lfg Linear factor graph
+     */
+    void combine(LinearFactorGraph &lfg);
+
+    /**
+     * Find all variables and their dimensions
+     * @return The set of all variable/dimension pairs
+     */
+    VariableSet variables() const;
+
+    /**
+     * Add zero-mean i.i.d. Gaussian prior terms to each variable
+     * @param sigma Standard deviation of Gaussian
+     */
+    LinearFactorGraph add_priors(double sigma) const;
+
+    /**
+     * Return (dense) matrix associated with factor graph
+     * @param ordering of variables needed for matrix column order
+     */
+    std::pair<Matrix,Vector> matrix (const Ordering& ordering) const;
+  };
+
+}

cpp/LinearFactorSet.h

@@ -0,0 +1,20 @@
+/**
+ * @file    LinearFactorSet.h
+ * @brief   Utility class: an STL set of linear factors, basically a wrappable typedef
+ * @author  Frank Dellaert
+ */
+
+#pragma once
+
+#include <set>
+#include <boost/shared_ptr.hpp>
+#include "LinearFactor.h"
+
+namespace gtsam {
+  
+  class LinearFactor;
+  
+  struct LinearFactorSet : std::set<boost::shared_ptr<LinearFactor> > {
+    LinearFactorSet() {}
+  };
+}

cpp/Makefile.am

@@ -0,0 +1,161 @@
+# on the Mac, libtool is called glibtool :-(
+# documentation see /opt/local/share/doc/libtool-1.5.26/manual.html
+if DARWIN
+  LIBTOOL = glibtool --tag=CXX
+else
+  LIBTOOL = libtool
+endif
+
+# the install destination
+includedir = ${prefix}/include/gtsam
+libdir = ${exec_prefix}/lib
+
+# library version
+current = 0  # The most recent interface number that this library implements. 
+revision = 0 # The implementation number of the current interface 
+age = 0 # The difference between the newest and oldest interfaces that	\
+this library implements. In other words, the library implements all	\
+the interface numbers in the range from number current - age to		\
+current.
+
+# from libtool manual:
+# Here are a set of rules to help you update your library version information:
+#   Start with version information of ‘0:0:0’ for each libtool library.
+#   - Update the version information only immediately before a public
+#   release of your software.
+#   - If the library source code has changed at all since the last
+#   update, then increment revision
+#   - If any interfaces have been added, removed, or changed since the
+#   last update, increment current, and set revision to 0.
+#   - If any interfaces have been added since the last public release,
+#   then increment age.
+#   - If any interfaces have been removed since the last public release,
+#   then set age to 0.
+version = $(current):$(revision):$(age)
+
+all: external_libs
+
+external_libs:
+	@echo Compiling CppUnitLite...; cd ../CppUnitLite; make all
+	@echo Compiling Colamd...;      cd ../colamd; make all
+
+clean : clean_external_libs
+	@echo Cleaning Cpp...;  rm -f *.o *.lo *.*~ libgtsam.la $(check_PROGRAMS)
+
+clean_external_libs:
+	@echo Cleaning CppUnitLite...; cd ../CppUnitLite; make clean
+	@echo Cleaning Colamd...; cd ../colamd; make clean
+
+# we specify the library in levels
+
+# basic Math
+sources = Vector.cpp svdcmp.cpp Matrix.cpp numericalDerivative.cpp 
+check_PROGRAMS = testVector testMatrix 
+testVector_SOURCES = testVector.cpp
+testMatrix_SOURCES = testMatrix.cpp
+
+testVector_LDADD = libgtsam.la
+testMatrix_LDADD = libgtsam.la
+
+# nodes
+sources += FGConfig.cpp Ordering.cpp LinearFactor.cpp NonlinearFactor.cpp ConditionalGaussian.cpp EqualityFactor.cpp DeltaFunction.cpp  
+check_PROGRAMS += testFGConfig testLinearFactor testConditionalGaussian testNonlinearFactor testEqualityFactor testDeltaFunction
+example = smallExample.cpp 
+testFGConfig_SOURCES            = testFGConfig.cpp
+testLinearFactor_SOURCES        = $(example) testLinearFactor.cpp
+testConditionalGaussian_SOURCES = $(example) testConditionalGaussian.cpp
+testNonlinearFactor_SOURCES     = $(example) testNonlinearFactor.cpp
+testEqualityFactor_SOURCES      = $(example) testEqualityFactor.cpp
+testDeltaFunction_SOURCES       = testDeltaFunction.cpp
+
+testFGConfig_LDADD            = libgtsam.la
+testLinearFactor_LDADD        = libgtsam.la
+testConditionalGaussian_LDADD = libgtsam.la
+testNonlinearFactor_LDADD     = libgtsam.la
+testEqualityFactor_LDADD	  = libgtsam.la
+testDeltaFunction_LDADD       = libgtsam.la
+
+# not the correct way, I'm sure: Kai ?
+timeLinearFactor: timeLinearFactor.cpp  
+timeLinearFactor: CXXFLAGS += -I /opt/local/include
+timeLinearFactor: LDFLAGS += -L.libs -lgtsam
+
+# graphs 
+sources += ConstrainedChordalBayesNet.cpp ChordalBayesNet.cpp FactorGraph.cpp LinearFactorGraph.cpp NonlinearFactorGraph.cpp 
+sources += ConstrainedNonlinearFactorGraph.cpp ConstrainedLinearFactorGraph.cpp 
+check_PROGRAMS += testChordalBayesNet testConstrainedChordalBayesNet testFactorgraph testLinearFactorGraph testNonlinearFactorGraph testConstrainedNonlinearFactorGraph testConstrainedLinearFactorGraph
+testChordalBayesNet_SOURCES      = $(example) testChordalBayesNet.cpp
+testConstrainedChordalBayesNet_SOURCES      = $(example) testConstrainedChordalBayesNet.cpp
+testFactorgraph_SOURCES          = testFactorgraph.cpp
+testLinearFactorGraph_SOURCES    = $(example) testLinearFactorGraph.cpp
+testNonlinearFactorGraph_SOURCES = $(example) testNonlinearFactorGraph.cpp
+testConstrainedNonlinearFactorGraph_SOURCES = $(example) testConstrainedNonlinearFactorGraph.cpp 
+testConstrainedLinearFactorGraph_SOURCES    = $(example) testConstrainedLinearFactorGraph.cpp
+
+testChordalBayesNet_LDADD      = libgtsam.la
+testFactorgraph_LDADD          = libgtsam.la
+testLinearFactorGraph_LDADD    = libgtsam.la
+testNonlinearFactorGraph_LDADD = libgtsam.la
+testConstrainedNonlinearFactorGraph_LDADD = libgtsam.la 
+testConstrainedLinearFactorGraph_LDADD    = libgtsam.la
+testConstrainedChordalBayesNet_LDADD      = libgtsam.la
+
+# geometry
+sources += Point2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp
+check_PROGRAMS += testPoint2 testPoint3 testRot3 testPose3 testCal3_S2
+testPoint2_SOURCES  = testPoint2.cpp
+testPoint3_SOURCES  = testPoint3.cpp
+testRot3_SOURCES    = testRot3.cpp
+testPose3_SOURCES   = testPose3.cpp
+testCal3_S2_SOURCES = testCal3_S2.cpp               
+
+testPoint2_LDADD    = libgtsam.la
+testPoint3_LDADD    = libgtsam.la
+testRot3_LDADD      = libgtsam.la
+testPose3_LDADD     = libgtsam.la
+testCal3_S2_LDADD   = libgtsam.la
+
+# simulated2D example
+sources += simulated2D.cpp 
+testSimulated2D_SOURCES = testSimulated2D.cpp
+testSimulated2D_LDADD = libgtsam.la
+check_PROGRAMS += testSimulated2D 
+
+# simulated3D example
+sources += Simulated3D.cpp 
+testSimulated3D_SOURCES = testSimulated3D.cpp
+testSimulated3D_LDADD = libgtsam.la
+check_PROGRAMS += testSimulated3D 
+
+# Visual SLAM
+sources += CalibratedCamera.cpp SimpleCamera.cpp VSLAMFactor.cpp
+check_PROGRAMS += testCalibratedCamera testSimpleCamera testVSLAMFactor
+testCalibratedCamera_SOURCES = testCalibratedCamera.cpp
+testCalibratedCamera_LDADD = libgtsam.la
+testSimpleCamera_SOURCES = testSimpleCamera.cpp
+testSimpleCamera_LDADD = libgtsam.la
+testVSLAMFactor_SOURCES = testVSLAMFactor.cpp
+testVSLAMFactor_LDADD = libgtsam.la
+
+
+# The header files will be installed in ~/include/gtsam
+headers = gtsam.h Value.h Factor.h LinearFactorSet.h Point2Prior.h Simulated2DOdometry.h Simulated2DMeasurement.h smallExample.h $(sources:.cpp=.h)
+
+# create both dynamic and static libraries
+AM_CXXFLAGS = -I$(boost) -fPIC
+lib_LTLIBRARIES = libgtsam.la
+libgtsam_la_SOURCES = $(sources)
+libgtsam_la_CPPFLAGS = $(AM_CXXFLAGS)
+libgtsam_la_LDFLAGS = -version-info $(version) -L../colamd -lcolamd #-lboost_serialization-mt
+
+# install the header files
+include_HEADERS = $(headers)
+
+AM_CXXFLAGS += -I.. 
+AM_LDFLAGS = -L../CppUnitLite -lCppUnitLite $(BOOST_LDFLAGS) $(boost_serialization) #-L. -lgtsam
+
+TESTS = $(check_PROGRAMS)
+
+# rule to run an executable
+%.run: % libgtsam.la
+	./$^

cpp/Matrix.cpp

@@ -0,0 +1,466 @@
+/**
+ * @file   Matrix.cpp
+ * @brief  matrix class
+ * @author Christian Potthast
+ */
+
+#include <stdarg.h>
+#include <string.h>
+#include <iomanip>
+
+#include <boost/foreach.hpp>
+#include <boost/numeric/ublas/lu.hpp>
+#include <boost/numeric/ublas/io.hpp>
+#include <boost/tuple/tuple.hpp>
+
+#include "Matrix.h"
+#include "Vector.h"
+#include "svdcmp.h"
+
+using namespace std;
+namespace ublas = boost::numeric::ublas;
+
+namespace gtsam {
+
+/* ************************************************************************* */
+Matrix Matrix_( size_t m, size_t n, const double* const data) {
+  Matrix A(m,n);
+  copy(data, data+m*n, A.data().begin());
+  return A;
+}
+
+/* ************************************************************************* */
+Matrix Matrix_( size_t m, size_t n, const Vector& v)
+{
+  Matrix A(m,n);
+  // column-wise copy
+  for( size_t j = 0, k=0  ; j < n ; j++)
+    for( size_t i = 0; i < m ; i++,k++)
+      A(i,j) = v(k);
+  return A;
+}
+
+/* ************************************************************************* */
+Matrix Matrix_(size_t m, size_t n, ...) {
+  Matrix A(m,n);
+  va_list ap;
+  va_start(ap, n);
+  for( size_t i = 0 ; i < m ; i++)
+    for( size_t j = 0 ; j < n ; j++) {
+      double value = va_arg(ap, double);
+      A(i,j) = value;
+    }
+  va_end(ap);
+  return A;
+}
+
+/* ************************************************************************* */
+/** create a matrix with value zero                                          */
+/* ************************************************************************* */
+Matrix zeros( size_t m, size_t n )
+{
+  Matrix A(m,n);
+  fill(A.data().begin(),A.data().end(),0.0);
+  return A;
+}
+
+/** 
+ * Identity matrix
+ */
+Matrix eye( size_t m, size_t n){
+  Matrix A = zeros(m,n);
+  for(size_t i = 0; i<min(m,n); i++) A(i,i)=1.0;
+  return A;
+}
+
+/* ************************************************************************* */ 
+/** Diagonal matrix values                                                   */
+/* ************************************************************************* */
+Matrix diag(const Vector& v) {
+  size_t m = v.size();
+  Matrix A = zeros(m,m);
+  for(size_t i = 0; i<m; i++) A(i,i)=v(i);
+  return A;
+}
+
+/* ************************************************************************* */
+/** Check if two matrizes are the same                                       */
+/* ************************************************************************* */
+bool equal_with_abs_tol(const Matrix& A, const Matrix& B, double tol){
+
+  size_t n1 = A.size2(), m1 = A.size1();
+  size_t n2 = B.size2(), m2 = B.size1();
+
+  if(m1!=m2 || n1!=n2) return false;
+
+  for(size_t i=0; i<m1; i++)
+    for(size_t j=0; j<n1; j++)
+      if(fabs(A(i,j) - B(i,j)) > tol)
+	return false;
+
+  return true;
+}
+
+/* ************************************************************************* */
+bool assert_equal(const Matrix& A, const Matrix& B, double tol) {
+
+  if (equal_with_abs_tol(A,B,tol)) return true;
+
+  size_t n1 = A.size2(), m1 = A.size1();
+  size_t n2 = B.size2(), m2 = B.size1();
+
+  cout << "not equal:" << endl;
+  print(A,"actual = ");
+  print(B,"expected = ");
+  if(m1!=m2 || n1!=n2)
+    cout << m1 << "," << n1 << " != " << m2 << "," << n2 << endl;
+  else
+    print(A-B, "actual - expected = ");
+  return false;
+}
+
+/* ************************************************************************ */
+/** negation                                                                */
+/* ************************************************************************ */
+/*
+Matrix operator-() const
+{
+  size_t m = size1(),n=size2();
+  Matrix M(m,n);
+  for(size_t i = 0; i < m; i++)
+    for(size_t j = 0; j < n; j++)
+      M(i,j) = -matrix_(i,j);
+  return M;  
+}
+*/
+
+/* ************************************************************************* */
+Vector Vector_(const Matrix& A)
+{
+  size_t m = A.size1(), n = A.size2();
+  Vector v(m*n);
+  for( size_t j = 0, k=0  ; j < n ; j++)
+    for( size_t i = 0; i < m ; i++,k++)
+      v(k) = A(i,j);
+  return v;
+}
+
+/* ************************************************************************* */
+void print(const Matrix& A, const string &s) {
+  size_t m = A.size1(), n = A.size2();
+
+  // print out all elements
+  cout << s << "[\n";
+  for( size_t i = 0 ; i < m ; i++) {
+    for( size_t j = 0 ; j < n ; j++) {
+      double aij = A(i,j);
+      cout << setw(9) << (fabs(aij)<1e-12 ? 0 : aij);
+    }
+    cout << endl;
+  }
+  cout << "]" << endl;
+}
+
+/* ************************************************************************* */
+Matrix sub(const Matrix& A, size_t i1, size_t i2, size_t j1, size_t j2) {
+  // using ublas is slower:
+  // Matrix B = Matrix(ublas::project(A,ublas::range(i1,i2+1),ublas::range(j1,j2+1)));
+  size_t m=i2-i1, n=j2-j1;
+  Matrix B(m,n);
+  for (size_t i=i1,k=0;i<i2;i++,k++)
+    memcpy(&B(k,0),&A(i,j1),n*sizeof(double));
+  return B;
+}
+
+/* ************************************************************************* */
+void solve(Matrix& A, Matrix& B)
+{
+  // perform LU-factorization
+  ublas::lu_factorize(A);
+
+  // backsubstitute
+  ublas::lu_substitute<const Matrix, Matrix>(A, B);
+}
+
+/* ************************************************************************* */
+Matrix inverse(const Matrix& originalA)
+{
+  Matrix A(originalA);
+  Matrix B = eye(A.size2());
+  solve(A,B);
+  return B;
+}
+
+/* ************************************************************************* */
+/** Householder QR factorization, Golub & Van Loan p 224, explicit version    */
+/* ************************************************************************* */
+pair<Matrix,Matrix> qr(const Matrix& A) {
+
+  const size_t m = A.size1(), n = A.size2(), kprime = min(m,n);
+  
+  Matrix Q=eye(m,m),R(A);
+  Vector v(m);
+
+  // loop over the kprime first columns 
+  for(size_t j=0; j < kprime; j++){
+
+    // we now work on the matrix (m-j)*(n-j) matrix A(j:end,j:end)
+    const size_t mm=m-j;
+
+    // copy column from matrix to xjm, i.e. x(j:m) = A(j:m,j)
+    Vector xjm(mm);
+    for(size_t k = 0 ; k < mm; k++)
+      xjm(k) = R(j+k, j);  
+        
+    // calculate the Householder vector v
+    double beta; Vector vjm;
+    boost::tie(beta,vjm) = house(xjm);
+
+    // pad with zeros to get m-dimensional vector v
+    for(size_t k = 0 ; k < m; k++) 
+      v(k) = k<j ? 0.0 : vjm(k-j);
+
+    // create Householder reflection matrix Qj = I-beta*v*v'
+    Matrix Qj = eye(m) - beta * Matrix(outer_prod(v,v));
+
+    R = Qj * R; // update R
+    Q = Q * Qj; // update Q
+
+  } // column j
+
+  return make_pair(Q,R);
+}
+
+/* ************************************************************************* */
+/** Imperative version of Householder rank 1 update
+ * i.e. do outer product update A = (I-beta vv')*A = A - v*(beta*A'*v)' = A - v*w'
+ * but only in relevant part, from row j onwards
+ * If called from householder_ does actually more work as first j columns 
+ * will not be touched. However, is called from LinearFactor.eliminate
+ * on a number of different matrices for which all columns change.
+ */
+/* ************************************************************************* */
+void householder_update(Matrix &A, int j, double beta, const Vector& vjm) {
+
+  const size_t m = A.size1(), n = A.size2();
+
+  // elegant but slow: A -= Matrix(outer_prod(v,beta*trans(A)*v));
+  // faster code below
+
+  // w = beta*transpose(A(j:m,:))*v(j:m)
+  Vector w(n);
+  for( size_t c = 0; c < n; c++) {
+    w(c) = 0.0;
+    // dangerous as relies on row-major scheme
+    const double *a = &A(j,c), * const v = &vjm(0);
+    for( size_t r=j, s=0 ; r < m ; r++, s++, a+=n )
+      // w(c) += A(r,c) * vjm(r-j)
+      w(c) += (*a) * v[s];
+    w(c) *= beta;
+  }
+
+  // rank 1 update A(j:m,:) -= v(j:m)*w'
+  for( size_t c = 0 ; c < n; c++) {
+    double wc = w(c);
+    double *a = &A(j,c); const double * const v =&vjm(0);
+    for( size_t r=j, s=0 ; r < m ; r++, s++, a+=n )
+      // A(r,c) -= vjm(r-j) * wjn(c-j);
+      (*a) -= v[s] * wc;
+  }
+}
+
+/* ************************************************************************* */
+/** Imperative version of Householder QR factorization, Golub & Van Loan p 224
+ * version with Householder vectors below diagonal, as in GVL
+ */
+/* ************************************************************************* */
+void householder_(Matrix &A, size_t k) 
+{
+  const size_t m = A.size1(), n = A.size2(), kprime = min(k,min(m,n));
+  
+  // loop over the kprime first columns 
+  for(size_t j=0; j < kprime; j++){
+    // below, the indices r,c always refer to original A
+
+    // copy column from matrix to xjm, i.e. x(j:m) = A(j:m,j)
+    Vector xjm(m-j);
+    for(size_t r = j ; r < m; r++)
+      xjm(r-j) = A(r,j);
+        
+    // calculate the Householder vector
+    double beta; Vector vjm;
+    boost::tie(beta,vjm) = house(xjm);
+
+    // do outer product update A = (I-beta vv')*A = A - v*(beta*A'*v)' = A - v*w'
+    householder_update(A, j, beta, vjm) ;
+
+    // the Householder vector is copied in the zeroed out part
+    for( size_t r = j+1 ; r < m ; r++ )
+      A(r,j) = vjm(r-j);
+
+  } // column j
+}
+
+/* ************************************************************************* */
+/** version with zeros below diagonal                                        */
+/* ************************************************************************* */
+void householder(Matrix &A, size_t k) {
+  householder_(A,k);
+  const size_t m = A.size1(), n = A.size2(), kprime = min(k,min(m,n));
+  for(size_t j=0; j < kprime; j++)
+    for( size_t i = j+1 ; i < m ; i++ )
+      A(i,j) = 0.0;
+}
+
+/* ************************************************************************* */
+Vector backsubstitution(const Matrix& R, const Vector& b)
+{
+  // result vector
+  Vector result(b.size());
+
+  double tmp = 0.0;
+  double div = 0.0;
+
+  int m = R.size1(), n = R.size2(), cols=n;
+
+  // check each row for non zero values
+  for( size_t i = m ; i > 0 ; i--){
+    cols--;
+    int j = n;
+
+    div = R(i-1, cols);
+
+    for( int ii = i ; ii < n ; ii++ ){
+      j = j - 1;
+      tmp = tmp + R(i-1,j) * result(j);
+    }
+    // assigne the result vector
+    result(i-1) = (b(i-1) - tmp) / div;
+    tmp = 0.0;
+  }
+
+  return result;
+}
+
+/* ************************************************************************* */
+Matrix stack(size_t nrMatrices, ...)
+{
+  int dimA1 = 0;
+  int dimA2 = 0;
+  va_list ap;
+  va_start(ap, nrMatrices);
+  for(size_t i = 0 ; i < nrMatrices ; i++) {
+    Matrix *M = va_arg(ap, Matrix *);
+    dimA1 += M->size1();
+    dimA2 =  M->size2();  // TODO: should check if all the same !
+  }
+  va_end(ap);
+  va_start(ap, nrMatrices);
+  Matrix A(dimA1, dimA2);
+  int vindex = 0;
+  for( size_t i = 0 ; i < nrMatrices ; i++) {
+    Matrix *M = va_arg(ap, Matrix *);
+    for(size_t d1 = 0; d1 < M->size1(); d1++)
+      for(size_t d2 = 0; d2 < M->size2(); d2++)
+	A(vindex+d1, d2) = (*M)(d1, d2);
+    vindex += M->size1();
+  }  
+
+  return A;
+}
+
+/* ************************************************************************* */
+Matrix collect(vector<const Matrix *> matrices)
+{
+  int dimA1 = 0;
+  int dimA2 = 0;
+  BOOST_FOREACH(const Matrix* M, matrices) {
+    dimA1 =  M->size1();  // TODO: should check if all the same !
+    dimA2 += M->size2();
+  }
+  Matrix A(dimA1, dimA2);
+  int hindex = 0;
+  BOOST_FOREACH(const Matrix* M, matrices) {
+    for(size_t d1 = 0; d1 < M->size1(); d1++)
+      for(size_t d2 = 0; d2 < M->size2(); d2++)
+	A(d1, d2+hindex) = (*M)(d1, d2);
+    hindex += M->size2();
+  }  
+
+  return A;
+}
+
+/* ************************************************************************* */
+Matrix collect(size_t nrMatrices, ...)
+{
+  vector<const Matrix *> matrices;
+  va_list ap;
+  va_start(ap, nrMatrices);
+  for( size_t i = 0 ; i < nrMatrices ; i++) {
+    Matrix *M = va_arg(ap, Matrix *);
+    matrices.push_back(M);
+  }
+return collect(matrices);
+}
+
+/* ************************************************************************* */
+Matrix skewSymmetric(double wx, double wy, double wz)
+{
+  return Matrix_(3,3,
+		  0.0, -wz, +wy,
+		  +wz, 0.0, -wx,
+		  -wy, +wx, 0.0);
+}
+
+/* ************************************************************************* */
+/** Numerical Recipes in C wrappers                                          
+ *  create Numerical Recipes in C structure
+ * pointers are subtracted by one to provide base 1 access 
+ */
+/* ************************************************************************* */
+double** createNRC(Matrix& A) {
+  const size_t m=A.size1();
+  double** a = new double* [m];
+  for(size_t i = 0; i < m; i++) 
+    a[i] = &A(i,0)-1;
+  return a;
+}
+
+/* ************************************************************************* */
+/** SVD                                                                      */
+/* ************************************************************************* */
+
+// version with in place modification of A
+void svd(Matrix& A, Vector& s, Matrix& V) {
+
+  const size_t m=A.size1(), n=A.size2();
+
+  double * q = new double[n]; // singular values
+
+  // create NRC matrices, u is in place
+  V = Matrix(n,n);
+  double **u = createNRC(A), **v = createNRC(V);
+
+  // perform SVD
+  // need to pass pointer - 1 in NRC routines so u[1][1] is first element !
+  svdcmp(u-1,m,n,q-1,v-1);
+	
+  // copy singular values back
+  s.resize(n);
+  copy(q,q+n,s.begin());
+
+  delete[] v;
+  delete[] q; //switched to array delete
+  delete[] u;
+
+}
+
+/* ************************************************************************* */
+void svd(const Matrix& A, Matrix& U, Vector& s, Matrix& V) {
+  U = A;      // copy
+  svd(U,s,V); // call in-place version
+}
+
+/* ************************************************************************* */
+
+} // namespace gtsam

cpp/Matrix.h

@@ -0,0 +1,201 @@
+/**
+ * @file    Matrix.h
+ * @brief   typedef and functions to augment Boost's ublas::matrix<double>
+ * @author  Christian Potthast
+ * @author  Kai Ni
+ * @author  Frank Dellaert
+ */
+
+// \callgraph
+
+#pragma once
+
+#include <boost/numeric/ublas/matrix.hpp>
+#include "Vector.h"
+
+/**
+ * Vector is a *global* typedef
+ * wrap-matlab does this typedef as well
+ * we use the default < double,row_major,unbounded_array<double> >
+ */
+
+#if ! defined (MEX_H)
+typedef boost::numeric::ublas::matrix<double> Matrix;
+#endif
+
+namespace gtsam {
+
+/**
+ *  constructor with size and initial data, row order !
+ */
+Matrix Matrix_( size_t m, size_t n, const double* const data);
+
+/**
+ *  constructor with size and vector data, column order !!!
+ */
+Matrix Matrix_( size_t m, size_t n, const Vector& v);
+
+/**
+ *  nice constructor, dangerous as number of arguments must be exactly right
+ *  and you have to pass doubles !!! always use 0.0 never 0
+*/
+Matrix Matrix_(size_t m, size_t n, ...);
+
+/**
+ * MATLAB like constructors
+ */
+Matrix zeros(size_t m, size_t n);
+Matrix eye(size_t m, size_t n);
+inline Matrix eye( size_t m ) { return eye(m,m); }
+Matrix diag(const Vector& v);
+
+/**
+ * equals with an tolerance
+ */
+bool equal_with_abs_tol(const Matrix& A, const Matrix& B, double tol = 1e-9);
+
+/**
+ * equality is just equal_with_abs_tol 1e-9
+ */
+inline bool operator==(const Matrix& A, const Matrix& B) {
+  return equal_with_abs_tol(A,B,1e-9);
+}
+
+/**
+ * inequality 
+ */
+inline bool operator!=(const Matrix& A, const Matrix& B) {
+  return !(A==B);
+ }
+
+/**
+ * equals with an tolerance, prints out message if unequal
+ */
+bool assert_equal(const Matrix& A, const Matrix& B, double tol = 1e-9);
+
+/**
+ * overload * for matrix-vector multiplication (as BOOST does not)
+ */
+inline Vector operator*(const Matrix& A, const Vector & v)  { 
+  if (A.size2()!=v.size()) throw(std::invalid_argument("Matrix operator* : A.n!=v.size"));
+  return Vector(prod(A,v));
+}
+
+/**
+ * overload * for matrix multiplication (as BOOST does not)
+ */
+inline Matrix operator*(const Matrix& A, const Matrix& B) {
+  if (A.size2()!=B.size1()) throw(std::invalid_argument("Matrix operator* : A.n!=B.m"));
+  return prod(A,B);
+}
+
+/**
+ * convert to column vector, column order !!!
+ */
+Vector Vector_(const Matrix& A);
+
+/**
+ * print a matrix
+ */
+void print(const Matrix& A, const std::string& s = "");
+
+/**
+ * extract submatrix, slice semantics, i.e. range = [i1,i2[ excluding i2
+ * @param A matrix
+ * @param i1 first row index
+ * @param i2 last  row index + 1
+ * @param j1 first col index
+ * @param j2 last  col index + 1
+ * @return submatrix A(i1:i2,j1:j2)
+ */
+Matrix sub(const Matrix& A, size_t i1, size_t i2, size_t j1, size_t j2);
+
+// left multiply with scalar
+//inline Matrix operator*(double s, const Matrix& A) { return A*s;}
+
+/**
+ * solve AX=B via in-place Lu factorization and backsubstitution
+ * After calling, A contains LU, B the solved RHS vectors
+ */
+void solve(Matrix& A, Matrix& B);
+
+/**
+ * invert A
+ */
+Matrix inverse(const Matrix& A);
+
+/**
+ * QR factorization, inefficient, best use imperative householder below
+ * m*n matrix -> m*m Q, m*n R
+ * @param A a matrix
+ * @return <Q,R> rotation matrix Q, upper triangular R
+ */ 
+std::pair<Matrix,Matrix> qr(const Matrix& A);
+
+/**
+ * Imperative version of Householder rank 1 update
+ */
+void householder_update(Matrix &A, int j, double beta, const Vector& vjm);
+
+/**
+ * Householder tranformation, Householder vectors below diagonal
+ * @param k number of columns to zero out below diagonal
+ * @param A matrix
+ * @return nothing: in place !!!
+ */
+void householder_(Matrix& A, size_t k);
+
+/**
+ * Householder tranformation, zeros below diagonal
+ * @param k number of columns to zero out below diagonal
+ * @param A matrix
+ * @return nothing: in place !!!
+ */
+void householder(Matrix& A, size_t k);
+
+/**
+ * backsubstitution
+ * @param R an upper triangular matrix
+ * @param b a RHS vector
+ * @return the solution of Rx=b
+ */ 
+Vector backsubstitution(const Matrix& R, const Vector& b);
+
+/**
+ * create a matrix by stacking other matrices 
+ */
+Matrix stack(size_t nrMatrices, ...);
+Matrix collect(std::vector<const Matrix *> matrices);
+Matrix collect(size_t nrMatrices, ...);
+
+/**
+ * skew symmetric matrix returns this:
+ *   0  -wz   wy
+ *  wz    0  -wx
+ * -wy   wx    0
+ * @param wx 3 dimensional vector
+ * @param wy
+ * @param wz
+ * @return a 3*3 skew symmetric matrix
+*/
+Matrix skewSymmetric(double wx, double wy, double wz);
+inline Matrix skewSymmetric(const Vector& w) { return skewSymmetric(w(0),w(1),w(2));}
+
+/**
+ * SVD computes economy SVD A=U*S*V'
+ * @param A an m*n matrix
+ * @param U output argument: m*n matrix
+ * @param S output argument: n-dim vector of singular values, *not* sorted !!!
+ * @param V output argument: n*n matrix
+ */ 
+void svd(const Matrix& A, Matrix& U, Vector& S, Matrix& V);
+
+// in-place version
+void svd(Matrix& A, Vector& S, Matrix& V);
+
+// macro for unit tests
+#define EQUALITY(expected,actual)\
+  { if (!assert_equal(expected,actual)) \
+    result_.addFailure(Failure(name_, __FILE__, __LINE__, #expected, #actual)); }
+
+} // namespace gtsam

cpp/NonlinearFactor.cpp

@@ -0,0 +1,119 @@
+/**
+ * @file    NonlinearFactor.cpp
+ * @brief   nonlinear factor versions which assume a Gaussian noise on a measurement 
+ * @brief   predicted by a non-linear function h nonlinearFactor
+ * @author  Kai Ni
+ * @author  Carlos Nieto
+ * @author  Christian Potthast
+ * @author  Frank Dellaert
+ */
+ 
+#include "NonlinearFactor.h"
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+NonlinearFactor::NonlinearFactor(const Vector& z,	                
+                                 const double sigma) 
+{ 
+  z_ =  z; 
+  sigma_ = sigma;
+}
+
+/* ************************************************************************* */
+NonlinearFactor1::NonlinearFactor1(const Vector& z,		                    
+                                   const double sigma,	                       
+                                   Vector (*h)(const Vector&),                      
+                                   const string& key1,                   
+                                   Matrix (*H)(const Vector&)) 
+  : NonlinearFactor(z, sigma), h_(h), key1_(key1), H_(H)
+{
+  keys_.push_front(key1);
+}
+
+/* ************************************************************************* */
+void NonlinearFactor1::print(const string& s) const {
+  cout << "NonLinearFactor1 " << s << endl;
+}
+
+/* ************************************************************************* */
+LinearFactor::shared_ptr NonlinearFactor1::linearize(const FGConfig& c) const 
+{
+  // get argument 1 from config
+  Vector x1 = c[key1_];
+
+  // Jacobian A = H(x1)/sigma
+  Matrix A = H_(x1)/sigma_;
+
+  // Right-hand-side b = error(c) = (z - h(x1))/sigma
+  Vector b = (z_ - h_(x1))/sigma_;
+
+  LinearFactor::shared_ptr p(new LinearFactor(key1_,A,b));
+  return p;
+}
+
+
+/* ************************************************************************* */
+/** http://artis.imag.fr/~Xavier.Decoret/resources/C++/operator==.html       */
+/* ************************************************************************* */
+bool NonlinearFactor1::equals(const Factor& f, double tol) const {
+  const NonlinearFactor1* p = dynamic_cast<const NonlinearFactor1*>(&f);
+  if (p == NULL) return false;
+  return NonlinearFactor::equals(*p,tol)
+    && (h_    == p->h_)
+    && (key1_ == p->key1_)
+    && (H_    == p->H_);
+}
+
+/* ************************************************************************* */
+NonlinearFactor2::NonlinearFactor2(const Vector& z,       
+				   const double sigma,
+				   Vector (*h)(const Vector&, const Vector&),
+				   const string& key1,
+                                   Matrix (*H1)(const Vector&, const Vector&),
+				   const string& key2,
+                                   Matrix (*H2)(const Vector&, const Vector&)
+				   ) 
+  : NonlinearFactor(z, sigma), h_(h), key1_(key1), H1_(H1), key2_(key2), H2_(H2)
+{ 
+  keys_.push_front(key1);
+  keys_.push_front(key2);
+}
+
+/* ************************************************************************* */
+void NonlinearFactor2::print(const string& s) const {
+  cout << "NonLinearFactor2 " << s << endl;
+}
+
+/* ************************************************************************* */
+LinearFactor::shared_ptr NonlinearFactor2::linearize(const FGConfig& c) const
+{
+  // get arguments from config
+  Vector x1 = c[key1_];
+  Vector x2 = c[key2_];
+
+  // Jacobian A = H(x)/sigma
+  Matrix A1 = H1_(x1,x2)/sigma_;
+  Matrix A2 = H2_(x1,x2)/sigma_;
+
+  // Right-hand-side b = (z - h(x))/sigma
+  Vector b = (z_ - h_(x1,x2))/sigma_;
+
+  LinearFactor::shared_ptr p(new LinearFactor(key1_,A1,key2_,A2,b));
+  return p;
+}
+
+/* ************************************************************************* */
+bool NonlinearFactor2::equals(const Factor& f, double tol) const {
+  const NonlinearFactor2* p = dynamic_cast<const NonlinearFactor2*>(&f);
+  if (p == NULL) return false;
+  return NonlinearFactor::equals(*p,tol)
+    && (h_    == p->h_)
+    && (key1_ == p->key1_)
+    && (H2_   == p->H1_)
+    && (key2_ == p->key2_)
+    && (H1_   == p->H2_);
+}
+
+/* ************************************************************************* */

cpp/NonlinearFactor.h

@@ -0,0 +1,156 @@
+/**
+ * @file    NonlinearFactor.h
+ * @brief   Non-linear factor class
+ * @author  Kai Ni
+ * @author  Carlos Nieto
+ * @author  Christian Potthast
+ * @author  Frank Dellaert
+ */
+
+
+// \callgraph
+
+#pragma once
+
+#include <list>
+
+#include <boost/shared_ptr.hpp>
+
+#include "Factor.h"
+#include "Matrix.h"
+#include "LinearFactor.h"
+
+/**
+ * Base Class
+ * Just has the measurement and noise model
+ */ 
+
+namespace gtsam {
+
+	// forward declaration of LinearFactor
+	//class LinearFactor;
+	//typedef boost::shared_ptr<LinearFactor> shared_ptr;
+
+  /**
+   * Nonlinear factor which assume Gaussian noise on a measurement
+   * predicted by a non-linear function h
+   */
+  class NonlinearFactor : public Factor
+  {
+  protected:
+
+    Vector z_;     // measurement
+    double sigma_; // noise standard deviation
+    std::list<std::string> keys_; // keys
+		
+  public:
+
+    /** Constructor */
+    NonlinearFactor(const Vector& z,	     // the measurement
+		    const double sigma);     // the variance
+
+    /** Vector of errors */
+    virtual Vector error_vector(const FGConfig& c) const = 0;
+
+    /** linearize to a LinearFactor */
+    virtual boost::shared_ptr<LinearFactor> linearize(const FGConfig& c) const = 0;
+
+    /** get functions */
+    double get_sigma() const {return sigma_;}
+    Vector get_measurement() const {return z_;}
+    std::list<std::string> get_keys() const {return keys_;}
+    void set_keys(std::list<std::string> keys) {keys_ = keys;}
+
+    /** calculate the error of the factor */
+    double error(const FGConfig& c) const {
+      Vector e = error_vector(c) / sigma_;
+      return 0.5 * inner_prod(trans(e),e);
+    };
+		
+    /** get the size of the factor */
+    std::size_t size() const{return keys_.size();}
+
+    /** Check if two NonlinearFactor objects are equal */
+    bool equals(const NonlinearFactor& other, double tol=1e-9) const {
+      return equal_with_abs_tol(z_,other.z_,tol) && fabs(sigma_ - other.sigma_)<=tol;
+    }
+
+    /** dump the information of the factor into a string **/
+    std::string dump() const{return "";}
+  };
+
+
+  /* ************************************************************************* */
+  /* a Gaussian nonlinear factor that takes 1 parameter                        */
+  /* ************************************************************************* */
+
+  class NonlinearFactor1 : public NonlinearFactor {
+  public:
+
+    /** Constructor */
+    NonlinearFactor1(const Vector& z,		  // measurement
+		     const double sigma,	  // variance
+		     Vector (*h)(const Vector&),  // measurement function
+		     const std::string& key1,     // key of the variable
+		     Matrix (*H)(const Vector&)); // derivative of the measurement function
+
+    void print(const std::string& s = "") const;
+
+    Vector (*h_)(const Vector&);
+    std::string key1_;
+    Matrix (*H_)(const Vector&);
+
+    /** error function */
+    inline Vector error_vector(const FGConfig& c) const { 
+      return z_ - h_(c[key1_]);
+    }
+
+    /** linearize a non-linearFactor1 to get a linearFactor1 */
+    boost::shared_ptr<LinearFactor> linearize(const FGConfig& c) const;
+
+    /** Check if two factors are equal */
+    bool equals(const Factor& f, double tol=1e-9) const;
+
+    std::string dump() {return "";}
+  };
+
+  /* ************************************************************************* */
+  /* A Gaussian nonlinear factor that takes 2 parameters                       */
+  /* ************************************************************************* */
+
+  class NonlinearFactor2 : public NonlinearFactor {
+  public:
+
+    /** Constructor */
+    NonlinearFactor2(const Vector& z,	                         // the measurement
+		     const double sigma,	                 // the variance
+		     Vector (*h)(const Vector&, const Vector&),  // the measurement function
+		     const std::string& key1,                    // key of the first variable
+		     Matrix (*H1)(const Vector&, const Vector&), // derivative of h in first variable
+		     const std::string& key2,                    // key of the second variable
+		     Matrix (*H2)(const Vector&, const Vector&));// derivative of h in second variable
+			
+    void print(const std::string& s = "") const;
+
+    Vector (*h_)(const Vector&, const Vector&);
+    std::string key1_;
+    Matrix (*H1_)(const Vector&, const Vector&);
+    std::string key2_;
+    Matrix (*H2_)(const Vector&, const Vector&);
+		
+    /** error function */
+    inline Vector error_vector(const FGConfig& c) const { 
+      return z_ - h_(c[key1_], c[key2_]); 
+    }
+
+    /** Linearize a non-linearFactor2 to get a linearFactor2 */
+    boost::shared_ptr<LinearFactor> linearize(const FGConfig& c) const;
+
+    /** Check if two factors are equal */
+    bool equals(const Factor& f, double tol=1e-9) const;
+
+    std::string dump() const{return "";};
+  };
+
+  /* ************************************************************************* */
+}

cpp/NonlinearFactorGraph.cpp

@@ -0,0 +1,254 @@
+/**
+ * @file    NonlinearFactorGraph.cpp
+ * @brief   Factor Graph Constsiting of non-linear factors
+ * @brief   nonlinearFactorGraph
+ * @author  Frank Dellaert
+ * @author  Carlos Nieto
+ * @author  Christian Potthast
+ */
+
+#include <math.h>
+#include <climits>
+#include <stdexcept>
+#include <boost/tuple/tuple.hpp>
+#include "NonlinearFactorGraph.h" 
+
+using namespace std;
+namespace gtsam {
+
+/* ************************************************************************* */
+LinearFactorGraph NonlinearFactorGraph::linearize(const FGConfig& config) const
+{
+	// TODO speed up the function either by returning a pointer or by
+	// returning the linearisation as a second argument and returning
+	// the reference
+
+	// create an empty linear FG
+	LinearFactorGraph linearFG;
+
+	// linearize all factors
+	for(const_iterator factor=factors.begin(); factor<factors.end(); factor++){
+		LinearFactor::shared_ptr lf = (*factor)->linearize(config);
+		linearFG.push_back(lf);
+	}
+
+	return linearFG;
+}
+
+/* ************************************************************************* */
+double calculate_error (const NonlinearFactorGraph& fg, const FGConfig& config, int verbosity) {
+	double newError = fg.error(config);
+	if (verbosity>=1) cout << "error: " << newError << endl;
+	return newError;
+}
+
+/* ************************************************************************* */
+bool check_convergence (double relativeErrorTreshold,
+		double absoluteErrorTreshold,
+		double currentError, double newError,
+		int verbosity)
+{
+	// check if diverges
+	double absoluteDecrease = currentError - newError;
+	if (verbosity>=2) cout << "absoluteDecrease: " << absoluteDecrease << endl;
+	if (absoluteDecrease<0)
+		throw overflow_error("NonlinearFactorGraph::optimize: error increased, diverges.");
+
+	// calculate relative error decrease and update currentError
+	double relativeDecrease = absoluteDecrease/currentError;
+	if (verbosity>=2) cout << "relativeDecrease: " << relativeDecrease << endl;
+	bool converged =
+			(relativeDecrease < relativeErrorTreshold) ||
+			(absoluteDecrease < absoluteErrorTreshold);
+	if (verbosity>=1 && converged) cout << "converged" << endl;
+	return converged;
+}
+
+/* ************************************************************************* */
+// linearize and solve, return delta config (TODO: return updated)
+/* ************************************************************************* */
+FGConfig NonlinearFactorGraph::iterate
+(const FGConfig& config, const Ordering& ordering) const
+{
+	LinearFactorGraph linear = linearize(config);   // linearize all factors
+	return linear.optimize(ordering);
+}
+
+/* ************************************************************************* */
+// One iteration of optimize, imperative :-(
+/* ************************************************************************* */
+double NonlinearFactorGraph::iterate
+(FGConfig& config, const Ordering& ordering, int verbosity) const
+{
+	FGConfig delta = iterate(config, ordering); // linearize and solve
+	if (verbosity>=4) delta.print("delta");     // maybe show output
+	config += delta;                            // update config
+	if (verbosity>=3) config.print("config");   // maybe show output
+	return calculate_error(*this,config,verbosity);
+}
+
+/* ************************************************************************* */
+Ordering NonlinearFactorGraph::getOrdering(FGConfig& config)
+{
+	LinearFactorGraph lfg = linearize(config);
+	return lfg.getOrdering();
+}
+/* ************************************************************************* */
+void NonlinearFactorGraph::optimize(FGConfig& config,
+		const Ordering& ordering,
+		double relativeErrorTreshold,
+		double absoluteErrorTreshold,
+		int verbosity) const
+		{
+	bool converged = false;
+	double currentError = calculate_error(*this,config, verbosity);
+
+	// while not converged
+	while(!converged) {
+		double newError = iterate(config, ordering, verbosity); // linearize, solve, update
+		converged = gtsam::check_convergence(relativeErrorTreshold, absoluteErrorTreshold,
+				currentError, newError, verbosity);
+		currentError = newError;
+	}
+		}
+
+/* ************************************************************************* */
+bool NonlinearFactorGraph::check_convergence(const FGConfig& config1,
+		const FGConfig& config2,
+		double relativeErrorTreshold,
+		double absoluteErrorTreshold,
+		int verbosity)
+{
+	double currentError = calculate_error(*this, config1, verbosity);
+	double newError     = calculate_error(*this, config2, verbosity);
+	return gtsam::check_convergence(relativeErrorTreshold, absoluteErrorTreshold,
+			currentError, newError, verbosity);
+
+}
+
+
+/* ************************************************************************* */
+// One attempt at a tempered Gauss-Newton step with given lambda
+/* ************************************************************************* */
+pair<FGConfig,double> NonlinearFactorGraph::try_lambda
+(const FGConfig& config, const LinearFactorGraph& linear,
+		double lambda, const Ordering& ordering, int verbosity) const
+		{
+	if (verbosity>=1)
+		cout << "trying lambda = " << lambda << endl;
+	LinearFactorGraph damped =
+			linear.add_priors(sqrt(lambda));                 // add prior-factors
+	if (verbosity>=7) damped.print("damped");
+	FGConfig delta = damped.optimize(ordering);        // solve
+	if (verbosity>=5) delta.print("delta");
+	FGConfig newConfig = config + delta;               // update config
+	if (verbosity>=5) newConfig.print("config");
+	double newError =                                  // calculate...
+			calculate_error(*this,newConfig,verbosity>=4);   // ...new error
+	return make_pair(newConfig,newError);              // return config and error
+		}
+
+/* ************************************************************************* */
+// One iteration of Levenberg Marquardt, imperative :-(
+/* ************************************************************************* */
+double NonlinearFactorGraph::iterateLM
+(FGConfig& config, double currentError, double& lambda, double lambdaFactor,
+		const Ordering& ordering, int verbosity) const
+		{
+	FGConfig newConfig;
+	double newError;
+	LinearFactorGraph linear = linearize(config); // linearize all factors once
+	if (verbosity>=6) linear.print("linear");
+
+	// try lambda steps with successively larger lambda until we achieve descent
+	boost::tie(newConfig,newError) = try_lambda(config, linear, lambda, ordering,verbosity);
+	while (newError > currentError) {
+		lambda = lambda * lambdaFactor; // be more cautious
+		boost::tie(newConfig,newError) = try_lambda(config, linear, lambda, ordering,verbosity);
+	}
+
+	// next time, let's be more adventerous
+	lambda = lambda / lambdaFactor;
+
+	// return result of this iteration
+	config = newConfig;
+	if (verbosity>=4) config.print("config"); // maybe show output
+	if (verbosity>=1) cout << "error: " << newError << endl;
+	return newError;
+		}
+
+/* ************************************************************************* */
+void NonlinearFactorGraph::optimizeLM(FGConfig& config,
+		const Ordering& ordering,
+		double relativeErrorTreshold,
+		double absoluteErrorTreshold,
+		int verbosity,
+		double lambda0,
+		double lambdaFactor) const
+		{
+	double lambda = lambda0;
+	bool converged = false;
+	double currentError = calculate_error(*this,config, verbosity);
+
+	if (verbosity>=4) config.print("config"); // maybe show output
+
+	// while not converged
+	while(!converged) {
+		// linearize, solve, update
+		double newError = iterateLM(config, currentError, lambda, lambdaFactor, ordering, verbosity);
+		converged = gtsam::check_convergence(relativeErrorTreshold, absoluteErrorTreshold,
+				currentError, newError, verbosity);
+		currentError = newError;
+	}
+		}
+
+/* ************************************************************************* */
+
+pair<LinearFactorGraph, FGConfig> NonlinearFactorGraph::OneIterationLM( FGConfig& config,
+		const Ordering& ordering,
+		double relativeErrorTreshold,
+		double absoluteErrorTreshold,
+		int verbosity,
+		double lambda0,
+		double lambdaFactor) {
+
+	double lambda = lambda0;
+	bool converged = false;
+	double currentError = calculate_error(*this,config, verbosity);
+
+	if (verbosity>=4) config.print("config"); // maybe show output
+
+	FGConfig newConfig;
+	double newError;
+	LinearFactorGraph linear = linearize(config); // linearize all factors once
+	if (verbosity>=6) linear.print("linear");
+
+	// try lambda steps with successively larger lambda until we achieve descent
+	boost::tie(newConfig,newError) = try_lambda(config, linear, lambda, ordering,verbosity);
+	while (newError > currentError) {
+		lambda = lambda * lambdaFactor; // be more cautious
+		boost::tie(newConfig,newError) = try_lambda(config, linear, lambda, ordering,verbosity);
+	}
+
+	// next time, let's be more adventerous
+	lambda = lambda / lambdaFactor;
+
+	// return result of this iteration
+	config = newConfig;
+	if (verbosity>=4) config.print("config"); // maybe show output
+	if (verbosity>=1) cout << "error: " << newError << endl;
+
+	pair<LinearFactorGraph, FGConfig> p(linear, newConfig);
+	return p;
+
+
+
+	// linearize, solve, update
+	//double newError = iterateLM(config, currentError, lambda, lambdaFactor, ordering, verbosity);
+
+}
+
+/* ************************************************************************* */
+
+
+} // namespace gtsam

cpp/NonlinearFactorGraph.h

@@ -0,0 +1,111 @@
+/**
+ * @file    NonlinearFactorGraph.h
+ * @brief   Factor Graph Constsiting of non-linear factors
+ * @author  Frank Dellaert
+ * @author  Carlos Nieto
+ * @author  Christian Potthast
+ */
+
+// \callgraph
+
+#pragma once
+
+#include "LinearFactorGraph.h"
+#include "FactorGraph.h"
+#include "NonlinearFactor.h"
+#include "ChordalBayesNet.h"
+#include <colamd/colamd.h>
+
+namespace gtsam {
+
+/** Factor Graph Constsiting of non-linear factors */
+class NonlinearFactorGraph : public FactorGraph<NonlinearFactor>
+{
+public: // internal, exposed for testing only, doc in .cpp file
+
+	FGConfig iterate(const FGConfig& config, const Ordering& ordering) const;
+
+	double iterate(FGConfig& config, const Ordering& ordering, int verbosity) const;
+
+	std::pair<FGConfig,double> try_lambda
+	(const FGConfig& config, const LinearFactorGraph& linear,
+			double lambda, const Ordering& ordering, int verbosity) const;
+
+	double iterateLM(FGConfig& config, double currentError,
+			double& lambda, double lambdaFactor,
+			const Ordering& ordering, int verbosity) const;
+
+	Ordering getOrdering(FGConfig& config);
+
+public: // these you will probably want to use
+	/**
+	 * linearize a non linear factor
+	 */
+	LinearFactorGraph linearize(const FGConfig& config) const;
+
+	/**
+	 * Optimize a solution for a non linear factor graph
+	 * Changes config in place
+	 * @param config       Reference to current configuration
+	 * @param ordering     Ordering to optimize with
+	 * @param relativeErrorTreshold
+	 * @param absoluteErrorTreshold
+	 * @param verbosity Integer specifying how much output to provide
+	 */
+	void optimize(FGConfig& config,
+			const Ordering& ordering,
+			double relativeErrorTreshold, double absoluteErrorTreshold,
+			int verbosity = 0) const;
+
+	/**
+	 * Given two configs, check whether the error of config2 is
+	 * different enough from the error for config1, or whether config1
+	 * is essentially optimal
+	 *
+	 * @param config1  Reference to first configuration
+	 * @param config2  Reference to second configuration
+	 * @param relativeErrorTreshold
+	 * @param absoluteErrorTreshold
+	 * @param verbosity Integer specifying how much output to provide
+	 */
+	bool check_convergence(const FGConfig& config1,
+			const FGConfig& config2,
+			double relativeErrorTreshold, double absoluteErrorTreshold,
+			int verbosity = 0);
+
+	/**
+	 * Optimize using Levenberg-Marquardt. Really Levenberg's
+	 * algorithm at this moment, as we just add I*\lambda to Hessian
+	 * H'H. The probabilistic explanation is very simple: every
+	 * variable gets an extra Gaussian prior that biases staying at
+	 * current value, with variance 1/lambda. This is done very easily
+	 * (but perhaps wastefully) by adding a prior factor for each of
+	 * the variables, after linearization.
+	 *
+	 * Changes config in place :-(
+	 *
+	 * @param config       Reference to current configuration
+	 * @param ordering     Ordering to optimize with
+	 * @param relativeErrorTreshold
+	 * @param absoluteErrorTreshold
+	 * @param verbosity    Integer specifying how much output to provide
+	 * @param lambda       Reference to current lambda
+	 * @param lambdaFactor Factor by which to decrease/increase lambda
+	 */
+	void optimizeLM(FGConfig& config,
+			const Ordering& ordering,
+			double relativeErrorTreshold, double absoluteErrorTreshold,
+			int verbosity = 0,
+			double lambda0=1e-5, double lambdaFactor=10) const;
+
+
+
+	std::pair<LinearFactorGraph, FGConfig> OneIterationLM( FGConfig& config, const Ordering& ordering,
+			double relativeErrorTreshold,
+			double absoluteErrorTreshold,
+			int verbosity,
+			double lambda0,
+			double lambdaFactor) ;
+
+};
+}

cpp/Ordering.cpp

@@ -0,0 +1,45 @@
+/**
+ * @file    Ordering.cpp
+ * @brief   Ordering
+ * @author  Christian Potthast
+ */
+
+#include <iostream>
+#include <string.h> //Added for linux compatibility
+#include "Ordering.h"
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+void Ordering::print() const {
+  std::cout << "Ordering:" << std::endl;
+  printf("Ordering size: %d \n", (int)this->size());
+  
+  for(size_t i = 0; i < this->size(); i++)
+    cout << (*this)[i] << " ";
+
+  cout << endl;
+}
+
+/* ************************************************************************* */
+bool Ordering::equals(Ordering &ord){	
+	if(this->size() != ord.size())
+		return false;
+	
+	vector<std::string>::iterator key;
+	vector<std::string>::iterator ord_key;
+	for(key = this->begin(); key != this->end(); key++){
+		for(ord_key = ord.begin(); ord_key != ord.end(); ord_key++){
+			if(strcmp((*key).c_str(), (*ord_key).c_str()) == 0)
+				break;
+			if(key == this->end())
+			  return false;
+		}
+	}
+	return true;
+}
+/* ************************************************************************* */
+
+
+

cpp/Ordering.h

@@ -0,0 +1,42 @@
+/**
+ * @file    Ordering.h
+ * @brief   Ordering of indices for eliminating a factor graph
+ * @author  Frank Dellaert
+ */
+
+
+#pragma once
+
+#include <vector>
+#include <string>
+
+// \namespace
+
+namespace gtsam {
+
+/**
+ * @class Ordering
+ * @brief ordering of indices for eliminating a factor graph
+ */
+class Ordering : public std::vector<std::string>
+{
+public:
+	/** Constructor  */
+	Ordering(){clear();}
+
+	Ordering(std::vector<std::string> strings_in) : std::vector<std::string> (strings_in) {}
+
+	/** Destructor */
+	~Ordering(){}
+
+	void print() const;
+
+	/**
+	 * check if two orderings are the same
+	 * @param ordering
+	 * @return bool
+	 */
+	bool equals(Ordering &ord);
+};
+
+}

cpp/Point2.cpp

@@ -0,0 +1,24 @@
+/**
+ * @file    Point2.cpp
+ * @brief   2D Point
+ * @author  Frank Dellaert
+ */
+
+#include "Point2.h"
+
+namespace gtsam {
+
+/* ************************************************************************* */
+bool assert_equal(const Point2& p, const Point2& q, double tol) {
+
+  if(fabs(p.x() - q.x()) < tol && fabs(p.y() - q.y()) < tol) return true;
+  printf("not equal:\n");
+  p.print("p = ");
+  q.print("q = ");
+  (p-q).print("p-q = ");
+  return false;
+}
+
+/* ************************************************************************* */
+
+} // namespace gtsam

cpp/Point2.h

@@ -0,0 +1,66 @@
+/**
+ * @file    Point2.h
+ * @brief   2D Point
+ * @author  Frank Dellaert
+ */
+
+#pragma once
+
+#include "Matrix.h"
+
+namespace gtsam {
+
+  /** A 2D point */
+  class Point2 {
+  private:
+    double x_, y_;  
+		
+  public:
+    Point2(): x_(0), y_(0) {}
+    Point2(const Point2 &p) : x_(p.x_), y_(p.y_) {}
+    Point2(double x, double y): x_(x), y_(y) {}
+    Point2(const Vector& v) : x_(v(0)), y_(v(1)) {}
+
+    /** get functions for x, y */
+    double x() const {return x_;}
+    double y() const {return y_;}
+
+    /** set functions for x, y */
+    void set_x(double x) {x_=x;}
+    void set_y(double y) {y_=y;}
+
+    /** return DOF, dimensionality of tangent space */
+    size_t dim() const { return 2;}
+		
+    /** Given 3-dim tangent vector, create new rotation */
+    Point2 exmap(const Vector& d) const { 
+      return Point2(x_+d(0),y_+d(1));
+    }
+		
+    /** return vectorized form (column-wise) */
+    Vector vector() const {
+      Vector v(2);
+      v(0)=x_;v(1)=y_;
+      return v;
+    }
+
+    /** operators */
+    inline bool   operator ==(const Point2& q) {return x_==q.x_ && q.y_==q.y_;}
+    inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);}
+    inline Point2 operator - (const Point2& q) const {return Point2(x_-q.x_,y_-q.y_);}
+
+    /** print with optional string */
+    void print(const std::string& s = "") const {
+      std::cout << s << "(" << x_ << ", " << y_ << ")" << std::endl;
+    }
+    
+    /** distance between two points */
+    double dist(const Point2& p2) const {
+      return sqrt(pow(x()-p2.x(),2.0) + pow(y()-p2.y(),2.0));
+    }
+  };
+
+  /** equals with an tolerance, prints out message if unequal*/
+  bool assert_equal(const Point2& p, const Point2& q, double tol = 1e-9);
+}
+

cpp/Point2Prior.h

@@ -0,0 +1,14 @@
+// Frank Dellaert
+// Simulated2D Prior
+
+#include "NonlinearFactor.h"
+#include "simulated2D.h"
+
+namespace gtsam {
+
+  struct Point2Prior : public NonlinearFactor1 {
+  Point2Prior(const Vector& mu, double sigma, const std::string& key):
+    NonlinearFactor1(mu, sigma, prior, key, Dprior) {}
+  };
+
+} // namespace gtsam

cpp/Point3.cpp

@@ -0,0 +1,84 @@
+/**
+ * @file  Point3.cpp
+ * @brief 3D Point
+ */
+
+#include "Point3.h"
+
+
+namespace gtsam {
+
+/* ************************************************************************* */
+
+bool Point3::operator== (const Point3& q) const {
+  return x_ == q.x_ && y_ == q.y_ && z_ == q.z_;
+}
+
+/* ************************************************************************* */
+Point3 Point3::operator+(const Point3& q) const {
+  return Point3( x_ + q.x_, y_ + q.y_, z_ + q.z_ );
+}
+
+/* ************************************************************************* */
+Point3 Point3::operator- (const Point3& q) const {
+  return Point3( x_ - q.x_, y_ - q.y_, z_ - q.z_ );
+}
+/* ************************************************************************* */
+Point3 Point3::operator*(double s) const {
+  return Point3(x_ * s, y_ * s, z_ * s);
+}
+/* ************************************************************************* */
+Point3 Point3::operator/(double s) const {
+  return Point3(x_ / s, y_ / s, z_ / s);
+}
+/* ************************************************************************* */
+Point3 operator*(double s, const Point3& p) { return p*s;}
+
+/* ************************************************************************* */
+Point3 add(const Point3 &p, const Point3 &q) {
+  return p+q;
+}
+/* ************************************************************************* */
+Matrix Dadd1(const Point3 &p, const Point3 &q) {
+  return eye(3,3);
+}
+/* ************************************************************************* */
+Matrix Dadd2(const Point3 &p, const Point3 &q) {
+  return eye(3,3);
+}
+/* ************************************************************************* */
+Point3 sub(const Point3 &p, const Point3 &q) {
+  return p+q;
+}
+/* ************************************************************************* */
+Matrix Dsub1(const Point3 &p, const Point3 &q) {
+  return eye(3,3);
+}
+/* ************************************************************************* */
+Matrix Dsub2(const Point3 &p, const Point3 &q) {
+  return -eye(3,3);
+}
+
+/* ************************************************************************* */
+bool Point3::equals(const Point3 & q, double tol) const {
+  return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol && fabs(z_ - q.z()) < tol);
+}
+/* ************************************************************************* */
+Point3 cross(const Point3 &p, const Point3 &q)
+{
+  return Point3( p.y_*q.z_ - p.z_*q.y_,
+		 p.z_*q.x_ - p.x_*q.z_,
+		 p.x_*q.y_ - p.y_*q.x_ );
+}
+/* ************************************************************************* */
+bool assert_equal(const Point3& p, const Point3& q, double tol) {
+  if(p.equals(q,tol)) return true;
+  printf("not equal:\n");
+  p.print("p = ");
+  q.print("q = ");
+  (p-q).print("p-q = ");
+  return false;
+}
+/* ************************************************************************* */
+
+} // namespace gtsam

cpp/Point3.h

@@ -0,0 +1,101 @@
+/**
+ * @file   Point3.h
+ * @brief  3D Point
+ * @author Alireza Fathi
+ * @author Christian Potthast
+ * @author Frank Dellaert
+ */
+
+// \callgraph
+
+#pragma once
+
+#include <boost/serialization/nvp.hpp>
+
+#include "Matrix.h"
+
+namespace gtsam {
+
+  /** A 3D point */
+  class Point3 {
+  private:
+    double x_, y_, z_;  
+		
+  public:
+    Point3(): x_(0), y_(0), z_(0) {}
+    Point3(const Point3 &p) : x_(p.x_), y_(p.y_), z_(p.z_) {}
+    Point3(double x, double y, double z): x_(x), y_(y), z_(z) {}
+    Point3(const Vector& v) : x_(v(0)), y_(v(1)), z_(v(2)) {}
+
+    /** return DOF, dimensionality of tangent space */
+    size_t dim() const { return 3;}
+		
+    /** Given 3-dim tangent vector, create new rotation*/
+    Point3 exmap(const Vector& d) const { return *this + d; }
+		
+    /** return vectorized form (column-wise)*/
+    Vector vector() const {
+      //double r[] = { x_, y_, z_ };
+      Vector v(3); v(0)=x_; v(1)=y_; v(2)=z_;
+      return v;
+    }
+
+    /** get functions for x, y, z */
+    double x() const {return x_;}
+    double y() const {return y_;}
+    double z() const {return z_;}
+
+    /** operators */
+    Point3 operator - () const { return Point3(-x_,-y_,-z_);}
+    bool   operator ==(const Point3& q) const;
+    Point3 operator + (const Point3& q) const;
+    Point3 operator - (const Point3& q) const;
+    Point3 operator * (double s) const;
+    Point3 operator / (double s) const;
+
+    /** print with optional string */
+    void print(const std::string& s = "") const {
+      std::cout << s << "(" << x_ << ", " << y_ <<  ", " << z_ << ")" << std::endl;
+    }
+
+    /** distance between two points */
+    double dist(const Point3& p2) const {
+      return sqrt(pow(x()-p2.x(),2.0) + pow(y()-p2.y(),2.0) + pow(z()-p2.z(),2.0));
+    }
+
+    /** equals with an tolerance */
+    bool equals(const Point3& p, double tol = 1e-9) const;
+
+    /** friends */
+    friend Point3 cross(const Point3 &p1, const Point3 &p2); 
+
+  private:
+    /** Serialization function */
+    friend class boost::serialization::access;
+    template<class Archive>
+      void serialize(Archive & ar, const unsigned int version)
+    {
+      ar & BOOST_SERIALIZATION_NVP(x_);
+      ar & BOOST_SERIALIZATION_NVP(y_);
+      ar & BOOST_SERIALIZATION_NVP(z_);
+    }
+  };
+
+  Point3 operator*(double s, const Point3& p);
+
+  /** add two points, add(p,q) is same as p+q */
+  Point3   add (const Point3 &p, const Point3 &q);
+  Matrix Dadd1(const Point3 &p, const Point3 &q);
+  Matrix Dadd2(const Point3 &p, const Point3 &q);
+
+  /** subtract two points, sub(p,q) is same as p-q */
+  Point3   sub (const Point3 &p, const Point3 &q);
+  Matrix Dsub1(const Point3 &p, const Point3 &q);
+  Matrix Dsub2(const Point3 &p, const Point3 &q);
+
+  /** cross product */
+  Point3 cross(const Point3 &p, const Point3 &q); 
+
+  /** equals with an tolerance, prints out message if unequal */
+  bool assert_equal(const Point3& p, const Point3& q, double tol = 1e-9);
+}

cpp/Pose3.cpp

@@ -0,0 +1,127 @@
+/**
+ * @file  Pose3.cpp
+ * @brief 3D Pose
+ */
+
+#include "Pose3.h"
+
+namespace gtsam {
+
+/* ************************************************************************* */
+Pose3 Pose3::exmap(const Vector& v) const { 
+  return Pose3(R_.exmap(sub(v,0,3)), t_.exmap(sub(v,3,6)));
+}
+
+/* ************************************************************************* */
+Point3 transform_from(const Pose3& pose, const Point3& p) {
+  return pose.R_ * p + pose.t_;
+}
+/* ************************************************************************* */
+/** 3by6                                                                     */
+/* ************************************************************************* */
+Matrix Dtransform_from1(const Pose3& pose, const Point3& p) {
+  Matrix DR = Drotate1(pose.rotation(), p);
+  Matrix Dt = Dadd1(pose.translation(), p);
+  return collect(2,&DR,&Dt);
+}
+/* ************************************************************************* */
+/** 3by3                                                                     */
+/* ************************************************************************* */
+Matrix Dtransform_from2(const Pose3& pose) {
+  return Drotate2(pose.rotation());
+}
+
+/* ************************************************************************* */
+Point3 transform_to(const Pose3& pose, const Point3& p) {
+		Point3 sub = p - pose.translation();
+		Point3 r = rotate(pose.rotation().inverse(), sub);
+		return r; 
+}
+/* ************************************************************************* */
+/** 3by6                                                                     */
+/* ************************************************************************* */
+Matrix Dtransform_to1(const Pose3& pose, const Point3& p) {
+  Point3 q = p - pose.translation();
+  Matrix D_r_R =   Dunrotate1(pose.rotation(),q);
+  Matrix D_r_t = - Dunrotate2(pose.rotation()); // negative because of sub
+
+  Matrix D_r_pose = collect(2,&D_r_R,&D_r_t);
+  return D_r_pose;
+}
+/* ************************************************************************* */
+/** 3by3                                                                     */
+/* ************************************************************************* */
+Matrix Dtransform_to2(const Pose3& pose) {
+  return Dunrotate2(pose.rotation());
+}
+
+/* ************************************************************************* */
+/** direct measurement of the deviation of a pose from the origin
+ * used as soft prior
+ */
+/* ************************************************************************* */
+Vector hPose (const Vector& x) {
+  Pose3 pose(x);                            // transform from vector to Pose3
+  Vector w = RQ(pose.rotation().matrix()); // get angle differences
+  Vector d = pose.translation().vector();  // get translation differences
+  return concatVectors(2,&w,&d);
+}
+
+/* ************************************************************************* */
+/** derivative of direct measurement
+ * 6*6, entry i,j is how measurement error will change 
+ */
+/* ************************************************************************* */
+Matrix DhPose(const Vector& x) {
+  Matrix H = eye(6,6);
+  return H;
+}
+
+/* ************************************************************************* */
+Pose3 Pose3::inverse() const
+{
+  Rot3 Rt = R_.inverse();
+  return Pose3(Rt,-(Rt*t_));
+}
+
+/* ************************************************************************* */
+bool Pose3::equals(const Pose3& pose, double tol) const
+{
+  return R_.equals(pose.rotation(),tol) && t_.equals(pose.translation(),tol);
+}
+
+/* ************************************************************************* */
+bool assert_equal(const Pose3& A, const Pose3& B, double tol)
+{
+  if(A.equals(B,tol)) return true;
+  printf("not equal:\n");
+  A.print("A");
+  B.print("B");
+  return false;
+}
+
+/* ************************************************************************* */
+Pose3 Pose3::transformPose_to(const Pose3& transform)
+{
+		Rot3 cRv = rotation() * Rot3(transform.rotation().inverse());
+		Point3 t = transform_to(transform, translation());
+		
+		return Pose3(cRv, t);
+}
+
+/* ************************************************************************* */
+Pose3 composeTransform(const Pose3& current, const Pose3& target)
+{
+		// reverse operation
+		Rot3 trans_rot = Rot3(target.rotation() * current.rotation().inverse()).inverse();
+
+		// get sub
+		Point3 sub = rotate(trans_rot, target.translation());
+		
+		// get final transform translation
+		Point3 trans_pt = current.translation() - sub;
+		
+		return Pose3(trans_rot, trans_pt);
+}
+
+} // namespace gtsam

cpp/Pose3.h

@@ -0,0 +1,122 @@
+/**
+ *@file  Pose3.h
+ *@brief 3D Pose
+ */
+
+// \callgraph
+
+#pragma once
+
+#include "Point3.h"
+#include "Rot3.h"
+
+namespace gtsam {
+
+/** A 3D pose (R,t) : (Rot3,Point3) */
+class Pose3 {
+private:
+	Rot3 R_;
+	Point3 t_;
+
+public:
+	Pose3() {} // default is origin
+	Pose3(const Pose3& pose):R_(pose.R_),t_(pose.t_) {}
+	Pose3(const Rot3& R, const Point3& t): R_(R), t_(t) {}
+
+	/** constructor from 4*4 matrix */
+	Pose3(const Matrix &T) :R_( T(0,0), T(0,1), T(0,2),
+			T(1,0), T(1,1), T(1,2),
+			T(2,0), T(2,1), T(2,2) ),
+			t_( T(0,3), T(1,3), T(2,3) ) {}
+
+	/** constructor from 12D vector */
+	Pose3(const Vector &V) :R_( V(0), V(3), V(6),
+			V(1), V(4), V(7),
+			V(2), V(5), V(8) ),
+			t_( V(9), V(10), V(11) ) {}
+
+	const Rot3& rotation() const {return R_;}
+
+	const Point3& translation() const {return t_;}
+
+	/** return DOF, dimensionality of tangent space = 6 */
+	size_t dim() const { return 6;}
+
+	/** Given 6-dim tangent vector, create new pose */
+	Pose3 exmap(const Vector& d) const;
+
+	/** inverse transformation */
+	Pose3 inverse() const;
+
+	/** composition */
+	inline Pose3 operator*(const Pose3& B) const {
+		return Pose3(matrix()*B.matrix());
+	}
+
+	/** return 12D vectorized form (column-wise) */
+	Vector vector() const {
+		Vector r = R_.vector(), t = t_.vector();
+		return concatVectors(2,&r,&t);
+	}
+
+	/** convert to 4*4 matrix */
+	Matrix matrix() const {
+		const double row4[] = {0,0,0,1};
+		Matrix A34 = Matrix_(3,4,vector()), A14 = Matrix_(1,4,row4);
+		return stack(2, &A34, &A14);
+	}
+
+	/** print with optional string */
+	void print(const std::string& s = "") const {
+		R_.print(s+".R");
+		t_.print(s+".t");
+	}
+
+	/** transforms */
+	Pose3 transformPose_to(const Pose3& transform);
+
+	/** assert equality up to a tolerance */
+	bool equals(const Pose3& pose, double tol=1e-9) const;
+
+	/** friends */
+	friend Point3 transform_from(const Pose3& pose, const Point3& p);
+	friend Point3 transform_to  (const Pose3& pose, const Point3& p);
+	friend Pose3 composeTransform(const Pose3& current, const Pose3& target);
+
+private:
+	/** Serialization function */
+	friend class boost::serialization::access;
+	template<class Archive>
+	void serialize(Archive & ar, const unsigned int version)
+	{
+		ar & BOOST_SERIALIZATION_NVP(R_);
+		ar & BOOST_SERIALIZATION_NVP(t_);
+	}
+}; // Pose3 class
+
+/** finds a transform from the current frame to the target frame given two coordinates of the same point */
+Pose3 composeTransform(const Pose3& current, const Pose3& target);
+
+/** receives the point in Pose coordinates and transforms it to world coordinates */
+Point3   transform_from (const Pose3& pose, const Point3& p);
+Matrix Dtransform_from1(const Pose3& pose, const Point3& p);
+Matrix Dtransform_from2(const Pose3& pose);  // does not depend on p !
+
+/** receives the point in world coordinates and transforms it to Pose coordinates */
+Point3   transform_to (const Pose3& pose, const Point3& p);
+Matrix Dtransform_to1(const Pose3& pose, const Point3& p);
+Matrix Dtransform_to2(const Pose3& pose);  // does not depend on p !
+
+/** direct measurement of a pose */
+Vector hPose (const Vector& x);
+
+/**
+ * derivative of direct measurement
+ * 12*6, entry i,j is how measurement error will change
+ */
+Matrix DhPose(const Vector& x);
+
+/** assert equality up to a tolerance */
+bool assert_equal(const Pose3& A, const Pose3& B, double tol=1e-9);
+
+} // namespace gtsam

cpp/Rot3.cpp

@@ -0,0 +1,173 @@
+/**
+ * @file    Rot3.cpp
+ * @brief   Rotation (internal: 3*3 matrix representation*)
+ * @author  Alireza Fathi
+ * @author  Christian Potthast
+ * @author  Frank Dellaert
+ */
+
+#include "Rot3.h"
+
+using namespace std;
+
+namespace gtsam {
+
+	/* ************************************************************************* */
+	/** faster than below ?                                                      */
+	/* ************************************************************************* */
+	Rot3 rodriguez(const Vector& w, double t) {
+		double l_w = 0.0;
+		for (int i = 0; i < 3; i++)
+			l_w += pow(w(i), 2.0);
+		if (l_w != 1.0) throw domain_error("rodriguez: length of w should be 1");
+
+		double ct = cos(t), st = sin(t);
+
+		Point3 r1 = Point3(ct + w(0) * w(0) * (1 - ct), w(1) * w(0) * (1 - ct) - w(
+				2) * st, w(1) * st + w(2) * w(0) * (1 - ct));
+		Point3 r2 = Point3(w(2) * st + w(0) * w(1) * (1 - ct), w(1) * w(1) * (1
+				- ct) + ct, -w(0) * st + w(2) * w(1) * (1 - ct));
+		Point3 r3 = Point3(-w(1) * st + w(0) * w(2) * (1 - ct), w(1) * w(2) * (1
+				- ct) + w(0) * st, ct + w(2) * w(2) * (1 - ct));
+
+		return Rot3(r1, r2, r2);
+	}
+
+	/* ************************************************************************* */
+	Rot3 rodriguez(double wx, double wy, double wz) {
+		if (wx == 0.0 && wy == 0.0 && wz == 0.0) return Rot3();
+		Matrix J = skewSymmetric(wx, wy, wz);
+		double t2 = wx * wx + wy * wy + wz * wz;
+		double t = sqrt(t2);
+		Matrix R = eye(3, 3) + sin(t) / t * J + (1.0 - cos(t)) / t2 * (J * J);
+		return R; // matrix constructor will be tripped
+	}
+
+	/* ************************************************************************* */
+	Rot3 rodriguez(const Vector& v) {
+		return rodriguez(v(0), v(1), v(2));
+	}
+
+	/* ************************************************************************* */
+	Rot3 exmap(const Rot3& R, const Vector& v) {
+		return rodriguez(v) * R;
+	}
+
+	/* ************************************************************************* */
+	Rot3 Rot3::exmap(const Vector& v) const {
+		if (zero(v)) return (*this);
+		return rodriguez(v) * (*this);
+	}
+
+	/* ************************************************************************* */
+	Point3 rotate(const Rot3& R, const Point3& p) {
+		return R * p;
+	}
+
+	/* ************************************************************************* */
+	Matrix Drotate1(const Rot3& R, const Point3& p) {
+		Point3 q = R * p;
+		return skewSymmetric(-q.x(), -q.y(), -q.z());
+	}
+
+	/* ************************************************************************* */
+	Matrix Drotate2(const Rot3& R) {
+		return R.matrix();
+	}
+
+	/* ************************************************************************* */
+	Point3 unrotate(const Rot3& R, const Point3& p) {
+		return R.unrotate(p);
+	}
+
+	/* ************************************************************************* */
+	bool Rot3::equals(const Rot3 & R, double tol) const {
+		return equal_with_abs_tol(matrix(), R.matrix(), tol);
+	}
+
+	/* ************************************************************************* */
+	/** see libraries/caml/geometry/math.lyx, derivative of unrotate              */
+	/* ************************************************************************* */
+	Matrix Dunrotate1(const Rot3 & R, const Point3 & p) {
+		Point3 q = R.unrotate(p);
+		return skewSymmetric(q.x(), q.y(), q.z()) * R.transpose();
+	}
+
+	/* ************************************************************************* */
+	Matrix Dunrotate2(const Rot3 & R) {
+		return R.transpose();
+	}
+
+	/* ************************************************************************* */
+	bool assert_equal(const Rot3 & A, const Rot3 & B, double tol) {
+	  if(A.equals(B,tol)) return true;
+	  printf("not equal:\n");
+	  A.print("A");
+	  B.print("B");
+	  return false;
+	}
+
+	/* ************************************************************************* */
+	/** This function receives a rotation 3 by 3 matrix and returns 3 rotation angles.
+	 *  The implementation is based on the algorithm in multiple view geometry
+	 *  the function returns a vector that its arguments are: thetax, thetay, thetaz in radians.
+	 */
+	/* ************************************************************************* */
+	Vector RQ(Matrix R) {
+		double Cx = R(2, 2) / (double) ((sqrt(pow((double) (R(2, 2)), 2.0) + pow(
+				(double) (R(2, 1)), 2.0)))); //cosX
+		double Sx = -R(2, 1) / (double) ((sqrt(pow((double) (R(2, 2)), 2.0) + pow(
+				(double) (R(2, 1)), 2.0)))); //sinX
+		Matrix Qx(3, 3);
+		for (int i = 0; i < 3; i++)
+			for (int j = 0; j < 3; j++)
+				Qx(i, j) = 0;
+
+		Qx(0, 0) = 1;
+		Qx(1, 1) = Cx;
+		Qx(1, 2) = -Sx;
+		Qx(2, 1) = Sx;
+		Qx(2, 2) = Cx;
+		R = R * Qx;
+		double Cy = R(2, 2) / (sqrt(pow((double) (R(2, 2)), 2.0) + pow((double) (R(
+				2, 0)), 2.0))); //cosY
+		double Sy = R(2, 0) / (sqrt(pow((double) (R(2, 2)), 2.0) + pow((double) (R(
+				2, 0)), 2.0))); //sinY
+		Matrix Qy(3, 3);
+		for (int i = 0; i < 3; i++)
+			for (int j = 0; j < 3; j++)
+				Qy(i, j) = 0;
+
+		Qy(0, 0) = Cy;
+		Qy(0, 2) = Sy;
+		Qy(1, 1) = 1;
+		Qy(2, 0) = -Sy;
+		Qy(2, 2) = Cy;
+		R = R * Qy;
+		double Cz = R(1, 1) / (sqrt(pow((double) (R(1, 1)), 2.0) + pow((double) (R(
+				1, 0)), 2.0))); //cosZ
+		double Sz = -R(1, 0) / (sqrt(pow((double) (R(1, 1)), 2.0) + pow(
+				(double) (R(1, 0)), 2.0)));//sinZ
+		Matrix Qz(3, 3);
+		for (int i = 0; i < 3; i++)
+			for (int j = 0; j < 3; j++)
+				Qz(i, j) = 0;
+		Qz(0, 0) = Cz;
+		Qz(0, 1) = -Sz;
+		Qz(1, 0) = Sz;
+		Qz(1, 1) = Cz;
+		Qz(2, 2) = 1;
+		R = R * Qz;
+		double pi = atan2(sqrt(2.0) / 2.0, sqrt(2.0) / 2.0) * 4.0;
+
+		Vector result(3);
+		result(0) = -atan2(Sx, Cx);
+		result(1) = -atan2(Sy, Cy);
+		result(2) = -atan2(Sz, Cz);
+
+		return result;
+	}
+
+/* ************************************************************************* */
+
+} // namespace gtsam

cpp/Rot3.h

@@ -0,0 +1,181 @@
+/**
+ * @file    Rot3.h
+ * @brief   Rotation
+ * @author  Alireza Fathi
+ * @author  Christian Potthast
+ * @author  Frank Dellaert
+ */
+
+// \callgraph
+
+#pragma once
+
+#include "Point3.h"
+
+namespace gtsam {
+
+  /* Rotation matrix */
+  class Rot3{
+  private:
+    /** we store columns ! */
+    Point3 r1_, r2_, r3_;  
+		
+  public:
+    // Matrix R;
+	 
+  /** default coonstructor, unit rotation */
+  Rot3() : r1_(Point3(1.0,0.0,0.0)), 
+      r2_(Point3(0.0,1.0,0.0)),
+      r3_(Point3(0.0,0.0,1.0)) {
+    }
+
+  /** constructor from columns */
+  Rot3(const Point3& r1, const Point3& r2, const Point3& r3) : r1_(r1), r2_(r2), r3_(r3) {
+    }
+		
+  /**  constructor from vector */
+  Rot3(const Vector &v) : 
+    r1_(Point3(v(0),v(1),v(2))),
+      r2_(Point3(v(3),v(4),v(5))),
+      r3_(Point3(v(6),v(7),v(8)))
+      {  }
+			
+  /** constructor from doubles in *row* order !!! */ 
+  Rot3(double R11, double R12, double R13,
+       double R21, double R22, double R23,
+       double R31, double R32, double R33) :
+    r1_(Point3(R11, R21, R31)),
+      r2_(Point3(R12, R22, R32)),
+      r3_(Point3(R13, R23, R33)) {}
+
+  /** constructor from matrix */
+  Rot3(const Matrix& R):
+    r1_(Point3(R(0,0), R(1,0), R(2,0))),
+      r2_(Point3(R(0,1), R(1,1), R(2,1))),
+      r3_(Point3(R(0,2), R(1,2), R(2,2))) {}
+
+    /** return DOF, dimensionality of tangent space */
+    size_t dim() const { return 3;}
+		
+    /** Given 3-dim tangent vector, create new rotation */
+    Rot3 exmap(const Vector& d) const;
+		
+    /** return vectorized form (column-wise)*/
+    Vector vector() const {
+      double r[] = { r1_.x(), r1_.y(), r1_.z(), 
+		     r2_.x(), r2_.y(), r2_.z(),
+		     r3_.x(), r3_.y(), r3_.z() };
+      Vector v(9);
+      copy(r,r+9,v.begin());
+      return v;  
+    }
+
+    /** return 3*3 rotation matrix */
+    Matrix matrix() const {
+      double r[] = { r1_.x(), r2_.x(), r3_.x(), 
+		     r1_.y(), r2_.y(), r3_.y(),
+		     r1_.z(), r2_.z(), r3_.z() };
+      return Matrix_(3,3, r);  
+    }
+
+    /** return 3*3 transpose (inverse) rotation matrix   */
+    Matrix transpose() const {
+      double r[] = { r1_.x(), r1_.y(), r1_.z(), 
+		     r2_.x(), r2_.y(), r2_.z(), 
+		     r3_.x(), r3_.y(), r3_.z()};
+      return Matrix_(3,3, r);  
+    }
+
+    /** inverse transformation  */
+    Rot3 inverse() const { return transpose();}
+		
+    /** composition */
+    inline Rot3 operator*(const Rot3& B) const { return Rot3(matrix()*B.matrix());}
+
+    /**  rotate from rotated to world, syntactic sugar = R*p  */
+    inline Point3 operator*(const Point3& p) const { return r1_ * p.x() + r2_ * p.y() + r3_ * p.z(); }
+
+    /** rotate from world to rotated = R'*p */
+    Point3 unrotate(const Point3& p) const {
+      return Point3(
+	r1_.x()*p.x() + r1_.y()*p.y() + r1_.z()*p.z(), 
+	r2_.x()*p.x() + r2_.y()*p.y() + r2_.z()*p.z(), 
+	r3_.x()*p.x() + r3_.y()*p.y() + r3_.z()*p.z()
+	);
+    }
+
+    /** print */
+    void print(const std::string& s="R") const { gtsam::print(matrix(), s);}
+
+    /** equals with an tolerance */
+    bool equals(const Rot3& p, double tol = 1e-9) const;
+
+    /** friends */
+    friend Matrix Dunrotate1(const Rot3& R, const Point3& p);
+
+  private:
+  	/** Serialization function */
+  	friend class boost::serialization::access;
+  	template<class Archive>
+  	void serialize(Archive & ar, const unsigned int version)
+  	{
+  		ar & BOOST_SERIALIZATION_NVP(r1_);
+  		ar & BOOST_SERIALIZATION_NVP(r2_);
+  		ar & BOOST_SERIALIZATION_NVP(r3_);
+  	}
+  };
+
+  /**
+   * Rodriguez' formula to compute an incremental rotation matrix
+   * @param   w is the rotation axis, unit length
+   * @param   theta rotation angle
+   * @return incremental rotation matrix
+   */
+  Rot3 rodriguez(const Vector& w, double theta);
+
+  /**
+   * Rodriguez' formula to compute an incremental rotation matrix
+   * @param wx
+   * @param wy
+   * @param wz 
+   * @return incremental rotation matrix
+   */
+  Rot3 rodriguez(double wx, double wy, double wz);
+
+  /**
+   * Rodriguez' formula to compute an incremental rotation matrix
+   * @param v a vector of incremental roll,pitch,yaw
+   * @return incremental rotation matrix
+   */
+  Rot3 rodriguez(const Vector& v);
+
+  /**
+   * Update Rotation with incremental rotation
+   * @param v a vector of incremental roll,pitch,yaw
+   * @param R a rotated frame
+   * @return incremental rotation matrix
+   */
+  Rot3 exmap(const Rot3& R, const Vector& v);
+
+  /**
+   * rotate point from rotated coordinate frame to 
+   * world = R*p
+   */
+  Point3    rotate(const Rot3& R, const Point3& p);
+  Matrix Drotate1(const Rot3& R, const Point3& p);
+  Matrix Drotate2(const Rot3& R); // does not depend on p !
+
+  /**
+   * rotate point from world to rotated 
+   * frame = R'*p
+   */
+  Point3    unrotate(const Rot3& R, const Point3& p);
+  Matrix Dunrotate1(const Rot3& R, const Point3& p);
+  Matrix Dunrotate2(const Rot3& R); // does not depend on p !
+
+  bool assert_equal(const Rot3& A, const Rot3& B, double tol=1e-9);
+
+  /** receives a rotation 3 by 3 matrix and returns 3 rotation angles.*/
+  Vector RQ(Matrix R);
+
+}

cpp/SimpleCamera.cpp

@@ -0,0 +1,67 @@
+/*
+ * SimpleCamera.cpp
+ *
+ *  Created on: Aug 16, 2009
+ *      Author: dellaert
+ */
+
+#include "SimpleCamera.h"
+#include "CalibratedCamera.h"
+
+namespace gtsam {
+
+	/* ************************************************************************* */
+
+	SimpleCamera::SimpleCamera(const Cal3_S2& K, const Pose3& pose) :
+		calibrated_(pose), K_(K) {
+	}
+
+	SimpleCamera::~SimpleCamera() {
+	}
+
+	Point2 SimpleCamera::project(const Point3 & P) const {
+		Point2 intrinsic = calibrated_.project(P);
+		Point2 projection = uncalibrate(K_, intrinsic);
+		return projection;
+	}
+
+	/* ************************************************************************* */
+	Point2 project(const SimpleCamera& camera, const Point3& point) {
+		return camera.project(point);
+	}
+
+	/* ************************************************************************* */
+	Matrix Dproject_pose(const SimpleCamera& camera, const Point3& point) {
+		Point2 intrinsic = project(camera.calibrated_, point);
+		Matrix D_intrinsic_pose = Dproject_pose(camera.calibrated_, point);
+		Matrix D_projection_intrinsic = Duncalibrate2(camera.K_, intrinsic);
+		Matrix D_projection_pose = D_projection_intrinsic * D_intrinsic_pose;
+		return D_projection_pose;
+	}
+
+	/* ************************************************************************* */
+	Matrix Dproject_point(const SimpleCamera& camera, const Point3& point) {
+		Point2 intrinsic = project(camera.calibrated_, point);
+		Matrix D_intrinsic_point = Dproject_point(camera.calibrated_, point);
+		Matrix D_projection_intrinsic = Duncalibrate2(camera.K_, intrinsic);
+		Matrix D_projection_point = D_projection_intrinsic * D_intrinsic_point;
+		return D_projection_point;
+	}
+
+	/* ************************************************************************* */
+	void Dproject_pose_point(const SimpleCamera& camera, const Point3& point,
+			Point2& projection, Matrix& D_projection_pose, Matrix& D_projection_point) {
+
+		Point2 intrinsic = project(camera.calibrated_, point);
+		Matrix D_intrinsic_pose = Dproject_pose(camera.calibrated_, point);
+		Matrix D_intrinsic_point = Dproject_point(camera.calibrated_, point);
+
+		projection = uncalibrate(camera.K_, intrinsic);
+		Matrix D_projection_intrinsic = Duncalibrate2(camera.K_, intrinsic);
+
+		D_projection_pose = D_projection_intrinsic * D_intrinsic_pose;
+		D_projection_point = D_projection_intrinsic * D_intrinsic_point;
+	}
+
+/* ************************************************************************* */
+}

cpp/SimpleCamera.h

@@ -0,0 +1,77 @@
+/*
+ * SimpleCamera.h
+ *
+ *  Created on: Aug 16, 2009
+ *      Author: dellaert
+ */
+
+#ifndef SIMPLECAMERA_H_
+#define SIMPLECAMERA_H_
+
+#include "CalibratedCamera.h"
+#include "Cal3_S2.h"
+
+namespace gtsam {
+
+	/**
+	 * A simple camera class with a Cal3_S2 calibration
+	 * Basically takes a Calibrated camera and adds calibration information
+	 * to produce measurements in pixels.
+	 * Not a sublass as a SimpleCamera *is not* a CalibratedCamera.
+	 */
+	class SimpleCamera {
+	private:
+		CalibratedCamera calibrated_; // Calibrated camera
+		Cal3_S2 K_; // Calibration
+
+	public:
+		SimpleCamera(const Cal3_S2& K, const Pose3& pose);
+		virtual ~SimpleCamera();
+
+		const Pose3& pose() const {
+			return calibrated_.pose();
+		}
+
+		const Cal3_S2& calibration() const {
+			return K_;
+		}
+
+		Point2 project(const Point3& P) const;
+
+		// Friends
+		friend Matrix Dproject_pose(const SimpleCamera& camera,  const Point3& point);
+		friend Matrix Dproject_point(const SimpleCamera& camera, const Point3& point);
+		friend void	Dproject_pose_point(const SimpleCamera& camera, const Point3& point,
+					Point2& projection, Matrix& D_projection_pose,
+					Matrix& D_projection_point);
+
+	};
+
+	/* ************************************************************************* */
+	// measurement functions and derivatives
+	/* ************************************************************************* */
+
+	/**
+	 * This function receives the camera pose and the landmark location and
+	 returns the location the point is supposed to appear in the image
+	 */
+	Point2 project(const SimpleCamera& camera, const Point3& point);
+
+	/**
+	 * Derivatives of project.
+	 */
+	Matrix Dproject_pose(const SimpleCamera& camera, const Point3& point);
+	Matrix Dproject_point(const SimpleCamera& camera, const Point3& point);
+
+	/**
+	 * super-duper combined evaluation + derivatives
+	 * saves a lot of time because a lot of computation is shared
+	 */
+	void
+			Dproject_pose_point(const SimpleCamera& camera, const Point3& point,
+					Point2& projection, Matrix& D_projection_pose,
+					Matrix& D_projection_point);
+
+}
+
+#endif /* SIMPLECAMERA_H_ */

cpp/Simulated2DMeasurement.h

@@ -0,0 +1,14 @@
+// Christian Potthast
+// Simulated2D Odometry
+
+#include "NonlinearFactor.h"
+#include "simulated2D.h"
+
+namespace gtsam {
+
+  struct Simulated2DMeasurement : public NonlinearFactor2 {
+  Simulated2DMeasurement(const Vector& z, double sigma, const std::string& key1, const std::string& key2):
+    NonlinearFactor2(z, sigma, mea, key1, Dmea1, key2, Dmea2) {}
+  };
+
+} // namespace gtsam

cpp/Simulated2DOdometry.h

@@ -0,0 +1,14 @@
+// Christian Potthast
+// Simulated2D Odometry
+
+#include "NonlinearFactor.h"
+#include "simulated2D.h"
+
+namespace gtsam {
+
+  struct Simulated2DOdometry : public NonlinearFactor2 {
+  Simulated2DOdometry(const Vector& z, double sigma, const std::string& key1, const std::string& key2):
+    NonlinearFactor2(z, sigma, odo, key1, Dodo1, key2, Dodo2) {}
+  };
+
+} // namespace gtsam

cpp/Simulated3D.cpp

@@ -0,0 +1,57 @@
+/**
+* @file   Simulated3D.cpp
+* @brief  measurement functions and derivatives for simulated 3D robot
+* @author Alex Cunningham
+**/
+
+#include "Simulated3D.h"
+
+namespace gtsam {
+
+Vector prior_3D (const Vector& x)
+{
+	return x;
+}
+
+Matrix Dprior_3D(const Vector& x)
+{
+	Matrix H = eye((int) x.size());
+	return H;
+}
+
+Vector odo_3D(const Vector& x1, const Vector& x2)
+{
+	return x2 - x1;
+}
+
+Matrix Dodo1_3D(const Vector& x1, const Vector& x2)
+{
+	Matrix H = -1 * eye((int) x1.size());
+	return H;
+}
+
+Matrix Dodo2_3D(const Vector& x1, const Vector& x2)
+{
+	Matrix H = eye((int) x1.size());
+	return H;
+}
+
+
+Vector mea_3D(const Vector& x,  const Vector& l)
+{
+	return l - x;
+}
+
+Matrix Dmea1_3D(const Vector& x, const Vector& l)
+{
+	Matrix H = -1 * eye((int) x.size());
+	return H;
+}
+
+Matrix Dmea2_3D(const Vector& x, const Vector& l)
+{
+	Matrix H = eye((int) x.size());
+	return H;
+}
+
+} // namespace gtsam

cpp/Simulated3D.h

@@ -0,0 +1,47 @@
+/**
+* @file   Simulated3D.h
+* @brief  measurement functions and derivatives for simulated 3D robot
+* @author Alex Cunningham
+**/
+
+// \callgraph
+
+#pragma once
+
+#include "NonlinearFactor.h"
+
+// \namespace
+
+namespace gtsam {
+	
+	/**
+	* Prior on a single pose
+	*/
+	Vector prior_3D (const Vector& x); 
+	Matrix Dprior_3D(const Vector& x);
+	
+	/**
+	* odometry between two poses
+	*/
+	Vector odo_3D(const Vector& x1, const Vector& x2);
+	Matrix Dodo1_3D(const Vector& x1, const Vector& x2);
+	Matrix Dodo2_3D(const Vector& x1, const Vector& x2);
+	
+	/**
+	*  measurement between landmark and pose
+	*/
+	Vector mea_3D(const Vector& x,  const Vector& l);
+	Matrix Dmea1_3D(const Vector& x, const Vector& l);
+	Matrix Dmea2_3D(const Vector& x, const Vector& l);
+	
+	struct Point2Prior3D : public NonlinearFactor1 {
+		Point2Prior3D(const Vector& mu, double sigma, const std::string& key):
+		NonlinearFactor1(mu, sigma, prior_3D, key, Dprior_3D) {}
+	};
+	
+	struct Simulated3DMeasurement : public NonlinearFactor2 {
+		Simulated3DMeasurement(const Vector& z, double sigma, const std::string& key1, const std::string& key2):
+		NonlinearFactor2(z, sigma, mea_3D, key1, Dmea1_3D, key2, Dmea2_3D) {}
+	};
+	
+} // namespace gtsam

cpp/VSLAMFactor.cpp

@@ -0,0 +1,101 @@
+/**
+ * @file    VSLAMFactor.cpp
+ * @brief   A non-linear factor specialized to the Visual SLAM problem
+ * @author  Alireza Fathi
+ */
+
+#include "VSLAMFactor.h"
+#include "SimpleCamera.h"
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+VSLAMFactor::VSLAMFactor(const Vector& z, double sigma, int cn, int ln, const Cal3_S2 &K)
+  : NonlinearFactor(z, sigma)
+{
+  cameraFrameNumber_ = cn;
+  landmarkNumber_ = ln;
+  char temp[100];
+  temp[0] = 0;
+  sprintf(temp, "x%d", cameraFrameNumber_);
+  cameraFrameName_ = string(temp);
+  temp[0] = 0;
+  sprintf(temp, "l%d", landmarkNumber_);
+  landmarkName_ = string(temp);
+  K_ = K;
+}
+
+/* ************************************************************************* */
+void VSLAMFactor::print(const std::string& s) const {
+  printf("%s %s %s\n", s.c_str(), cameraFrameName_.c_str(), landmarkName_.c_str());
+  ::print(z_, s+".z");
+}
+
+/* ************************************************************************* */
+Vector VSLAMFactor::error_vector(const FGConfig& c) const {
+  Pose3  pose     = c[cameraFrameName_];
+  Point3 landmark = c[landmarkName_];
+
+  // Right-hand-side b = (z - h(x))/sigma
+  Vector h = project(SimpleCamera(K_,pose), landmark).vector();
+
+  return (z_ - h);
+}
+
+/* ************************************************************************* */
+LinearFactor::shared_ptr VSLAMFactor::linearize(const FGConfig& c) const
+{
+  // get arguments from config
+  Pose3  pose     = c[cameraFrameName_]; // cast from Vector to Pose3 !!!
+  Point3 landmark = c[landmarkName_];    // cast from Vector to Point3 !!
+
+  SimpleCamera camera(K_,pose);
+
+  // Jacobians
+  Matrix Dh1 = Dproject_pose(camera, landmark);
+  Matrix Dh2 = Dproject_point(camera, landmark);
+
+  // Right-hand-side b = (z - h(x))
+  Vector h = project(camera, landmark).vector();
+  Vector b = z_ - h;
+
+  // Make new linearfactor, divide by sigma
+  LinearFactor::shared_ptr
+    p(new LinearFactor(cameraFrameName_, Dh1/sigma_, landmarkName_, Dh2/sigma_, b/sigma_));
+  return p;
+}
+
+/* ************************************************************************* */
+bool VSLAMFactor::equals(const Factor& f, double tol) const {
+  const VSLAMFactor* p = dynamic_cast<const VSLAMFactor*>(&f);
+  if (p == NULL) goto fail;
+  if (cameraFrameNumber_ != p->cameraFrameNumber_ || landmarkNumber_ != p->landmarkNumber_) goto fail;
+  if (!equal_with_abs_tol(z_,p->z_,tol)) goto fail;
+  return true;
+
+ fail:
+  print("actual");
+  p->print("expected");
+  return false;
+}
+
+/* ************************************************************************* */
+string VSLAMFactor::dump() const
+{
+  int i = getCameraFrameNumber();
+  int j = getLandmarkNumber();
+  double sigma = get_sigma();
+  Vector z = get_measurement();
+  char buffer[200];
+  buffer[0] = 0;
+  sprintf(buffer, "1 %d %d %f %d", i, j , sigma, z.size());
+  for(size_t i = 0; i < z.size(); i++)
+    sprintf(buffer, "%s %f", buffer, z(i));
+  sprintf(buffer, "%s %s", buffer, K_.dump().c_str());
+
+  return string(buffer);
+	string temp;
+	return temp;
+}
+

cpp/VSLAMFactor.h

@@ -0,0 +1,70 @@
+/**
+ * @file    VSLAMFactor.h
+ * @brief   A Nonlinear Factor, specialized for visual SLAM
+ * @author  Alireza Fathi
+ */
+
+#pragma once
+
+#include "NonlinearFactor.h"
+#include "LinearFactor.h"
+#include "Cal3_S2.h"
+#include "Pose3.h"
+
+/**
+ * Non-linear factor for a constraint derived from a 2D measurement,
+ * i.e. the main building block for visual SLAM.
+ */
+class VSLAMFactor : public gtsam::NonlinearFactor
+{
+
+ private:
+  int cameraFrameNumber_, landmarkNumber_;
+  std::string cameraFrameName_, landmarkName_;
+  gtsam::Cal3_S2 K_; // Calibration stored in each factor. FD: need to think about this.
+
+ public:
+
+  typedef boost::shared_ptr<VSLAMFactor> shared_ptr; // shorthand for a smart pointer to a factor
+
+  /**
+   * Constructor
+   * @param z is the 2 dimensional location of point in image (the measurement)
+   * @param sigma is the standard deviation
+   * @param cameraFrameNumber is basically the frame number
+   * @param landmarkNumber is the index of the landmark
+   * @param K the constant calibration
+   */
+  VSLAMFactor(const Vector& z, double sigma, int cameraFrameNumber, int landmarkNumber, const gtsam::Cal3_S2& K);
+
+
+  /**
+   * print
+   * @param s optional string naming the factor
+   */
+  void print(const std::string& s="VSLAMFactor") const;
+
+  /**
+   * calculate the error of the factor
+   */
+  Vector error_vector(const gtsam::FGConfig&) const;
+
+  /**
+   * linerarization 
+   */
+  gtsam::LinearFactor::shared_ptr linearize(const gtsam::FGConfig&) const;
+
+  /**
+   * equals 
+   */
+  bool equals(const gtsam::Factor&, double tol=1e-9) const;
+
+  int getCameraFrameNumber() const { return cameraFrameNumber_; }
+  int getLandmarkNumber()    const { return landmarkNumber_;    }
+
+  /**
+   * dump the information of the factor into a string
+   */
+  std::string dump() const;
+};
+

cpp/Value.h

@@ -0,0 +1,49 @@
+/**
+ * @file    Value.h
+ * @brief   Abstract base class for values that can be updated using exmap
+ * @author  Frank Dellaert
+ */
+
+// \callgraph
+
+#pragma once
+
+#include "Vector.h"
+
+namespace gtsam {
+
+  /**
+   * The value class should be templated with the derived class, e.g.
+   * class Rot3 : public Value<rot3>. This allows us to define the
+   * return type of exmap as a Rot3 as well.
+   */
+  template <class Derived> class Value {
+
+  private:
+
+    const size_t dim_; // dimensionality of tangent space, e.g. 3 for Rot3
+
+  public:
+
+    Value(size_t dim) : dim_ (dim) {}
+
+    /**
+     * dimensionality of tangent space, e.g. 3 for Rot3
+     */
+    size_t dim() { return dim_;} 
+
+    /**
+     * print
+     * @param s optional string naming the factor
+     */
+    virtual void print(const std::string& s="") const = 0;
+
+    /**
+     * Exponential map: add a delta vector, addition for most simple
+     * types, but fully exponential map for types such as Rot3, which
+     * takes a 3-dim delta vector to update a 9-dim representation.
+     * equality up to tolerance
+     */
+    virtual Derived exmap(const Vector& delta) const = 0;
+  };
+}

cpp/Vector.cpp

@@ -0,0 +1,226 @@
+/**
+* @file    Vector.cpp
+* @brief   typedef and functions to augment Boost's ublas::vector<double>
+* @author  Kai Ni
+* @author  Frank Dellaert
+*/
+
+#include <stdarg.h>
+#include <iostream>
+#include <sstream>
+#include <iomanip>
+
+#ifdef WIN32
+#include <Windows.h>
+#endif
+
+#include <boost/random/normal_distribution.hpp>
+#include <boost/random/variate_generator.hpp>
+
+#include "Vector.h"
+
+using namespace std;
+
+namespace gtsam {
+  
+  void odprintf(const char *format, ...)
+  {
+    char    buf[4096], *p = buf;
+    int     n;
+    
+    va_list args;
+    va_start(args, format);
+    #ifdef WIN32
+    n = _vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
+    #else
+    n = vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
+    #endif
+    va_end(args);
+    
+    /*
+    p += (n < 0) ? sizeof buf - 3 : n;
+    
+    while ( p > buf  &&  isspace(p[-1]) )
+      *--p = '\0';
+    
+    *p++ = '\r';
+    *p++ = '\n';
+    *p   = '\0';
+    */
+    
+    #ifdef WIN32
+    OutputDebugString(buf);
+    #else    
+    printf(buf);
+    #endif
+    
+  }
+  
+  /* ************************************************************************* */
+  Vector Vector_( size_t m, const double* const data) {
+    Vector v(m);
+    copy(data, data+m, v.data().begin());
+    return v;
+  }
+  
+  /* ************************************************************************* */
+  Vector Vector_(size_t m, ...) {
+    Vector v(m);
+    va_list ap;
+    va_start(ap, m);
+    for( size_t i = 0 ; i < m ; i++) {
+      double value = va_arg(ap, double);
+      v(i) = value;
+    }
+    va_end(ap);
+    return v;
+  }
+  
+  /* ************************************************************************* */
+  bool zero(const Vector& v) {
+    bool result = true;
+    size_t n = v.size();
+    for( size_t j = 0 ; j < n ; j++)
+      result = result && (v(j) == 0.0);
+    return result;
+  }
+  
+  /* ************************************************************************* */
+  Vector zero(size_t n) { 
+    Vector v(n); fill_n(v.begin(),n,0);
+    return v;
+  }
+  
+  /* ************************************************************************* */
+  void print(const Vector& v, const string& s) {
+    size_t n = v.size();
+    odprintf("%s[", s.c_str());
+    for(size_t i=0; i<n; i++)
+      odprintf("%g%s", v[i], (i<n-1 ? "; " : ""));
+    odprintf("]\n");
+  }
+  
+  /* ************************************************************************* */
+  string dump(const Vector& v)
+  {
+	  ostringstream oss;
+	  oss << "[";
+	  size_t n = v.size();
+	  for(size_t i=0; i<n; i++)
+		  oss << v[i] << (i<n-1 ? "; " : "");
+	  oss << "]";
+	  return oss.str();
+  }
+
+  /* ************************************************************************* */
+  bool operator==(const Vector& vec1,const Vector& vec2) {
+    Vector::const_iterator it1 = vec1.begin();
+    Vector::const_iterator it2 = vec2.begin();
+    size_t m = vec1.size();
+    for(size_t i=0; i<m; i++)
+      if(it1[i] != it2[i])
+      return false;
+    return true;
+  }
+  
+  /* ************************************************************************* */
+  bool equal_with_abs_tol(const Vector& vec1, const Vector& vec2, double tol) {
+    Vector::const_iterator it1 = vec1.begin();
+    Vector::const_iterator it2 = vec2.begin();
+    if (vec1.size()!=vec2.size()) return false;
+    for(size_t i=0; i<vec1.size(); i++)
+      if(fabs(it1[i] - it2[i]) > tol)
+      return false;
+    return true;
+  }
+  
+  /* ************************************************************************* */
+  bool assert_equal(const Vector& vec1, const Vector& vec2, double tol) {
+    if (equal_with_abs_tol(vec1,vec2,tol)) return true;
+    cout << "not equal:" << endl;
+    print(vec1, "v1");
+    print(vec2, "v2");
+    return false;
+  }
+  
+  /* ************************************************************************* */
+  Vector sub(const Vector &v, size_t i1, size_t i2) {
+    size_t n = i2-i1;
+    Vector v_return(n);
+    for( size_t i = 0; i < n; i++ )
+      v_return(i) = v(i1 + i);
+    return v_return;
+  }
+  
+  /* ************************************************************************* */
+  pair<double, Vector > house(Vector &x)
+  {
+    const double x02 = x(0)*x(0);
+    const double sigma = inner_prod(trans(x),x) - x02;
+    double beta = 0.0;
+    
+    Vector v(x); v(0) = 1.0;
+    
+    if( sigma == 0.0 )
+      beta = 0.0;
+    else {
+      double mu = sqrt(x02 + sigma);
+      if( x(0) <= 0.0 )
+        v(0) = x(0) - mu;
+      else
+        v(0) = -sigma / (x(0) + mu);
+      
+      const double v02 = v(0)*v(0);
+      beta = 2.0 * v02 / (sigma + v02);
+      v = v / v(0);
+    }
+    
+    return make_pair(beta, v);
+  }
+  
+  /* ************************************************************************* */
+  Vector concatVectors(size_t nrVectors, ...)
+  {
+    int dimA = 0;
+    va_list ap;
+    va_start(ap, nrVectors);
+    Vector* V;
+    for( size_t i = 0 ; i < nrVectors ; i++)
+    {
+      V = va_arg(ap, Vector*);
+      dimA += V->size();
+    }
+    va_end(ap);
+    va_start(ap, nrVectors);
+    Vector A(dimA);
+    int index = 0;
+    for( size_t i = 0 ; i < nrVectors ; i++)
+    {
+      V = va_arg(ap, Vector *);
+      for(size_t d = 0; d < V->size(); d++)
+        A(d+index) = (*V)(d);
+      index += V->size();
+    }  
+    
+    return A;
+  }
+  
+  /* ************************************************************************* */
+  Vector rand_vector_norm(size_t dim, double mean, double sigma)
+  {
+    boost::normal_distribution<double> norm_dist(mean, sigma);
+    boost::variate_generator<boost::minstd_rand&, boost::normal_distribution<double> > norm(generator, norm_dist);
+    
+    Vector v(dim);
+    Vector::iterator it_v;
+    for(it_v=v.begin(); it_v!=v.end(); it_v++)
+      *it_v = norm();
+    
+    return v;
+  }
+  
+  /* ************************************************************************* */
+  
+  
+  
+} // namespace gtsam

cpp/Vector.h

@@ -0,0 +1,114 @@
+/**
+ * @file    Vector.h
+ * @brief   typedef and functions to augment Boost's ublas::vector<double>
+ * @author  Kai Ni
+ * @author  Frank Dellaert
+ */
+
+// \callgraph
+
+#pragma once
+
+#include <boost/numeric/ublas/vector.hpp>
+#include <boost/random/linear_congruential.hpp>
+
+// Vector is a *global* typedef
+// wrap-matlab does this typedef as well
+#if ! defined (MEX_H)
+typedef boost::numeric::ublas::vector<double> Vector;
+#endif
+
+namespace gtsam {
+
+
+void odprintf(const char *format, ...);
+
+/**
+ *  constructor with size and initial data, row order !
+ */
+Vector Vector_( size_t m, const double* const data);
+
+/**
+ *  nice constructor, dangerous as number of arguments must be exactly right
+ *  and you have to pass doubles !!! always use 0.0 never 0
+ */
+Vector Vector_(size_t m, ...);
+
+/**
+ * Create zero vector
+ * @ param size
+ */
+Vector zero(size_t n);
+	
+/**
+ * check if all zero
+ */
+bool zero(const Vector& v);
+	
+/**
+ * print with optional string
+ */
+void print(const Vector& v, const std::string& s = "");
+
+/**
+ * returns a string suitable for printing
+ */
+std::string dump(const Vector& v);
+
+/**
+ * operator==()
+ */
+bool operator==(const Vector& vec1,const Vector& vec2);
+
+/**
+ * VecA == VecB up to tolerance
+ */
+bool equal_with_abs_tol(const Vector& vec1, const Vector& vec2, double tol=1e-9);
+
+/**
+ * Same, prints if error
+ * @param vec1 Vector
+ * @param vec2 Vector
+ * @param tol 1e-9
+ * @return bool
+ */
+bool assert_equal(const Vector& vec1, const Vector& vec2, double tol=1e-9);
+
+/**
+ * vector multiplication by scalar
+inline Vector operator*(double s, const Vector& vec ) {
+return vec*s;
+}
+*/
+
+/**
+ * extract subvector, slice semantics, i.e. range = [i1,i2[ excluding i2
+ * @param v Vector
+ * @param i1 first row index
+ * @param i2 last  row index + 1
+ * @return subvector v(i1:i2)
+ */
+Vector sub(const Vector &v, size_t i1, size_t i2);
+
+/**
+ * house(x,j) computes HouseHolder vector v and scaling factor beta
+ *  from x, such that the corresponding Householder reflection zeroes out
+ *  all but x.(j), j is base 0. Golub & Van Loan p 210.
+ */
+std::pair<double,Vector> house(Vector &x);
+
+/**
+ * concatenate Vectors
+ */
+Vector concatVectors(size_t nrVectors, ...);
+
+
+/**
+ * random vector
+ */
+Vector rand_vector_norm(size_t dim, double mean = 0, double sigma = 1);
+
+} // namespace gtsam
+
+static boost::minstd_rand generator(42u);
+

cpp/gtsam.h

@@ -0,0 +1,147 @@
+class FGConfig {
+  FGConfig();
+  Vector get(string name) const;
+  bool contains(string name) const;
+  size_t size() const;
+  void insert(string name, Vector val);
+  void print() const;
+  bool equals(const FGConfig& expected, double tol) const;
+  void clear();
+};
+
+class LinearFactorSet {
+  LinearFactorSet();
+  void insert(LinearFactor* factor);
+};
+
+class LinearFactor {
+  LinearFactor(string key1,
+	       Matrix A1,
+	       Vector b_in);
+  LinearFactor(string key1,
+	       Matrix A1,
+	       string key2,
+	       Matrix A2,
+	       Vector b_in);
+  LinearFactor(string key1,
+	       Matrix A1,
+	       string key2,
+	       Matrix A2,
+	       string key3,
+	       Matrix A3,
+	       Vector b_in);
+  bool empty() const;
+  Vector get_b() const;
+  Matrix get_A(string key) const;
+  double error(const FGConfig& c) const;
+  bool involves(string key) const;
+  void print() const;
+  bool equals(const LinearFactor& lf, double tol) const;
+  pair<Matrix,Vector> matrix(const Ordering& ordering) const;
+};
+
+class ConditionalGaussian {
+  ConditionalGaussian();
+  ConditionalGaussian(Vector d,
+		      Matrix R);
+  ConditionalGaussian(Vector d,
+		      Matrix R,
+		      string name1,
+		      Matrix S);
+  ConditionalGaussian(Vector d,
+		      Matrix R,
+		      string name1,
+		      Matrix S,
+		      string name2,
+		      Matrix T);
+  void print() const;
+  Vector solve(const FGConfig& x);
+  void add(string key, Matrix S);
+  bool equals(const ConditionalGaussian &cg) const;
+};
+
+class Ordering {
+  Ordering();
+  void push_back(string s);
+  void print() const;
+};
+
+class ChordalBayesNet {
+  ChordalBayesNet();
+  void insert(string name, ConditionalGaussian* node);
+  ConditionalGaussian* get(string name);
+  FGConfig* optimize();
+  void print() const;
+  bool equals(const ChordalBayesNet& cbn) const;
+  pair<Matrix,Vector> matrix() const;
+};
+
+class LinearFactorGraph {
+  LinearFactorGraph();
+
+  size_t size() const;
+  void push_back(LinearFactor* ptr_f);
+  double error(const FGConfig& c) const;
+  double probPrime(const FGConfig& c) const;
+  void print() const;
+  bool equals(const LinearFactorGraph& lfgraph) const;
+
+  FGConfig optimize(const Ordering& ordering);
+  LinearFactor* combine_factors(string key);
+  ConditionalGaussian* eliminate_one(string key);
+  ChordalBayesNet* eliminate(const Ordering& ordering);
+  pair<Matrix,Vector> matrix(const Ordering& ordering) const;
+};
+
+class Point2 {
+  Point2();
+  Point2(double x, double y);
+  double x();
+  double y();
+  size_t dim() const;
+  void print() const;
+};
+
+class Point3 {
+  Point3();
+  Point3(double x, double y, double z);
+  Point3(Vector v);
+  size_t dim() const;
+  Point3 exmap(Vector d) const;
+  Vector vector() const;
+  double x();
+  double y();
+  double z();
+  void print() const;
+}; 
+
+class Point2Prior {
+  Point2Prior(Vector mu, double sigma, string key);
+  Vector error_vector(const FGConfig& c) const;
+  LinearFactor* linearize(const FGConfig& c) const;
+  double get_sigma();
+  Vector get_measurement();
+  double error(const FGConfig& c) const;
+  void print() const;
+};
+
+class Simulated2DOdometry {
+  Simulated2DOdometry(Vector odo, double sigma, string key, string key2);
+  Vector error_vector(const FGConfig& c) const;
+  LinearFactor* linearize(const FGConfig& c) const;
+  double get_sigma();
+  Vector get_measurement();
+  double error(const FGConfig& c) const;
+  void print() const;
+};
+
+class Simulated2DMeasurement {
+  Simulated2DMeasurement(Vector odo, double sigma, string key, string key2);
+  Vector error_vector(const FGConfig& c) const;
+  LinearFactor* linearize(const FGConfig& c) const;
+  double get_sigma();
+  Vector get_measurement();
+  double error(const FGConfig& c) const;
+  void print() const;
+};
+

cpp/gtsam.sln

@@ -0,0 +1,20 @@
+
+Microsoft Visual Studio Solution File, Format Version 10.00
+# Visual Studio 2008
+Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "gtsam", "gtsam.vcproj", "{8AC1F57D-77D6-4B79-B50C-2508F076EBA3}"
+EndProject
+Global
+	GlobalSection(SolutionConfigurationPlatforms) = preSolution
+		Debug|Win32 = Debug|Win32
+		Release|Win32 = Release|Win32
+	EndGlobalSection
+	GlobalSection(ProjectConfigurationPlatforms) = postSolution
+		{8AC1F57D-77D6-4B79-B50C-2508F076EBA3}.Debug|Win32.ActiveCfg = Debug|Win32
+		{8AC1F57D-77D6-4B79-B50C-2508F076EBA3}.Debug|Win32.Build.0 = Debug|Win32
+		{8AC1F57D-77D6-4B79-B50C-2508F076EBA3}.Release|Win32.ActiveCfg = Release|Win32
+		{8AC1F57D-77D6-4B79-B50C-2508F076EBA3}.Release|Win32.Build.0 = Release|Win32
+	EndGlobalSection
+	GlobalSection(SolutionProperties) = preSolution
+		HideSolutionNode = FALSE
+	EndGlobalSection
+EndGlobal

cpp/gtsam.vcproj

@@ -0,0 +1,362 @@
+<?xml version="1.0" encoding="gb2312"?>
+<VisualStudioProject
+	ProjectType="Visual C++"
+	Version="9.00"
+	Name="gtsam"
+	ProjectGUID="{8AC1F57D-77D6-4B79-B50C-2508F076EBA3}"
+	RootNamespace="gtsam"
+	Keyword="Win32Proj"
+	TargetFrameworkVersion="196613"
+	>
+	<Platforms>
+		<Platform
+			Name="Win32"
+		/>
+	</Platforms>
+	<ToolFiles>
+	</ToolFiles>
+	<Configurations>
+		<Configuration
+			Name="Debug|Win32"
+			OutputDirectory="$(SolutionDir)$(ConfigurationName)"
+			IntermediateDirectory="$(ConfigurationName)"
+			ConfigurationType="4"
+			CharacterSet="2"
+			>
+			<Tool
+				Name="VCPreBuildEventTool"
+			/>
+			<Tool
+				Name="VCCustomBuildTool"
+			/>
+			<Tool
+				Name="VCXMLDataGeneratorTool"
+			/>
+			<Tool
+				Name="VCWebServiceProxyGeneratorTool"
+			/>
+			<Tool
+				Name="VCMIDLTool"
+			/>
+			<Tool
+				Name="VCCLCompilerTool"
+				Optimization="0"
+				AdditionalIncludeDirectories="&quot;$(BOOST_DIR)&quot;"
+				PreprocessorDefinitions="WIN32;_DEBUG;_LIB;_CRT_SECURE_NO_WARNINGS;_SCL_SECURE_NO_WARNINGS"
+				MinimalRebuild="true"
+				BasicRuntimeChecks="3"
+				RuntimeLibrary="3"
+				UsePrecompiledHeader="0"
+				WarningLevel="3"
+				DebugInformationFormat="3"
+			/>
+			<Tool
+				Name="VCManagedResourceCompilerTool"
+			/>
+			<Tool
+				Name="VCResourceCompilerTool"
+			/>
+			<Tool
+				Name="VCPreLinkEventTool"
+			/>
+			<Tool
+				Name="VCLibrarianTool"
+				OutputFile="$(ProjectDir)\$(ConfigurationName)\lib$(ProjectName).lib"
+			/>
+			<Tool
+				Name="VCALinkTool"
+			/>
+			<Tool
+				Name="VCXDCMakeTool"
+			/>
+			<Tool
+				Name="VCBscMakeTool"
+			/>
+			<Tool
+				Name="VCFxCopTool"
+			/>
+			<Tool
+				Name="VCPostBuildEventTool"
+				CommandLine="mkdir  $(ProjectDir)\..\..\..\libs\;&#x0D;&#x0A;copy $(ProjectDir)\$(ConfigurationName)\lib$(ProjectName).lib  $(ProjectDir)\..\..\..\libs\;&#x0D;&#x0A;mkdir  $(ProjectDir)\..\..\..\include\;&#x0D;&#x0A;mkdir  $(ProjectDir)\..\..\..\include\gtsam\;&#x0D;&#x0A;copy  $(ProjectDir)\*.h $(ProjectDir)\..\..\..\include\gtsam\;&#x0D;&#x0A;"
+			/>
+		</Configuration>
+		<Configuration
+			Name="Release|Win32"
+			OutputDirectory="$(SolutionDir)$(ConfigurationName)"
+			IntermediateDirectory="$(ConfigurationName)"
+			ConfigurationType="2"
+			CharacterSet="1"
+			WholeProgramOptimization="1"
+			>
+			<Tool
+				Name="VCPreBuildEventTool"
+			/>
+			<Tool
+				Name="VCCustomBuildTool"
+			/>
+			<Tool
+				Name="VCXMLDataGeneratorTool"
+			/>
+			<Tool
+				Name="VCWebServiceProxyGeneratorTool"
+			/>
+			<Tool
+				Name="VCMIDLTool"
+			/>
+			<Tool
+				Name="VCCLCompilerTool"
+				Optimization="2"
+				EnableIntrinsicFunctions="true"
+				PreprocessorDefinitions="WIN32;NDEBUG;_WINDOWS;_USRDLL;GTSAM_EXPORTS"
+				RuntimeLibrary="2"
+				EnableFunctionLevelLinking="true"
+				UsePrecompiledHeader="0"
+				WarningLevel="3"
+				DebugInformationFormat="3"
+			/>
+			<Tool
+				Name="VCManagedResourceCompilerTool"
+			/>
+			<Tool
+				Name="VCResourceCompilerTool"
+			/>
+			<Tool
+				Name="VCPreLinkEventTool"
+			/>
+			<Tool
+				Name="VCLinkerTool"
+				LinkIncremental="1"
+				GenerateDebugInformation="true"
+				SubSystem="2"
+				OptimizeReferences="2"
+				EnableCOMDATFolding="2"
+				TargetMachine="1"
+			/>
+			<Tool
+				Name="VCALinkTool"
+			/>
+			<Tool
+				Name="VCManifestTool"
+			/>
+			<Tool
+				Name="VCXDCMakeTool"
+			/>
+			<Tool
+				Name="VCBscMakeTool"
+			/>
+			<Tool
+				Name="VCFxCopTool"
+			/>
+			<Tool
+				Name="VCAppVerifierTool"
+			/>
+			<Tool
+				Name="VCPostBuildEventTool"
+			/>
+		</Configuration>
+	</Configurations>
+	<References>
+	</References>
+	<Files>
+		<Filter
+			Name="Source Files"
+			Filter="cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx"
+			UniqueIdentifier="{4FC737F1-C7A5-4376-A066-2A32D752A2FF}"
+			>
+			<File
+				RelativePath=".\cal3_S2.cpp"
+				>
+			</File>
+			<File
+				RelativePath=".\chordalBayesNet.cpp"
+				>
+			</File>
+			<File
+				RelativePath=".\conditionalGaussian.cpp"
+				>
+			</File>
+			<File
+				RelativePath=".\factorGraph.cpp"
+				>
+			</File>
+			<File
+				RelativePath=".\fgConfig.cpp"
+				>
+			</File>
+			<File
+				RelativePath=".\linearFactor.cpp"
+				>
+			</File>
+			<File
+				RelativePath=".\linearFactorGraph.cpp"
+				>
+			</File>
+			<File
+				RelativePath=".\matrix.cpp"
+				>
+			</File>
+			<File
+				RelativePath=".\nonlinearFactor.cpp"
+				>
+			</File>
+			<File
+				RelativePath=".\nonlinearFactorGraph.cpp"
+				>
+			</File>
+			<File
+				RelativePath=".\numericalDerivative.cpp"
+				>
+			</File>
+			<File
+				RelativePath=".\ordering.cpp"
+				>
+			</File>
+			<File
+				RelativePath=".\Point2.cpp"
+				>
+			</File>
+			<File
+				RelativePath=".\Point3.cpp"
+				>
+			</File>
+			<File
+				RelativePath=".\Pose3.cpp"
+				>
+			</File>
+			<File
+				RelativePath=".\Rot3.cpp"
+				>
+			</File>
+			<File
+				RelativePath=".\simulated2D.cpp"
+				>
+			</File>
+			<File
+				RelativePath=".\smallExample.cpp"
+				>
+			</File>
+			<File
+				RelativePath=".\svdcmp.cpp"
+				>
+			</File>
+			<File
+				RelativePath=".\vector.cpp"
+				>
+			</File>
+		</Filter>
+		<Filter
+			Name="Header Files"
+			Filter="h;hpp;hxx;hm;inl;inc;xsd"
+			UniqueIdentifier="{93995380-89BD-4b04-88EB-625FBE52EBFB}"
+			>
+			<File
+				RelativePath=".\cal3_S2.h"
+				>
+			</File>
+			<File
+				RelativePath=".\chordalBayesNet.h"
+				>
+			</File>
+			<File
+				RelativePath=".\conditionalGaussian.h"
+				>
+			</File>
+			<File
+				RelativePath=".\factor.h"
+				>
+			</File>
+			<File
+				RelativePath=".\factorGraph.h"
+				>
+			</File>
+			<File
+				RelativePath=".\fgConfig.h"
+				>
+			</File>
+			<File
+				RelativePath=".\gtsam.h"
+				>
+			</File>
+			<File
+				RelativePath=".\linearFactor.h"
+				>
+			</File>
+			<File
+				RelativePath=".\linearFactorGraph.h"
+				>
+			</File>
+			<File
+				RelativePath=".\LinearFactorSet.h"
+				>
+			</File>
+			<File
+				RelativePath=".\matrix.h"
+				>
+			</File>
+			<File
+				RelativePath=".\nonlinearFactor.h"
+				>
+			</File>
+			<File
+				RelativePath=".\nonlinearFactorGraph.h"
+				>
+			</File>
+			<File
+				RelativePath=".\numericalDerivative.h"
+				>
+			</File>
+			<File
+				RelativePath=".\ordering.h"
+				>
+			</File>
+			<File
+				RelativePath=".\Point2.h"
+				>
+			</File>
+			<File
+				RelativePath=".\Point3.h"
+				>
+			</File>
+			<File
+				RelativePath=".\Pose3.h"
+				>
+			</File>
+			<File
+				RelativePath=".\Rot3.h"
+				>
+			</File>
+			<File
+				RelativePath=".\simulated2D.h"
+				>
+			</File>
+			<File
+				RelativePath=".\smallExample.h"
+				>
+			</File>
+			<File
+				RelativePath=".\svdcmp.h"
+				>
+			</File>
+			<File
+				RelativePath=".\Value.h"
+				>
+			</File>
+			<File
+				RelativePath=".\vector.h"
+				>
+			</File>
+		</Filter>
+		<Filter
+			Name="Resource Files"
+			Filter="rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav"
+			UniqueIdentifier="{67DA6AB6-F800-4c08-8B7A-83BB121AAD01}"
+			>
+		</Filter>
+		<File
+			RelativePath=".\Makefile"
+			>
+		</File>
+	</Files>
+	<Globals>
+	</Globals>
+</VisualStudioProject>

cpp/manual.mk

@@ -0,0 +1,139 @@
+# Makefile for gtsam/cpp
+# Author Frank Dellaert
+
+# on the Mac, libtool is called glibtool :-(
+# documentation see /opt/local/share/doc/libtool-1.5.26/manual.html
+ifeq ($(shell uname),Darwin)
+  LIBTOOL = glibtool
+else
+  LIBTOOL = libtool
+endif
+
+INSTALL = install
+
+# C++ flags
+CXXFLAGS += -isystem $(BOOST_DIR) -O5
+CXXFLAGS += -DBOOST_UBLAS_NDEBUG
+
+# specify the source files
+# basic
+sources = Vector.cpp svdcmp.cpp Matrix.cpp numericalDerivative.cpp Ordering.cpp
+# nodes
+sources += FGConfig.cpp LinearFactor.cpp ConditionalGaussian.cpp NonlinearFactor.cpp 
+# graphs
+sources += FactorGraph.cpp LinearFactorGraph.cpp NonlinearFactorGraph.cpp ChordalBayesNet.cpp
+# geometry
+sources += Point2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp
+
+# The header files will be installed in ~/include/gtsam
+headers = Value.h factor.h linearfactorset.h $(sources:.cpp=.h)
+
+# conventional object files
+object_files = $(sources:.cpp=.o)
+
+# For libtool to build a shared library, we need "shared" object files with extension .lo
+shared_object_files = $(sources:.cpp=.lo)
+
+# rule for shared compiling shared_object_files
+%.lo: %.o
+
+# rule for shared compiling shared_object_files
+%.lo: %.cpp
+	$(LIBTOOL) --tag=CXX --mode=compile $(COMPILE.cpp) $(OUTPUT_OPTION) $<
+
+# library version
+current = 0  # The most recent interface number that this library implements. 
+revision = 0 # The implementation number of the current interface 
+age = 0 # The difference between the newest and oldest interfaces that	\
+this library implements. In other words, the library implements all	\
+the interface numbers in the range from number current - age to		\
+current.
+# from libtool manual:
+# Here are a set of rules to help you update your library version information:
+#   Start with version information of ‘0:0:0’ for each libtool library.
+#   Update the version information only immediately before a public release of your software.
+#   If the library source code has changed at all since the last update, then increment revision
+#   If any interfaces have been added, removed, or changed since the last update, increment current, and set revision to 0.
+#   If any interfaces have been added since the last public release, then increment age.
+#   If any interfaces have been removed since the last public release, then set age to 0.
+version = $(current):$(revision):$(age)
+
+# this builds the shared library
+# note that libgtsam.la is the libtool target
+# the actual library is built in the hidden subdirectory .libs
+libgtsam.la : $(shared_object_files) 
+	$(LIBTOOL) --tag=CXX --mode=link g++ -version-info $(version) -o libgtsam.la -rpath $(HOME)/lib $(shared_object_files)
+
+# shortcut
+lib: libgtsam.la
+
+# this builds the static library (used for unit testing)
+# object files will be only ones remade if a file is touched because deps broken
+libgtsam.a : $(shared_object_files) $(object_files) 
+	$(LIBTOOL) --tag=CXX --mode=link g++ -o libgtsam.a -static $(HOME)/lib $(shared_object_files)
+
+# and this installs the shared library
+install: libgtsam.la
+	$(INSTALL) -d $(HOME)/include/gtsam
+	rm -f $(HOME)/include/gtsam/typedefs.h
+	cp -f $(headers) $(HOME)/include/gtsam
+	$(LIBTOOL) --mode=install cp libgtsam.la $(HOME)/lib/libgtsam.la
+
+# create the MATLAB toolbox
+interfacePath = .
+moduleName = gtsam
+toolboxpath = $(HOME)/toolbox/gtsam
+mexFlags = "-I$(BOOST_DIR) -I$(HOME)/include -I$(HOME)/include/gtsam -L$(HOME)/lib -lgtsam"
+matlab:
+	wrap $(interfacePath) $(moduleName) $(toolboxpath) $(mexFlags)
+
+# unit tests
+unit-tests = $(shell ls test*.cpp) 
+unit-tests: $(unit-tests:.cpp=.run)
+
+# timing tests
+timing-tests = $(shell ls time*.cpp) 
+timing-tests: $(timing-tests:.cpp=.run)
+
+# local executables are for testing and timing
+executables =  $(unit-tests:.cpp=) $(timing-tests:.cpp=)
+
+# link flags
+INCDIR ?= $(HOME)/include
+LIBDIR ?= $(HOME)/lib
+$(executables) : simulated2D.o smallExample.o libgtsam.a
+$(executables) : LDFLAGS += -I. -I$(INCDIR)
+$(executables) : LDLIBS += libgtsam.a -L$(LIBDIR) -lCppUnitLite
+
+tests: unit-tests timing-tests
+clean-tests:
+	-rm -rf $(executables)
+
+# make a version of timeLinearFactor instrumented for Saturn profiler
+saturn: timeLinearFactor
+saturn: CXXFLAGS += -finstrument-functions
+saturn: LDLIBS += -lSaturn
+
+# rule to run an executable
+%.run: %
+	./$^
+
+# clean will remove the hidden .libs directory by libtool as well
+clean: clean-tests
+	-rm -rf *.d *.o *.lo *.a *.la .libs *.dSYM
+
+.PHONY: clean clean-tests unit-tests timing-tests matlab deps
+
+# dependecy generation as described in
+# http://www.wlug.org.nz/MakefileHowto
+all-sources = $(sources) smallExample.cpp simulated2D.cpp
+
+# when building object files, -MMD specifies dependency generation into .d files
+(all-sources:.cpp=.o): CXXFLAGS += -MMD
+
+deps := $(all-sources:.cpp=.d)
+deps: $(all-sources)
+	$(CXX) -MMD -E $(CXXFLAGS) -I. -I$(INCDIR) $(all-sources) > /dev/null
+
+-include $(deps)
+

cpp/numericalDerivative.cpp

@@ -0,0 +1,60 @@
+/**
+ * @file    numericalDerivative.cpp
+ * @brief   Some functions to compute numerical derivatives
+ * @author  Frank Dellaert
+ */
+
+#include "numericalDerivative.h"
+
+namespace gtsam {
+
+/* ************************************************************************* */
+  Matrix NumericalDerivative11
+  (Vector (*h)(const Vector&), const Vector& x_, double delta) {
+    Vector x(x_), hx = h(x);
+    double factor = 1.0/(2.0*delta);
+    const size_t m = hx.size(), n = x.size();
+    Matrix H = zeros(m,n);
+    for (size_t j=0;j<n;j++) {
+      x(j) +=   delta; Vector hxplus = h(x);
+      x(j) -= 2*delta; Vector hxmin  = h(x);
+      x(j) +=   delta; Vector dh = (hxplus-hxmin)*factor;
+      for (size_t i=0;i<m;i++) H(i,j) = dh(i);
+    }
+    return H;
+  }
+
+/* ************************************************************************* */
+  Matrix NumericalDerivative21
+  (Vector (*h)(const Vector&, const Vector&), const Vector& x1_, const Vector& x2, double delta) {
+    Vector x1(x1_), hx = h(x1,x2);
+    double factor = 1.0/(2.0*delta);
+    const size_t m = hx.size(), n = x1.size();
+    Matrix H = zeros(m,n);
+    for (size_t j=0;j<n;j++) {
+      x1(j) +=   delta; Vector hxplus = h(x1,x2);
+      x1(j) -= 2*delta; Vector hxmin  = h(x1,x2);
+      x1(j) +=   delta; Vector dh = (hxplus-hxmin)*factor;
+      for (size_t i=0;i<m;i++) H(i,j) = dh(i);
+    }
+    return H;
+  }
+
+/* ************************************************************************* */
+  Matrix NumericalDerivative22
+  (Vector (*h)(const Vector&, const Vector&), const Vector& x1, const Vector& x2_, double delta) {
+    Vector x2(x2_), hx = h(x1,x2);
+    double factor = 1.0/(2.0*delta);
+    const size_t m = hx.size(), n = x2.size();
+    Matrix H = zeros(m,n);
+    for (size_t j=0;j<n;j++) {
+      x2(j) +=   delta; Vector hxplus = h(x1,x2);
+      x2(j) -= 2*delta; Vector hxmin  = h(x1,x2);
+      x2(j) +=   delta; Vector dh = (hxplus-hxmin)*factor;
+      for (size_t i=0;i<m;i++) H(i,j) = dh(i);
+    }
+    return H;
+  }
+/* ************************************************************************* */
+
+}

cpp/numericalDerivative.h

@@ -0,0 +1,150 @@
+/**
+ * @file    numericalDerivative.h
+ * @brief   Some functions to compute numerical derivatives
+ * @author  Frank Dellaert
+ */
+
+// \callgraph
+
+#pragma once
+
+#include "Matrix.h"
+
+namespace gtsam {
+
+  /**
+   * Compute numerical derivative in arument 1 of unary function
+   * @param h unary function yielding m-vector
+   * @param x n-dimensional value at which to evaluate h
+   * @param delta increment for numerical derivative
+   * @return m*n Jacobian computed via central differencing
+   */
+  Matrix NumericalDerivative11
+   (Vector (*h)(const Vector&), const Vector& x, double delta=1e-5);
+
+  /**
+	* templated version (starts with LOWERCASE n)
+	 * Class Y is the output arguement
+	 * Class X is the input argument
+   * both classes need dim, exmap, vector
+   */
+  template<class Y, class X>
+    Matrix numericalDerivative11
+    (Y (*h)(const X&), const X& x, double delta=1e-5) {
+    Vector hx = h(x).vector();
+    double factor = 1.0/(2.0*delta);
+    const size_t m = hx.size(), n = x.dim();
+    Vector d(n,0.0);
+    Matrix H = zeros(m,n);
+    for (size_t j=0;j<n;j++) {
+      d(j) +=   delta; Vector hxplus = h(x.exmap(d)).vector(); 
+      d(j) -= 2*delta; Vector hxmin  = h(x.exmap(d)).vector();
+      d(j) +=   delta; Vector dh = (hxplus-hxmin)*factor;
+      for (size_t i=0;i<m;i++) H(i,j) = dh(i);
+    }
+    return H;
+  }
+
+  /**
+   * Compute numerical derivative in argument 1 of binary function
+   * @param h binary function yielding m-vector
+   * @param x1 n-dimensional first argument value
+   * @param x2 second argument value
+   * @param delta increment for numerical derivative
+   * @return m*n Jacobian computed via central differencing
+   */
+  Matrix NumericalDerivative21
+    (Vector (*h)(const Vector&, const Vector&), const Vector& x1, const Vector& x2, double delta=1e-5);
+
+  /**
+   * templated version (starts with LOWERCASE n)
+   * classes need dim, exmap, vector
+   */
+   template<class Y, class X1, class X2>
+    Matrix numericalDerivative21
+    (Y (*h)(const X1&, const X2&), 
+     const X1& x1, const X2& x2, double delta=1e-5) 
+  {
+    Vector hx = h(x1,x2).vector();
+    double factor = 1.0/(2.0*delta);
+    const size_t m = hx.size(), n = x1.dim();
+    Vector d(n,0.0);
+    Matrix H = zeros(m,n);
+    for (size_t j=0;j<n;j++) {
+      d(j) +=   delta; Vector hxplus = h(x1.exmap(d),x2).vector();
+      d(j) -= 2*delta; Vector hxmin  = h(x1.exmap(d),x2).vector();
+      d(j) +=   delta; Vector dh = (hxplus-hxmin)*factor;
+      for (size_t i=0;i<m;i++) H(i,j) = dh(i);
+    }
+    return H;
+  }
+
+  /**
+   * Compute numerical derivative in arument 2 of binary function
+   * @param h binary function yielding m-vector
+   * @param x1 first argument value
+   * @param x2 n-dimensional second argument value
+   * @param delta increment for numerical derivative
+   * @return m*n Jacobian computed via central differencing
+   */
+  Matrix NumericalDerivative22
+    (Vector (*h)(const Vector&, const Vector&), const Vector& x1, const Vector& x2, double delta=1e-5);
+
+  /**
+   *  templated version (starts with LOWERCASE n)
+   * classes need dim, exmap, vector
+   */
+  template<class Y, class X1, class X2>
+    Matrix numericalDerivative22
+    (Y (*h)(const X1&, const X2&), 
+     const X1& x1, const X2& x2, double delta=1e-5) 
+  {
+    Vector hx = h(x1,x2).vector();
+    double factor = 1.0/(2.0*delta);
+    const size_t m = hx.size(), n = x2.dim();
+    Vector d(n,0.0);
+    Matrix H = zeros(m,n);
+    for (size_t j=0;j<n;j++) {
+      d(j) +=   delta; Vector hxplus = h(x1,x2.exmap(d)).vector();
+      d(j) -= 2*delta; Vector hxmin  = h(x1,x2.exmap(d)).vector();
+      d(j) +=   delta; Vector dh = (hxplus-hxmin)*factor;
+      for (size_t i=0;i<m;i++) H(i,j) = dh(i);
+    }
+    return H;
+  }
+
+  /**
+   * Compute numerical derivative in argument 1 of binary function
+   * @param h binary function yielding m-vector
+   * @param x1 n-dimensional first argument value
+   * @param x2 second argument value
+   * @param delta increment for numerical derivative
+   * @return m*n Jacobian computed via central differencing
+   */
+  Matrix NumericalDerivative31
+    (Vector (*h)(const Vector&, const Vector&, const Vector&), const Vector& x1, const Vector& x2, const Vector& x3, double delta=1e-5);
+
+  /**
+   * templated version (starts with LOWERCASE n)
+   * classes need dim, exmap, vector
+   */
+  template<class Y, class X1, class X2, class X3>
+    Matrix numericalDerivative31
+    (Y (*h)(const X1&, const X2&, const X3&), 
+     const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) 
+  {
+    Vector hx = h(x1,x2,x3).vector();
+    double factor = 1.0/(2.0*delta);
+    const size_t m = hx.size(), n = x1.dim();
+    Vector d(n,0.0);
+    Matrix H = zeros(m,n);
+    for (size_t j=0;j<n;j++) {
+      d(j) +=   delta; Vector hxplus = h(x1.exmap(d),x2,x3).vector();
+      d(j) -= 2*delta; Vector hxmin  = h(x1.exmap(d),x2,x3).vector();
+      d(j) +=   delta; Vector dh = (hxplus-hxmin)*factor;
+      for (size_t i=0;i<m;i++) H(i,j) = dh(i);
+    }
+    return H;
+  }
+
+}

cpp/simulated2D.cpp

@@ -0,0 +1,67 @@
+/**
+ * @file    simulated2D.cpp
+ * @brief   measurement functions and derivatives for simulated 2D robot
+ * @author  Frank Dellaert
+ */
+
+#include "simulated2D.h"
+
+namespace gtsam {
+
+
+/** prior on a single pose */
+
+/* ************************************************************************* */
+Vector prior (const Vector& x) {return x;}
+
+/* ************************************************************************* */
+Matrix Dprior(const Vector& x) {
+  Matrix H(2,2);
+  H(0,0)=1;  H(0,1)=0;
+  H(1,0)=0;  H(1,1)=1;
+  return H;
+}
+/* ************************************************************************* */
+
+/** odometry between two poses */                                
+
+/* ************************************************************************* */
+Vector odo(const Vector& x1, const Vector& x2) {return x2 - x1;}
+Matrix Dodo1(const Vector& x1, const Vector& x2) {
+  Matrix H(2,2);
+  H(0,0)=-1;  H(0,1)= 0;
+  H(1,0)= 0;  H(1,1)=-1;
+  return H;
+}
+
+/* ************************************************************************* */
+Matrix Dodo2(const Vector& x1, const Vector& x2) {
+  Matrix H(2,2);
+  H(0,0)= 1;  H(0,1)= 0;
+  H(1,0)= 0;  H(1,1)= 1;
+  return H;
+}
+
+/* ************************************************************************* */
+
+/** measurement between landmark and pose */
+
+/* ************************************************************************* */
+Vector mea(const Vector& x,  const Vector& l)  {return l  - x;}
+Matrix Dmea1(const Vector& x, const Vector& l) {
+  Matrix H(2,2);
+  H(0,0)=-1;  H(0,1)= 0;
+  H(1,0)= 0;  H(1,1)=-1;
+  return H;
+}
+
+/* ************************************************************************* */
+Matrix Dmea2(const Vector& x, const Vector& l) {
+  Matrix H(2,2);
+  H(0,0)= 1;  H(0,1)= 0;
+  H(1,0)= 0;  H(1,1)= 1;
+  return H;
+}
+
+/* ************************************************************************* */
+} // namespace gtsam

cpp/simulated2D.h

@@ -0,0 +1,37 @@
+/**
+ * @file    simulated2D.h
+ * @brief   measurement functions and derivatives for simulated 2D robot
+ * @author  Frank Dellaert
+ */
+
+// \callgraph
+
+#pragma once
+
+#include "NonlinearFactor.h"
+
+// \namespace
+
+namespace gtsam {
+
+  /**
+   * Prior on a single pose
+   */
+  Vector prior (const Vector& x);
+  Matrix Dprior(const Vector& x);
+
+  /**
+   * odometry between two poses
+   */
+  Vector odo(const Vector& x1, const Vector& x2);
+  Matrix Dodo1(const Vector& x1, const Vector& x2);
+  Matrix Dodo2(const Vector& x1, const Vector& x2);
+
+  /**
+   *  measurement between landmark and pose
+   */
+  Vector mea(const Vector& x,  const Vector& l);
+  Matrix Dmea1(const Vector& x, const Vector& l);
+  Matrix Dmea2(const Vector& x, const Vector& l);
+
+} // namespace gtsam

cpp/smallExample.cpp

@@ -0,0 +1,336 @@
+/**
+ * @file    smallExample.cpp
+ * @brief   Create small example with two poses and one landmark
+ * @brief   smallExample
+ * @author  Carlos Nieto
+ * @author  Frank dellaert
+ */
+
+
+
+/*STL/C++*/
+#include <iostream>
+#include <string>
+
+using namespace std;
+
+#include "Matrix.h"
+#include "NonlinearFactor.h"
+#include "EqualityFactor.h"
+#include "DeltaFunction.h"
+#include "smallExample.h"
+#include "Point2Prior.h"
+#include "Simulated2DOdometry.h"
+#include "Simulated2DMeasurement.h"
+#include "simulated2D.h"
+
+namespace gtsam {
+
+  typedef boost::shared_ptr<NonlinearFactor> shared;
+
+/* ************************************************************************* */
+NonlinearFactorGraph createNonlinearFactorGraph()
+{
+    // Create
+    NonlinearFactorGraph nlfg;
+
+    // prior on x1
+    double sigma1=0.1;
+    Vector mu(2); mu(0) = 0 ; mu(1) = 0;
+    shared f1(new Point2Prior(mu, sigma1, "x1"));
+    nlfg.push_back(f1);
+
+    // odometry between x1 and x2
+    double sigma2=0.1;
+    Vector z2(2); z2(0) = 1.5 ; z2(1) = 0;
+    shared f2(new Simulated2DOdometry(z2, sigma2, "x1", "x2"));
+    nlfg.push_back(f2);
+
+    // measurement between x1 and l1
+    double sigma3=0.2;
+    Vector z3(2); z3(0) = 0. ; z3(1) = -1.;
+    shared f3(new Simulated2DMeasurement(z3, sigma3, "x1", "l1"));
+    nlfg.push_back(f3);
+
+    // measurement between x2 and l1
+    double sigma4=0.2;
+    Vector z4(2); z4(0)= -1.5 ; z4(1) = -1.;
+    shared f4(new Simulated2DMeasurement(z4, sigma4, "x2", "l1"));
+    nlfg.push_back(f4);
+
+    return nlfg;
+}
+
+/* ************************************************************************* */
+ConstrainedLinearFactorGraph createConstrainedLinearFactorGraph()
+{
+	ConstrainedLinearFactorGraph graph;
+
+	// add an equality factor
+	Vector v1(2); v1(0)=1.;v1(1)=2.;
+	EqualityFactor::shared_ptr f1(new EqualityFactor(v1, "x0"));
+	graph.push_back_eq(f1);
+
+	// add a normal linear factor
+	Matrix A21 = -1 * eye(2);
+
+	Matrix A22 = eye(2);
+
+	Vector b(2);
+	b(0) = 2 ; b(1) = 3;
+
+	double sigma = 0.1;
+	LinearFactor::shared_ptr f2(new LinearFactor("x0", A21/sigma,  "x1", A22/sigma, b/sigma));
+	graph.push_back(f2);
+
+
+	return graph;
+}
+
+/* ************************************************************************* */
+ConstrainedNonlinearFactorGraph createConstrainedNonlinearFactorGraph()
+{
+	ConstrainedNonlinearFactorGraph graph;
+	FGConfig c = createConstrainedConfig();
+
+	// equality constraint for initial pose
+	EqualityFactor::shared_ptr f1(new EqualityFactor(c["x0"], "x0"));
+	graph.push_back_eq(f1);
+
+    // odometry between x0 and x1
+    double sigma=0.1;
+    shared f2(new Simulated2DOdometry(c["x1"]-c["x0"], sigma, "x0", "x1"));
+    graph.push_back(f2);
+
+	return graph;
+}
+
+/* ************************************************************************* */
+FGConfig createConfig()
+{
+    Vector v_x1(2); v_x1(0) = 0.;  v_x1(1) = 0.;
+    Vector v_x2(2); v_x2(0) = 1.5; v_x2(1) = 0.;
+    Vector v_l1(2); v_l1(0) = 0.;  v_l1(1) = -1.;
+    FGConfig c;
+    c.insert("x1", v_x1);
+    c.insert("x2", v_x2);
+    c.insert("l1", v_l1);
+    return c;
+}
+
+/* ************************************************************************* */
+FGConfig createNoisyConfig()
+{
+    Vector v_x1(2); v_x1(0) = 0.1;  v_x1(1) = 0.1;
+    Vector v_x2(2); v_x2(0) = 1.4;  v_x2(1) = 0.2;
+    Vector v_l1(2); v_l1(0) = 0.1;  v_l1(1) = -1.1;
+    FGConfig c;
+    c.insert("x1", v_x1);
+    c.insert("x2", v_x2);
+    c.insert("l1", v_l1);
+    return c;
+}
+
+/* ************************************************************************* */
+FGConfig createConstrainedConfig()
+{
+	FGConfig config;
+
+	Vector x0(2); x0(0)=1.0; x0(1)=2.0;
+	config.insert("x0", x0);
+
+	Vector x1(2); x1(0)=3.0; x1(1)=5.0;
+	config.insert("x1", x1);
+
+	return config;
+}
+
+/* ************************************************************************* */
+FGConfig createConstrainedLinConfig()
+{
+	FGConfig config;
+
+	Vector x0(2); x0(0)=1.0; x0(1)=2.0; // value doesn't actually matter
+	config.insert("x0", x0);
+
+	Vector x1(2); x1(0)=2.3; x1(1)=5.3;
+	config.insert("x1", x1);
+
+	return config;
+}
+
+/* ************************************************************************* */
+FGConfig createConstrainedCorrectDelta()
+{
+	FGConfig config;
+
+	Vector x0(2); x0(0)=0.; x0(1)=0.;
+	config.insert("x0", x0);
+
+	Vector x1(2); x1(0)= 0.7; x1(1)= -0.3;
+	config.insert("x1", x1);
+
+	return config;
+}
+
+/* ************************************************************************* */
+FGConfig createCorrectDelta() {
+  Vector v_x1(2); v_x1(0) = -0.1;  v_x1(1) = -0.1;
+  Vector v_x2(2); v_x2(0) =  0.1;  v_x2(1) = -0.2;
+  Vector v_l1(2); v_l1(0) = -0.1;  v_l1(1) =  0.1;
+  FGConfig c;
+  c.insert("x1", v_x1);
+  c.insert("x2", v_x2);
+  c.insert("l1", v_l1);
+  return c;
+}
+
+/* ************************************************************************* */
+FGConfig createZeroDelta() {
+  Vector v_x1(2); v_x1(0) = 0;  v_x1(1) = 0;
+  Vector v_x2(2); v_x2(0) = 0;  v_x2(1) = 0;
+  Vector v_l1(2); v_l1(0) = 0;  v_l1(1) = 0;
+  FGConfig c;
+  c.insert("x1", v_x1);
+  c.insert("x2", v_x2);
+  c.insert("l1", v_l1);
+  return c;
+}
+
+/* ************************************************************************* */
+LinearFactorGraph createLinearFactorGraph()
+{
+  FGConfig c = createNoisyConfig();
+  
+  // Create
+  LinearFactorGraph fg;
+
+  // prior on x1
+  Matrix A11(2,2);
+  A11(0,0) = 10; A11(0,1) =  0;
+  A11(1,0) =  0; A11(1,1) = 10;
+
+  Vector b = - c["x1"]/0.1;
+
+  LinearFactor::shared_ptr f1(new LinearFactor("x1", A11, b));
+  fg.push_back(f1);
+
+  // odometry between x1 and x2
+  Matrix A21(2,2);
+  A21(0,0) = -10 ; A21(0,1) =   0;
+  A21(1,0) =   0 ; A21(1,1) = -10;
+
+  Matrix A22(2,2);
+  A22(0,0) = 10 ; A22(0,1) =  0;
+  A22(1,0) =  0 ; A22(1,1) = 10;
+
+  // Vector b(2);
+  b(0) = 2 ; b(1) = -1;
+
+  LinearFactor::shared_ptr f2(new LinearFactor("x1", A21,  "x2", A22, b));
+  fg.push_back(f2);
+
+  // measurement between x1 and l1
+  Matrix A31(2,2);
+  A31(0,0) = -5; A31(0,1) =  0;
+  A31(1,0) =  0; A31(1,1) = -5;
+
+  Matrix A32(2,2);
+  A32(0,0) = 5 ; A32(0,1) = 0;
+  A32(1,0) = 0 ; A32(1,1) = 5;
+
+  b(0) = 0 ; b(1) = 1;
+
+  LinearFactor::shared_ptr f3(new LinearFactor("x1", A31, "l1", A32, b));
+  fg.push_back(f3);
+
+  // measurement between x2 and l1
+  Matrix A41(2,2);
+  A41(0,0) = -5 ; A41(0,1) =  0;
+  A41(1,0) =  0 ; A41(1,1) = -5;
+
+  Matrix A42(2,2);
+  A42(0,0) = 5 ; A42(0,1) = 0;
+  A42(1,0) = 0 ; A42(1,1) = 5;
+
+  b(0)= -1 ; b(1) = 1.5;
+
+  LinearFactor::shared_ptr f4(new LinearFactor("x2", A41, "l1", A42, b));
+  fg.push_back(f4);
+
+  return fg;
+}
+
+/* ************************************************************************* */
+/** create small Chordal Bayes Net x <- y
+ * x y d
+ * 1 1 9
+ *   1 5
+ */
+ChordalBayesNet createSmallChordalBayesNet()
+{
+  Matrix R11 = Matrix_(1,1,1.0), S12 = Matrix_(1,1,1.0);
+  Matrix                          R22 = Matrix_(1,1,1.0);
+  Vector d1(1), d2(1);
+  d1(0) = 9; d2(0) = 5;
+  
+  // define nodes and specify in reverse topological sort (i.e. parents last)
+  ConditionalGaussian::shared_ptr
+    x(new ConditionalGaussian(d1,R11,"y",S12)),
+    y(new ConditionalGaussian(d2,R22));
+  ChordalBayesNet cbn;
+  cbn.insert("x",x);
+  cbn.insert("y",y);
+
+  return cbn;
+}
+
+/* ************************************************************************* */
+ConstrainedChordalBayesNet createConstrainedChordalBayesNet()
+{
+	ConstrainedChordalBayesNet cbn;
+	FGConfig c = createConstrainedConfig();
+
+	// add regular conditional gaussian - no parent
+	Matrix R = eye(2);
+	Vector d = c["x1"];
+	double sigma = 0.1;
+	ConditionalGaussian::shared_ptr f1(new ConditionalGaussian(d/sigma, R/sigma));
+	cbn.insert("x1", f1);
+
+	// add a delta function to the cbn
+	DeltaFunction::shared_ptr f2(new DeltaFunction(c["x0"], "x0"));
+	cbn.insert_df("x0", f2);
+
+	return cbn;
+}
+
+/* ************************************************************************* */
+// Some nonlinear functions to optimize
+/* ************************************************************************* */
+namespace optimize {
+  Vector h(const Vector& v) {
+    double x = v(0);
+    return Vector_(2,cos(x),sin(x));
+  };
+  Matrix H(const Vector& v) {
+    double x = v(0);
+    return Matrix_(2,1,-sin(x),cos(x));
+  };
+}
+
+/* ************************************************************************* */
+NonlinearFactorGraph createReallyNonlinearFactorGraph()
+{
+  NonlinearFactorGraph fg;
+  Vector z = Vector_(2,1.0,0.0);
+  double sigma = 0.1;
+  boost::shared_ptr<NonlinearFactor1> 
+    factor(new NonlinearFactor1(z,sigma,&optimize::h,"x",&optimize::H));
+  fg.push_back(factor);
+  return fg;
+}
+
+/* ************************************************************************* */
+
+} // namespace gtsam

cpp/smallExample.h

@@ -0,0 +1,96 @@
+/**
+ * @file    smallExample.h
+ * @brief   Create small example with two poses and one landmark
+ * @brief   smallExample
+ * @author  Carlos Nieto
+ */
+
+
+// \callgraph
+
+
+#pragma once
+
+#include "ConstrainedNonlinearFactorGraph.h"
+#include "ConstrainedChordalBayesNet.h"
+#include "ConstrainedLinearFactorGraph.h"
+#include "NonlinearFactorGraph.h"
+#include "FGConfig.h"
+
+// \namespace
+
+namespace gtsam {
+
+/**
+ * Create small example for non-linear factor graph
+ */
+NonlinearFactorGraph createNonlinearFactorGraph() ;
+
+/**
+ * Create small example constrained factor graph
+ */
+ConstrainedLinearFactorGraph createConstrainedLinearFactorGraph();
+
+/**
+ * Create small example constrained nonlinear factor graph
+ */
+ConstrainedNonlinearFactorGraph createConstrainedNonlinearFactorGraph();
+
+/**
+ * Create configuration to go with it
+ * The ground truth configuration for the example above
+ */
+FGConfig createConfig();
+
+/**
+ * Create configuration for constrained example
+ * This is the ground truth version
+ */
+FGConfig createConstrainedConfig();
+
+/**
+ * create a noisy configuration for a nonlinear factor graph
+ */
+FGConfig createNoisyConfig();
+
+/**
+ * Zero delta config
+ */
+FGConfig createZeroDelta();
+
+/**
+ * Delta config that, when added to noisyConfig, returns the ground truth
+ */
+FGConfig createCorrectDelta();
+
+/**
+ * create a linear factor graph
+ * The non-linear graph above evaluated at NoisyConfig
+ */
+LinearFactorGraph createLinearFactorGraph();
+
+/**
+ * create small Chordal Bayes Net x <- y
+ */
+ChordalBayesNet createSmallChordalBayesNet();
+
+/**
+ * create small Constrained Chordal Bayes Net (from other constrained example)
+ */
+ConstrainedChordalBayesNet createConstrainedChordalBayesNet();
+
+/**
+ * Create really non-linear factor graph (cos/sin)
+ */
+NonlinearFactorGraph createReallyNonlinearFactorGraph();
+
+/**
+ * Create a noisy configuration for linearization
+ */
+FGConfig createConstrainedLinConfig();
+
+/**
+ * Create the correct delta configuration
+ */
+FGConfig createConstrainedCorrectDelta();
+}

cpp/svdcmp.cpp

@@ -0,0 +1,254 @@
+/**
+ * @file    svdcmp.cpp
+ * @brief   SVD decomposition adapted from NRC
+ * @author  Alireza Fathi
+ * @author  Frank Dellaert
+ */
+#include <stdexcept>
+#include <math.h>    /* for 'fabs' */
+#include <iostream>
+#include <vector>
+
+using namespace std;
+
+
+#define SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a))
+static double sqrarg;
+#define SQR(a) ((sqrarg=(a)) == 0.0 ? 0.0 : sqrarg*sqrarg)
+static double maxarg1,maxarg2;
+#define FMAX(a,b) (maxarg1=(a),maxarg2=(b),(maxarg1) > (maxarg2) ?\
+        (maxarg1) : (maxarg2))
+static int iminarg1,iminarg2;
+#define IMIN(a,b) (iminarg1=(a),iminarg2=(b),(iminarg1) < (iminarg2) ?\
+        (iminarg1) : (iminarg2))
+
+
+/* ************************************************************************* */
+/*
+double pythag(double a, double b)
+{
+  double absa = 0.0, absb = 0.0;
+  absa=fabs(a);
+  absb=fabs(b);
+  if (absa > absb) return absa*sqrt(1.0+SQR(absb/absa));
+  else return (absb == 0.0 ? 0.0 : absb*sqrt(1.0+SQR(absa/absb)));
+}
+*/
+
+/* ************************************************************************* */
+double pythag(double a, double b)
+{
+  double absa = 0.0, absb = 0.0;
+  absa=fabs(a);
+  absb=fabs(b);
+  if (absa > absb) return absa*sqrt(1.0+SQR(absb/absa));
+  else {
+    if(absb == 0.0)
+      return 0.0;
+    else
+     return (absb * sqrt(1.0 + SQR(absa/absb))); 
+  }
+}
+
+/* ************************************************************************* */
+void svdcmp(double **a, int m, int n, double w[], double **v)
+{
+  int flag,i,its,j,jj,k,l,nm;
+  double anorm,c,f,g,h,s,scale,x,y,z;
+
+  //vector sizes:
+  // w[n] - q-1 passed in
+  // a[m] - u-1 passed in 
+  // v[n] - v-1 passed in
+
+  //Current progress on verifying array bounds:
+  // rv1 references have been fixed
+
+  double *rv1 = new double[n];
+
+  g= 0.0;
+  scale= 0.0;
+  anorm= 0.0;
+  for (i=1;i<=n;i++) {
+    l=i+1;
+    rv1[i-1]=scale*g;
+    g=s=scale=0.0;
+    if (i <= m) {
+      for (k=i;k<=m;k++) scale += fabs(a[k][i]); 
+      if (scale) {
+	for (k=i;k<=m;k++) {
+	  a[k][i] /= scale;  
+	  s += a[k][i]*a[k][i];
+	}
+	f=a[i][i];
+	g = -SIGN(sqrt(s),f);
+	h=f*g-s;
+	a[i][i]=f-g;
+	for (j=l;j<=n;j++) {
+	  for (s=0.0,k=i;k<=m;k++) s += a[k][i]*a[k][j];
+	  f=s/h;
+	  for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
+	}
+	for (k=i;k<=m;k++) a[k][i] *= scale;
+      }
+    }
+    w[i]=scale *g; 
+    g=s=scale=0.0;
+    if (i <= m && i != n) {
+      for (k=l;k<=n;k++) scale += fabs(a[i][k]);
+      if (scale) {
+	for (k=l;k<=n;k++) {
+	  a[i][k] /= scale;
+	  s += a[i][k]*a[i][k];
+	}
+	f=a[i][l];
+	g = -SIGN(sqrt(s),f);
+	h=f*g-s;
+	a[i][l]=f-g;
+	for (k=l;k<=n;k++) 
+	  {
+	    rv1[k-1]=a[i][k]/h;
+	  }
+
+	for (j=l;j<=m;j++) {
+	  for (s=0.0,k=l;k<=n;k++) 
+	    s += a[j][k]*a[i][k];
+	  for (k=l;k<=n;k++) 
+	    {
+	      a[j][k] += s*rv1[k-1];
+	    }
+	}
+	for (k=l;k<=n;k++) a[i][k] *= scale;
+      }
+    }
+    anorm=FMAX(anorm,(fabs(w[i])+fabs(rv1[i-1]))); 
+
+  }
+  for (i=n;i>=1;i--) {
+    if (i < n) {
+      if (g) {
+	for (j=l;j<=n;j++)
+	  v[j][i]=(a[i][j]/a[i][l])/g;
+	for (j=l;j<=n;j++) {
+	  for (s=0.0,k=l;k<=n;k++) s += a[i][k]*v[k][j];
+	  for (k=l;k<=n;k++) v[k][j] += s*v[k][i];
+	}
+      }
+      for (j=l;j<=n;j++) v[i][j]=v[j][i]=0.0;
+    }
+    v[i][i]=1.0;
+    g=rv1[i-1];
+    l=i;
+  }
+  for (i=IMIN(m,n);i>=1;i--) {
+    l=i+1;
+    g=w[i];
+    for (j=l;j<=n;j++) a[i][j]=0.0;
+    if (g) {
+      g=1.0/g;
+      for (j=l;j<=n;j++) {
+	for (s=0.0,k=l;k<=m;k++) s += a[k][i]*a[k][j];
+	f=(s/a[i][i])*g;
+	for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
+      }
+      for (j=i;j<=m;j++) a[j][i] *= g;
+    } else for (j=i;j<=m;j++) a[j][i]=0.0;
+    ++a[i][i];
+  }
+  for (k=n;k>=1;k--) {
+    for (its=1;its<=30;its++) {
+      flag=1;
+      for (l=k;l>=1;l--) {
+	nm=l-1;
+	if ((double)(fabs(rv1[l-1])+anorm) == anorm) {
+	  flag=0;
+	  break;
+	}
+	if ((double)(fabs(w[nm])+anorm) == anorm) break; 
+      }
+      if (flag) {
+	c=0.0;
+	s=1.0;
+	for (i=l;i<=k;i++) {
+	  f=s*rv1[i-1];
+	  rv1[i-1]=c*rv1[i-1];
+	  if ((double)(fabs(f)+anorm) == anorm) break;
+	  g=w[i];  
+	  h=pythag(f,g);
+	  w[i]=h;
+	  h=1.0/h;
+	  c=g*h;
+	  s = -f*h;
+	  for (j=1;j<=m;j++) {
+	    y=a[j][nm];
+	    z=a[j][i];
+	    a[j][nm]=y*c+z*s;
+	    a[j][i]=z*c-y*s;
+	  }
+	}
+      }
+      z=w[k]; 
+      if (l == k) {
+	if (z < 0.0) {
+	  w[k] = -z;  
+	  for (j=1;j<=n;j++) v[j][k] = -v[j][k];
+	}
+	break;
+      }
+      if (its == 30) throw(std::domain_error("no convergence in 30 svdcmp iterations"));
+      x=w[l];  
+      nm=k-1;
+      y=w[nm];  
+      g=rv1[nm-1] ;
+      h=rv1[k-1];
+      f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
+      g=pythag(f,1.0);
+      f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
+      c=s=1.0;
+      for (j=l;j<=nm;j++) {
+	i=j+1;
+	g=rv1[i-1];
+	y=w[i]; 
+	h=s*g;
+	g=c*g;
+	z=pythag(f,h);
+	rv1[j-1]=z;
+	c=f/z;
+	s=h/z;
+	f=x*c+g*s;
+	g = g*c-x*s;
+	h=y*s;
+	y *= c;
+	for (jj=1;jj<=n;jj++) {
+	  x=v[jj][j];
+	  z=v[jj][i];
+	  v[jj][j]=x*c+z*s;
+	  v[jj][i]=z*c-x*s;
+	}
+	z=pythag(f,h);
+	w[j]=z; 
+	if (z) {
+	  z=1.0/z;
+	  c=f*z;
+	  s=h*z;
+	}
+	f=c*g+s*y;
+	x=c*y-s*g;
+	for (jj=1;jj<=m;jj++) {
+	  y=a[jj][j];
+	  z=a[jj][i];
+	  a[jj][j]=y*c+z*s;
+	  a[jj][i]=z*c-y*s;
+	}
+      }
+      rv1[l-1]=0.0;
+      rv1[k-1]=f;
+      w[k]=x; 
+    }
+  }
+
+  delete[] rv1;
+  
+}
+
+/* ************************************************************************* */

cpp/svdcmp.h

@@ -0,0 +1,14 @@
+/**
+ * @file    svdcmp.h
+ * @brief   SVD decomposition adapted from NRC
+ * @author  Alireza Fathi
+ * @author  Frank Dellaert
+ */
+
+// \callgraph
+
+#pragma once
+
+/** SVD decomposition */
+void svdcmp(double **a, int m, int n, double w[], double **v);
+

cpp/testCal3_S2.cpp

@@ -0,0 +1,49 @@
+/**
+ * @file  testCal3_S2.cpp
+ * @brief Unit tests for transform derivatives
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include "numericalDerivative.h"
+#include "Cal3_S2.h"
+
+using namespace gtsam;
+
+Cal3_S2 K(500, 500, 0.1, 640 / 2, 480 / 2);
+Point2 p(1, -2);
+double tol = 1e-8;
+
+/** transform derivatives */
+
+/* ************************************************************************* */
+TEST( Cal3_S2, Duncalibrate1)
+{
+	Matrix computed = Duncalibrate1(K, p);
+	Matrix numerical = numericalDerivative21(uncalibrate, K, p);
+	CHECK(assert_equal(numerical,computed,tol));
+}
+
+/* ************************************************************************* */
+TEST( Cal3_S2, Duncalibrate2)
+{
+	Matrix computed = Duncalibrate2(K, p);
+	Matrix numerical = numericalDerivative22(uncalibrate, K, p);
+	CHECK(assert_equal(numerical,computed,tol));
+}
+
+/* ************************************************************************* */
+TEST( Cal3_S2, assert_equal)
+{
+	CHECK(assert_equal(K,K,tol));
+
+	Cal3_S2 K1(501, 500, 0.1, 640 / 2, 480 / 2);
+	CHECK(!K.equals(K1,tol));
+}
+
+/* ************************************************************************* */
+int main() {
+	TestResult tr;
+	return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
+

cpp/testCalibratedCamera.cpp

@@ -0,0 +1,89 @@
+/**
+ * Frank Dellaert
+ * brief: test CalibratedCamera class
+ * based on testVSLAMFactor.cpp
+ */
+
+#include <iostream>
+
+#include <CppUnitLite/TestHarness.h>
+#include "numericalDerivative.h"
+#include "CalibratedCamera.h"
+
+using namespace std;
+using namespace gtsam;
+
+const Pose3 pose1(Matrix_(3,3,
+				      1., 0., 0.,
+				      0.,-1., 0.,
+				      0., 0.,-1.
+				      ),
+			      Point3(0,0,500));
+ 
+const CalibratedCamera camera(pose1);
+
+const Point3 point1(-80.0,-80.0, 0.0);
+const Point3 point2(-80.0, 80.0, 0.0);
+const Point3 point3( 80.0, 80.0, 0.0);
+const Point3 point4( 80.0,-80.0, 0.0);
+
+/* ************************************************************************* */
+TEST( CalibratedCamera, constructor)
+{
+  CHECK(assert_equal( camera.pose(), pose1));
+}
+
+/* ************************************************************************* */
+TEST( CalibratedCamera, project)
+{
+  CHECK(assert_equal( camera.project(point1), Point2(-.16,  .16) ));
+  CHECK(assert_equal( camera.project(point2), Point2(-.16, -.16) ));
+  CHECK(assert_equal( camera.project(point3), Point2( .16, -.16) ));
+  CHECK(assert_equal( camera.project(point4), Point2( .16,  .16) ));
+}
+
+/* ************************************************************************* */
+
+TEST( CalibratedCamera, Dproject_to_camera1) {
+	Point3 pp(155,233,131);
+	Matrix test1 = Dproject_to_camera1(pp);
+	Matrix test2 = numericalDerivative11(project_to_camera, pp);
+	CHECK(assert_equal(test1, test2));
+}
+
+/* ************************************************************************* */
+Point2 project2(const Pose3& pose, const Point3& point) {
+	return project(CalibratedCamera(pose), point);
+}
+
+TEST( CalibratedCamera, Dproject_pose)
+{
+	Matrix computed = Dproject_pose(camera, point1);
+	Matrix numerical = numericalDerivative21(project2, pose1, point1);
+	CHECK(assert_equal(computed, numerical,1e-7));
+}
+
+TEST( CalibratedCamera, Dproject_point)
+{
+	Matrix computed = Dproject_point(camera, point1);
+	Matrix numerical = numericalDerivative22(project2, pose1, point1);
+	CHECK(assert_equal(computed, numerical,1e-7));
+}
+
+TEST( CalibratedCamera, Dproject_point_pose)
+{
+  Point2 result;
+	Matrix Dpose, Dpoint;
+	Dproject_pose_point(camera, point1, result, Dpose, Dpoint);
+	Matrix numerical_pose  = numericalDerivative21(project2, pose1, point1);
+	Matrix numerical_point = numericalDerivative22(project2, pose1, point1);
+  CHECK(assert_equal(result, Point2(-.16,  .16) ));
+	CHECK(assert_equal(Dpose,  numerical_pose, 1e-7));
+	CHECK(assert_equal(Dpoint, numerical_point,1e-7));
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; TestRegistry::runAllTests(tr); return 0; }
+/* ************************************************************************* */
+
+

cpp/testChordalBayesNet.cpp

@@ -0,0 +1,130 @@
+/**
+ * @file    testChordalBayesNet.cpp
+ * @brief   Unit tests for ChordalBayesNet
+ * @author  Frank Dellaert
+ */
+
+
+// STL/C++
+#include <iostream>
+#include <sstream>
+#include <CppUnitLite/TestHarness.h>
+#include <boost/tuple/tuple.hpp>
+#include <boost/foreach.hpp>
+
+#ifdef HAVE_BOOST_SERIALIZATION
+#include <boost/archive/text_oarchive.hpp>
+#include <boost/archive/text_iarchive.hpp>
+#endif //HAVE_BOOST_SERIALIZATION
+
+#include "ChordalBayesNet.h"
+#include "smallExample.h"
+
+
+
+using namespace gtsam;
+
+/* ************************************************************************* */
+TEST( ChordalBayesNet, constructor )
+{
+  // small Bayes Net x <- y
+  // x y d
+  // 1 1 9
+  //   1 5
+  Matrix R11 = Matrix_(1,1,1.0), S12 = Matrix_(1,1,1.0);
+  Matrix                         R22 = Matrix_(1,1,1.0);
+  Vector d1(1), d2(1);
+  d1(0) = 9; d2(0) = 5;
+  
+  // define nodes and specify in reverse topological sort (i.e. parents last)
+  ConditionalGaussian x(d1,R11,"y",S12), y(d2,R22);
+
+  // check small example which uses constructor
+  ChordalBayesNet cbn = createSmallChordalBayesNet();
+  CHECK( x.equals(*cbn["x"]) );
+  CHECK( y.equals(*cbn["y"]) );
+}
+
+/* ************************************************************************* */
+TEST( ChordalBayesNet, matrix )
+{
+  // Create a test graph
+  ChordalBayesNet cbn = createSmallChordalBayesNet();
+
+  Matrix R; Vector d;
+  boost::tie(R,d) = cbn.matrix(); // find matrix and RHS
+
+  Matrix R1 = Matrix_(2,2,
+		      1.0, 1.0,
+		      0.0, 1.0
+    );
+  Vector d1 = Vector_(2, 9.0, 5.0);
+
+  EQUALITY(R,R1);
+  CHECK(d==d1);
+}
+
+/* ************************************************************************* */
+TEST( ChordalBayesNet, optimize )
+{
+  // optimize small Bayes Net
+  ChordalBayesNet cbn = createSmallChordalBayesNet();
+  boost::shared_ptr<FGConfig> actual = cbn.optimize();
+
+  FGConfig expected;
+  Vector x(1), y(1);
+  x(0) = 4; y(0) = 5;
+  expected.insert("x",x);
+  expected.insert("y",y);
+
+  CHECK(actual->equals(expected));
+}
+
+/* ************************************************************************* */
+#ifdef HAVE_BOOST_SERIALIZATION
+TEST( ChordalBayesNet, serialize )
+{
+//     //create a starting CBN
+//     ChordalBayesNet cbn = createSmallChordalBayesNet();
+//     
+//     //serialize the CBN
+//     std::ostringstream in_archive_stream;
+//     boost::archive::text_oarchive in_archive(in_archive_stream);
+//     in_archive << cbn;
+//     std::string serialized = in_archive_stream.str();
+//     
+//     //DEBUG
+//     std::cout << "CBN Raw string: [" << serialized << "]" << std::endl;
+//     
+//     //remove newlines/carriage returns
+//     std::string clean;
+//     BOOST_FOREACH(char s, serialized)
+//     {
+// 	if (s != '\n') 
+// 	{
+// 	    //copy in character
+// 	    clean.append(std::string(1,s));
+// 	}
+// 	else
+// 	{
+// 	    std::cout << "   Newline character found!" << std::endl;
+// 	    //replace with an identifiable string
+// 	    clean.append(std::string(1,' '));
+// 	}
+//     }
+// 
+//     
+//     std::cout << "Cleaned CBN String: [" << clean << "]" << std::endl;
+//     
+//     //deserialize the CBN
+//     std::istringstream out_archive_stream(clean);
+//     boost::archive::text_iarchive out_archive(out_archive_stream);
+//     ChordalBayesNet output;
+//     out_archive >> output;
+//     CHECK(cbn.equals(output));
+}
+#endif //HAVE_BOOST_SERIALIZATION
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
+/* ************************************************************************* */