commit d80fa24a9f83a93e6f85d2e943c4122f26b2d4e2 Author: Richard Roberts Date: Fri Aug 21 22:23:24 2009 +0000 Fixing directory structure diff --git a/AUTHORS b/AUTHORS new file mode 100644 index 000000000..70de2564a --- /dev/null +++ b/AUTHORS @@ -0,0 +1,4 @@ +Frank Dellaert +Christian Potthast +Alireza Fathi +Carlos Nieto diff --git a/COPYING b/COPYING new file mode 100644 index 000000000..81b0297c1 --- /dev/null +++ b/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. \ No newline at end of file diff --git a/CppUnitLite/Failure.cpp b/CppUnitLite/Failure.cpp new file mode 100644 index 000000000..a422bc975 --- /dev/null +++ b/CppUnitLite/Failure.cpp @@ -0,0 +1,50 @@ + + +#include "Failure.h" + +#include +#include + + +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; +} + + diff --git a/CppUnitLite/Failure.h b/CppUnitLite/Failure.h new file mode 100644 index 000000000..10a586b22 --- /dev/null +++ b/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 diff --git a/CppUnitLite/Makefile b/CppUnitLite/Makefile new file mode 100644 index 000000000..2ef78a3bf --- /dev/null +++ b/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: diff --git a/CppUnitLite/SimpleString.cpp b/CppUnitLite/SimpleString.cpp new file mode 100644 index 000000000..1d1212c2d --- /dev/null +++ b/CppUnitLite/SimpleString.cpp @@ -0,0 +1,95 @@ + + +#include "SimpleString.h" +#include +#include +#include + + +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); +} + + diff --git a/CppUnitLite/SimpleString.h b/CppUnitLite/SimpleString.h new file mode 100644 index 000000000..25e10827b --- /dev/null +++ b/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 diff --git a/CppUnitLite/Test.cpp b/CppUnitLite/Test.cpp new file mode 100644 index 000000000..9990628b8 --- /dev/null +++ b/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; + +} + diff --git a/CppUnitLite/Test.h b/CppUnitLite/Test.h new file mode 100644 index 000000000..0da5e152c --- /dev/null +++ b/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 +#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 diff --git a/CppUnitLite/TestHarness.h b/CppUnitLite/TestHarness.h new file mode 100644 index 000000000..64cc750f8 --- /dev/null +++ b/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 + diff --git a/CppUnitLite/TestRegistry.cpp b/CppUnitLite/TestRegistry.cpp new file mode 100644 index 000000000..dc1a33f1e --- /dev/null +++ b/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(); +} + + + diff --git a/CppUnitLite/TestRegistry.h b/CppUnitLite/TestRegistry.h new file mode 100644 index 000000000..3a477c947 --- /dev/null +++ b/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 diff --git a/CppUnitLite/TestResult.cpp b/CppUnitLite/TestResult.cpp new file mode 100644 index 000000000..32acc2718 --- /dev/null +++ b/CppUnitLite/TestResult.cpp @@ -0,0 +1,40 @@ + +#include "TestResult.h" +#include "Failure.h" + +#include + + +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"); +} diff --git a/CppUnitLite/TestResult.h b/CppUnitLite/TestResult.h new file mode 100644 index 000000000..052a392c1 --- /dev/null +++ b/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 diff --git a/INSTALL b/INSTALL new file mode 100644 index 000000000..5458714e1 --- /dev/null +++ b/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. + diff --git a/LICENSE b/LICENSE new file mode 100644 index 000000000..47889e5eb --- /dev/null +++ b/LICENSE @@ -0,0 +1 @@ +The GTSAM library has not been licensed yet. See also COPYING diff --git a/Makefile.am b/Makefile.am new file mode 100644 index 000000000..5723d8197 --- /dev/null +++ b/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 diff --git a/README b/README new file mode 100644 index 000000000..568112606 --- /dev/null +++ b/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. + diff --git a/autogen.sh b/autogen.sh new file mode 100755 index 000000000..fd0be89ce --- /dev/null +++ b/autogen.sh @@ -0,0 +1,2 @@ +#!/bin/sh +autoreconf --force --install -I config -I m4 diff --git a/colamd/Makefile b/colamd/Makefile new file mode 100755 index 000000000..927e58405 --- /dev/null +++ b/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: + diff --git a/colamd/UFconfig.h b/colamd/UFconfig.h new file mode 100755 index 000000000..54208d58b --- /dev/null +++ b/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 + +/* ========================================================================== */ +/* === 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 diff --git a/colamd/colamd.c b/colamd/colamd.c new file mode 100755 index 000000000..5fe20d628 --- /dev/null +++ b/colamd/colamd.c @@ -0,0 +1,3611 @@ +/* ========================================================================== */ +/* === colamd/symamd - a sparse matrix column ordering algorithm ============ */ +/* ========================================================================== */ + +/* COLAMD / SYMAMD + + colamd: an approximate minimum degree column ordering algorithm, + for LU factorization of symmetric or unsymmetric matrices, + QR factorization, least squares, interior point methods for + linear programming problems, and other related problems. + + symamd: an approximate minimum degree ordering algorithm for Cholesky + factorization of symmetric matrices. + + Purpose: + + Colamd computes a permutation Q such that the Cholesky factorization of + (AQ)'(AQ) has less fill-in and requires fewer floating point operations + than A'A. This also provides a good ordering for sparse partial + pivoting methods, P(AQ) = LU, where Q is computed prior to numerical + factorization, and P is computed during numerical factorization via + conventional partial pivoting with row interchanges. Colamd is the + column ordering method used in SuperLU, part of the ScaLAPACK library. + It is also available as built-in function in MATLAB Version 6, + available from MathWorks, Inc. (http://www.mathworks.com). This + routine can be used in place of colmmd in MATLAB. + + Symamd computes a permutation P of a symmetric matrix A such that the + Cholesky factorization of PAP' has less fill-in and requires fewer + floating point operations than A. Symamd constructs a matrix M such + that M'M has the same nonzero pattern of A, and then orders the columns + of M using colmmd. The column ordering of M is then returned as the + row and column ordering P of A. + + 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. + + Copyright and License: + + Copyright (c) 1998-2007, Timothy A. Davis, All Rights Reserved. + COLAMD is also available under alternate licenses, contact T. Davis + for details. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 + USA + + Permission is hereby granted to use or copy this program under the + terms of the GNU LGPL, provided that the Copyright, this License, + and the Availability of the original version is retained on all copies. + User documentation of any code that uses this code or any modified + version of this code must cite the Copyright, this License, the + Availability note, and "Used by permission." Permission to modify + the code and to distribute modified code is granted, provided the + Copyright, this License, and the Availability note are retained, + and a notice that the code was modified is included. + + 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.c + file. It requires the colamd.h file. It is required by the colamdmex.c + and symamdmex.c files, for the MATLAB interface to colamd and symamd. + Appears as ACM Algorithm 836. + + See the ChangeLog file for changes since Version 1.0. + + References: + + T. A. Davis, J. R. Gilbert, S. Larimore, E. Ng, An approximate column + minimum degree ordering algorithm, ACM Transactions on Mathematical + Software, vol. 30, no. 3., pp. 353-376, 2004. + + T. A. Davis, J. R. Gilbert, S. Larimore, E. Ng, Algorithm 836: COLAMD, + an approximate column minimum degree ordering algorithm, ACM + Transactions on Mathematical Software, vol. 30, no. 3., pp. 377-380, + 2004. + +*/ + +/* ========================================================================== */ +/* === Description of user-callable routines ================================ */ +/* ========================================================================== */ + +/* COLAMD includes both int and UF_long versions of all its routines. The + * description below is for the int version. For UF_long, all int arguments + * become UF_long. UF_long is normally defined as long, except for WIN64. + + ---------------------------------------------------------------------------- + colamd_recommended: + ---------------------------------------------------------------------------- + + C syntax: + + #include "colamd.h" + size_t colamd_recommended (int nnz, int n_row, int n_col) ; + size_t colamd_l_recommended (UF_long nnz, UF_long n_row, + UF_long n_col) ; + + Purpose: + + Returns recommended value of Alen for use by colamd. Returns 0 + if any input argument is negative. The use of this routine + is optional. Not needed for symamd, which dynamically allocates + its own memory. + + Note that in v2.4 and earlier, these routines returned int or long. + They now return a value of type size_t. + + Arguments (all input arguments): + + int nnz ; Number of nonzeros in the matrix A. This must + be the same value as p [n_col] in the call to + colamd - otherwise you will get a wrong value + of the recommended memory to use. + + int n_row ; Number of rows in the matrix A. + + int n_col ; Number of columns in the matrix A. + + ---------------------------------------------------------------------------- + colamd_set_defaults: + ---------------------------------------------------------------------------- + + C syntax: + + #include "colamd.h" + colamd_set_defaults (double knobs [COLAMD_KNOBS]) ; + colamd_l_set_defaults (double knobs [COLAMD_KNOBS]) ; + + Purpose: + + Sets the default parameters. The use of this routine is optional. + + Arguments: + + double knobs [COLAMD_KNOBS] ; Output only. + + NOTE: the meaning of the dense row/col knobs has changed in v2.4 + + knobs [0] and knobs [1] control dense row and col detection: + + Colamd: rows with more than + max (16, knobs [COLAMD_DENSE_ROW] * sqrt (n_col)) + entries are removed prior to ordering. Columns with more than + max (16, knobs [COLAMD_DENSE_COL] * sqrt (MIN (n_row,n_col))) + entries are removed prior to + ordering, and placed last in the output column ordering. + + Symamd: uses only knobs [COLAMD_DENSE_ROW], which is knobs [0]. + Rows and columns with more than + max (16, knobs [COLAMD_DENSE_ROW] * sqrt (n)) + entries are removed prior to ordering, and placed last in the + output ordering. + + COLAMD_DENSE_ROW and COLAMD_DENSE_COL are defined as 0 and 1, + respectively, in colamd.h. Default values of these two knobs + are both 10. Currently, only knobs [0] and knobs [1] are + used, but future versions may use more knobs. If so, they will + be properly set to their defaults by the future version of + colamd_set_defaults, so that the code that calls colamd will + not need to change, assuming that you either use + colamd_set_defaults, or pass a (double *) NULL pointer as the + knobs array to colamd or symamd. + + knobs [2]: aggressive absorption + + knobs [COLAMD_AGGRESSIVE] controls whether or not to do + aggressive absorption during the ordering. Default is TRUE. + + + ---------------------------------------------------------------------------- + colamd: + ---------------------------------------------------------------------------- + + C syntax: + + #include "colamd.h" + int colamd (int n_row, int n_col, int Alen, int *A, int *p, + double knobs [COLAMD_KNOBS], int stats [COLAMD_STATS]) ; + UF_long colamd_l (UF_long n_row, UF_long n_col, UF_long Alen, + UF_long *A, UF_long *p, double knobs [COLAMD_KNOBS], + UF_long stats [COLAMD_STATS]) ; + + Purpose: + + Computes a column ordering (Q) of A such that P(AQ)=LU or + (AQ)'AQ=LL' have less fill-in and require fewer floating point + operations than factorizing the unpermuted matrix A or A'A, + respectively. + + Returns: + + TRUE (1) if successful, FALSE (0) otherwise. + + Arguments: + + int n_row ; Input argument. + + Number of rows in the matrix A. + Restriction: n_row >= 0. + Colamd returns FALSE if n_row is negative. + + int n_col ; Input argument. + + Number of columns in the matrix A. + Restriction: n_col >= 0. + Colamd returns FALSE if n_col is negative. + + int Alen ; Input argument. + + Restriction (see note): + Alen >= 2*nnz + 6*(n_col+1) + 4*(n_row+1) + n_col + Colamd returns FALSE if these conditions are not met. + + Note: this restriction makes an modest assumption regarding + the size of the two typedef's structures in colamd.h. + We do, however, guarantee that + + Alen >= colamd_recommended (nnz, n_row, n_col) + + will be sufficient. Note: the macro version does not check + for integer overflow, and thus is not recommended. Use + the colamd_recommended routine instead. + + int A [Alen] ; Input argument, undefined on output. + + A is an integer array of size Alen. Alen must be at least as + large as the bare minimum value given above, but this is very + low, and can result in excessive run time. For best + performance, we recommend that Alen be greater than or equal to + colamd_recommended (nnz, n_row, n_col), which adds + nnz/5 to the bare minimum value given above. + + On input, the row indices of the entries in column c of the + matrix are held in A [(p [c]) ... (p [c+1]-1)]. The row indices + in a given column c need not be in ascending order, and + duplicate row indices may be be present. However, colamd will + work a little faster if both of these conditions are met + (Colamd puts the matrix into this format, if it finds that the + the conditions are not met). + + The matrix is 0-based. That is, rows are in the range 0 to + n_row-1, and columns are in the range 0 to n_col-1. Colamd + returns FALSE if any row index is out of range. + + The contents of A are modified during ordering, and are + undefined on output. + + int p [n_col+1] ; Both input and output argument. + + p is an integer array of size n_col+1. On input, it holds the + "pointers" for the column form of the matrix A. Column c of + the matrix A is held in A [(p [c]) ... (p [c+1]-1)]. The first + entry, p [0], must be zero, and p [c] <= p [c+1] must hold + for all c in the range 0 to n_col-1. The value p [n_col] is + thus the total number of entries in the pattern of the matrix A. + Colamd returns FALSE if these conditions are not met. + + On output, if colamd returns TRUE, the array p holds the column + permutation (Q, for P(AQ)=LU or (AQ)'(AQ)=LL'), where p [0] is + the first column index in the new ordering, and p [n_col-1] is + the last. That is, p [k] = j means that column j of A is the + kth pivot column, in AQ, where k is in the range 0 to n_col-1 + (p [0] = j means that column j of A is the first column in AQ). + + If colamd returns FALSE, then no permutation is returned, and + p is undefined on output. + + double knobs [COLAMD_KNOBS] ; Input argument. + + See colamd_set_defaults for a description. + + int stats [COLAMD_STATS] ; Output argument. + + Statistics on the ordering, and error status. + See colamd.h for related definitions. + Colamd returns FALSE if stats is not present. + + stats [0]: number of dense or empty rows ignored. + + stats [1]: number of dense or empty columns ignored (and + ordered last in the output permutation p) + Note that a row can become "empty" if it + contains only "dense" and/or "empty" columns, + and similarly a column can become "empty" if it + only contains "dense" and/or "empty" rows. + + stats [2]: number of garbage collections performed. + This can be excessively high if Alen is close + to the minimum required value. + + stats [3]: status code. < 0 is an error code. + > 1 is a warning or notice. + + 0 OK. Each column of the input matrix contained + row indices in increasing order, with no + duplicates. + + 1 OK, but columns of input matrix were jumbled + (unsorted columns or duplicate entries). Colamd + had to do some extra work to sort the matrix + first and remove duplicate entries, but it + still was able to return a valid permutation + (return value of colamd was TRUE). + + stats [4]: highest numbered column that + is unsorted or has duplicate + entries. + stats [5]: last seen duplicate or + unsorted row index. + stats [6]: number of duplicate or + unsorted row indices. + + -1 A is a null pointer + + -2 p is a null pointer + + -3 n_row is negative + + stats [4]: n_row + + -4 n_col is negative + + stats [4]: n_col + + -5 number of nonzeros in matrix is negative + + stats [4]: number of nonzeros, p [n_col] + + -6 p [0] is nonzero + + stats [4]: p [0] + + -7 A is too small + + stats [4]: required size + stats [5]: actual size (Alen) + + -8 a column has a negative number of entries + + stats [4]: column with < 0 entries + stats [5]: number of entries in col + + -9 a row index is out of bounds + + stats [4]: column with bad row index + stats [5]: bad row index + stats [6]: n_row, # of rows of matrx + + -10 (unused; see symamd.c) + + -999 (unused; see symamd.c) + + Future versions may return more statistics in the stats array. + + Example: + + See http://www.cise.ufl.edu/research/sparse/colamd/example.c + for a complete example. + + To order the columns of a 5-by-4 matrix with 11 nonzero entries in + the following nonzero pattern + + x 0 x 0 + x 0 x x + 0 x x 0 + 0 0 x x + x x 0 0 + + with default knobs and no output statistics, do the following: + + #include "colamd.h" + #define ALEN 100 + 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) ; + + The permutation is returned in the array p, and A is destroyed. + + ---------------------------------------------------------------------------- + symamd: + ---------------------------------------------------------------------------- + + C syntax: + + #include "colamd.h" + int symamd (int n, int *A, int *p, int *perm, + double knobs [COLAMD_KNOBS], int stats [COLAMD_STATS], + void (*allocate) (size_t, size_t), void (*release) (void *)) ; + UF_long symamd_l (UF_long n, UF_long *A, UF_long *p, UF_long *perm, + double knobs [COLAMD_KNOBS], UF_long stats [COLAMD_STATS], + void (*allocate) (size_t, size_t), void (*release) (void *)) ; + + Purpose: + + The symamd routine computes an ordering P of a symmetric sparse + matrix A such that the Cholesky factorization PAP' = LL' remains + sparse. It is based on a column ordering of a matrix M constructed + so that the nonzero pattern of M'M is the same as A. The matrix A + is assumed to be symmetric; only the strictly lower triangular part + is accessed. You must pass your selected memory allocator (usually + calloc/free or mxCalloc/mxFree) to symamd, for it to allocate + memory for the temporary matrix M. + + Returns: + + TRUE (1) if successful, FALSE (0) otherwise. + + Arguments: + + int n ; Input argument. + + Number of rows and columns in the symmetrix matrix A. + Restriction: n >= 0. + Symamd returns FALSE if n is negative. + + int A [nnz] ; Input argument. + + A is an integer array of size nnz, where nnz = p [n]. + + The row indices of the entries in column c of the matrix are + held in A [(p [c]) ... (p [c+1]-1)]. The row indices in a + given column c need not be in ascending order, and duplicate + row indices may be present. However, symamd will run faster + if the columns are in sorted order with no duplicate entries. + + The matrix is 0-based. That is, rows are in the range 0 to + n-1, and columns are in the range 0 to n-1. Symamd + returns FALSE if any row index is out of range. + + The contents of A are not modified. + + int p [n+1] ; Input argument. + + p is an integer array of size n+1. On input, it holds the + "pointers" for the column form of the matrix A. Column c of + the matrix A is held in A [(p [c]) ... (p [c+1]-1)]. The first + entry, p [0], must be zero, and p [c] <= p [c+1] must hold + for all c in the range 0 to n-1. The value p [n] is + thus the total number of entries in the pattern of the matrix A. + Symamd returns FALSE if these conditions are not met. + + The contents of p are not modified. + + int perm [n+1] ; Output argument. + + On output, if symamd returns TRUE, the array perm holds the + permutation P, where perm [0] is the first index in the new + ordering, and perm [n-1] is the last. That is, perm [k] = j + means that row and column j of A is the kth column in PAP', + where k is in the range 0 to n-1 (perm [0] = j means + that row and column j of A are the first row and column in + PAP'). The array is used as a workspace during the ordering, + which is why it must be of length n+1, not just n. + + double knobs [COLAMD_KNOBS] ; Input argument. + + See colamd_set_defaults for a description. + + int stats [COLAMD_STATS] ; Output argument. + + Statistics on the ordering, and error status. + See colamd.h for related definitions. + Symamd returns FALSE if stats is not present. + + stats [0]: number of dense or empty row and columns ignored + (and ordered last in the output permutation + perm). Note that a row/column can become + "empty" if it contains only "dense" and/or + "empty" columns/rows. + + stats [1]: (same as stats [0]) + + stats [2]: number of garbage collections performed. + + stats [3]: status code. < 0 is an error code. + > 1 is a warning or notice. + + 0 OK. Each column of the input matrix contained + row indices in increasing order, with no + duplicates. + + 1 OK, but columns of input matrix were jumbled + (unsorted columns or duplicate entries). Symamd + had to do some extra work to sort the matrix + first and remove duplicate entries, but it + still was able to return a valid permutation + (return value of symamd was TRUE). + + stats [4]: highest numbered column that + is unsorted or has duplicate + entries. + stats [5]: last seen duplicate or + unsorted row index. + stats [6]: number of duplicate or + unsorted row indices. + + -1 A is a null pointer + + -2 p is a null pointer + + -3 (unused, see colamd.c) + + -4 n is negative + + stats [4]: n + + -5 number of nonzeros in matrix is negative + + stats [4]: # of nonzeros (p [n]). + + -6 p [0] is nonzero + + stats [4]: p [0] + + -7 (unused) + + -8 a column has a negative number of entries + + stats [4]: column with < 0 entries + stats [5]: number of entries in col + + -9 a row index is out of bounds + + stats [4]: column with bad row index + stats [5]: bad row index + stats [6]: n_row, # of rows of matrx + + -10 out of memory (unable to allocate temporary + workspace for M or count arrays using the + "allocate" routine passed into symamd). + + Future versions may return more statistics in the stats array. + + void * (*allocate) (size_t, size_t) + + A pointer to a function providing memory allocation. The + allocated memory must be returned initialized to zero. For a + C application, this argument should normally be a pointer to + calloc. For a MATLAB mexFunction, the routine mxCalloc is + passed instead. + + void (*release) (size_t, size_t) + + A pointer to a function that frees memory allocated by the + memory allocation routine above. For a C application, this + argument should normally be a pointer to free. For a MATLAB + mexFunction, the routine mxFree is passed instead. + + + ---------------------------------------------------------------------------- + colamd_report: + ---------------------------------------------------------------------------- + + C syntax: + + #include "colamd.h" + colamd_report (int stats [COLAMD_STATS]) ; + colamd_l_report (UF_long stats [COLAMD_STATS]) ; + + Purpose: + + Prints the error status and statistics recorded in the stats + array on the standard error output (for a standard C routine) + or on the MATLAB output (for a mexFunction). + + Arguments: + + int stats [COLAMD_STATS] ; Input only. Statistics from colamd. + + + ---------------------------------------------------------------------------- + symamd_report: + ---------------------------------------------------------------------------- + + C syntax: + + #include "colamd.h" + symamd_report (int stats [COLAMD_STATS]) ; + symamd_l_report (UF_long stats [COLAMD_STATS]) ; + + Purpose: + + Prints the error status and statistics recorded in the stats + array on the standard error output (for a standard C routine) + or on the MATLAB output (for a mexFunction). + + Arguments: + + int stats [COLAMD_STATS] ; Input only. Statistics from symamd. + + +*/ + +/* ========================================================================== */ +/* === Scaffolding code definitions ======================================== */ +/* ========================================================================== */ + +/* Ensure that debugging is turned off: */ +#ifndef NDEBUG +#define NDEBUG +#endif + +/* turn on debugging by uncommenting the following line + #undef NDEBUG +*/ + +/* + Our "scaffolding code" philosophy: In our opinion, well-written library + code should keep its "debugging" code, and just normally have it turned off + by the compiler so as not to interfere with performance. This serves + several purposes: + + (1) assertions act as comments to the reader, telling you what the code + expects at that point. All assertions will always be true (unless + there really is a bug, of course). + + (2) leaving in the scaffolding code assists anyone who would like to modify + the code, or understand the algorithm (by reading the debugging output, + one can get a glimpse into what the code is doing). + + (3) (gasp!) for actually finding bugs. This code has been heavily tested + and "should" be fully functional and bug-free ... but you never know... + + The code will become outrageously slow when debugging is + enabled. To control the level of debugging output, set an environment + variable D to 0 (little), 1 (some), 2, 3, or 4 (lots). When debugging, + you should see the following message on the standard output: + + colamd: debug version, D = 1 (THIS WILL BE SLOW!) + + or a similar message for symamd. If you don't, then debugging has not + been enabled. + +*/ + +/* ========================================================================== */ +/* === Include files ======================================================== */ +/* ========================================================================== */ + +#include "colamd.h" +#include +#include + +#ifdef MATLAB_MEX_FILE +#include "mex.h" +#include "matrix.h" +#endif /* MATLAB_MEX_FILE */ + +#if !defined (NPRINT) || !defined (NDEBUG) +#include +#endif + +#ifndef NULL +#define NULL ((void *) 0) +#endif + +/* ========================================================================== */ +/* === int or UF_long ======================================================= */ +/* ========================================================================== */ + +/* define UF_long */ +#include "UFconfig.h" + +#ifdef DLONG + +#define Int UF_long +#define ID UF_long_id +#define Int_MAX UF_long_max + +#define COLAMD_recommended colamd_l_recommended +#define COLAMD_set_defaults colamd_l_set_defaults +#define COLAMD_MAIN colamd_l +#define SYMAMD_MAIN symamd_l +#define COLAMD_report colamd_l_report +#define SYMAMD_report symamd_l_report + +#else + +#define Int int +#define ID "%d" +#define Int_MAX INT_MAX + +#define COLAMD_recommended colamd_recommended +#define COLAMD_set_defaults colamd_set_defaults +#define COLAMD_MAIN colamd +#define SYMAMD_MAIN symamd +#define COLAMD_report colamd_report +#define SYMAMD_report symamd_report + +#endif + +/* ========================================================================== */ +/* === Row and Column structures ============================================ */ +/* ========================================================================== */ + +/* User code that makes use of the colamd/symamd routines need not directly */ +/* reference these structures. They are used only for colamd_recommended. */ + +typedef struct Colamd_Col_struct +{ + Int start ; /* index for A of first row in this column, or DEAD */ + /* if column is dead */ + Int length ; /* number of rows in this column */ + union + { + Int thickness ; /* number of original columns represented by this */ + /* col, if the column is alive */ + Int parent ; /* parent in parent tree super-column structure, if */ + /* the column is dead */ + } shared1 ; + union + { + Int score ; /* the score used to maintain heap, if col is alive */ + Int order ; /* pivot ordering of this column, if col is dead */ + } shared2 ; + union + { + Int headhash ; /* head of a hash bucket, if col is at the head of */ + /* a degree list */ + Int hash ; /* hash value, if col is not in a degree list */ + Int prev ; /* previous column in degree list, if col is in a */ + /* degree list (but not at the head of a degree list) */ + } shared3 ; + union + { + Int degree_next ; /* next column, if col is in a degree list */ + Int hash_next ; /* next column, if col is in a hash list */ + } shared4 ; + +} Colamd_Col ; + +typedef struct Colamd_Row_struct +{ + Int start ; /* index for A of first col in this row */ + Int length ; /* number of principal columns in this row */ + union + { + Int degree ; /* number of principal & non-principal columns in row */ + Int p ; /* used as a row pointer in init_rows_cols () */ + } shared1 ; + union + { + Int mark ; /* for computing set differences and marking dead rows*/ + Int first_column ;/* first column in row (used in garbage collection) */ + } shared2 ; + +} Colamd_Row ; + +/* ========================================================================== */ +/* === Definitions ========================================================== */ +/* ========================================================================== */ + +/* Routines are either PUBLIC (user-callable) or PRIVATE (not user-callable) */ +#define PUBLIC +#define PRIVATE static + +#define DENSE_DEGREE(alpha,n) \ + ((Int) MAX (16.0, (alpha) * sqrt ((double) (n)))) + +#define MAX(a,b) (((a) > (b)) ? (a) : (b)) +#define MIN(a,b) (((a) < (b)) ? (a) : (b)) + +#define ONES_COMPLEMENT(r) (-(r)-1) + +/* -------------------------------------------------------------------------- */ +/* Change for version 2.1: define TRUE and FALSE only if not yet defined */ +/* -------------------------------------------------------------------------- */ + +#ifndef TRUE +#define TRUE (1) +#endif + +#ifndef FALSE +#define FALSE (0) +#endif + +/* -------------------------------------------------------------------------- */ + +#define EMPTY (-1) + +/* Row and column status */ +#define ALIVE (0) +#define DEAD (-1) + +/* Column status */ +#define DEAD_PRINCIPAL (-1) +#define DEAD_NON_PRINCIPAL (-2) + +/* Macros for row and column status update and checking. */ +#define ROW_IS_DEAD(r) ROW_IS_MARKED_DEAD (Row[r].shared2.mark) +#define ROW_IS_MARKED_DEAD(row_mark) (row_mark < ALIVE) +#define ROW_IS_ALIVE(r) (Row [r].shared2.mark >= ALIVE) +#define COL_IS_DEAD(c) (Col [c].start < ALIVE) +#define COL_IS_ALIVE(c) (Col [c].start >= ALIVE) +#define COL_IS_DEAD_PRINCIPAL(c) (Col [c].start == DEAD_PRINCIPAL) +#define KILL_ROW(r) { Row [r].shared2.mark = DEAD ; } +#define KILL_PRINCIPAL_COL(c) { Col [c].start = DEAD_PRINCIPAL ; } +#define KILL_NON_PRINCIPAL_COL(c) { Col [c].start = DEAD_NON_PRINCIPAL ; } + +/* ========================================================================== */ +/* === Colamd reporting mechanism =========================================== */ +/* ========================================================================== */ + +#if defined (MATLAB_MEX_FILE) || defined (MATHWORKS) +/* In MATLAB, matrices are 1-based to the user, but 0-based internally */ +#define INDEX(i) ((i)+1) +#else +/* In C, matrices are 0-based and indices are reported as such in *_report */ +#define INDEX(i) (i) +#endif + +/* All output goes through the PRINTF macro. */ +#define PRINTF(params) { if (colamd_printf != NULL) (void) colamd_printf params ; } + +/* ========================================================================== */ +/* === Prototypes of PRIVATE routines ======================================= */ +/* ========================================================================== */ + +PRIVATE Int init_rows_cols +( + Int n_row, + Int n_col, + Colamd_Row Row [], + Colamd_Col Col [], + Int A [], + Int p [], + Int stats [COLAMD_STATS] +) ; + +PRIVATE void init_scoring +( + Int n_row, + Int n_col, + Colamd_Row Row [], + Colamd_Col Col [], + Int A [], + Int head [], + double knobs [COLAMD_KNOBS], + Int *p_n_row2, + Int *p_n_col2, + Int *p_max_deg +) ; + +PRIVATE Int find_ordering +( + Int n_row, + Int n_col, + Int Alen, + Colamd_Row Row [], + Colamd_Col Col [], + Int A [], + Int head [], + Int n_col2, + Int max_deg, + Int pfree, + Int aggressive +) ; + +PRIVATE void order_children +( + Int n_col, + Colamd_Col Col [], + Int p [] +) ; + +PRIVATE void detect_super_cols +( + +#ifndef NDEBUG + Int n_col, + Colamd_Row Row [], +#endif /* NDEBUG */ + + Colamd_Col Col [], + Int A [], + Int head [], + Int row_start, + Int row_length +) ; + +PRIVATE Int garbage_collection +( + Int n_row, + Int n_col, + Colamd_Row Row [], + Colamd_Col Col [], + Int A [], + Int *pfree +) ; + +PRIVATE Int clear_mark +( + Int tag_mark, + Int max_mark, + Int n_row, + Colamd_Row Row [] +) ; + +PRIVATE void print_report +( + char *method, + Int stats [COLAMD_STATS] +) ; + +/* ========================================================================== */ +/* === Debugging prototypes and definitions ================================= */ +/* ========================================================================== */ + +#ifndef NDEBUG + +#include + +/* colamd_debug is the *ONLY* global variable, and is only */ +/* present when debugging */ + +PRIVATE Int colamd_debug = 0 ; /* debug print level */ + +#define DEBUG0(params) { PRINTF (params) ; } +#define DEBUG1(params) { if (colamd_debug >= 1) PRINTF (params) ; } +#define DEBUG2(params) { if (colamd_debug >= 2) PRINTF (params) ; } +#define DEBUG3(params) { if (colamd_debug >= 3) PRINTF (params) ; } +#define DEBUG4(params) { if (colamd_debug >= 4) PRINTF (params) ; } + +#ifdef MATLAB_MEX_FILE +#define ASSERT(expression) (mxAssert ((expression), "")) +#else +#define ASSERT(expression) (assert (expression)) +#endif /* MATLAB_MEX_FILE */ + +PRIVATE void colamd_get_debug /* gets the debug print level from getenv */ +( + char *method +) ; + +PRIVATE void debug_deg_lists +( + Int n_row, + Int n_col, + Colamd_Row Row [], + Colamd_Col Col [], + Int head [], + Int min_score, + Int should, + Int max_deg +) ; + +PRIVATE void debug_mark +( + Int n_row, + Colamd_Row Row [], + Int tag_mark, + Int max_mark +) ; + +PRIVATE void debug_matrix +( + Int n_row, + Int n_col, + Colamd_Row Row [], + Colamd_Col Col [], + Int A [] +) ; + +PRIVATE void debug_structures +( + Int n_row, + Int n_col, + Colamd_Row Row [], + Colamd_Col Col [], + Int A [], + Int n_col2 +) ; + +#else /* NDEBUG */ + +/* === No debugging ========================================================= */ + +#define DEBUG0(params) ; +#define DEBUG1(params) ; +#define DEBUG2(params) ; +#define DEBUG3(params) ; +#define DEBUG4(params) ; + +#define ASSERT(expression) + +#endif /* NDEBUG */ + +/* ========================================================================== */ +/* === USER-CALLABLE ROUTINES: ============================================== */ +/* ========================================================================== */ + +/* ========================================================================== */ +/* === colamd_recommended =================================================== */ +/* ========================================================================== */ + +/* + The colamd_recommended routine returns the suggested size for Alen. This + value has been determined to provide good balance between the number of + garbage collections and the memory requirements for colamd. If any + argument is negative, or if integer overflow occurs, a 0 is returned as an + error condition. 2*nnz space is required for the row and column + indices of the matrix. COLAMD_C (n_col) + COLAMD_R (n_row) space is + required for the Col and Row arrays, respectively, which are internal to + colamd (roughly 6*n_col + 4*n_row). An additional n_col space is the + minimal amount of "elbow room", and nnz/5 more space is recommended for + run time efficiency. + + Alen is approximately 2.2*nnz + 7*n_col + 4*n_row + 10. + + This function is not needed when using symamd. +*/ + +/* add two values of type size_t, and check for integer overflow */ +static size_t t_add (size_t a, size_t b, int *ok) +{ + (*ok) = (*ok) && ((a + b) >= MAX (a,b)) ; + return ((*ok) ? (a + b) : 0) ; +} + +/* compute a*k where k is a small integer, and check for integer overflow */ +static size_t t_mult (size_t a, size_t k, int *ok) +{ + size_t i, s = 0 ; + for (i = 0 ; i < k ; i++) + { + s = t_add (s, a, ok) ; + } + return (s) ; +} + +/* size of the Col and Row structures */ +#define COLAMD_C(n_col,ok) \ + ((t_mult (t_add (n_col, 1, ok), sizeof (Colamd_Col), ok) / sizeof (Int))) + +#define COLAMD_R(n_row,ok) \ + ((t_mult (t_add (n_row, 1, ok), sizeof (Colamd_Row), ok) / sizeof (Int))) + + +PUBLIC size_t COLAMD_recommended /* returns recommended value of Alen. */ +( + /* === Parameters ======================================================= */ + + Int nnz, /* number of nonzeros in A */ + Int n_row, /* number of rows in A */ + Int n_col /* number of columns in A */ +) +{ + size_t s, c, r ; + int ok = TRUE ; + if (nnz < 0 || n_row < 0 || n_col < 0) + { + return (0) ; + } + s = t_mult (nnz, 2, &ok) ; /* 2*nnz */ + c = COLAMD_C (n_col, &ok) ; /* size of column structures */ + r = COLAMD_R (n_row, &ok) ; /* size of row structures */ + s = t_add (s, c, &ok) ; + s = t_add (s, r, &ok) ; + s = t_add (s, n_col, &ok) ; /* elbow room */ + s = t_add (s, nnz/5, &ok) ; /* elbow room */ + ok = ok && (s < Int_MAX) ; + return (ok ? s : 0) ; +} + + +/* ========================================================================== */ +/* === colamd_set_defaults ================================================== */ +/* ========================================================================== */ + +/* + The colamd_set_defaults routine sets the default values of the user- + controllable parameters for colamd and symamd: + + Colamd: rows with more than max (16, knobs [0] * sqrt (n_col)) + entries are removed prior to ordering. Columns with more than + max (16, knobs [1] * sqrt (MIN (n_row,n_col))) entries are removed + prior to ordering, and placed last in the output column ordering. + + Symamd: Rows and columns with more than max (16, knobs [0] * sqrt (n)) + entries are removed prior to ordering, and placed last in the + output ordering. + + knobs [0] dense row control + + knobs [1] dense column control + + knobs [2] if nonzero, do aggresive absorption + + knobs [3..19] unused, but future versions might use this + +*/ + +PUBLIC void COLAMD_set_defaults +( + /* === Parameters ======================================================= */ + + double knobs [COLAMD_KNOBS] /* knob array */ +) +{ + /* === Local variables ================================================== */ + + Int i ; + + if (!knobs) + { + return ; /* no knobs to initialize */ + } + for (i = 0 ; i < COLAMD_KNOBS ; i++) + { + knobs [i] = 0 ; + } + knobs [COLAMD_DENSE_ROW] = 10 ; + knobs [COLAMD_DENSE_COL] = 10 ; + knobs [COLAMD_AGGRESSIVE] = TRUE ; /* default: do aggressive absorption*/ +} + + +/* ========================================================================== */ +/* === symamd =============================================================== */ +/* ========================================================================== */ + +PUBLIC Int SYMAMD_MAIN /* return TRUE if OK, FALSE otherwise */ +( + /* === Parameters ======================================================= */ + + 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+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) */ +) +{ + /* === Local variables ================================================== */ + + Int *count ; /* length of each column of M, and col pointer*/ + Int *mark ; /* mark array for finding duplicate entries */ + Int *M ; /* row indices of matrix M */ + size_t Mlen ; /* length of M */ + Int n_row ; /* number of rows in M */ + Int nnz ; /* number of entries in A */ + Int i ; /* row index of A */ + Int j ; /* column index of A */ + Int k ; /* row index of M */ + Int mnz ; /* number of nonzeros in M */ + Int pp ; /* index into a column of A */ + Int last_row ; /* last row seen in the current column */ + Int length ; /* number of nonzeros in a column */ + + double cknobs [COLAMD_KNOBS] ; /* knobs for colamd */ + double default_knobs [COLAMD_KNOBS] ; /* default knobs for colamd */ + +#ifndef NDEBUG + colamd_get_debug ("symamd") ; +#endif /* NDEBUG */ + + /* === Check the input arguments ======================================== */ + + if (!stats) + { + DEBUG0 (("symamd: stats not present\n")) ; + return (FALSE) ; + } + for (i = 0 ; i < COLAMD_STATS ; i++) + { + stats [i] = 0 ; + } + stats [COLAMD_STATUS] = COLAMD_OK ; + stats [COLAMD_INFO1] = -1 ; + stats [COLAMD_INFO2] = -1 ; + + if (!A) + { + stats [COLAMD_STATUS] = COLAMD_ERROR_A_not_present ; + DEBUG0 (("symamd: A not present\n")) ; + return (FALSE) ; + } + + if (!p) /* p is not present */ + { + stats [COLAMD_STATUS] = COLAMD_ERROR_p_not_present ; + DEBUG0 (("symamd: p not present\n")) ; + return (FALSE) ; + } + + if (n < 0) /* n must be >= 0 */ + { + stats [COLAMD_STATUS] = COLAMD_ERROR_ncol_negative ; + stats [COLAMD_INFO1] = n ; + DEBUG0 (("symamd: n negative %d\n", n)) ; + return (FALSE) ; + } + + nnz = p [n] ; + if (nnz < 0) /* nnz must be >= 0 */ + { + stats [COLAMD_STATUS] = COLAMD_ERROR_nnz_negative ; + stats [COLAMD_INFO1] = nnz ; + DEBUG0 (("symamd: number of entries negative %d\n", nnz)) ; + return (FALSE) ; + } + + if (p [0] != 0) + { + stats [COLAMD_STATUS] = COLAMD_ERROR_p0_nonzero ; + stats [COLAMD_INFO1] = p [0] ; + DEBUG0 (("symamd: p[0] not zero %d\n", p [0])) ; + return (FALSE) ; + } + + /* === If no knobs, set default knobs =================================== */ + + if (!knobs) + { + COLAMD_set_defaults (default_knobs) ; + knobs = default_knobs ; + } + + /* === Allocate count and mark ========================================== */ + + count = (Int *) ((*allocate) (n+1, sizeof (Int))) ; + if (!count) + { + stats [COLAMD_STATUS] = COLAMD_ERROR_out_of_memory ; + DEBUG0 (("symamd: allocate count (size %d) failed\n", n+1)) ; + return (FALSE) ; + } + + mark = (Int *) ((*allocate) (n+1, sizeof (Int))) ; + if (!mark) + { + stats [COLAMD_STATUS] = COLAMD_ERROR_out_of_memory ; + (*release) ((void *) count) ; + DEBUG0 (("symamd: allocate mark (size %d) failed\n", n+1)) ; + return (FALSE) ; + } + + /* === Compute column counts of M, check if A is valid ================== */ + + stats [COLAMD_INFO3] = 0 ; /* number of duplicate or unsorted row indices*/ + + for (i = 0 ; i < n ; i++) + { + mark [i] = -1 ; + } + + for (j = 0 ; j < n ; j++) + { + last_row = -1 ; + + length = p [j+1] - p [j] ; + if (length < 0) + { + /* column pointers must be non-decreasing */ + stats [COLAMD_STATUS] = COLAMD_ERROR_col_length_negative ; + stats [COLAMD_INFO1] = j ; + stats [COLAMD_INFO2] = length ; + (*release) ((void *) count) ; + (*release) ((void *) mark) ; + DEBUG0 (("symamd: col %d negative length %d\n", j, length)) ; + return (FALSE) ; + } + + for (pp = p [j] ; pp < p [j+1] ; pp++) + { + i = A [pp] ; + if (i < 0 || i >= n) + { + /* row index i, in column j, is out of bounds */ + stats [COLAMD_STATUS] = COLAMD_ERROR_row_index_out_of_bounds ; + stats [COLAMD_INFO1] = j ; + stats [COLAMD_INFO2] = i ; + stats [COLAMD_INFO3] = n ; + (*release) ((void *) count) ; + (*release) ((void *) mark) ; + DEBUG0 (("symamd: row %d col %d out of bounds\n", i, j)) ; + return (FALSE) ; + } + + if (i <= last_row || mark [i] == j) + { + /* row index is unsorted or repeated (or both), thus col */ + /* is jumbled. This is a notice, not an error condition. */ + stats [COLAMD_STATUS] = COLAMD_OK_BUT_JUMBLED ; + stats [COLAMD_INFO1] = j ; + stats [COLAMD_INFO2] = i ; + (stats [COLAMD_INFO3]) ++ ; + DEBUG1 (("symamd: row %d col %d unsorted/duplicate\n", i, j)) ; + } + + if (i > j && mark [i] != j) + { + /* row k of M will contain column indices i and j */ + count [i]++ ; + count [j]++ ; + } + + /* mark the row as having been seen in this column */ + mark [i] = j ; + + last_row = i ; + } + } + + /* v2.4: removed free(mark) */ + + /* === Compute column pointers of M ===================================== */ + + /* use output permutation, perm, for column pointers of M */ + perm [0] = 0 ; + for (j = 1 ; j <= n ; j++) + { + perm [j] = perm [j-1] + count [j-1] ; + } + for (j = 0 ; j < n ; j++) + { + count [j] = perm [j] ; + } + + /* === Construct M ====================================================== */ + + mnz = perm [n] ; + n_row = mnz / 2 ; + Mlen = COLAMD_recommended (mnz, n_row, n) ; + M = (Int *) ((*allocate) (Mlen, sizeof (Int))) ; + DEBUG0 (("symamd: M is %d-by-%d with %d entries, Mlen = %g\n", + n_row, n, mnz, (double) Mlen)) ; + + if (!M) + { + stats [COLAMD_STATUS] = COLAMD_ERROR_out_of_memory ; + (*release) ((void *) count) ; + (*release) ((void *) mark) ; + DEBUG0 (("symamd: allocate M (size %g) failed\n", (double) Mlen)) ; + return (FALSE) ; + } + + k = 0 ; + + if (stats [COLAMD_STATUS] == COLAMD_OK) + { + /* Matrix is OK */ + for (j = 0 ; j < n ; j++) + { + ASSERT (p [j+1] - p [j] >= 0) ; + for (pp = p [j] ; pp < p [j+1] ; pp++) + { + i = A [pp] ; + ASSERT (i >= 0 && i < n) ; + if (i > j) + { + /* row k of M contains column indices i and j */ + M [count [i]++] = k ; + M [count [j]++] = k ; + k++ ; + } + } + } + } + else + { + /* Matrix is jumbled. Do not add duplicates to M. Unsorted cols OK. */ + DEBUG0 (("symamd: Duplicates in A.\n")) ; + for (i = 0 ; i < n ; i++) + { + mark [i] = -1 ; + } + for (j = 0 ; j < n ; j++) + { + ASSERT (p [j+1] - p [j] >= 0) ; + for (pp = p [j] ; pp < p [j+1] ; pp++) + { + i = A [pp] ; + ASSERT (i >= 0 && i < n) ; + if (i > j && mark [i] != j) + { + /* row k of M contains column indices i and j */ + M [count [i]++] = k ; + M [count [j]++] = k ; + k++ ; + mark [i] = j ; + } + } + } + /* v2.4: free(mark) moved below */ + } + + /* count and mark no longer needed */ + (*release) ((void *) count) ; + (*release) ((void *) mark) ; /* v2.4: free (mark) moved here */ + ASSERT (k == n_row) ; + + /* === Adjust the knobs for M =========================================== */ + + for (i = 0 ; i < COLAMD_KNOBS ; i++) + { + cknobs [i] = knobs [i] ; + } + + /* there are no dense rows in M */ + cknobs [COLAMD_DENSE_ROW] = -1 ; + cknobs [COLAMD_DENSE_COL] = knobs [COLAMD_DENSE_ROW] ; + + /* === Order the columns of M =========================================== */ + + /* v2.4: colamd cannot fail here, so the error check is removed */ + (void) COLAMD_MAIN (n_row, n, (Int) Mlen, M, perm, cknobs, stats) ; + + /* Note that the output permutation is now in perm */ + + /* === get the statistics for symamd from colamd ======================== */ + + /* a dense column in colamd means a dense row and col in symamd */ + stats [COLAMD_DENSE_ROW] = stats [COLAMD_DENSE_COL] ; + + /* === Free M =========================================================== */ + + (*release) ((void *) M) ; + DEBUG0 (("symamd: done.\n")) ; + return (TRUE) ; + +} + +/* ========================================================================== */ +/* === colamd =============================================================== */ +/* ========================================================================== */ + +/* + The colamd routine computes a column ordering Q of a sparse matrix + A such that the LU factorization P(AQ) = LU remains sparse, where P is + selected via partial pivoting. The routine can also be viewed as + providing a permutation Q such that the Cholesky factorization + (AQ)'(AQ) = LL' remains sparse. +*/ + +PUBLIC Int COLAMD_MAIN /* returns TRUE if successful, FALSE otherwise*/ +( + /* === Parameters ======================================================= */ + + Int n_row, /* number of rows in A */ + Int n_col, /* number of columns in A */ + Int Alen, /* length of A */ + Int A [], /* row indices of A */ + Int p [], /* pointers to columns in A */ + double knobs [COLAMD_KNOBS],/* parameters (uses defaults if NULL) */ + Int stats [COLAMD_STATS] /* output statistics and error codes */ +) +{ + /* === Local variables ================================================== */ + + Int i ; /* loop index */ + Int nnz ; /* nonzeros in A */ + size_t Row_size ; /* size of Row [], in integers */ + size_t Col_size ; /* size of Col [], in integers */ + size_t need ; /* minimum required length of A */ + Colamd_Row *Row ; /* pointer into A of Row [0..n_row] array */ + Colamd_Col *Col ; /* pointer into A of Col [0..n_col] array */ + Int n_col2 ; /* number of non-dense, non-empty columns */ + Int n_row2 ; /* number of non-dense, non-empty rows */ + Int ngarbage ; /* number of garbage collections performed */ + Int max_deg ; /* maximum row degree */ + double default_knobs [COLAMD_KNOBS] ; /* default knobs array */ + Int aggressive ; /* do aggressive absorption */ + int ok ; + +#ifndef NDEBUG + colamd_get_debug ("colamd") ; +#endif /* NDEBUG */ + + /* === Check the input arguments ======================================== */ + + if (!stats) + { + DEBUG0 (("colamd: stats not present\n")) ; + return (FALSE) ; + } + for (i = 0 ; i < COLAMD_STATS ; i++) + { + stats [i] = 0 ; + } + stats [COLAMD_STATUS] = COLAMD_OK ; + stats [COLAMD_INFO1] = -1 ; + stats [COLAMD_INFO2] = -1 ; + + if (!A) /* A is not present */ + { + stats [COLAMD_STATUS] = COLAMD_ERROR_A_not_present ; + DEBUG0 (("colamd: A not present\n")) ; + return (FALSE) ; + } + + if (!p) /* p is not present */ + { + stats [COLAMD_STATUS] = COLAMD_ERROR_p_not_present ; + DEBUG0 (("colamd: p not present\n")) ; + return (FALSE) ; + } + + if (n_row < 0) /* n_row must be >= 0 */ + { + stats [COLAMD_STATUS] = COLAMD_ERROR_nrow_negative ; + stats [COLAMD_INFO1] = n_row ; + DEBUG0 (("colamd: nrow negative %d\n", n_row)) ; + return (FALSE) ; + } + + if (n_col < 0) /* n_col must be >= 0 */ + { + stats [COLAMD_STATUS] = COLAMD_ERROR_ncol_negative ; + stats [COLAMD_INFO1] = n_col ; + DEBUG0 (("colamd: ncol negative %d\n", n_col)) ; + return (FALSE) ; + } + + nnz = p [n_col] ; + if (nnz < 0) /* nnz must be >= 0 */ + { + stats [COLAMD_STATUS] = COLAMD_ERROR_nnz_negative ; + stats [COLAMD_INFO1] = nnz ; + DEBUG0 (("colamd: number of entries negative %d\n", nnz)) ; + return (FALSE) ; + } + + if (p [0] != 0) + { + stats [COLAMD_STATUS] = COLAMD_ERROR_p0_nonzero ; + stats [COLAMD_INFO1] = p [0] ; + DEBUG0 (("colamd: p[0] not zero %d\n", p [0])) ; + return (FALSE) ; + } + + /* === If no knobs, set default knobs =================================== */ + + if (!knobs) + { + COLAMD_set_defaults (default_knobs) ; + knobs = default_knobs ; + } + + aggressive = (knobs [COLAMD_AGGRESSIVE] != FALSE) ; + + /* === Allocate the Row and Col arrays from array A ===================== */ + + ok = TRUE ; + Col_size = COLAMD_C (n_col, &ok) ; /* size of Col array of structs */ + Row_size = COLAMD_R (n_row, &ok) ; /* size of Row array of structs */ + + /* need = 2*nnz + n_col + Col_size + Row_size ; */ + need = t_mult (nnz, 2, &ok) ; + need = t_add (need, n_col, &ok) ; + need = t_add (need, Col_size, &ok) ; + need = t_add (need, Row_size, &ok) ; + + if (!ok || need > (size_t) Alen || need > Int_MAX) + { + /* not enough space in array A to perform the ordering */ + stats [COLAMD_STATUS] = COLAMD_ERROR_A_too_small ; + stats [COLAMD_INFO1] = need ; + stats [COLAMD_INFO2] = Alen ; + DEBUG0 (("colamd: Need Alen >= %d, given only Alen = %d\n", need,Alen)); + return (FALSE) ; + } + + Alen -= Col_size + Row_size ; + Col = (Colamd_Col *) &A [Alen] ; + Row = (Colamd_Row *) &A [Alen + Col_size] ; + + /* === Construct the row and column data structures ===================== */ + + if (!init_rows_cols (n_row, n_col, Row, Col, A, p, stats)) + { + /* input matrix is invalid */ + DEBUG0 (("colamd: Matrix invalid\n")) ; + return (FALSE) ; + } + + /* === Initialize scores, kill dense rows/columns ======================= */ + + init_scoring (n_row, n_col, Row, Col, A, p, knobs, + &n_row2, &n_col2, &max_deg) ; + + /* === Order the supercolumns =========================================== */ + + ngarbage = find_ordering (n_row, n_col, Alen, Row, Col, A, p, + n_col2, max_deg, 2*nnz, aggressive) ; + + /* === Order the non-principal columns ================================== */ + + order_children (n_col, Col, p) ; + + /* === Return statistics in stats ======================================= */ + + stats [COLAMD_DENSE_ROW] = n_row - n_row2 ; + stats [COLAMD_DENSE_COL] = n_col - n_col2 ; + stats [COLAMD_DEFRAG_COUNT] = ngarbage ; + DEBUG0 (("colamd: done.\n")) ; + return (TRUE) ; +} + + +/* ========================================================================== */ +/* === colamd_report ======================================================== */ +/* ========================================================================== */ + +PUBLIC void COLAMD_report +( + Int stats [COLAMD_STATS] +) +{ + print_report ("colamd", stats) ; +} + + +/* ========================================================================== */ +/* === symamd_report ======================================================== */ +/* ========================================================================== */ + +PUBLIC void SYMAMD_report +( + Int stats [COLAMD_STATS] +) +{ + print_report ("symamd", stats) ; +} + + + +/* ========================================================================== */ +/* === NON-USER-CALLABLE ROUTINES: ========================================== */ +/* ========================================================================== */ + +/* There are no user-callable routines beyond this point in the file */ + + +/* ========================================================================== */ +/* === init_rows_cols ======================================================= */ +/* ========================================================================== */ + +/* + Takes the column form of the matrix in A and creates the row form of the + matrix. Also, row and column attributes are stored in the Col and Row + structs. If the columns are un-sorted or contain duplicate row indices, + this routine will also sort and remove duplicate row indices from the + column form of the matrix. Returns FALSE if the matrix is invalid, + TRUE otherwise. Not user-callable. +*/ + +PRIVATE Int init_rows_cols /* returns TRUE if OK, or FALSE otherwise */ +( + /* === Parameters ======================================================= */ + + Int n_row, /* number of rows of A */ + Int n_col, /* number of columns of A */ + Colamd_Row Row [], /* of size n_row+1 */ + Colamd_Col Col [], /* of size n_col+1 */ + Int A [], /* row indices of A, of size Alen */ + Int p [], /* pointers to columns in A, of size n_col+1 */ + Int stats [COLAMD_STATS] /* colamd statistics */ +) +{ + /* === Local variables ================================================== */ + + Int col ; /* a column index */ + Int row ; /* a row index */ + Int *cp ; /* a column pointer */ + Int *cp_end ; /* a pointer to the end of a column */ + Int *rp ; /* a row pointer */ + Int *rp_end ; /* a pointer to the end of a row */ + Int last_row ; /* previous row */ + + /* === Initialize columns, and check column pointers ==================== */ + + for (col = 0 ; col < n_col ; col++) + { + Col [col].start = p [col] ; + Col [col].length = p [col+1] - p [col] ; + + if (Col [col].length < 0) + { + /* column pointers must be non-decreasing */ + stats [COLAMD_STATUS] = COLAMD_ERROR_col_length_negative ; + stats [COLAMD_INFO1] = col ; + stats [COLAMD_INFO2] = Col [col].length ; + DEBUG0 (("colamd: col %d length %d < 0\n", col, Col [col].length)) ; + return (FALSE) ; + } + + Col [col].shared1.thickness = 1 ; + Col [col].shared2.score = 0 ; + Col [col].shared3.prev = EMPTY ; + Col [col].shared4.degree_next = EMPTY ; + } + + /* p [0..n_col] no longer needed, used as "head" in subsequent routines */ + + /* === Scan columns, compute row degrees, and check row indices ========= */ + + stats [COLAMD_INFO3] = 0 ; /* number of duplicate or unsorted row indices*/ + + for (row = 0 ; row < n_row ; row++) + { + Row [row].length = 0 ; + Row [row].shared2.mark = -1 ; + } + + for (col = 0 ; col < n_col ; col++) + { + last_row = -1 ; + + cp = &A [p [col]] ; + cp_end = &A [p [col+1]] ; + + while (cp < cp_end) + { + row = *cp++ ; + + /* make sure row indices within range */ + if (row < 0 || row >= n_row) + { + stats [COLAMD_STATUS] = COLAMD_ERROR_row_index_out_of_bounds ; + stats [COLAMD_INFO1] = col ; + stats [COLAMD_INFO2] = row ; + stats [COLAMD_INFO3] = n_row ; + DEBUG0 (("colamd: row %d col %d out of bounds\n", row, col)) ; + return (FALSE) ; + } + + if (row <= last_row || Row [row].shared2.mark == col) + { + /* row index are unsorted or repeated (or both), thus col */ + /* is jumbled. This is a notice, not an error condition. */ + stats [COLAMD_STATUS] = COLAMD_OK_BUT_JUMBLED ; + stats [COLAMD_INFO1] = col ; + stats [COLAMD_INFO2] = row ; + (stats [COLAMD_INFO3]) ++ ; + DEBUG1 (("colamd: row %d col %d unsorted/duplicate\n",row,col)); + } + + if (Row [row].shared2.mark != col) + { + Row [row].length++ ; + } + else + { + /* this is a repeated entry in the column, */ + /* it will be removed */ + Col [col].length-- ; + } + + /* mark the row as having been seen in this column */ + Row [row].shared2.mark = col ; + + last_row = row ; + } + } + + /* === Compute row pointers ============================================= */ + + /* row form of the matrix starts directly after the column */ + /* form of matrix in A */ + Row [0].start = p [n_col] ; + Row [0].shared1.p = Row [0].start ; + Row [0].shared2.mark = -1 ; + for (row = 1 ; row < n_row ; row++) + { + Row [row].start = Row [row-1].start + Row [row-1].length ; + Row [row].shared1.p = Row [row].start ; + Row [row].shared2.mark = -1 ; + } + + /* === Create row form ================================================== */ + + if (stats [COLAMD_STATUS] == COLAMD_OK_BUT_JUMBLED) + { + /* if cols jumbled, watch for repeated row indices */ + for (col = 0 ; col < n_col ; col++) + { + cp = &A [p [col]] ; + cp_end = &A [p [col+1]] ; + while (cp < cp_end) + { + row = *cp++ ; + if (Row [row].shared2.mark != col) + { + A [(Row [row].shared1.p)++] = col ; + Row [row].shared2.mark = col ; + } + } + } + } + else + { + /* if cols not jumbled, we don't need the mark (this is faster) */ + for (col = 0 ; col < n_col ; col++) + { + cp = &A [p [col]] ; + cp_end = &A [p [col+1]] ; + while (cp < cp_end) + { + A [(Row [*cp++].shared1.p)++] = col ; + } + } + } + + /* === Clear the row marks and set row degrees ========================== */ + + for (row = 0 ; row < n_row ; row++) + { + Row [row].shared2.mark = 0 ; + Row [row].shared1.degree = Row [row].length ; + } + + /* === See if we need to re-create columns ============================== */ + + if (stats [COLAMD_STATUS] == COLAMD_OK_BUT_JUMBLED) + { + DEBUG0 (("colamd: reconstructing column form, matrix jumbled\n")) ; + +#ifndef NDEBUG + /* make sure column lengths are correct */ + for (col = 0 ; col < n_col ; col++) + { + p [col] = Col [col].length ; + } + for (row = 0 ; row < n_row ; row++) + { + rp = &A [Row [row].start] ; + rp_end = rp + Row [row].length ; + while (rp < rp_end) + { + p [*rp++]-- ; + } + } + for (col = 0 ; col < n_col ; col++) + { + ASSERT (p [col] == 0) ; + } + /* now p is all zero (different than when debugging is turned off) */ +#endif /* NDEBUG */ + + /* === Compute col pointers ========================================= */ + + /* col form of the matrix starts at A [0]. */ + /* Note, we may have a gap between the col form and the row */ + /* form if there were duplicate entries, if so, it will be */ + /* removed upon the first garbage collection */ + Col [0].start = 0 ; + p [0] = Col [0].start ; + for (col = 1 ; col < n_col ; col++) + { + /* note that the lengths here are for pruned columns, i.e. */ + /* no duplicate row indices will exist for these columns */ + Col [col].start = Col [col-1].start + Col [col-1].length ; + p [col] = Col [col].start ; + } + + /* === Re-create col form =========================================== */ + + for (row = 0 ; row < n_row ; row++) + { + rp = &A [Row [row].start] ; + rp_end = rp + Row [row].length ; + while (rp < rp_end) + { + A [(p [*rp++])++] = row ; + } + } + } + + /* === Done. Matrix is not (or no longer) jumbled ====================== */ + + return (TRUE) ; +} + + +/* ========================================================================== */ +/* === init_scoring ========================================================= */ +/* ========================================================================== */ + +/* + Kills dense or empty columns and rows, calculates an initial score for + each column, and places all columns in the degree lists. Not user-callable. +*/ + +PRIVATE void init_scoring +( + /* === Parameters ======================================================= */ + + Int n_row, /* number of rows of A */ + Int n_col, /* number of columns of A */ + Colamd_Row Row [], /* of size n_row+1 */ + Colamd_Col Col [], /* of size n_col+1 */ + Int A [], /* column form and row form of A */ + Int head [], /* of size n_col+1 */ + double knobs [COLAMD_KNOBS],/* parameters */ + Int *p_n_row2, /* number of non-dense, non-empty rows */ + Int *p_n_col2, /* number of non-dense, non-empty columns */ + Int *p_max_deg /* maximum row degree */ +) +{ + /* === Local variables ================================================== */ + + Int c ; /* a column index */ + Int r, row ; /* a row index */ + Int *cp ; /* a column pointer */ + Int deg ; /* degree of a row or column */ + Int *cp_end ; /* a pointer to the end of a column */ + Int *new_cp ; /* new column pointer */ + Int col_length ; /* length of pruned column */ + Int score ; /* current column score */ + Int n_col2 ; /* number of non-dense, non-empty columns */ + Int n_row2 ; /* number of non-dense, non-empty rows */ + Int dense_row_count ; /* remove rows with more entries than this */ + Int dense_col_count ; /* remove cols with more entries than this */ + Int min_score ; /* smallest column score */ + Int max_deg ; /* maximum row degree */ + Int next_col ; /* Used to add to degree list.*/ + +#ifndef NDEBUG + Int debug_count ; /* debug only. */ +#endif /* NDEBUG */ + + /* === Extract knobs ==================================================== */ + + /* Note: if knobs contains a NaN, this is undefined: */ + if (knobs [COLAMD_DENSE_ROW] < 0) + { + /* only remove completely dense rows */ + dense_row_count = n_col-1 ; + } + else + { + dense_row_count = DENSE_DEGREE (knobs [COLAMD_DENSE_ROW], n_col) ; + } + if (knobs [COLAMD_DENSE_COL] < 0) + { + /* only remove completely dense columns */ + dense_col_count = n_row-1 ; + } + else + { + dense_col_count = + DENSE_DEGREE (knobs [COLAMD_DENSE_COL], MIN (n_row, n_col)) ; + } + + DEBUG1 (("colamd: densecount: %d %d\n", dense_row_count, dense_col_count)) ; + max_deg = 0 ; + n_col2 = n_col ; + n_row2 = n_row ; + + /* === Kill empty columns =============================================== */ + + /* Put the empty columns at the end in their natural order, so that LU */ + /* factorization can proceed as far as possible. */ + for (c = n_col-1 ; c >= 0 ; c--) + { + deg = Col [c].length ; + if (deg == 0) + { + /* this is a empty column, kill and order it last */ + Col [c].shared2.order = --n_col2 ; + KILL_PRINCIPAL_COL (c) ; + } + } + DEBUG1 (("colamd: null columns killed: %d\n", n_col - n_col2)) ; + + /* === Kill dense columns =============================================== */ + + /* Put the dense columns at the end, in their natural order */ + for (c = n_col-1 ; c >= 0 ; c--) + { + /* skip any dead columns */ + if (COL_IS_DEAD (c)) + { + continue ; + } + deg = Col [c].length ; + if (deg > dense_col_count) + { + /* this is a dense column, kill and order it last */ + Col [c].shared2.order = --n_col2 ; + /* decrement the row degrees */ + cp = &A [Col [c].start] ; + cp_end = cp + Col [c].length ; + while (cp < cp_end) + { + Row [*cp++].shared1.degree-- ; + } + KILL_PRINCIPAL_COL (c) ; + } + } + DEBUG1 (("colamd: Dense and null columns killed: %d\n", n_col - n_col2)) ; + + /* === Kill dense and empty rows ======================================== */ + + for (r = 0 ; r < n_row ; r++) + { + deg = Row [r].shared1.degree ; + ASSERT (deg >= 0 && deg <= n_col) ; + if (deg > dense_row_count || deg == 0) + { + /* kill a dense or empty row */ + KILL_ROW (r) ; + --n_row2 ; + } + else + { + /* keep track of max degree of remaining rows */ + max_deg = MAX (max_deg, deg) ; + } + } + DEBUG1 (("colamd: Dense and null rows killed: %d\n", n_row - n_row2)) ; + + /* === Compute initial column scores ==================================== */ + + /* At this point the row degrees are accurate. They reflect the number */ + /* of "live" (non-dense) columns in each row. No empty rows exist. */ + /* Some "live" columns may contain only dead rows, however. These are */ + /* pruned in the code below. */ + + /* now find the initial matlab score for each column */ + for (c = n_col-1 ; c >= 0 ; c--) + { + /* skip dead column */ + if (COL_IS_DEAD (c)) + { + continue ; + } + score = 0 ; + cp = &A [Col [c].start] ; + new_cp = cp ; + cp_end = cp + Col [c].length ; + while (cp < cp_end) + { + /* get a row */ + row = *cp++ ; + /* skip if dead */ + if (ROW_IS_DEAD (row)) + { + continue ; + } + /* compact the column */ + *new_cp++ = row ; + /* add row's external degree */ + score += Row [row].shared1.degree - 1 ; + /* guard against integer overflow */ + score = MIN (score, n_col) ; + } + /* determine pruned column length */ + col_length = (Int) (new_cp - &A [Col [c].start]) ; + if (col_length == 0) + { + /* a newly-made null column (all rows in this col are "dense" */ + /* and have already been killed) */ + DEBUG2 (("Newly null killed: %d\n", c)) ; + Col [c].shared2.order = --n_col2 ; + KILL_PRINCIPAL_COL (c) ; + } + else + { + /* set column length and set score */ + ASSERT (score >= 0) ; + ASSERT (score <= n_col) ; + Col [c].length = col_length ; + Col [c].shared2.score = score ; + } + } + DEBUG1 (("colamd: Dense, null, and newly-null columns killed: %d\n", + n_col-n_col2)) ; + + /* At this point, all empty rows and columns are dead. All live columns */ + /* are "clean" (containing no dead rows) and simplicial (no supercolumns */ + /* yet). Rows may contain dead columns, but all live rows contain at */ + /* least one live column. */ + +#ifndef NDEBUG + debug_structures (n_row, n_col, Row, Col, A, n_col2) ; +#endif /* NDEBUG */ + + /* === Initialize degree lists ========================================== */ + +#ifndef NDEBUG + debug_count = 0 ; +#endif /* NDEBUG */ + + /* clear the hash buckets */ + for (c = 0 ; c <= n_col ; c++) + { + head [c] = EMPTY ; + } + min_score = n_col ; + /* place in reverse order, so low column indices are at the front */ + /* of the lists. This is to encourage natural tie-breaking */ + for (c = n_col-1 ; c >= 0 ; c--) + { + /* only add principal columns to degree lists */ + if (COL_IS_ALIVE (c)) + { + DEBUG4 (("place %d score %d minscore %d ncol %d\n", + c, Col [c].shared2.score, min_score, n_col)) ; + + /* === Add columns score to DList =============================== */ + + score = Col [c].shared2.score ; + + ASSERT (min_score >= 0) ; + ASSERT (min_score <= n_col) ; + ASSERT (score >= 0) ; + ASSERT (score <= n_col) ; + ASSERT (head [score] >= EMPTY) ; + + /* now add this column to dList at proper score location */ + next_col = head [score] ; + Col [c].shared3.prev = EMPTY ; + Col [c].shared4.degree_next = next_col ; + + /* if there already was a column with the same score, set its */ + /* previous pointer to this new column */ + if (next_col != EMPTY) + { + Col [next_col].shared3.prev = c ; + } + head [score] = c ; + + /* see if this score is less than current min */ + min_score = MIN (min_score, score) ; + +#ifndef NDEBUG + debug_count++ ; +#endif /* NDEBUG */ + + } + } + +#ifndef NDEBUG + DEBUG1 (("colamd: Live cols %d out of %d, non-princ: %d\n", + debug_count, n_col, n_col-debug_count)) ; + ASSERT (debug_count == n_col2) ; + debug_deg_lists (n_row, n_col, Row, Col, head, min_score, n_col2, max_deg) ; +#endif /* NDEBUG */ + + /* === Return number of remaining columns, and max row degree =========== */ + + *p_n_col2 = n_col2 ; + *p_n_row2 = n_row2 ; + *p_max_deg = max_deg ; +} + + +/* ========================================================================== */ +/* === find_ordering ======================================================== */ +/* ========================================================================== */ + +/* + Order the principal columns of the supercolumn form of the matrix + (no supercolumns on input). Uses a minimum approximate column minimum + degree ordering method. Not user-callable. +*/ + +PRIVATE Int find_ordering /* return the number of garbage collections */ +( + /* === Parameters ======================================================= */ + + Int n_row, /* number of rows of A */ + Int n_col, /* number of columns of A */ + Int Alen, /* size of A, 2*nnz + n_col or larger */ + Colamd_Row Row [], /* of size n_row+1 */ + Colamd_Col Col [], /* of size n_col+1 */ + Int A [], /* column form and row form of A */ + Int head [], /* of size n_col+1 */ + Int n_col2, /* Remaining columns to order */ + Int max_deg, /* Maximum row degree */ + Int pfree, /* index of first free slot (2*nnz on entry) */ + Int aggressive +) +{ + /* === Local variables ================================================== */ + + Int k ; /* current pivot ordering step */ + Int pivot_col ; /* current pivot column */ + Int *cp ; /* a column pointer */ + Int *rp ; /* a row pointer */ + Int pivot_row ; /* current pivot row */ + Int *new_cp ; /* modified column pointer */ + Int *new_rp ; /* modified row pointer */ + Int pivot_row_start ; /* pointer to start of pivot row */ + Int pivot_row_degree ; /* number of columns in pivot row */ + Int pivot_row_length ; /* number of supercolumns in pivot row */ + Int pivot_col_score ; /* score of pivot column */ + Int needed_memory ; /* free space needed for pivot row */ + Int *cp_end ; /* pointer to the end of a column */ + Int *rp_end ; /* pointer to the end of a row */ + Int row ; /* a row index */ + Int col ; /* a column index */ + Int max_score ; /* maximum possible score */ + Int cur_score ; /* score of current column */ + unsigned Int hash ; /* hash value for supernode detection */ + Int head_column ; /* head of hash bucket */ + Int first_col ; /* first column in hash bucket */ + Int tag_mark ; /* marker value for mark array */ + Int row_mark ; /* Row [row].shared2.mark */ + Int set_difference ; /* set difference size of row with pivot row */ + Int min_score ; /* smallest column score */ + Int col_thickness ; /* "thickness" (no. of columns in a supercol) */ + Int max_mark ; /* maximum value of tag_mark */ + Int pivot_col_thickness ; /* number of columns represented by pivot col */ + Int prev_col ; /* Used by Dlist operations. */ + Int next_col ; /* Used by Dlist operations. */ + Int ngarbage ; /* number of garbage collections performed */ + +#ifndef NDEBUG + Int debug_d ; /* debug loop counter */ + Int debug_step = 0 ; /* debug loop counter */ +#endif /* NDEBUG */ + + /* === Initialization and clear mark ==================================== */ + + max_mark = INT_MAX - n_col ; /* INT_MAX defined in */ + tag_mark = clear_mark (0, max_mark, n_row, Row) ; + min_score = 0 ; + ngarbage = 0 ; + DEBUG1 (("colamd: Ordering, n_col2=%d\n", n_col2)) ; + + /* === Order the columns ================================================ */ + + for (k = 0 ; k < n_col2 ; /* 'k' is incremented below */) + { + +#ifndef NDEBUG + if (debug_step % 100 == 0) + { + DEBUG2 (("\n... Step k: %d out of n_col2: %d\n", k, n_col2)) ; + } + else + { + DEBUG3 (("\n----------Step k: %d out of n_col2: %d\n", k, n_col2)) ; + } + debug_step++ ; + debug_deg_lists (n_row, n_col, Row, Col, head, + min_score, n_col2-k, max_deg) ; + debug_matrix (n_row, n_col, Row, Col, A) ; +#endif /* NDEBUG */ + + /* === Select pivot column, and order it ============================ */ + + /* make sure degree list isn't empty */ + ASSERT (min_score >= 0) ; + ASSERT (min_score <= n_col) ; + ASSERT (head [min_score] >= EMPTY) ; + +#ifndef NDEBUG + for (debug_d = 0 ; debug_d < min_score ; debug_d++) + { + ASSERT (head [debug_d] == EMPTY) ; + } +#endif /* NDEBUG */ + + /* get pivot column from head of minimum degree list */ + while (head [min_score] == EMPTY && min_score < n_col) + { + min_score++ ; + } + pivot_col = head [min_score] ; + ASSERT (pivot_col >= 0 && pivot_col <= n_col) ; + next_col = Col [pivot_col].shared4.degree_next ; + head [min_score] = next_col ; + if (next_col != EMPTY) + { + Col [next_col].shared3.prev = EMPTY ; + } + + ASSERT (COL_IS_ALIVE (pivot_col)) ; + + /* remember score for defrag check */ + pivot_col_score = Col [pivot_col].shared2.score ; + + /* the pivot column is the kth column in the pivot order */ + Col [pivot_col].shared2.order = k ; + + /* increment order count by column thickness */ + pivot_col_thickness = Col [pivot_col].shared1.thickness ; + k += pivot_col_thickness ; + ASSERT (pivot_col_thickness > 0) ; + DEBUG3 (("Pivot col: %d thick %d\n", pivot_col, pivot_col_thickness)) ; + + /* === Garbage_collection, if necessary ============================= */ + + needed_memory = MIN (pivot_col_score, n_col - k) ; + if (pfree + needed_memory >= Alen) + { + pfree = garbage_collection (n_row, n_col, Row, Col, A, &A [pfree]) ; + ngarbage++ ; + /* after garbage collection we will have enough */ + ASSERT (pfree + needed_memory < Alen) ; + /* garbage collection has wiped out the Row[].shared2.mark array */ + tag_mark = clear_mark (0, max_mark, n_row, Row) ; + +#ifndef NDEBUG + debug_matrix (n_row, n_col, Row, Col, A) ; +#endif /* NDEBUG */ + } + + /* === Compute pivot row pattern ==================================== */ + + /* get starting location for this new merged row */ + pivot_row_start = pfree ; + + /* initialize new row counts to zero */ + pivot_row_degree = 0 ; + + /* tag pivot column as having been visited so it isn't included */ + /* in merged pivot row */ + Col [pivot_col].shared1.thickness = -pivot_col_thickness ; + + /* pivot row is the union of all rows in the pivot column pattern */ + cp = &A [Col [pivot_col].start] ; + cp_end = cp + Col [pivot_col].length ; + while (cp < cp_end) + { + /* get a row */ + row = *cp++ ; + DEBUG4 (("Pivot col pattern %d %d\n", ROW_IS_ALIVE (row), row)) ; + /* skip if row is dead */ + if (ROW_IS_ALIVE (row)) + { + rp = &A [Row [row].start] ; + rp_end = rp + Row [row].length ; + while (rp < rp_end) + { + /* get a column */ + col = *rp++ ; + /* add the column, if alive and untagged */ + col_thickness = Col [col].shared1.thickness ; + if (col_thickness > 0 && COL_IS_ALIVE (col)) + { + /* tag column in pivot row */ + Col [col].shared1.thickness = -col_thickness ; + ASSERT (pfree < Alen) ; + /* place column in pivot row */ + A [pfree++] = col ; + pivot_row_degree += col_thickness ; + } + } + } + } + + /* clear tag on pivot column */ + Col [pivot_col].shared1.thickness = pivot_col_thickness ; + max_deg = MAX (max_deg, pivot_row_degree) ; + +#ifndef NDEBUG + DEBUG3 (("check2\n")) ; + debug_mark (n_row, Row, tag_mark, max_mark) ; +#endif /* NDEBUG */ + + /* === Kill all rows used to construct pivot row ==================== */ + + /* also kill pivot row, temporarily */ + cp = &A [Col [pivot_col].start] ; + cp_end = cp + Col [pivot_col].length ; + while (cp < cp_end) + { + /* may be killing an already dead row */ + row = *cp++ ; + DEBUG3 (("Kill row in pivot col: %d\n", row)) ; + KILL_ROW (row) ; + } + + /* === Select a row index to use as the new pivot row =============== */ + + pivot_row_length = pfree - pivot_row_start ; + if (pivot_row_length > 0) + { + /* pick the "pivot" row arbitrarily (first row in col) */ + pivot_row = A [Col [pivot_col].start] ; + DEBUG3 (("Pivotal row is %d\n", pivot_row)) ; + } + else + { + /* there is no pivot row, since it is of zero length */ + pivot_row = EMPTY ; + ASSERT (pivot_row_length == 0) ; + } + ASSERT (Col [pivot_col].length > 0 || pivot_row_length == 0) ; + + /* === Approximate degree computation =============================== */ + + /* Here begins the computation of the approximate degree. The column */ + /* score is the sum of the pivot row "length", plus the size of the */ + /* set differences of each row in the column minus the pattern of the */ + /* pivot row itself. The column ("thickness") itself is also */ + /* excluded from the column score (we thus use an approximate */ + /* external degree). */ + + /* The time taken by the following code (compute set differences, and */ + /* add them up) is proportional to the size of the data structure */ + /* being scanned - that is, the sum of the sizes of each column in */ + /* the pivot row. Thus, the amortized time to compute a column score */ + /* is proportional to the size of that column (where size, in this */ + /* context, is the column "length", or the number of row indices */ + /* in that column). The number of row indices in a column is */ + /* monotonically non-decreasing, from the length of the original */ + /* column on input to colamd. */ + + /* === Compute set differences ====================================== */ + + DEBUG3 (("** Computing set differences phase. **\n")) ; + + /* pivot row is currently dead - it will be revived later. */ + + DEBUG3 (("Pivot row: ")) ; + /* for each column in pivot row */ + rp = &A [pivot_row_start] ; + rp_end = rp + pivot_row_length ; + while (rp < rp_end) + { + col = *rp++ ; + ASSERT (COL_IS_ALIVE (col) && col != pivot_col) ; + DEBUG3 (("Col: %d\n", col)) ; + + /* clear tags used to construct pivot row pattern */ + col_thickness = -Col [col].shared1.thickness ; + ASSERT (col_thickness > 0) ; + Col [col].shared1.thickness = col_thickness ; + + /* === Remove column from degree list =========================== */ + + cur_score = Col [col].shared2.score ; + prev_col = Col [col].shared3.prev ; + next_col = Col [col].shared4.degree_next ; + ASSERT (cur_score >= 0) ; + ASSERT (cur_score <= n_col) ; + ASSERT (cur_score >= EMPTY) ; + if (prev_col == EMPTY) + { + head [cur_score] = next_col ; + } + else + { + Col [prev_col].shared4.degree_next = next_col ; + } + if (next_col != EMPTY) + { + Col [next_col].shared3.prev = prev_col ; + } + + /* === Scan the column ========================================== */ + + cp = &A [Col [col].start] ; + cp_end = cp + Col [col].length ; + while (cp < cp_end) + { + /* get a row */ + row = *cp++ ; + row_mark = Row [row].shared2.mark ; + /* skip if dead */ + if (ROW_IS_MARKED_DEAD (row_mark)) + { + continue ; + } + ASSERT (row != pivot_row) ; + set_difference = row_mark - tag_mark ; + /* check if the row has been seen yet */ + if (set_difference < 0) + { + ASSERT (Row [row].shared1.degree <= max_deg) ; + set_difference = Row [row].shared1.degree ; + } + /* subtract column thickness from this row's set difference */ + set_difference -= col_thickness ; + ASSERT (set_difference >= 0) ; + /* absorb this row if the set difference becomes zero */ + if (set_difference == 0 && aggressive) + { + DEBUG3 (("aggressive absorption. Row: %d\n", row)) ; + KILL_ROW (row) ; + } + else + { + /* save the new mark */ + Row [row].shared2.mark = set_difference + tag_mark ; + } + } + } + +#ifndef NDEBUG + debug_deg_lists (n_row, n_col, Row, Col, head, + min_score, n_col2-k-pivot_row_degree, max_deg) ; +#endif /* NDEBUG */ + + /* === Add up set differences for each column ======================= */ + + DEBUG3 (("** Adding set differences phase. **\n")) ; + + /* for each column in pivot row */ + rp = &A [pivot_row_start] ; + rp_end = rp + pivot_row_length ; + while (rp < rp_end) + { + /* get a column */ + col = *rp++ ; + ASSERT (COL_IS_ALIVE (col) && col != pivot_col) ; + hash = 0 ; + cur_score = 0 ; + cp = &A [Col [col].start] ; + /* compact the column */ + new_cp = cp ; + cp_end = cp + Col [col].length ; + + DEBUG4 (("Adding set diffs for Col: %d.\n", col)) ; + + while (cp < cp_end) + { + /* get a row */ + row = *cp++ ; + ASSERT(row >= 0 && row < n_row) ; + row_mark = Row [row].shared2.mark ; + /* skip if dead */ + if (ROW_IS_MARKED_DEAD (row_mark)) + { + DEBUG4 ((" Row %d, dead\n", row)) ; + continue ; + } + DEBUG4 ((" Row %d, set diff %d\n", row, row_mark-tag_mark)); + ASSERT (row_mark >= tag_mark) ; + /* compact the column */ + *new_cp++ = row ; + /* compute hash function */ + hash += row ; + /* add set difference */ + cur_score += row_mark - tag_mark ; + /* integer overflow... */ + cur_score = MIN (cur_score, n_col) ; + } + + /* recompute the column's length */ + Col [col].length = (Int) (new_cp - &A [Col [col].start]) ; + + /* === Further mass elimination ================================= */ + + if (Col [col].length == 0) + { + DEBUG4 (("further mass elimination. Col: %d\n", col)) ; + /* nothing left but the pivot row in this column */ + KILL_PRINCIPAL_COL (col) ; + pivot_row_degree -= Col [col].shared1.thickness ; + ASSERT (pivot_row_degree >= 0) ; + /* order it */ + Col [col].shared2.order = k ; + /* increment order count by column thickness */ + k += Col [col].shared1.thickness ; + } + else + { + /* === Prepare for supercolumn detection ==================== */ + + DEBUG4 (("Preparing supercol detection for Col: %d.\n", col)) ; + + /* save score so far */ + Col [col].shared2.score = cur_score ; + + /* add column to hash table, for supercolumn detection */ + hash %= n_col + 1 ; + + DEBUG4 ((" Hash = %d, n_col = %d.\n", hash, n_col)) ; + ASSERT (((Int) hash) <= n_col) ; + + head_column = head [hash] ; + if (head_column > EMPTY) + { + /* degree list "hash" is non-empty, use prev (shared3) of */ + /* first column in degree list as head of hash bucket */ + first_col = Col [head_column].shared3.headhash ; + Col [head_column].shared3.headhash = col ; + } + else + { + /* degree list "hash" is empty, use head as hash bucket */ + first_col = - (head_column + 2) ; + head [hash] = - (col + 2) ; + } + Col [col].shared4.hash_next = first_col ; + + /* save hash function in Col [col].shared3.hash */ + Col [col].shared3.hash = (Int) hash ; + ASSERT (COL_IS_ALIVE (col)) ; + } + } + + /* The approximate external column degree is now computed. */ + + /* === Supercolumn detection ======================================== */ + + DEBUG3 (("** Supercolumn detection phase. **\n")) ; + + detect_super_cols ( + +#ifndef NDEBUG + n_col, Row, +#endif /* NDEBUG */ + + Col, A, head, pivot_row_start, pivot_row_length) ; + + /* === Kill the pivotal column ====================================== */ + + KILL_PRINCIPAL_COL (pivot_col) ; + + /* === Clear mark =================================================== */ + + tag_mark = clear_mark (tag_mark+max_deg+1, max_mark, n_row, Row) ; + +#ifndef NDEBUG + DEBUG3 (("check3\n")) ; + debug_mark (n_row, Row, tag_mark, max_mark) ; +#endif /* NDEBUG */ + + /* === Finalize the new pivot row, and column scores ================ */ + + DEBUG3 (("** Finalize scores phase. **\n")) ; + + /* for each column in pivot row */ + rp = &A [pivot_row_start] ; + /* compact the pivot row */ + new_rp = rp ; + rp_end = rp + pivot_row_length ; + while (rp < rp_end) + { + col = *rp++ ; + /* skip dead columns */ + if (COL_IS_DEAD (col)) + { + continue ; + } + *new_rp++ = col ; + /* add new pivot row to column */ + A [Col [col].start + (Col [col].length++)] = pivot_row ; + + /* retrieve score so far and add on pivot row's degree. */ + /* (we wait until here for this in case the pivot */ + /* row's degree was reduced due to mass elimination). */ + cur_score = Col [col].shared2.score + pivot_row_degree ; + + /* calculate the max possible score as the number of */ + /* external columns minus the 'k' value minus the */ + /* columns thickness */ + max_score = n_col - k - Col [col].shared1.thickness ; + + /* make the score the external degree of the union-of-rows */ + cur_score -= Col [col].shared1.thickness ; + + /* make sure score is less or equal than the max score */ + cur_score = MIN (cur_score, max_score) ; + ASSERT (cur_score >= 0) ; + + /* store updated score */ + Col [col].shared2.score = cur_score ; + + /* === Place column back in degree list ========================= */ + + ASSERT (min_score >= 0) ; + ASSERT (min_score <= n_col) ; + ASSERT (cur_score >= 0) ; + ASSERT (cur_score <= n_col) ; + ASSERT (head [cur_score] >= EMPTY) ; + next_col = head [cur_score] ; + Col [col].shared4.degree_next = next_col ; + Col [col].shared3.prev = EMPTY ; + if (next_col != EMPTY) + { + Col [next_col].shared3.prev = col ; + } + head [cur_score] = col ; + + /* see if this score is less than current min */ + min_score = MIN (min_score, cur_score) ; + + } + +#ifndef NDEBUG + debug_deg_lists (n_row, n_col, Row, Col, head, + min_score, n_col2-k, max_deg) ; +#endif /* NDEBUG */ + + /* === Resurrect the new pivot row ================================== */ + + if (pivot_row_degree > 0) + { + /* update pivot row length to reflect any cols that were killed */ + /* during super-col detection and mass elimination */ + Row [pivot_row].start = pivot_row_start ; + Row [pivot_row].length = (Int) (new_rp - &A[pivot_row_start]) ; + ASSERT (Row [pivot_row].length > 0) ; + Row [pivot_row].shared1.degree = pivot_row_degree ; + Row [pivot_row].shared2.mark = 0 ; + /* pivot row is no longer dead */ + + DEBUG1 (("Resurrect Pivot_row %d deg: %d\n", + pivot_row, pivot_row_degree)) ; + } + } + + /* === All principal columns have now been ordered ====================== */ + + return (ngarbage) ; +} + + +/* ========================================================================== */ +/* === order_children ======================================================= */ +/* ========================================================================== */ + +/* + The find_ordering routine has ordered all of the principal columns (the + representatives of the supercolumns). The non-principal columns have not + yet been ordered. This routine orders those columns by walking up the + parent tree (a column is a child of the column which absorbed it). The + final permutation vector is then placed in p [0 ... n_col-1], with p [0] + being the first column, and p [n_col-1] being the last. It doesn't look + like it at first glance, but be assured that this routine takes time linear + in the number of columns. Although not immediately obvious, the time + taken by this routine is O (n_col), that is, linear in the number of + columns. Not user-callable. +*/ + +PRIVATE void order_children +( + /* === Parameters ======================================================= */ + + Int n_col, /* number of columns of A */ + Colamd_Col Col [], /* of size n_col+1 */ + Int p [] /* p [0 ... n_col-1] is the column permutation*/ +) +{ + /* === Local variables ================================================== */ + + Int i ; /* loop counter for all columns */ + Int c ; /* column index */ + Int parent ; /* index of column's parent */ + Int order ; /* column's order */ + + /* === Order each non-principal column ================================== */ + + for (i = 0 ; i < n_col ; i++) + { + /* find an un-ordered non-principal column */ + ASSERT (COL_IS_DEAD (i)) ; + if (!COL_IS_DEAD_PRINCIPAL (i) && Col [i].shared2.order == EMPTY) + { + parent = i ; + /* once found, find its principal parent */ + do + { + parent = Col [parent].shared1.parent ; + } while (!COL_IS_DEAD_PRINCIPAL (parent)) ; + + /* now, order all un-ordered non-principal columns along path */ + /* to this parent. collapse tree at the same time */ + c = i ; + /* get order of parent */ + order = Col [parent].shared2.order ; + + do + { + ASSERT (Col [c].shared2.order == EMPTY) ; + + /* order this column */ + Col [c].shared2.order = order++ ; + /* collaps tree */ + Col [c].shared1.parent = parent ; + + /* get immediate parent of this column */ + c = Col [c].shared1.parent ; + + /* continue until we hit an ordered column. There are */ + /* guarranteed not to be anymore unordered columns */ + /* above an ordered column */ + } while (Col [c].shared2.order == EMPTY) ; + + /* re-order the super_col parent to largest order for this group */ + Col [parent].shared2.order = order ; + } + } + + /* === Generate the permutation ========================================= */ + + for (c = 0 ; c < n_col ; c++) + { + p [Col [c].shared2.order] = c ; + } +} + + +/* ========================================================================== */ +/* === detect_super_cols ==================================================== */ +/* ========================================================================== */ + +/* + Detects supercolumns by finding matches between columns in the hash buckets. + Check amongst columns in the set A [row_start ... row_start + row_length-1]. + The columns under consideration are currently *not* in the degree lists, + and have already been placed in the hash buckets. + + The hash bucket for columns whose hash function is equal to h is stored + as follows: + + if head [h] is >= 0, then head [h] contains a degree list, so: + + head [h] is the first column in degree bucket h. + Col [head [h]].headhash gives the first column in hash bucket h. + + otherwise, the degree list is empty, and: + + -(head [h] + 2) is the first column in hash bucket h. + + For a column c in a hash bucket, Col [c].shared3.prev is NOT a "previous + column" pointer. Col [c].shared3.hash is used instead as the hash number + for that column. The value of Col [c].shared4.hash_next is the next column + in the same hash bucket. + + Assuming no, or "few" hash collisions, the time taken by this routine is + linear in the sum of the sizes (lengths) of each column whose score has + just been computed in the approximate degree computation. + Not user-callable. +*/ + +PRIVATE void detect_super_cols +( + /* === Parameters ======================================================= */ + +#ifndef NDEBUG + /* these two parameters are only needed when debugging is enabled: */ + Int n_col, /* number of columns of A */ + Colamd_Row Row [], /* of size n_row+1 */ +#endif /* NDEBUG */ + + Colamd_Col Col [], /* of size n_col+1 */ + Int A [], /* row indices of A */ + Int head [], /* head of degree lists and hash buckets */ + Int row_start, /* pointer to set of columns to check */ + Int row_length /* number of columns to check */ +) +{ + /* === Local variables ================================================== */ + + Int hash ; /* hash value for a column */ + Int *rp ; /* pointer to a row */ + Int c ; /* a column index */ + Int super_c ; /* column index of the column to absorb into */ + Int *cp1 ; /* column pointer for column super_c */ + Int *cp2 ; /* column pointer for column c */ + Int length ; /* length of column super_c */ + Int prev_c ; /* column preceding c in hash bucket */ + Int i ; /* loop counter */ + Int *rp_end ; /* pointer to the end of the row */ + Int col ; /* a column index in the row to check */ + Int head_column ; /* first column in hash bucket or degree list */ + Int first_col ; /* first column in hash bucket */ + + /* === Consider each column in the row ================================== */ + + rp = &A [row_start] ; + rp_end = rp + row_length ; + while (rp < rp_end) + { + col = *rp++ ; + if (COL_IS_DEAD (col)) + { + continue ; + } + + /* get hash number for this column */ + hash = Col [col].shared3.hash ; + ASSERT (hash <= n_col) ; + + /* === Get the first column in this hash bucket ===================== */ + + head_column = head [hash] ; + if (head_column > EMPTY) + { + first_col = Col [head_column].shared3.headhash ; + } + else + { + first_col = - (head_column + 2) ; + } + + /* === Consider each column in the hash bucket ====================== */ + + for (super_c = first_col ; super_c != EMPTY ; + super_c = Col [super_c].shared4.hash_next) + { + ASSERT (COL_IS_ALIVE (super_c)) ; + ASSERT (Col [super_c].shared3.hash == hash) ; + length = Col [super_c].length ; + + /* prev_c is the column preceding column c in the hash bucket */ + prev_c = super_c ; + + /* === Compare super_c with all columns after it ================ */ + + for (c = Col [super_c].shared4.hash_next ; + c != EMPTY ; c = Col [c].shared4.hash_next) + { + ASSERT (c != super_c) ; + ASSERT (COL_IS_ALIVE (c)) ; + ASSERT (Col [c].shared3.hash == hash) ; + + /* not identical if lengths or scores are different */ + if (Col [c].length != length || + Col [c].shared2.score != Col [super_c].shared2.score) + { + prev_c = c ; + continue ; + } + + /* compare the two columns */ + cp1 = &A [Col [super_c].start] ; + cp2 = &A [Col [c].start] ; + + for (i = 0 ; i < length ; i++) + { + /* the columns are "clean" (no dead rows) */ + ASSERT (ROW_IS_ALIVE (*cp1)) ; + ASSERT (ROW_IS_ALIVE (*cp2)) ; + /* row indices will same order for both supercols, */ + /* no gather scatter nessasary */ + if (*cp1++ != *cp2++) + { + break ; + } + } + + /* the two columns are different if the for-loop "broke" */ + if (i != length) + { + prev_c = c ; + continue ; + } + + /* === Got it! two columns are identical =================== */ + + ASSERT (Col [c].shared2.score == Col [super_c].shared2.score) ; + + Col [super_c].shared1.thickness += Col [c].shared1.thickness ; + Col [c].shared1.parent = super_c ; + KILL_NON_PRINCIPAL_COL (c) ; + /* order c later, in order_children() */ + Col [c].shared2.order = EMPTY ; + /* remove c from hash bucket */ + Col [prev_c].shared4.hash_next = Col [c].shared4.hash_next ; + } + } + + /* === Empty this hash bucket ======================================= */ + + if (head_column > EMPTY) + { + /* corresponding degree list "hash" is not empty */ + Col [head_column].shared3.headhash = EMPTY ; + } + else + { + /* corresponding degree list "hash" is empty */ + head [hash] = EMPTY ; + } + } +} + + +/* ========================================================================== */ +/* === garbage_collection =================================================== */ +/* ========================================================================== */ + +/* + Defragments and compacts columns and rows in the workspace A. Used when + all avaliable memory has been used while performing row merging. Returns + the index of the first free position in A, after garbage collection. The + time taken by this routine is linear is the size of the array A, which is + itself linear in the number of nonzeros in the input matrix. + Not user-callable. +*/ + +PRIVATE Int garbage_collection /* returns the new value of pfree */ +( + /* === Parameters ======================================================= */ + + Int n_row, /* number of rows */ + Int n_col, /* number of columns */ + Colamd_Row Row [], /* row info */ + Colamd_Col Col [], /* column info */ + Int A [], /* A [0 ... Alen-1] holds the matrix */ + Int *pfree /* &A [0] ... pfree is in use */ +) +{ + /* === Local variables ================================================== */ + + Int *psrc ; /* source pointer */ + Int *pdest ; /* destination pointer */ + Int j ; /* counter */ + Int r ; /* a row index */ + Int c ; /* a column index */ + Int length ; /* length of a row or column */ + +#ifndef NDEBUG + Int debug_rows ; + DEBUG2 (("Defrag..\n")) ; + for (psrc = &A[0] ; psrc < pfree ; psrc++) ASSERT (*psrc >= 0) ; + debug_rows = 0 ; +#endif /* NDEBUG */ + + /* === Defragment the columns =========================================== */ + + pdest = &A[0] ; + for (c = 0 ; c < n_col ; c++) + { + if (COL_IS_ALIVE (c)) + { + psrc = &A [Col [c].start] ; + + /* move and compact the column */ + ASSERT (pdest <= psrc) ; + Col [c].start = (Int) (pdest - &A [0]) ; + length = Col [c].length ; + for (j = 0 ; j < length ; j++) + { + r = *psrc++ ; + if (ROW_IS_ALIVE (r)) + { + *pdest++ = r ; + } + } + Col [c].length = (Int) (pdest - &A [Col [c].start]) ; + } + } + + /* === Prepare to defragment the rows =================================== */ + + for (r = 0 ; r < n_row ; r++) + { + if (ROW_IS_DEAD (r) || (Row [r].length == 0)) + { + /* This row is already dead, or is of zero length. Cannot compact + * a row of zero length, so kill it. NOTE: in the current version, + * there are no zero-length live rows. Kill the row (for the first + * time, or again) just to be safe. */ + KILL_ROW (r) ; + } + else + { + /* save first column index in Row [r].shared2.first_column */ + psrc = &A [Row [r].start] ; + Row [r].shared2.first_column = *psrc ; + ASSERT (ROW_IS_ALIVE (r)) ; + /* flag the start of the row with the one's complement of row */ + *psrc = ONES_COMPLEMENT (r) ; +#ifndef NDEBUG + debug_rows++ ; +#endif /* NDEBUG */ + } + } + + /* === Defragment the rows ============================================== */ + + psrc = pdest ; + while (psrc < pfree) + { + /* find a negative number ... the start of a row */ + if (*psrc++ < 0) + { + psrc-- ; + /* get the row index */ + r = ONES_COMPLEMENT (*psrc) ; + ASSERT (r >= 0 && r < n_row) ; + /* restore first column index */ + *psrc = Row [r].shared2.first_column ; + ASSERT (ROW_IS_ALIVE (r)) ; + ASSERT (Row [r].length > 0) ; + /* move and compact the row */ + ASSERT (pdest <= psrc) ; + Row [r].start = (Int) (pdest - &A [0]) ; + length = Row [r].length ; + for (j = 0 ; j < length ; j++) + { + c = *psrc++ ; + if (COL_IS_ALIVE (c)) + { + *pdest++ = c ; + } + } + Row [r].length = (Int) (pdest - &A [Row [r].start]) ; + ASSERT (Row [r].length > 0) ; +#ifndef NDEBUG + debug_rows-- ; +#endif /* NDEBUG */ + } + } + /* ensure we found all the rows */ + ASSERT (debug_rows == 0) ; + + /* === Return the new value of pfree ==================================== */ + + return ((Int) (pdest - &A [0])) ; +} + + +/* ========================================================================== */ +/* === clear_mark =========================================================== */ +/* ========================================================================== */ + +/* + Clears the Row [].shared2.mark array, and returns the new tag_mark. + Return value is the new tag_mark. Not user-callable. +*/ + +PRIVATE Int clear_mark /* return the new value for tag_mark */ +( + /* === Parameters ======================================================= */ + + Int tag_mark, /* new value of tag_mark */ + Int max_mark, /* max allowed value of tag_mark */ + + Int n_row, /* number of rows in A */ + Colamd_Row Row [] /* Row [0 ... n_row-1].shared2.mark is set to zero */ +) +{ + /* === Local variables ================================================== */ + + Int r ; + + if (tag_mark <= 0 || tag_mark >= max_mark) + { + for (r = 0 ; r < n_row ; r++) + { + if (ROW_IS_ALIVE (r)) + { + Row [r].shared2.mark = 0 ; + } + } + tag_mark = 1 ; + } + + return (tag_mark) ; +} + + +/* ========================================================================== */ +/* === print_report ========================================================= */ +/* ========================================================================== */ + +PRIVATE void print_report +( + char *method, + Int stats [COLAMD_STATS] +) +{ + + Int i1, i2, i3 ; + + PRINTF (("\n%s version %d.%d, %s: ", method, + COLAMD_MAIN_VERSION, COLAMD_SUB_VERSION, COLAMD_DATE)) ; + + if (!stats) + { + PRINTF (("No statistics available.\n")) ; + return ; + } + + i1 = stats [COLAMD_INFO1] ; + i2 = stats [COLAMD_INFO2] ; + i3 = stats [COLAMD_INFO3] ; + + if (stats [COLAMD_STATUS] >= 0) + { + PRINTF (("OK. ")) ; + } + else + { + PRINTF (("ERROR. ")) ; + } + + switch (stats [COLAMD_STATUS]) + { + + case COLAMD_OK_BUT_JUMBLED: + + PRINTF(("Matrix has unsorted or duplicate row indices.\n")) ; + + PRINTF(("%s: number of duplicate or out-of-order row indices: %d\n", + method, i3)) ; + + PRINTF(("%s: last seen duplicate or out-of-order row index: %d\n", + method, INDEX (i2))) ; + + PRINTF(("%s: last seen in column: %d", + method, INDEX (i1))) ; + + /* no break - fall through to next case instead */ + + case COLAMD_OK: + + PRINTF(("\n")) ; + + PRINTF(("%s: number of dense or empty rows ignored: %d\n", + method, stats [COLAMD_DENSE_ROW])) ; + + PRINTF(("%s: number of dense or empty columns ignored: %d\n", + method, stats [COLAMD_DENSE_COL])) ; + + PRINTF(("%s: number of garbage collections performed: %d\n", + method, stats [COLAMD_DEFRAG_COUNT])) ; + break ; + + case COLAMD_ERROR_A_not_present: + + PRINTF(("Array A (row indices of matrix) not present.\n")) ; + break ; + + case COLAMD_ERROR_p_not_present: + + PRINTF(("Array p (column pointers for matrix) not present.\n")) ; + break ; + + case COLAMD_ERROR_nrow_negative: + + PRINTF(("Invalid number of rows (%d).\n", i1)) ; + break ; + + case COLAMD_ERROR_ncol_negative: + + PRINTF(("Invalid number of columns (%d).\n", i1)) ; + break ; + + case COLAMD_ERROR_nnz_negative: + + PRINTF(("Invalid number of nonzero entries (%d).\n", i1)) ; + break ; + + case COLAMD_ERROR_p0_nonzero: + + PRINTF(("Invalid column pointer, p [0] = %d, must be zero.\n", i1)); + break ; + + case COLAMD_ERROR_A_too_small: + + PRINTF(("Array A too small.\n")) ; + PRINTF((" Need Alen >= %d, but given only Alen = %d.\n", + i1, i2)) ; + break ; + + case COLAMD_ERROR_col_length_negative: + + PRINTF + (("Column %d has a negative number of nonzero entries (%d).\n", + INDEX (i1), i2)) ; + break ; + + case COLAMD_ERROR_row_index_out_of_bounds: + + PRINTF + (("Row index (row %d) out of bounds (%d to %d) in column %d.\n", + INDEX (i2), INDEX (0), INDEX (i3-1), INDEX (i1))) ; + break ; + + case COLAMD_ERROR_out_of_memory: + + PRINTF(("Out of memory.\n")) ; + break ; + + /* v2.4: internal-error case deleted */ + } +} + + + + +/* ========================================================================== */ +/* === colamd debugging routines ============================================ */ +/* ========================================================================== */ + +/* When debugging is disabled, the remainder of this file is ignored. */ + +#ifndef NDEBUG + + +/* ========================================================================== */ +/* === debug_structures ===================================================== */ +/* ========================================================================== */ + +/* + At this point, all empty rows and columns are dead. All live columns + are "clean" (containing no dead rows) and simplicial (no supercolumns + yet). Rows may contain dead columns, but all live rows contain at + least one live column. +*/ + +PRIVATE void debug_structures +( + /* === Parameters ======================================================= */ + + Int n_row, + Int n_col, + Colamd_Row Row [], + Colamd_Col Col [], + Int A [], + Int n_col2 +) +{ + /* === Local variables ================================================== */ + + Int i ; + Int c ; + Int *cp ; + Int *cp_end ; + Int len ; + Int score ; + Int r ; + Int *rp ; + Int *rp_end ; + Int deg ; + + /* === Check A, Row, and Col ============================================ */ + + for (c = 0 ; c < n_col ; c++) + { + if (COL_IS_ALIVE (c)) + { + len = Col [c].length ; + score = Col [c].shared2.score ; + DEBUG4 (("initial live col %5d %5d %5d\n", c, len, score)) ; + ASSERT (len > 0) ; + ASSERT (score >= 0) ; + ASSERT (Col [c].shared1.thickness == 1) ; + cp = &A [Col [c].start] ; + cp_end = cp + len ; + while (cp < cp_end) + { + r = *cp++ ; + ASSERT (ROW_IS_ALIVE (r)) ; + } + } + else + { + i = Col [c].shared2.order ; + ASSERT (i >= n_col2 && i < n_col) ; + } + } + + for (r = 0 ; r < n_row ; r++) + { + if (ROW_IS_ALIVE (r)) + { + i = 0 ; + len = Row [r].length ; + deg = Row [r].shared1.degree ; + ASSERT (len > 0) ; + ASSERT (deg > 0) ; + rp = &A [Row [r].start] ; + rp_end = rp + len ; + while (rp < rp_end) + { + c = *rp++ ; + if (COL_IS_ALIVE (c)) + { + i++ ; + } + } + ASSERT (i > 0) ; + } + } +} + + +/* ========================================================================== */ +/* === debug_deg_lists ====================================================== */ +/* ========================================================================== */ + +/* + Prints the contents of the degree lists. Counts the number of columns + in the degree list and compares it to the total it should have. Also + checks the row degrees. +*/ + +PRIVATE void debug_deg_lists +( + /* === Parameters ======================================================= */ + + Int n_row, + Int n_col, + Colamd_Row Row [], + Colamd_Col Col [], + Int head [], + Int min_score, + Int should, + Int max_deg +) +{ + /* === Local variables ================================================== */ + + Int deg ; + Int col ; + Int have ; + Int row ; + + /* === Check the degree lists =========================================== */ + + if (n_col > 10000 && colamd_debug <= 0) + { + return ; + } + have = 0 ; + DEBUG4 (("Degree lists: %d\n", min_score)) ; + for (deg = 0 ; deg <= n_col ; deg++) + { + col = head [deg] ; + if (col == EMPTY) + { + continue ; + } + DEBUG4 (("%d:", deg)) ; + while (col != EMPTY) + { + DEBUG4 ((" %d", col)) ; + have += Col [col].shared1.thickness ; + ASSERT (COL_IS_ALIVE (col)) ; + col = Col [col].shared4.degree_next ; + } + DEBUG4 (("\n")) ; + } + DEBUG4 (("should %d have %d\n", should, have)) ; + ASSERT (should == have) ; + + /* === Check the row degrees ============================================ */ + + if (n_row > 10000 && colamd_debug <= 0) + { + return ; + } + for (row = 0 ; row < n_row ; row++) + { + if (ROW_IS_ALIVE (row)) + { + ASSERT (Row [row].shared1.degree <= max_deg) ; + } + } +} + + +/* ========================================================================== */ +/* === debug_mark =========================================================== */ +/* ========================================================================== */ + +/* + Ensures that the tag_mark is less that the maximum and also ensures that + each entry in the mark array is less than the tag mark. +*/ + +PRIVATE void debug_mark +( + /* === Parameters ======================================================= */ + + Int n_row, + Colamd_Row Row [], + Int tag_mark, + Int max_mark +) +{ + /* === Local variables ================================================== */ + + Int r ; + + /* === Check the Row marks ============================================== */ + + ASSERT (tag_mark > 0 && tag_mark <= max_mark) ; + if (n_row > 10000 && colamd_debug <= 0) + { + return ; + } + for (r = 0 ; r < n_row ; r++) + { + ASSERT (Row [r].shared2.mark < tag_mark) ; + } +} + + +/* ========================================================================== */ +/* === debug_matrix ========================================================= */ +/* ========================================================================== */ + +/* + Prints out the contents of the columns and the rows. +*/ + +PRIVATE void debug_matrix +( + /* === Parameters ======================================================= */ + + Int n_row, + Int n_col, + Colamd_Row Row [], + Colamd_Col Col [], + Int A [] +) +{ + /* === Local variables ================================================== */ + + Int r ; + Int c ; + Int *rp ; + Int *rp_end ; + Int *cp ; + Int *cp_end ; + + /* === Dump the rows and columns of the matrix ========================== */ + + if (colamd_debug < 3) + { + return ; + } + DEBUG3 (("DUMP MATRIX:\n")) ; + for (r = 0 ; r < n_row ; r++) + { + DEBUG3 (("Row %d alive? %d\n", r, ROW_IS_ALIVE (r))) ; + if (ROW_IS_DEAD (r)) + { + continue ; + } + DEBUG3 (("start %d length %d degree %d\n", + Row [r].start, Row [r].length, Row [r].shared1.degree)) ; + rp = &A [Row [r].start] ; + rp_end = rp + Row [r].length ; + while (rp < rp_end) + { + c = *rp++ ; + DEBUG4 ((" %d col %d\n", COL_IS_ALIVE (c), c)) ; + } + } + + for (c = 0 ; c < n_col ; c++) + { + DEBUG3 (("Col %d alive? %d\n", c, COL_IS_ALIVE (c))) ; + if (COL_IS_DEAD (c)) + { + continue ; + } + DEBUG3 (("start %d length %d shared1 %d shared2 %d\n", + Col [c].start, Col [c].length, + Col [c].shared1.thickness, Col [c].shared2.score)) ; + cp = &A [Col [c].start] ; + cp_end = cp + Col [c].length ; + while (cp < cp_end) + { + r = *cp++ ; + DEBUG4 ((" %d row %d\n", ROW_IS_ALIVE (r), r)) ; + } + } +} + +PRIVATE void colamd_get_debug +( + char *method +) +{ + FILE *f ; + colamd_debug = 0 ; /* no debug printing */ + f = fopen ("debug", "r") ; + if (f == (FILE *) NULL) + { + colamd_debug = 0 ; + } + else + { + fscanf (f, "%d", &colamd_debug) ; + fclose (f) ; + } + DEBUG0 (("%s: debug version, D = %d (THIS WILL BE SLOW!)\n", + method, colamd_debug)) ; +} + +#endif /* NDEBUG */ diff --git a/colamd/colamd.h b/colamd/colamd.h new file mode 100755 index 000000000..26372d8fa --- /dev/null +++ b/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 + +/* ========================================================================== */ +/* === 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 */ diff --git a/colamd/colamd_global.c b/colamd/colamd_global.c new file mode 100755 index 000000000..4d1ae2230 --- /dev/null +++ b/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 +int (*colamd_printf) (const char *, ...) = printf ; +#endif +#else +int (*colamd_printf) (const char *, ...) = ((void *) 0) ; +#endif + diff --git a/colamd/main.cpp b/colamd/main.cpp new file mode 100755 index 000000000..da728b446 --- /dev/null +++ b/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; +} diff --git a/configure.ac b/configure.ac new file mode 100644 index 000000000..8b9f1b91c --- /dev/null +++ b/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 serialization flag]) + + boost_serialization=$withval ]) +AC_SUBST([boost_serialization]) + +#AC_DEFINE([TEST_AC_DEFINE], [""], [testing of ac_define]) + + +AC_OUTPUT diff --git a/cpp/.cvsignore b/cpp/.cvsignore new file mode 100644 index 000000000..67155973c --- /dev/null +++ b/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 diff --git a/cpp/Cal3_S2.cpp b/cpp/Cal3_S2.cpp new file mode 100644 index 000000000..a3df39934 --- /dev/null +++ b/cpp/Cal3_S2.cpp @@ -0,0 +1,76 @@ +/** + * @file Cal3_S2.cpp + * @brief The most common 5DOF 3D->2D calibration + * @author Frank Dellaert + */ + +#include +#include + +#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 diff --git a/cpp/Cal3_S2.h b/cpp/Cal3_S2.h new file mode 100644 index 000000000..1d3be0804 --- /dev/null +++ b/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); +} diff --git a/cpp/CalibratedCamera.cpp b/cpp/CalibratedCamera.cpp new file mode 100644 index 000000000..ed9317307 --- /dev/null +++ b/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 + } + +/* ************************************************************************* */ +} diff --git a/cpp/CalibratedCamera.h b/cpp/CalibratedCamera.h new file mode 100644 index 000000000..cd48e7acf --- /dev/null +++ b/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_ */ diff --git a/cpp/ChordalBayesNet.cpp b/cpp/ChordalBayesNet.cpp new file mode 100644 index 000000000..4bd11fb8c --- /dev/null +++ b/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 +#include +#include +#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::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 ChordalBayesNet::optimize() +{ + boost::shared_ptr result(new FGConfig); + result = optimize(result); + return result; +} + +/* ************************************************************************* */ +boost::shared_ptr ChordalBayesNet::optimize(const boost::shared_ptr &c) +{ + boost::shared_ptr 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 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 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;iget_R(); + for (size_t i=0;iparentsBegin(); + 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 +#include +#include + +#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 shared_ptr; + +protected: + + /** nodes keys stored in topological sort order, i.e. from parents to children */ + std::list keys; + + /** nodes stored on key */ + std::map nodes; + + typedef std::map::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::const_iterator const_iterator; + const_iterator const begin() {return nodes.begin();} + const_iterator const end() {return nodes.end();} + + /** optimize */ + boost::shared_ptr optimize(); + boost::shared_ptr optimize(const boost::shared_ptr &c); + + /** print */ + void print() const; + + /** check equality */ + bool equals(const ChordalBayesNet& cbn) const; + + /** + * Return (dense) upper-triangular matrix representation + */ + std::pair matrix() const; + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(keys); + ar & BOOST_SERIALIZATION_NVP(nodes); + } + +}; + +} /// namespace gtsam diff --git a/cpp/ConditionalGaussian.cpp b/cpp/ConditionalGaussian.cpp new file mode 100644 index 000000000..fa7dc0b46 --- /dev/null +++ b/cpp/ConditionalGaussian.cpp @@ -0,0 +1,104 @@ +/** + * @file ConditionalGaussian.cpp + * @brief Conditional Gaussian Base class + * @author Christian Potthast + */ + + +#include +#include +#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::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::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::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::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; + +} + +/* ************************************************************************* */ diff --git a/cpp/ConditionalGaussian.h b/cpp/ConditionalGaussian.h new file mode 100644 index 000000000..83c68aea4 --- /dev/null +++ b/cpp/ConditionalGaussian.h @@ -0,0 +1,130 @@ +/** + * @file ConditionalGaussian.h + * @brief Conditional Gaussian Base class + * @author Christian Potthast + */ + + +// \callgraph + +#pragma once + +#include +#include +#include +#include +#include +#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::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 parents_; + + /** the RHS vector */ + Vector d_; + + public: + typedef boost::shared_ptr 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 + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(R_); + ar & BOOST_SERIALIZATION_NVP(d_); + ar & BOOST_SERIALIZATION_NVP(parents_); + } + }; +} diff --git a/cpp/ConstrainedChordalBayesNet.cpp b/cpp/ConstrainedChordalBayesNet.cpp new file mode 100644 index 000000000..32c52154d --- /dev/null +++ b/cpp/ConstrainedChordalBayesNet.cpp @@ -0,0 +1,134 @@ +/* + * ConstrainedChordalBayesNet.cpp + * + * Created on: Aug 11, 2009 + * Author: alexgc + */ + +#include +#include +#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 p1; + BOOST_FOREACH(p1, delta_nodes) + { + cout << " " << p1.first << endl; + p1.second->print(); + } + pair p2; + BOOST_FOREACH(p2, nodes) + { + cout << " " << p2.first << endl; + p2.second->print(); + } +} + +boost::shared_ptr ConstrainedChordalBayesNet::optimize() +{ + boost::shared_ptr empty(new FGConfig); + return optimize(empty); +} + +boost::shared_ptr ConstrainedChordalBayesNet::optimize(const boost::shared_ptr &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 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 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 p; + BOOST_FOREACH(p, nodes) + { + ret.insert(p.first, p.second); + } + return ret; +} + +} diff --git a/cpp/ConstrainedChordalBayesNet.h b/cpp/ConstrainedChordalBayesNet.h new file mode 100644 index 000000000..4deffe83a --- /dev/null +++ b/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 delta_nodes; + +public: + typedef std::map::const_iterator const_delta_iterator; + typedef std::map::iterator delta_iterator; + +public: + typedef boost::shared_ptr 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 optimize(); + boost::shared_ptr optimize(const boost::shared_ptr &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_ */ diff --git a/cpp/ConstrainedLinearFactorGraph.cpp b/cpp/ConstrainedLinearFactorGraph.cpp new file mode 100644 index 000000000..d29b89a06 --- /dev/null +++ b/cpp/ConstrainedLinearFactorGraph.cpp @@ -0,0 +1,220 @@ +/* + * ConstrainedLinearFactorGraph.cpp + * + * Created on: Aug 10, 2009 + * Author: alexgc + */ + +#include +#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 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 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 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 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 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; +} + + + +} diff --git a/cpp/ConstrainedLinearFactorGraph.h b/cpp/ConstrainedLinearFactorGraph.h new file mode 100644 index 000000000..e218a4a28 --- /dev/null +++ b/cpp/ConstrainedLinearFactorGraph.h @@ -0,0 +1,132 @@ +/* + * ConstrainedLinearFactorGraph.h + * + * Created on: Aug 10, 2009 + * Author: alexgc + */ + +#ifndef CONSTRAINEDLINEARFACTORGRAPH_H_ +#define CONSTRAINEDLINEARFACTORGRAPH_H_ + +#include +#include + +#include "LinearFactorGraph.h" +#include "EqualityFactor.h" +#include "ConstrainedChordalBayesNet.h" + +namespace gtsam { + +class ConstrainedLinearFactorGraph: public LinearFactorGraph { + +protected: + std::vector eq_factors; /// collection of equality factors + +public: + // iterators for equality constraints - same interface as linear factors + typedef std::vector::const_iterator eq_const_iterator; + typedef std::vector::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_ */ diff --git a/cpp/ConstrainedNonlinearFactorGraph.cpp b/cpp/ConstrainedNonlinearFactorGraph.cpp new file mode 100644 index 000000000..49e30d98e --- /dev/null +++ b/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(); factorlinearize(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_factorlinearize(); + ret.push_back_eq(eq); + } + + return ret; +} + +NonlinearFactorGraph ConstrainedNonlinearFactorGraph::convert() const +{ + NonlinearFactorGraph ret; + BOOST_FOREACH(boost::shared_ptr f, factors) + { + ret.push_back(f); + } + return ret; +} + +} diff --git a/cpp/ConstrainedNonlinearFactorGraph.h b/cpp/ConstrainedNonlinearFactorGraph.h new file mode 100644 index 000000000..d907c5b0a --- /dev/null +++ b/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 eq_factors; + +public: + // iterators over equality factors + typedef std::vector::const_iterator eq_const_iterator; + typedef std::vector::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_ */ diff --git a/cpp/DeltaFunction.cpp b/cpp/DeltaFunction.cpp new file mode 100644 index 000000000..8f5d07341 --- /dev/null +++ b/cpp/DeltaFunction.cpp @@ -0,0 +1,60 @@ +/* + * DeltaFunction.cpp + * + * Created on: Aug 11, 2009 + * Author: alexgc + */ + +#include +#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; +} + +} diff --git a/cpp/DeltaFunction.h b/cpp/DeltaFunction.h new file mode 100644 index 000000000..6e5d25588 --- /dev/null +++ b/cpp/DeltaFunction.h @@ -0,0 +1,63 @@ +/* + * DeltaFunction.h + * + * Created on: Aug 11, 2009 + * Author: alexgc + */ + +#ifndef DELTAFUNCTION_H_ +#define DELTAFUNCTION_H_ + +#include +#include +#include +#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 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_ */ diff --git a/cpp/Doxyfile b/cpp/Doxyfile new file mode 100644 index 000000000..c1d20b1f8 --- /dev/null +++ b/cpp/Doxyfile @@ -0,0 +1,1417 @@ +# Doxyfile 1.5.6 + +# This file describes the settings to be used by the documentation system +# doxygen (www.doxygen.org) for a project +# +# All text after a hash (#) is considered a comment and will be ignored +# The format is: +# TAG = value [value, ...] +# For lists items can also be appended using: +# TAG += value [value, ...] +# Values that contain spaces should be placed between quotes (" ") + +#--------------------------------------------------------------------------- +# Project related configuration options +#--------------------------------------------------------------------------- + +# This tag specifies the encoding used for all characters in the config file +# that follow. The default is UTF-8 which is also the encoding used for all +# text before the first occurrence of this tag. Doxygen uses libiconv (or the +# iconv built into libc) for the transcoding. See +# http://www.gnu.org/software/libiconv for the list of possible encodings. + +DOXYFILE_ENCODING = UTF-8 + +# The PROJECT_NAME tag is a single word (or a sequence of words surrounded +# by quotes) that should identify the project. + +PROJECT_NAME = gtsam + +# The PROJECT_NUMBER tag can be used to enter a project or revision number. +# This could be handy for archiving the generated documentation or +# if some version control system is used. + +PROJECT_NUMBER = + +# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) +# base path where the generated documentation will be put. +# If a relative path is entered, it will be relative to the location +# where doxygen was started. If left blank the current directory will be used. + +OUTPUT_DIRECTORY = + +# If the CREATE_SUBDIRS tag is set to YES, then doxygen will create +# 4096 sub-directories (in 2 levels) under the output directory of each output +# format and will distribute the generated files over these directories. +# Enabling this option can be useful when feeding doxygen a huge amount of +# source files, where putting all generated files in the same directory would +# otherwise cause performance problems for the file system. + +CREATE_SUBDIRS = NO + +# The OUTPUT_LANGUAGE tag is used to specify the language in which all +# documentation generated by doxygen is written. Doxygen will use this +# information to generate all constant output in the proper language. +# The default language is English, other supported languages are: +# Afrikaans, Arabic, Brazilian, Catalan, Chinese, Chinese-Traditional, +# Croatian, Czech, Danish, Dutch, Farsi, Finnish, French, German, Greek, +# Hungarian, Italian, Japanese, Japanese-en (Japanese with English messages), +# Korean, Korean-en, Lithuanian, Norwegian, Macedonian, Persian, Polish, +# Portuguese, Romanian, Russian, Serbian, Slovak, Slovene, Spanish, Swedish, +# and Ukrainian. + +OUTPUT_LANGUAGE = English + +# If the BRIEF_MEMBER_DESC tag is set to YES (the default) Doxygen will +# include brief member descriptions after the members that are listed in +# the file and class documentation (similar to JavaDoc). +# Set to NO to disable this. + +BRIEF_MEMBER_DESC = YES + +# If the REPEAT_BRIEF tag is set to YES (the default) Doxygen will prepend +# the brief description of a member or function before the detailed description. +# Note: if both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the +# brief descriptions will be completely suppressed. + +REPEAT_BRIEF = YES + +# This tag implements a quasi-intelligent brief description abbreviator +# that is used to form the text in various listings. Each string +# in this list, if found as the leading text of the brief description, will be +# stripped from the text and the result after processing the whole list, is +# used as the annotated text. Otherwise, the brief description is used as-is. +# If left blank, the following values are used ("$name" is automatically +# replaced with the name of the entity): "The $name class" "The $name widget" +# "The $name file" "is" "provides" "specifies" "contains" +# "represents" "a" "an" "the" + +ABBREVIATE_BRIEF = + +# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then +# Doxygen will generate a detailed section even if there is only a brief +# description. + +ALWAYS_DETAILED_SEC = NO + +# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all +# inherited members of a class in the documentation of that class as if those +# members were ordinary class members. Constructors, destructors and assignment +# operators of the base classes will not be shown. + +INLINE_INHERITED_MEMB = NO + +# If the FULL_PATH_NAMES tag is set to YES then Doxygen will prepend the full +# path before files name in the file list and in the header files. If set +# to NO the shortest path that makes the file name unique will be used. + +FULL_PATH_NAMES = YES + +# If the FULL_PATH_NAMES tag is set to YES then the STRIP_FROM_PATH tag +# can be used to strip a user-defined part of the path. Stripping is +# only done if one of the specified strings matches the left-hand part of +# the path. The tag can be used to show relative paths in the file list. +# If left blank the directory from which doxygen is run is used as the +# path to strip. + +STRIP_FROM_PATH = + +# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of +# the path mentioned in the documentation of a class, which tells +# the reader which header file to include in order to use a class. +# If left blank only the name of the header file containing the class +# definition is used. Otherwise one should specify the include paths that +# are normally passed to the compiler using the -I flag. + +STRIP_FROM_INC_PATH = + +# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter +# (but less readable) file names. This can be useful is your file systems +# doesn't support long names like on DOS, Mac, or CD-ROM. + +SHORT_NAMES = NO + +# If the JAVADOC_AUTOBRIEF tag is set to YES then Doxygen +# will interpret the first line (until the first dot) of a JavaDoc-style +# comment as the brief description. If set to NO, the JavaDoc +# comments will behave just like regular Qt-style comments +# (thus requiring an explicit @brief command for a brief description.) + +JAVADOC_AUTOBRIEF = NO + +# If the QT_AUTOBRIEF tag is set to YES then Doxygen will +# interpret the first line (until the first dot) of a Qt-style +# comment as the brief description. If set to NO, the comments +# will behave just like regular Qt-style comments (thus requiring +# an explicit \brief command for a brief description.) + +QT_AUTOBRIEF = NO + +# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make Doxygen +# treat a multi-line C++ special comment block (i.e. a block of //! or /// +# comments) as a brief description. This used to be the default behaviour. +# The new default is to treat a multi-line C++ comment block as a detailed +# description. Set this tag to YES if you prefer the old behaviour instead. + +MULTILINE_CPP_IS_BRIEF = NO + +# If the DETAILS_AT_TOP tag is set to YES then Doxygen +# will output the detailed description near the top, like JavaDoc. +# If set to NO, the detailed description appears after the member +# documentation. + +DETAILS_AT_TOP = NO + +# If the INHERIT_DOCS tag is set to YES (the default) then an undocumented +# member inherits the documentation from any documented member that it +# re-implements. + +INHERIT_DOCS = YES + +# If the SEPARATE_MEMBER_PAGES tag is set to YES, then doxygen will produce +# a new page for each member. If set to NO, the documentation of a member will +# be part of the file/class/namespace that contains it. + +SEPARATE_MEMBER_PAGES = NO + +# The TAB_SIZE tag can be used to set the number of spaces in a tab. +# Doxygen uses this value to replace tabs by spaces in code fragments. + +TAB_SIZE = 8 + +# This tag can be used to specify a number of aliases that acts +# as commands in the documentation. An alias has the form "name=value". +# For example adding "sideeffect=\par Side Effects:\n" will allow you to +# put the command \sideeffect (or @sideeffect) in the documentation, which +# will result in a user-defined paragraph with heading "Side Effects:". +# You can put \n's in the value part of an alias to insert newlines. + +ALIASES = + +# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C +# sources only. Doxygen will then generate output that is more tailored for C. +# For instance, some of the names that are used will be different. The list +# of all members will be omitted, etc. + +OPTIMIZE_OUTPUT_FOR_C = NO + +# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java +# sources only. Doxygen will then generate output that is more tailored for +# Java. For instance, namespaces will be presented as packages, qualified +# scopes will look different, etc. + +OPTIMIZE_OUTPUT_JAVA = NO + +# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran +# sources only. Doxygen will then generate output that is more tailored for +# Fortran. + +OPTIMIZE_FOR_FORTRAN = NO + +# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL +# sources. Doxygen will then generate output that is tailored for +# VHDL. + +OPTIMIZE_OUTPUT_VHDL = NO + +# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want +# to include (a tag file for) the STL sources as input, then you should +# set this tag to YES in order to let doxygen match functions declarations and +# definitions whose arguments contain STL classes (e.g. func(std::string); v.s. +# func(std::string) {}). This also make the inheritance and collaboration +# diagrams that involve STL classes more complete and accurate. + +BUILTIN_STL_SUPPORT = NO + +# If you use Microsoft's C++/CLI language, you should set this option to YES to +# enable parsing support. + +CPP_CLI_SUPPORT = NO + +# Set the SIP_SUPPORT tag to YES if your project consists of sip sources only. +# Doxygen will parse them like normal C++ but will assume all classes use public +# instead of private inheritance when no explicit protection keyword is present. + +SIP_SUPPORT = NO + +# For Microsoft's IDL there are propget and propput attributes to indicate getter +# and setter methods for a property. Setting this option to YES (the default) +# will make doxygen to replace the get and set methods by a property in the +# documentation. This will only work if the methods are indeed getting or +# setting a simple type. If this is not the case, or you want to show the +# methods anyway, you should set this option to NO. + +IDL_PROPERTY_SUPPORT = YES + +# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC +# tag is set to YES, then doxygen will reuse the documentation of the first +# member in the group (if any) for the other members of the group. By default +# all members of a group must be documented explicitly. + +DISTRIBUTE_GROUP_DOC = NO + +# Set the SUBGROUPING tag to YES (the default) to allow class member groups of +# the same type (for instance a group of public functions) to be put as a +# subgroup of that type (e.g. under the Public Functions section). Set it to +# NO to prevent subgrouping. Alternatively, this can be done per class using +# the \nosubgrouping command. + +SUBGROUPING = YES + +# When TYPEDEF_HIDES_STRUCT is enabled, a typedef of a struct, union, or enum +# is documented as struct, union, or enum with the name of the typedef. So +# typedef struct TypeS {} TypeT, will appear in the documentation as a struct +# with name TypeT. When disabled the typedef will appear as a member of a file, +# namespace, or class. And the struct will be named TypeS. This can typically +# be useful for C code in case the coding convention dictates that all compound +# types are typedef'ed and only the typedef is referenced, never the tag name. + +TYPEDEF_HIDES_STRUCT = NO + +#--------------------------------------------------------------------------- +# Build related configuration options +#--------------------------------------------------------------------------- + +# If the EXTRACT_ALL tag is set to YES doxygen will assume all entities in +# documentation are documented, even if no documentation was available. +# Private class members and static file members will be hidden unless +# the EXTRACT_PRIVATE and EXTRACT_STATIC tags are set to YES + +EXTRACT_ALL = YES + +# If the EXTRACT_PRIVATE tag is set to YES all private members of a class +# will be included in the documentation. + +EXTRACT_PRIVATE = YES + +# If the EXTRACT_STATIC tag is set to YES all static members of a file +# will be included in the documentation. + +EXTRACT_STATIC = YES + +# If the EXTRACT_LOCAL_CLASSES tag is set to YES classes (and structs) +# defined locally in source files will be included in the documentation. +# If set to NO only classes defined in header files are included. + +EXTRACT_LOCAL_CLASSES = YES + +# This flag is only useful for Objective-C code. When set to YES local +# methods, which are defined in the implementation section but not in +# the interface are included in the documentation. +# If set to NO (the default) only methods in the interface are included. + +EXTRACT_LOCAL_METHODS = NO + +# If this flag is set to YES, the members of anonymous namespaces will be +# extracted and appear in the documentation as a namespace called +# 'anonymous_namespace{file}', where file will be replaced with the base +# name of the file that contains the anonymous namespace. By default +# anonymous namespace are hidden. + +EXTRACT_ANON_NSPACES = NO + +# If the HIDE_UNDOC_MEMBERS tag is set to YES, Doxygen will hide all +# undocumented members of documented classes, files or namespaces. +# If set to NO (the default) these members will be included in the +# various overviews, but no documentation section is generated. +# This option has no effect if EXTRACT_ALL is enabled. + +HIDE_UNDOC_MEMBERS = NO + +# If the HIDE_UNDOC_CLASSES tag is set to YES, Doxygen will hide all +# undocumented classes that are normally visible in the class hierarchy. +# If set to NO (the default) these classes will be included in the various +# overviews. This option has no effect if EXTRACT_ALL is enabled. + +HIDE_UNDOC_CLASSES = NO + +# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, Doxygen will hide all +# friend (class|struct|union) declarations. +# If set to NO (the default) these declarations will be included in the +# documentation. + +HIDE_FRIEND_COMPOUNDS = NO + +# If the HIDE_IN_BODY_DOCS tag is set to YES, Doxygen will hide any +# documentation blocks found inside the body of a function. +# If set to NO (the default) these blocks will be appended to the +# function's detailed documentation block. + +HIDE_IN_BODY_DOCS = NO + +# The INTERNAL_DOCS tag determines if documentation +# that is typed after a \internal command is included. If the tag is set +# to NO (the default) then the documentation will be excluded. +# Set it to YES to include the internal documentation. + +INTERNAL_DOCS = NO + +# If the CASE_SENSE_NAMES tag is set to NO then Doxygen will only generate +# file names in lower-case letters. If set to YES upper-case letters are also +# allowed. This is useful if you have classes or files whose names only differ +# in case and if your file system supports case sensitive file names. Windows +# and Mac users are advised to set this option to NO. + +CASE_SENSE_NAMES = YES + +# If the HIDE_SCOPE_NAMES tag is set to NO (the default) then Doxygen +# will show members with their full class and namespace scopes in the +# documentation. If set to YES the scope will be hidden. + +HIDE_SCOPE_NAMES = NO + +# If the SHOW_INCLUDE_FILES tag is set to YES (the default) then Doxygen +# will put a list of the files that are included by a file in the documentation +# of that file. + +SHOW_INCLUDE_FILES = YES + +# If the INLINE_INFO tag is set to YES (the default) then a tag [inline] +# is inserted in the documentation for inline members. + +INLINE_INFO = YES + +# If the SORT_MEMBER_DOCS tag is set to YES (the default) then doxygen +# will sort the (detailed) documentation of file and class members +# alphabetically by member name. If set to NO the members will appear in +# declaration order. + +SORT_MEMBER_DOCS = YES + +# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the +# brief documentation of file, namespace and class members alphabetically +# by member name. If set to NO (the default) the members will appear in +# declaration order. + +SORT_BRIEF_DOCS = NO + +# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the +# hierarchy of group names into alphabetical order. If set to NO (the default) +# the group names will appear in their defined order. + +SORT_GROUP_NAMES = NO + +# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be +# sorted by fully-qualified names, including namespaces. If set to +# NO (the default), the class list will be sorted only by class name, +# not including the namespace part. +# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES. +# Note: This option applies only to the class list, not to the +# alphabetical list. + +SORT_BY_SCOPE_NAME = NO + +# The GENERATE_TODOLIST tag can be used to enable (YES) or +# disable (NO) the todo list. This list is created by putting \todo +# commands in the documentation. + +GENERATE_TODOLIST = YES + +# The GENERATE_TESTLIST tag can be used to enable (YES) or +# disable (NO) the test list. This list is created by putting \test +# commands in the documentation. + +GENERATE_TESTLIST = YES + +# The GENERATE_BUGLIST tag can be used to enable (YES) or +# disable (NO) the bug list. This list is created by putting \bug +# commands in the documentation. + +GENERATE_BUGLIST = YES + +# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or +# disable (NO) the deprecated list. This list is created by putting +# \deprecated commands in the documentation. + +GENERATE_DEPRECATEDLIST = YES + +# The ENABLED_SECTIONS tag can be used to enable conditional +# documentation sections, marked by \if sectionname ... \endif. + +ENABLED_SECTIONS = + +# The MAX_INITIALIZER_LINES tag determines the maximum number of lines +# the initial value of a variable or define consists of for it to appear in +# the documentation. If the initializer consists of more lines than specified +# here it will be hidden. Use a value of 0 to hide initializers completely. +# The appearance of the initializer of individual variables and defines in the +# documentation can be controlled using \showinitializer or \hideinitializer +# command in the documentation regardless of this setting. + +MAX_INITIALIZER_LINES = 30 + +# Set the SHOW_USED_FILES tag to NO to disable the list of files generated +# at the bottom of the documentation of classes and structs. If set to YES the +# list will mention the files that were used to generate the documentation. + +SHOW_USED_FILES = YES + +# If the sources in your project are distributed over multiple directories +# then setting the SHOW_DIRECTORIES tag to YES will show the directory hierarchy +# in the documentation. The default is NO. + +SHOW_DIRECTORIES = NO + +# Set the SHOW_FILES tag to NO to disable the generation of the Files page. +# This will remove the Files entry from the Quick Index and from the +# Folder Tree View (if specified). The default is YES. + +SHOW_FILES = YES + +# Set the SHOW_NAMESPACES tag to NO to disable the generation of the +# Namespaces page. This will remove the Namespaces entry from the Quick Index +# and from the Folder Tree View (if specified). The default is YES. + +SHOW_NAMESPACES = YES + +# The FILE_VERSION_FILTER tag can be used to specify a program or script that +# doxygen should invoke to get the current version for each file (typically from +# the version control system). Doxygen will invoke the program by executing (via +# popen()) the command , where is the value of +# the FILE_VERSION_FILTER tag, and is the name of an input file +# provided by doxygen. Whatever the program writes to standard output +# is used as the file version. See the manual for examples. + +FILE_VERSION_FILTER = + +#--------------------------------------------------------------------------- +# configuration options related to warning and progress messages +#--------------------------------------------------------------------------- + +# The QUIET tag can be used to turn on/off the messages that are generated +# by doxygen. Possible values are YES and NO. If left blank NO is used. + +QUIET = NO + +# The WARNINGS tag can be used to turn on/off the warning messages that are +# generated by doxygen. Possible values are YES and NO. If left blank +# NO is used. + +WARNINGS = YES + +# If WARN_IF_UNDOCUMENTED is set to YES, then doxygen will generate warnings +# for undocumented members. If EXTRACT_ALL is set to YES then this flag will +# automatically be disabled. + +WARN_IF_UNDOCUMENTED = YES + +# If WARN_IF_DOC_ERROR is set to YES, doxygen will generate warnings for +# potential errors in the documentation, such as not documenting some +# parameters in a documented function, or documenting parameters that +# don't exist or using markup commands wrongly. + +WARN_IF_DOC_ERROR = YES + +# This WARN_NO_PARAMDOC option can be abled to get warnings for +# functions that are documented, but have no documentation for their parameters +# or return value. If set to NO (the default) doxygen will only warn about +# wrong or incomplete parameter documentation, but not about the absence of +# documentation. + +WARN_NO_PARAMDOC = NO + +# The WARN_FORMAT tag determines the format of the warning messages that +# doxygen can produce. The string should contain the $file, $line, and $text +# tags, which will be replaced by the file and line number from which the +# warning originated and the warning text. Optionally the format may contain +# $version, which will be replaced by the version of the file (if it could +# be obtained via FILE_VERSION_FILTER) + +WARN_FORMAT = "$file:$line: $text" + +# The WARN_LOGFILE tag can be used to specify a file to which warning +# and error messages should be written. If left blank the output is written +# to stderr. + +WARN_LOGFILE = + +#--------------------------------------------------------------------------- +# configuration options related to the input files +#--------------------------------------------------------------------------- + +# The INPUT tag can be used to specify the files and/or directories that contain +# documented source files. You may enter file names like "myfile.cpp" or +# directories like "/usr/src/myproject". Separate the files or directories +# with spaces. + +INPUT = + +# This tag can be used to specify the character encoding of the source files +# that doxygen parses. Internally doxygen uses the UTF-8 encoding, which is +# also the default input encoding. Doxygen uses libiconv (or the iconv built +# into libc) for the transcoding. See http://www.gnu.org/software/libiconv for +# the list of possible encodings. + +INPUT_ENCODING = UTF-8 + +# If the value of the INPUT tag contains directories, you can use the +# FILE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp +# and *.h) to filter out the source-files in the directories. If left +# blank the following patterns are tested: +# *.c *.cc *.cxx *.cpp *.c++ *.java *.ii *.ixx *.ipp *.i++ *.inl *.h *.hh *.hxx +# *.hpp *.h++ *.idl *.odl *.cs *.php *.php3 *.inc *.m *.mm *.py *.f90 + +FILE_PATTERNS = + +# The RECURSIVE tag can be used to turn specify whether or not subdirectories +# should be searched for input files as well. Possible values are YES and NO. +# If left blank NO is used. + +RECURSIVE = NO + +# The EXCLUDE tag can be used to specify files and/or directories that should +# excluded from the INPUT source files. This way you can easily exclude a +# subdirectory from a directory tree whose root is specified with the INPUT tag. + +EXCLUDE = + +# The EXCLUDE_SYMLINKS tag can be used select whether or not files or +# directories that are symbolic links (a Unix filesystem feature) are excluded +# from the input. + +EXCLUDE_SYMLINKS = NO + +# If the value of the INPUT tag contains directories, you can use the +# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude +# certain files from those directories. Note that the wildcards are matched +# against the file with absolute path, so to exclude all test directories +# for example use the pattern */test/* + +EXCLUDE_PATTERNS = + +# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names +# (namespaces, classes, functions, etc.) that should be excluded from the +# output. The symbol name can be a fully qualified name, a word, or if the +# wildcard * is used, a substring. Examples: ANamespace, AClass, +# AClass::ANamespace, ANamespace::*Test + +EXCLUDE_SYMBOLS = + +# The EXAMPLE_PATH tag can be used to specify one or more files or +# directories that contain example code fragments that are included (see +# the \include command). + +EXAMPLE_PATH = + +# If the value of the EXAMPLE_PATH tag contains directories, you can use the +# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp +# and *.h) to filter out the source-files in the directories. If left +# blank all files are included. + +EXAMPLE_PATTERNS = + +# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be +# searched for input files to be used with the \include or \dontinclude +# commands irrespective of the value of the RECURSIVE tag. +# Possible values are YES and NO. If left blank NO is used. + +EXAMPLE_RECURSIVE = NO + +# The IMAGE_PATH tag can be used to specify one or more files or +# directories that contain image that are included in the documentation (see +# the \image command). + +IMAGE_PATH = + +# The INPUT_FILTER tag can be used to specify a program that doxygen should +# invoke to filter for each input file. Doxygen will invoke the filter program +# by executing (via popen()) the command , where +# is the value of the INPUT_FILTER tag, and is the name of an +# input file. Doxygen will then use the output that the filter program writes +# to standard output. If FILTER_PATTERNS is specified, this tag will be +# ignored. + +INPUT_FILTER = + +# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern +# basis. Doxygen will compare the file name with each pattern and apply the +# filter if there is a match. The filters are a list of the form: +# pattern=filter (like *.cpp=my_cpp_filter). See INPUT_FILTER for further +# info on how filters are used. If FILTER_PATTERNS is empty, INPUT_FILTER +# is applied to all files. + +FILTER_PATTERNS = + +# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using +# INPUT_FILTER) will be used to filter the input files when producing source +# files to browse (i.e. when SOURCE_BROWSER is set to YES). + +FILTER_SOURCE_FILES = NO + +#--------------------------------------------------------------------------- +# configuration options related to source browsing +#--------------------------------------------------------------------------- + +# If the SOURCE_BROWSER tag is set to YES then a list of source files will +# be generated. Documented entities will be cross-referenced with these sources. +# Note: To get rid of all source code in the generated output, make sure also +# VERBATIM_HEADERS is set to NO. + +SOURCE_BROWSER = NO + +# Setting the INLINE_SOURCES tag to YES will include the body +# of functions and classes directly in the documentation. + +INLINE_SOURCES = YES + +# Setting the STRIP_CODE_COMMENTS tag to YES (the default) will instruct +# doxygen to hide any special comment blocks from generated source code +# fragments. Normal C and C++ comments will always remain visible. + +STRIP_CODE_COMMENTS = YES + +# If the REFERENCED_BY_RELATION tag is set to YES +# then for each documented function all documented +# functions referencing it will be listed. + +REFERENCED_BY_RELATION = NO + +# If the REFERENCES_RELATION tag is set to YES +# then for each documented function all documented entities +# called/used by that function will be listed. + +REFERENCES_RELATION = NO + +# If the REFERENCES_LINK_SOURCE tag is set to YES (the default) +# and SOURCE_BROWSER tag is set to YES, then the hyperlinks from +# functions in REFERENCES_RELATION and REFERENCED_BY_RELATION lists will +# link to the source code. Otherwise they will link to the documentstion. + +REFERENCES_LINK_SOURCE = YES + +# If the USE_HTAGS tag is set to YES then the references to source code +# will point to the HTML generated by the htags(1) tool instead of doxygen +# built-in source browser. The htags tool is part of GNU's global source +# tagging system (see http://www.gnu.org/software/global/global.html). You +# will need version 4.8.6 or higher. + +USE_HTAGS = NO + +# If the VERBATIM_HEADERS tag is set to YES (the default) then Doxygen +# will generate a verbatim copy of the header file for each class for +# which an include is specified. Set to NO to disable this. + +VERBATIM_HEADERS = NO + +#--------------------------------------------------------------------------- +# configuration options related to the alphabetical class index +#--------------------------------------------------------------------------- + +# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index +# of all compounds will be generated. Enable this if the project +# contains a lot of classes, structs, unions or interfaces. + +ALPHABETICAL_INDEX = NO + +# If the alphabetical index is enabled (see ALPHABETICAL_INDEX) then +# the COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns +# in which this list will be split (can be a number in the range [1..20]) + +COLS_IN_ALPHA_INDEX = 5 + +# In case all classes in a project start with a common prefix, all +# classes will be put under the same header in the alphabetical index. +# The IGNORE_PREFIX tag can be used to specify one or more prefixes that +# should be ignored while generating the index headers. + +IGNORE_PREFIX = + +#--------------------------------------------------------------------------- +# configuration options related to the HTML output +#--------------------------------------------------------------------------- + +# If the GENERATE_HTML tag is set to YES (the default) Doxygen will +# generate HTML output. + +GENERATE_HTML = YES + +# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `html' will be used as the default path. + +HTML_OUTPUT = html + +# The HTML_FILE_EXTENSION tag can be used to specify the file extension for +# each generated HTML page (for example: .htm,.php,.asp). If it is left blank +# doxygen will generate files with .html extension. + +HTML_FILE_EXTENSION = .html + +# The HTML_HEADER tag can be used to specify a personal HTML header for +# each generated HTML page. If it is left blank doxygen will generate a +# standard header. + +HTML_HEADER = + +# The HTML_FOOTER tag can be used to specify a personal HTML footer for +# each generated HTML page. If it is left blank doxygen will generate a +# standard footer. + +HTML_FOOTER = + +# The HTML_STYLESHEET tag can be used to specify a user-defined cascading +# style sheet that is used by each HTML page. It can be used to +# fine-tune the look of the HTML output. If the tag is left blank doxygen +# will generate a default style sheet. Note that doxygen will try to copy +# the style sheet file to the HTML output directory, so don't put your own +# stylesheet in the HTML output directory as well, or it will be erased! + +HTML_STYLESHEET = + +# If the HTML_ALIGN_MEMBERS tag is set to YES, the members of classes, +# files or namespaces will be aligned in HTML using tables. If set to +# NO a bullet list will be used. + +HTML_ALIGN_MEMBERS = YES + +# If the GENERATE_HTMLHELP tag is set to YES, additional index files +# will be generated that can be used as input for tools like the +# Microsoft HTML help workshop to generate a compiled HTML help file (.chm) +# of the generated HTML documentation. + +GENERATE_HTMLHELP = NO + +# If the GENERATE_DOCSET tag is set to YES, additional index files +# will be generated that can be used as input for Apple's Xcode 3 +# integrated development environment, introduced with OSX 10.5 (Leopard). +# To create a documentation set, doxygen will generate a Makefile in the +# HTML output directory. Running make will produce the docset in that +# directory and running "make install" will install the docset in +# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find +# it at startup. + +GENERATE_DOCSET = NO + +# When GENERATE_DOCSET tag is set to YES, this tag determines the name of the +# feed. A documentation feed provides an umbrella under which multiple +# documentation sets from a single provider (such as a company or product suite) +# can be grouped. + +DOCSET_FEEDNAME = "Doxygen generated docs" + +# When GENERATE_DOCSET tag is set to YES, this tag specifies a string that +# should uniquely identify the documentation set bundle. This should be a +# reverse domain-name style string, e.g. com.mycompany.MyDocSet. Doxygen +# will append .docset to the name. + +DOCSET_BUNDLE_ID = org.doxygen.Project + +# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML +# documentation will contain sections that can be hidden and shown after the +# page has loaded. For this to work a browser that supports +# JavaScript and DHTML is required (for instance Mozilla 1.0+, Firefox +# Netscape 6.0+, Internet explorer 5.0+, Konqueror, or Safari). + +HTML_DYNAMIC_SECTIONS = NO + +# If the GENERATE_HTMLHELP tag is set to YES, the CHM_FILE tag can +# be used to specify the file name of the resulting .chm file. You +# can add a path in front of the file if the result should not be +# written to the html output directory. + +CHM_FILE = + +# If the GENERATE_HTMLHELP tag is set to YES, the HHC_LOCATION tag can +# be used to specify the location (absolute path including file name) of +# the HTML help compiler (hhc.exe). If non-empty doxygen will try to run +# the HTML help compiler on the generated index.hhp. + +HHC_LOCATION = + +# If the GENERATE_HTMLHELP tag is set to YES, the GENERATE_CHI flag +# controls if a separate .chi index file is generated (YES) or that +# it should be included in the master .chm file (NO). + +GENERATE_CHI = NO + +# If the GENERATE_HTMLHELP tag is set to YES, the CHM_INDEX_ENCODING +# is used to encode HtmlHelp index (hhk), content (hhc) and project file +# content. + +CHM_INDEX_ENCODING = + +# If the GENERATE_HTMLHELP tag is set to YES, the BINARY_TOC flag +# controls whether a binary table of contents is generated (YES) or a +# normal table of contents (NO) in the .chm file. + +BINARY_TOC = NO + +# The TOC_EXPAND flag can be set to YES to add extra items for group members +# to the contents of the HTML help documentation and to the tree view. + +TOC_EXPAND = NO + +# The DISABLE_INDEX tag can be used to turn on/off the condensed index at +# top of each HTML page. The value NO (the default) enables the index and +# the value YES disables it. + +DISABLE_INDEX = NO + +# This tag can be used to set the number of enum values (range [1..20]) +# that doxygen will group on one line in the generated HTML documentation. + +ENUM_VALUES_PER_LINE = 4 + +# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index +# structure should be generated to display hierarchical information. +# If the tag value is set to FRAME, a side panel will be generated +# containing a tree-like index structure (just like the one that +# is generated for HTML Help). For this to work a browser that supports +# JavaScript, DHTML, CSS and frames is required (for instance Mozilla 1.0+, +# Netscape 6.0+, Internet explorer 5.0+, or Konqueror). Windows users are +# probably better off using the HTML help feature. Other possible values +# for this tag are: HIERARCHIES, which will generate the Groups, Directories, +# and Class Hiererachy pages using a tree view instead of an ordered list; +# ALL, which combines the behavior of FRAME and HIERARCHIES; and NONE, which +# disables this behavior completely. For backwards compatibility with previous +# releases of Doxygen, the values YES and NO are equivalent to FRAME and NONE +# respectively. + +GENERATE_TREEVIEW = YES + +# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be +# used to set the initial width (in pixels) of the frame in which the tree +# is shown. + +TREEVIEW_WIDTH = 250 + +# Use this tag to change the font size of Latex formulas included +# as images in the HTML documentation. The default is 10. Note that +# when you change the font size after a successful doxygen run you need +# to manually remove any form_*.png images from the HTML output directory +# to force them to be regenerated. + +FORMULA_FONTSIZE = 10 + +#--------------------------------------------------------------------------- +# configuration options related to the LaTeX output +#--------------------------------------------------------------------------- + +# If the GENERATE_LATEX tag is set to YES (the default) Doxygen will +# generate Latex output. + +GENERATE_LATEX = NO + +# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `latex' will be used as the default path. + +LATEX_OUTPUT = latex + +# The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be +# invoked. If left blank `latex' will be used as the default command name. + +LATEX_CMD_NAME = latex + +# The MAKEINDEX_CMD_NAME tag can be used to specify the command name to +# generate index for LaTeX. If left blank `makeindex' will be used as the +# default command name. + +MAKEINDEX_CMD_NAME = makeindex + +# If the COMPACT_LATEX tag is set to YES Doxygen generates more compact +# LaTeX documents. This may be useful for small projects and may help to +# save some trees in general. + +COMPACT_LATEX = NO + +# The PAPER_TYPE tag can be used to set the paper type that is used +# by the printer. Possible values are: a4, a4wide, letter, legal and +# executive. If left blank a4wide will be used. + +PAPER_TYPE = a4wide + +# The EXTRA_PACKAGES tag can be to specify one or more names of LaTeX +# packages that should be included in the LaTeX output. + +EXTRA_PACKAGES = + +# The LATEX_HEADER tag can be used to specify a personal LaTeX header for +# the generated latex document. The header should contain everything until +# the first chapter. If it is left blank doxygen will generate a +# standard header. Notice: only use this tag if you know what you are doing! + +LATEX_HEADER = + +# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated +# is prepared for conversion to pdf (using ps2pdf). The pdf file will +# contain links (just like the HTML output) instead of page references +# This makes the output suitable for online browsing using a pdf viewer. + +PDF_HYPERLINKS = YES + +# If the USE_PDFLATEX tag is set to YES, pdflatex will be used instead of +# plain latex in the generated Makefile. Set this option to YES to get a +# higher quality PDF documentation. + +USE_PDFLATEX = YES + +# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \\batchmode. +# command to the generated LaTeX files. This will instruct LaTeX to keep +# running if errors occur, instead of asking the user for help. +# This option is also used when generating formulas in HTML. + +LATEX_BATCHMODE = NO + +# If LATEX_HIDE_INDICES is set to YES then doxygen will not +# include the index chapters (such as File Index, Compound Index, etc.) +# in the output. + +LATEX_HIDE_INDICES = NO + +#--------------------------------------------------------------------------- +# configuration options related to the RTF output +#--------------------------------------------------------------------------- + +# If the GENERATE_RTF tag is set to YES Doxygen will generate RTF output +# The RTF output is optimized for Word 97 and may not look very pretty with +# other RTF readers or editors. + +GENERATE_RTF = NO + +# The RTF_OUTPUT tag is used to specify where the RTF docs will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `rtf' will be used as the default path. + +RTF_OUTPUT = rtf + +# If the COMPACT_RTF tag is set to YES Doxygen generates more compact +# RTF documents. This may be useful for small projects and may help to +# save some trees in general. + +COMPACT_RTF = NO + +# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated +# will contain hyperlink fields. The RTF file will +# contain links (just like the HTML output) instead of page references. +# This makes the output suitable for online browsing using WORD or other +# programs which support those fields. +# Note: wordpad (write) and others do not support links. + +RTF_HYPERLINKS = NO + +# Load stylesheet definitions from file. Syntax is similar to doxygen's +# config file, i.e. a series of assignments. You only have to provide +# replacements, missing definitions are set to their default value. + +RTF_STYLESHEET_FILE = + +# Set optional variables used in the generation of an rtf document. +# Syntax is similar to doxygen's config file. + +RTF_EXTENSIONS_FILE = + +#--------------------------------------------------------------------------- +# configuration options related to the man page output +#--------------------------------------------------------------------------- + +# If the GENERATE_MAN tag is set to YES (the default) Doxygen will +# generate man pages + +GENERATE_MAN = NO + +# The MAN_OUTPUT tag is used to specify where the man pages will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `man' will be used as the default path. + +MAN_OUTPUT = man + +# The MAN_EXTENSION tag determines the extension that is added to +# the generated man pages (default is the subroutine's section .3) + +MAN_EXTENSION = .3 + +# If the MAN_LINKS tag is set to YES and Doxygen generates man output, +# then it will generate one additional man file for each entity +# documented in the real man page(s). These additional files +# only source the real man page, but without them the man command +# would be unable to find the correct page. The default is NO. + +MAN_LINKS = NO + +#--------------------------------------------------------------------------- +# configuration options related to the XML output +#--------------------------------------------------------------------------- + +# If the GENERATE_XML tag is set to YES Doxygen will +# generate an XML file that captures the structure of +# the code including all documentation. + +GENERATE_XML = NO + +# The XML_OUTPUT tag is used to specify where the XML pages will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `xml' will be used as the default path. + +XML_OUTPUT = xml + +# The XML_SCHEMA tag can be used to specify an XML schema, +# which can be used by a validating XML parser to check the +# syntax of the XML files. + +XML_SCHEMA = + +# The XML_DTD tag can be used to specify an XML DTD, +# which can be used by a validating XML parser to check the +# syntax of the XML files. + +XML_DTD = + +# If the XML_PROGRAMLISTING tag is set to YES Doxygen will +# dump the program listings (including syntax highlighting +# and cross-referencing information) to the XML output. Note that +# enabling this will significantly increase the size of the XML output. + +XML_PROGRAMLISTING = YES + +#--------------------------------------------------------------------------- +# configuration options for the AutoGen Definitions output +#--------------------------------------------------------------------------- + +# If the GENERATE_AUTOGEN_DEF tag is set to YES Doxygen will +# generate an AutoGen Definitions (see autogen.sf.net) file +# that captures the structure of the code including all +# documentation. Note that this feature is still experimental +# and incomplete at the moment. + +GENERATE_AUTOGEN_DEF = NO + +#--------------------------------------------------------------------------- +# configuration options related to the Perl module output +#--------------------------------------------------------------------------- + +# If the GENERATE_PERLMOD tag is set to YES Doxygen will +# generate a Perl module file that captures the structure of +# the code including all documentation. Note that this +# feature is still experimental and incomplete at the +# moment. + +GENERATE_PERLMOD = NO + +# If the PERLMOD_LATEX tag is set to YES Doxygen will generate +# the necessary Makefile rules, Perl scripts and LaTeX code to be able +# to generate PDF and DVI output from the Perl module output. + +PERLMOD_LATEX = NO + +# If the PERLMOD_PRETTY tag is set to YES the Perl module output will be +# nicely formatted so it can be parsed by a human reader. This is useful +# if you want to understand what is going on. On the other hand, if this +# tag is set to NO the size of the Perl module output will be much smaller +# and Perl will parse it just the same. + +PERLMOD_PRETTY = YES + +# The names of the make variables in the generated doxyrules.make file +# are prefixed with the string contained in PERLMOD_MAKEVAR_PREFIX. +# This is useful so different doxyrules.make files included by the same +# Makefile don't overwrite each other's variables. + +PERLMOD_MAKEVAR_PREFIX = + +#--------------------------------------------------------------------------- +# Configuration options related to the preprocessor +#--------------------------------------------------------------------------- + +# If the ENABLE_PREPROCESSING tag is set to YES (the default) Doxygen will +# evaluate all C-preprocessor directives found in the sources and include +# files. + +ENABLE_PREPROCESSING = YES + +# If the MACRO_EXPANSION tag is set to YES Doxygen will expand all macro +# names in the source code. If set to NO (the default) only conditional +# compilation will be performed. Macro expansion can be done in a controlled +# way by setting EXPAND_ONLY_PREDEF to YES. + +MACRO_EXPANSION = NO + +# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES +# then the macro expansion is limited to the macros specified with the +# PREDEFINED and EXPAND_AS_DEFINED tags. + +EXPAND_ONLY_PREDEF = NO + +# If the SEARCH_INCLUDES tag is set to YES (the default) the includes files +# in the INCLUDE_PATH (see below) will be search if a #include is found. + +SEARCH_INCLUDES = YES + +# The INCLUDE_PATH tag can be used to specify one or more directories that +# contain include files that are not input files but should be processed by +# the preprocessor. + +INCLUDE_PATH = + +# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard +# patterns (like *.h and *.hpp) to filter out the header-files in the +# directories. If left blank, the patterns specified with FILE_PATTERNS will +# be used. + +INCLUDE_FILE_PATTERNS = + +# The PREDEFINED tag can be used to specify one or more macro names that +# are defined before the preprocessor is started (similar to the -D option of +# gcc). The argument of the tag is a list of macros of the form: name +# or name=definition (no spaces). If the definition and the = are +# omitted =1 is assumed. To prevent a macro definition from being +# undefined via #undef or recursively expanded use the := operator +# instead of the = operator. + +PREDEFINED = + +# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then +# this tag can be used to specify a list of macro names that should be expanded. +# The macro definition that is found in the sources will be used. +# Use the PREDEFINED tag if you want to use a different macro definition. + +EXPAND_AS_DEFINED = + +# If the SKIP_FUNCTION_MACROS tag is set to YES (the default) then +# doxygen's preprocessor will remove all function-like macros that are alone +# on a line, have an all uppercase name, and do not end with a semicolon. Such +# function macros are typically used for boiler-plate code, and will confuse +# the parser if not removed. + +SKIP_FUNCTION_MACROS = YES + +#--------------------------------------------------------------------------- +# Configuration::additions related to external references +#--------------------------------------------------------------------------- + +# The TAGFILES option can be used to specify one or more tagfiles. +# Optionally an initial location of the external documentation +# can be added for each tagfile. The format of a tag file without +# this location is as follows: +# TAGFILES = file1 file2 ... +# Adding location for the tag files is done as follows: +# TAGFILES = file1=loc1 "file2 = loc2" ... +# where "loc1" and "loc2" can be relative or absolute paths or +# URLs. If a location is present for each tag, the installdox tool +# does not have to be run to correct the links. +# Note that each tag file must have a unique name +# (where the name does NOT include the path) +# If a tag file is not located in the directory in which doxygen +# is run, you must also specify the path to the tagfile here. + +TAGFILES = + +# When a file name is specified after GENERATE_TAGFILE, doxygen will create +# a tag file that is based on the input files it reads. + +GENERATE_TAGFILE = + +# If the ALLEXTERNALS tag is set to YES all external classes will be listed +# in the class index. If set to NO only the inherited external classes +# will be listed. + +ALLEXTERNALS = NO + +# If the EXTERNAL_GROUPS tag is set to YES all external groups will be listed +# in the modules index. If set to NO, only the current project's groups will +# be listed. + +EXTERNAL_GROUPS = YES + +# The PERL_PATH should be the absolute path and name of the perl script +# interpreter (i.e. the result of `which perl'). + +PERL_PATH = /usr/bin/perl + +#--------------------------------------------------------------------------- +# Configuration options related to the dot tool +#--------------------------------------------------------------------------- + +# If the CLASS_DIAGRAMS tag is set to YES (the default) Doxygen will +# generate a inheritance diagram (in HTML, RTF and LaTeX) for classes with base +# or super classes. Setting the tag to NO turns the diagrams off. Note that +# this option is superseded by the HAVE_DOT option below. This is only a +# fallback. It is recommended to install and use dot, since it yields more +# powerful graphs. + +CLASS_DIAGRAMS = YES + +# You can define message sequence charts within doxygen comments using the \msc +# command. Doxygen will then run the mscgen tool (see +# http://www.mcternan.me.uk/mscgen/) to produce the chart and insert it in the +# documentation. The MSCGEN_PATH tag allows you to specify the directory where +# the mscgen tool resides. If left empty the tool is assumed to be found in the +# default search path. + +MSCGEN_PATH = + +# If set to YES, the inheritance and collaboration graphs will hide +# inheritance and usage relations if the target is undocumented +# or is not a class. + +HIDE_UNDOC_RELATIONS = YES + +# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is +# available from the path. This tool is part of Graphviz, a graph visualization +# toolkit from AT&T and Lucent Bell Labs. The other options in this section +# have no effect if this option is set to NO (the default) + +HAVE_DOT = NO + +# By default doxygen will write a font called FreeSans.ttf to the output +# directory and reference it in all dot files that doxygen generates. This +# font does not include all possible unicode characters however, so when you need +# these (or just want a differently looking font) you can specify the font name +# using DOT_FONTNAME. You need need to make sure dot is able to find the font, +# which can be done by putting it in a standard location or by setting the +# DOTFONTPATH environment variable or by setting DOT_FONTPATH to the directory +# containing the font. + +DOT_FONTNAME = FreeSans + +# By default doxygen will tell dot to use the output directory to look for the +# FreeSans.ttf font (which doxygen will put there itself). If you specify a +# different font using DOT_FONTNAME you can set the path where dot +# can find it using this tag. + +DOT_FONTPATH = + +# If the CLASS_GRAPH and HAVE_DOT tags are set to YES then doxygen +# will generate a graph for each documented class showing the direct and +# indirect inheritance relations. Setting this tag to YES will force the +# the CLASS_DIAGRAMS tag to NO. + +CLASS_GRAPH = YES + +# If the COLLABORATION_GRAPH and HAVE_DOT tags are set to YES then doxygen +# will generate a graph for each documented class showing the direct and +# indirect implementation dependencies (inheritance, containment, and +# class references variables) of the class with other documented classes. + +COLLABORATION_GRAPH = YES + +# If the GROUP_GRAPHS and HAVE_DOT tags are set to YES then doxygen +# will generate a graph for groups, showing the direct groups dependencies + +GROUP_GRAPHS = YES + +# If the UML_LOOK tag is set to YES doxygen will generate inheritance and +# collaboration diagrams in a style similar to the OMG's Unified Modeling +# Language. + +UML_LOOK = NO + +# If set to YES, the inheritance and collaboration graphs will show the +# relations between templates and their instances. + +TEMPLATE_RELATIONS = NO + +# If the ENABLE_PREPROCESSING, SEARCH_INCLUDES, INCLUDE_GRAPH, and HAVE_DOT +# tags are set to YES then doxygen will generate a graph for each documented +# file showing the direct and indirect include dependencies of the file with +# other documented files. + +INCLUDE_GRAPH = YES + +# If the ENABLE_PREPROCESSING, SEARCH_INCLUDES, INCLUDED_BY_GRAPH, and +# HAVE_DOT tags are set to YES then doxygen will generate a graph for each +# documented header file showing the documented files that directly or +# indirectly include this file. + +INCLUDED_BY_GRAPH = YES + +# If the CALL_GRAPH and HAVE_DOT options are set to YES then +# doxygen will generate a call dependency graph for every global function +# or class method. Note that enabling this option will significantly increase +# the time of a run. So in most cases it will be better to enable call graphs +# for selected functions only using the \callgraph command. + +CALL_GRAPH = YES + +# If the CALLER_GRAPH and HAVE_DOT tags are set to YES then +# doxygen will generate a caller dependency graph for every global function +# or class method. Note that enabling this option will significantly increase +# the time of a run. So in most cases it will be better to enable caller +# graphs for selected functions only using the \callergraph command. + +CALLER_GRAPH = NO + +# If the GRAPHICAL_HIERARCHY and HAVE_DOT tags are set to YES then doxygen +# will graphical hierarchy of all classes instead of a textual one. + +GRAPHICAL_HIERARCHY = YES + +# If the DIRECTORY_GRAPH, SHOW_DIRECTORIES and HAVE_DOT tags are set to YES +# then doxygen will show the dependencies a directory has on other directories +# in a graphical way. The dependency relations are determined by the #include +# relations between the files in the directories. + +DIRECTORY_GRAPH = YES + +# The DOT_IMAGE_FORMAT tag can be used to set the image format of the images +# generated by dot. Possible values are png, jpg, or gif +# If left blank png will be used. + +DOT_IMAGE_FORMAT = png + +# The tag DOT_PATH can be used to specify the path where the dot tool can be +# found. If left blank, it is assumed the dot tool can be found in the path. + +DOT_PATH = + +# The DOTFILE_DIRS tag can be used to specify one or more directories that +# contain dot files that are included in the documentation (see the +# \dotfile command). + +DOTFILE_DIRS = + +# The DOT_GRAPH_MAX_NODES tag can be used to set the maximum number of +# nodes that will be shown in the graph. If the number of nodes in a graph +# becomes larger than this value, doxygen will truncate the graph, which is +# visualized by representing a node as a red box. Note that doxygen if the +# number of direct children of the root node in a graph is already larger than +# DOT_GRAPH_MAX_NODES then the graph will not be shown at all. Also note +# that the size of a graph can be further restricted by MAX_DOT_GRAPH_DEPTH. + +DOT_GRAPH_MAX_NODES = 50 + +# The MAX_DOT_GRAPH_DEPTH tag can be used to set the maximum depth of the +# graphs generated by dot. A depth value of 3 means that only nodes reachable +# from the root by following a path via at most 3 edges will be shown. Nodes +# that lay further from the root node will be omitted. Note that setting this +# option to 1 or 2 may greatly reduce the computation time needed for large +# code bases. Also note that the size of a graph can be further restricted by +# DOT_GRAPH_MAX_NODES. Using a depth of 0 means no depth restriction. + +MAX_DOT_GRAPH_DEPTH = 0 + +# Set the DOT_TRANSPARENT tag to YES to generate images with a transparent +# background. This is enabled by default, which results in a transparent +# background. Warning: Depending on the platform used, enabling this option +# may lead to badly anti-aliased labels on the edges of a graph (i.e. they +# become hard to read). + +DOT_TRANSPARENT = YES + +# Set the DOT_MULTI_TARGETS tag to YES allow dot to generate multiple output +# files in one run (i.e. multiple -o and -T options on the command line). This +# makes dot run faster, but since only newer versions of dot (>1.8.10) +# support this, this feature is disabled by default. + +DOT_MULTI_TARGETS = NO + +# If the GENERATE_LEGEND tag is set to YES (the default) Doxygen will +# generate a legend page explaining the meaning of the various boxes and +# arrows in the dot generated graphs. + +GENERATE_LEGEND = YES + +# If the DOT_CLEANUP tag is set to YES (the default) Doxygen will +# remove the intermediate dot files that are used to generate +# the various graphs. + +DOT_CLEANUP = YES + +#--------------------------------------------------------------------------- +# Configuration::additions related to the search engine +#--------------------------------------------------------------------------- + +# The SEARCHENGINE tag specifies whether or not a search engine should be +# used. If set to NO the values of all tags below this one will be ignored. + +SEARCHENGINE = NO diff --git a/cpp/EqualityFactor.cpp b/cpp/EqualityFactor.cpp new file mode 100644 index 000000000..66c8e2d64 --- /dev/null +++ b/cpp/EqualityFactor.cpp @@ -0,0 +1,76 @@ +/* + * EqualityFactor.cpp + * + * Created on: Aug 10, 2009 + * Author: alexgc + */ + +#include "EqualityFactor.h" +#include + +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(&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; +} + +} diff --git a/cpp/EqualityFactor.h b/cpp/EqualityFactor.h new file mode 100644 index 000000000..f5d68d7ff --- /dev/null +++ b/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 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_ */ diff --git a/cpp/FGConfig.cpp b/cpp/FGConfig.cpp new file mode 100644 index 000000000..d869fab18 --- /dev/null +++ b/cpp/FGConfig.cpp @@ -0,0 +1,95 @@ +/** + * @file FGConfig.cpp + * @brief Factor Graph Configuration + * @brief fgConfig + * @author Carlos Nieto + * @author Christian Potthast + */ + +#include +#include +#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; +} + +/* ************************************************************************* */ + diff --git a/cpp/FGConfig.h b/cpp/FGConfig.h new file mode 100644 index 000000000..1701343f5 --- /dev/null +++ b/cpp/FGConfig.h @@ -0,0 +1,94 @@ +/** + * @file FGConfig.h + * @brief Factor Graph Configuration + * @author Carlos Nieto + * @author Christian Potthast + */ + +// \callgraph + +#pragma once + +#include +#include + +#include "Value.h" + +namespace gtsam { + + /** Factor Graph Configuration */ + class FGConfig { + + protected: + /** Map from string indices to values */ + std::map values; + + public: + typedef std::map::iterator iterator; + typedef std::map::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 get_names(){ + std::vector 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 + void serialize(Archive & ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(values); + } + }; +} diff --git a/cpp/Factor.h b/cpp/Factor.h new file mode 100644 index 000000000..621428eb3 --- /dev/null +++ b/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 +#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_ { + }; + + /** + * 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; + }; +} diff --git a/cpp/FactorGraph.cpp b/cpp/FactorGraph.cpp new file mode 100644 index 000000000..8b4628599 --- /dev/null +++ b/cpp/FactorGraph.cpp @@ -0,0 +1,9 @@ +/** + * @file FactorGraph.cpp + * @brief Factor Graph Base Class + * @author Carlos Nieto + */ + +#include "FactorGraph.h" + + diff --git a/cpp/FactorGraph.h b/cpp/FactorGraph.h new file mode 100644 index 000000000..48c6505d6 --- /dev/null +++ b/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 +#include +#include +#include +#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 FactorGraph + { + public: + typedef typename boost::shared_ptr shared_factor; + typedef typename std::vector::iterator iterator; + typedef typename std::vector::const_iterator const_iterator; + + protected: + /** Collection of factors */ + std::vector factors; + std::map > node_to_factors_; + + public: + + /** get the factors to a specific node */ + const std::list& 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;iequals(*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 diff --git a/cpp/LinearFactor.cpp b/cpp/LinearFactor.cpp new file mode 100644 index 000000000..b20a9257b --- /dev/null +++ b/cpp/LinearFactor.cpp @@ -0,0 +1,245 @@ +/** + * @file LinearFactor.cpp + * @brief Linear Factor....A Gaussian + * @brief linearFactor + * @author Christian Potthast + */ + +#include +#include +#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& 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(&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& separator) const { + if(involves(key)) { + string j; Matrix A; + FOREACH_PAIR(j,A,As) + if(j != key) separator.insert(j); + } +} + +/* ************************************************************************* */ +pair LinearFactor::matrix(const Ordering& ordering) const { + + // get pointers to the matrices + vector 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 & 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 +MutableLinearFactor::eliminate(const string& key) +{ + // start empty remaining factor to be returned + boost::shared_ptr 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 mset_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); +} + +/* ************************************************************************* */ diff --git a/cpp/LinearFactor.h b/cpp/LinearFactor.h new file mode 100644 index 000000000..bdbd51fb7 --- /dev/null +++ b/cpp/LinearFactor.h @@ -0,0 +1,245 @@ +/** + * @file LinearFactor.h + * @brief Linear Factor....A Gaussian + * @brief linearFactor + * @author Christian Potthast + */ + +// \callgraph + +#pragma once + +#include +#include +#include +#include +#include + +#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 shared_ptr; + typedef std::map::iterator iterator; + typedef std::map::const_iterator const_iterator; + +protected: + + std::map 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::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& separator) const; + + /** + * Return (dense) matrix associated with factor + * @param ordering of variables needed for matrix column order + */ + std::pair 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 & 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 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 diff --git a/cpp/LinearFactorGraph.cpp b/cpp/LinearFactorGraph.cpp new file mode 100644 index 000000000..ea70ac53b --- /dev/null +++ b/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 +#include +#include +#include + +#include + +#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 LinearFactorGraph::find_separator(const string& key) const +{ + set 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 +LinearFactorGraph::combine_factors(const string& key) +{ + LinearFactorSet found = find_factors_and_remove(key); + boost::shared_ptr 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 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 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 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 _symbolicElimintationOrder; + + Ordering result; + + map > symbolToMatrixElement; + _symbolicLength = 0; + _symbolicColLength = 0; + int matrixRow = 0; + int symNRows = 0; + int symNCols = 0; + for(vector::const_iterator it = begin(); it != end(); it++) + { + symNRows++; + for(map::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 >::iterator it = symbolToMatrixElement.begin(); it != symbolToMatrixElement.end(); it++) + { + for(vector::iterator rit = it->second.begin(); rit != it->second.end(); rit++) + _symbolicMatrix[count++] = (*rit); + _symbolicColumns[colCount++] = count; + } + + vector initialOrder; + for(map >::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::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; +} + diff --git a/cpp/LinearFactorGraph.h b/cpp/LinearFactorGraph.h new file mode 100644 index 000000000..a20889e94 --- /dev/null +++ b/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 + +#include "LinearFactor.h" +#include "FactorGraph.h" +#include "ChordalBayesNet.h" + +namespace gtsam { + + /** Linear Factor Graph where all factors are Gaussians */ + class LinearFactorGraph : public FactorGraph { + 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 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 + 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 (const Ordering& ordering) const; + }; + +} diff --git a/cpp/LinearFactorSet.h b/cpp/LinearFactorSet.h new file mode 100644 index 000000000..30e2f0f67 --- /dev/null +++ b/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 +#include +#include "LinearFactor.h" + +namespace gtsam { + + class LinearFactor; + + struct LinearFactorSet : std::set > { + LinearFactorSet() {} + }; +} diff --git a/cpp/Makefile.am b/cpp/Makefile.am new file mode 100644 index 000000000..8d9bc7363 --- /dev/null +++ b/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 + ./$^ diff --git a/cpp/Matrix.cpp b/cpp/Matrix.cpp new file mode 100644 index 000000000..b747e058f --- /dev/null +++ b/cpp/Matrix.cpp @@ -0,0 +1,466 @@ +/** + * @file Matrix.cpp + * @brief matrix class + * @author Christian Potthast + */ + +#include +#include +#include + +#include +#include +#include +#include + +#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 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(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 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 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 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 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 diff --git a/cpp/Matrix.h b/cpp/Matrix.h new file mode 100644 index 000000000..801f3100a --- /dev/null +++ b/cpp/Matrix.h @@ -0,0 +1,201 @@ +/** + * @file Matrix.h + * @brief typedef and functions to augment Boost's ublas::matrix + * @author Christian Potthast + * @author Kai Ni + * @author Frank Dellaert + */ + +// \callgraph + +#pragma once + +#include +#include "Vector.h" + +/** + * Vector is a *global* typedef + * wrap-matlab does this typedef as well + * we use the default < double,row_major,unbounded_array > + */ + +#if ! defined (MEX_H) +typedef boost::numeric::ublas::matrix 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 rotation matrix Q, upper triangular R + */ +std::pair 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 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 diff --git a/cpp/NonlinearFactor.cpp b/cpp/NonlinearFactor.cpp new file mode 100644 index 000000000..fc4ec4a97 --- /dev/null +++ b/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(&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(&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_); +} + +/* ************************************************************************* */ diff --git a/cpp/NonlinearFactor.h b/cpp/NonlinearFactor.h new file mode 100644 index 000000000..7997b5bc9 --- /dev/null +++ b/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 + +#include + +#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 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 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 linearize(const FGConfig& c) const = 0; + + /** get functions */ + double get_sigma() const {return sigma_;} + Vector get_measurement() const {return z_;} + std::list get_keys() const {return keys_;} + void set_keys(std::list 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 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 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 "";}; + }; + + /* ************************************************************************* */ +} diff --git a/cpp/NonlinearFactorGraph.cpp b/cpp/NonlinearFactorGraph.cpp new file mode 100644 index 000000000..f7bb91e97 --- /dev/null +++ b/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 +#include +#include +#include +#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(); factorlinearize(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 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 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 p(linear, newConfig); + return p; + + + + // linearize, solve, update + //double newError = iterateLM(config, currentError, lambda, lambdaFactor, ordering, verbosity); + +} + +/* ************************************************************************* */ + + +} // namespace gtsam diff --git a/cpp/NonlinearFactorGraph.h b/cpp/NonlinearFactorGraph.h new file mode 100644 index 000000000..af55339d1 --- /dev/null +++ b/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 + +namespace gtsam { + +/** Factor Graph Constsiting of non-linear factors */ +class NonlinearFactorGraph : public FactorGraph +{ +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 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 OneIterationLM( FGConfig& config, const Ordering& ordering, + double relativeErrorTreshold, + double absoluteErrorTreshold, + int verbosity, + double lambda0, + double lambdaFactor) ; + +}; +} diff --git a/cpp/Ordering.cpp b/cpp/Ordering.cpp new file mode 100644 index 000000000..ade78a67b --- /dev/null +++ b/cpp/Ordering.cpp @@ -0,0 +1,45 @@ +/** + * @file Ordering.cpp + * @brief Ordering + * @author Christian Potthast + */ + +#include +#include //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::iterator key; + vector::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; +} +/* ************************************************************************* */ + + + diff --git a/cpp/Ordering.h b/cpp/Ordering.h new file mode 100644 index 000000000..691ac4cf4 --- /dev/null +++ b/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 +#include + +// \namespace + +namespace gtsam { + +/** + * @class Ordering + * @brief ordering of indices for eliminating a factor graph + */ +class Ordering : public std::vector +{ +public: + /** Constructor */ + Ordering(){clear();} + + Ordering(std::vector strings_in) : std::vector (strings_in) {} + + /** Destructor */ + ~Ordering(){} + + void print() const; + + /** + * check if two orderings are the same + * @param ordering + * @return bool + */ + bool equals(Ordering &ord); +}; + +} diff --git a/cpp/Point2.cpp b/cpp/Point2.cpp new file mode 100644 index 000000000..8e7acf675 --- /dev/null +++ b/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 diff --git a/cpp/Point2.h b/cpp/Point2.h new file mode 100644 index 000000000..93b25f73f --- /dev/null +++ b/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); +} + diff --git a/cpp/Point2Prior.h b/cpp/Point2Prior.h new file mode 100644 index 000000000..886c2223e --- /dev/null +++ b/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 diff --git a/cpp/Point3.cpp b/cpp/Point3.cpp new file mode 100644 index 000000000..0c2dd30a1 --- /dev/null +++ b/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 diff --git a/cpp/Point3.h b/cpp/Point3.h new file mode 100644 index 000000000..bc25dcfc8 --- /dev/null +++ b/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 + +#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 + 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); +} diff --git a/cpp/Pose3.cpp b/cpp/Pose3.cpp new file mode 100644 index 000000000..c4a139b62 --- /dev/null +++ b/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 diff --git a/cpp/Pose3.h b/cpp/Pose3.h new file mode 100644 index 000000000..0ec0819a7 --- /dev/null +++ b/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 + 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 diff --git a/cpp/Rot3.cpp b/cpp/Rot3.cpp new file mode 100644 index 000000000..438e4ec06 --- /dev/null +++ b/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 diff --git a/cpp/Rot3.h b/cpp/Rot3.h new file mode 100644 index 000000000..b5cc028d3 --- /dev/null +++ b/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 + 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); + +} diff --git a/cpp/SimpleCamera.cpp b/cpp/SimpleCamera.cpp new file mode 100644 index 000000000..c207291a7 --- /dev/null +++ b/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; + } + +/* ************************************************************************* */ +} diff --git a/cpp/SimpleCamera.h b/cpp/SimpleCamera.h new file mode 100644 index 000000000..40f0f3b0f --- /dev/null +++ b/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_ */ diff --git a/cpp/Simulated2DMeasurement.h b/cpp/Simulated2DMeasurement.h new file mode 100644 index 000000000..d355ebbc1 --- /dev/null +++ b/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 diff --git a/cpp/Simulated2DOdometry.h b/cpp/Simulated2DOdometry.h new file mode 100644 index 000000000..3de11dde9 --- /dev/null +++ b/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 diff --git a/cpp/Simulated3D.cpp b/cpp/Simulated3D.cpp new file mode 100644 index 000000000..a3c8b749e --- /dev/null +++ b/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 \ No newline at end of file diff --git a/cpp/Simulated3D.h b/cpp/Simulated3D.h new file mode 100644 index 000000000..1cde91065 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/cpp/VSLAMFactor.cpp b/cpp/VSLAMFactor.cpp new file mode 100644 index 000000000..82358d162 --- /dev/null +++ b/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(&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; +} + diff --git a/cpp/VSLAMFactor.h b/cpp/VSLAMFactor.h new file mode 100644 index 000000000..ae281fd63 --- /dev/null +++ b/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 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; +}; + diff --git a/cpp/Value.h b/cpp/Value.h new file mode 100644 index 000000000..da91ec20e --- /dev/null +++ b/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. This allows us to define the + * return type of exmap as a Rot3 as well. + */ + template 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; + }; +} diff --git a/cpp/Vector.cpp b/cpp/Vector.cpp new file mode 100644 index 000000000..feefbf2f5 --- /dev/null +++ b/cpp/Vector.cpp @@ -0,0 +1,226 @@ +/** +* @file Vector.cpp +* @brief typedef and functions to augment Boost's ublas::vector +* @author Kai Ni +* @author Frank Dellaert +*/ + +#include +#include +#include +#include + +#ifdef WIN32 +#include +#endif + +#include +#include + +#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 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 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 norm_dist(mean, sigma); + boost::variate_generator > 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 diff --git a/cpp/Vector.h b/cpp/Vector.h new file mode 100644 index 000000000..6ad5237b8 --- /dev/null +++ b/cpp/Vector.h @@ -0,0 +1,114 @@ +/** + * @file Vector.h + * @brief typedef and functions to augment Boost's ublas::vector + * @author Kai Ni + * @author Frank Dellaert + */ + +// \callgraph + +#pragma once + +#include +#include + +// Vector is a *global* typedef +// wrap-matlab does this typedef as well +#if ! defined (MEX_H) +typedef boost::numeric::ublas::vector 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 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); + diff --git a/cpp/gtsam.h b/cpp/gtsam.h new file mode 100644 index 000000000..c3cf30acb --- /dev/null +++ b/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(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() 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(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; +}; + diff --git a/cpp/gtsam.sln b/cpp/gtsam.sln new file mode 100644 index 000000000..a2912451c --- /dev/null +++ b/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 diff --git a/cpp/gtsam.vcproj b/cpp/gtsam.vcproj new file mode 100644 index 000000000..ec6d32a5a --- /dev/null +++ b/cpp/gtsam.vcproj @@ -0,0 +1,362 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/cpp/manual.mk b/cpp/manual.mk new file mode 100644 index 000000000..b130459f4 --- /dev/null +++ b/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) + diff --git a/cpp/numericalDerivative.cpp b/cpp/numericalDerivative.cpp new file mode 100644 index 000000000..ffcaaf34f --- /dev/null +++ b/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 + 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 + 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 + 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 + 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 +#include + +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 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 + factor(new NonlinearFactor1(z,sigma,&optimize::h,"x",&optimize::H)); + fg.push_back(factor); + return fg; +} + +/* ************************************************************************* */ + +} // namespace gtsam diff --git a/cpp/smallExample.h b/cpp/smallExample.h new file mode 100644 index 000000000..91034aec8 --- /dev/null +++ b/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(); +} diff --git a/cpp/svdcmp.cpp b/cpp/svdcmp.cpp new file mode 100644 index 000000000..c730f30e0 --- /dev/null +++ b/cpp/svdcmp.cpp @@ -0,0 +1,254 @@ +/** + * @file svdcmp.cpp + * @brief SVD decomposition adapted from NRC + * @author Alireza Fathi + * @author Frank Dellaert + */ +#include +#include /* for 'fabs' */ +#include +#include + +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; + +} + +/* ************************************************************************* */ diff --git a/cpp/svdcmp.h b/cpp/svdcmp.h new file mode 100644 index 000000000..aa3d87b48 --- /dev/null +++ b/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); + diff --git a/cpp/testCal3_S2.cpp b/cpp/testCal3_S2.cpp new file mode 100644 index 000000000..1cd1d14ce --- /dev/null +++ b/cpp/testCal3_S2.cpp @@ -0,0 +1,49 @@ +/** + * @file testCal3_S2.cpp + * @brief Unit tests for transform derivatives + */ + +#include +#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); +} +/* ************************************************************************* */ + diff --git a/cpp/testCalibratedCamera.cpp b/cpp/testCalibratedCamera.cpp new file mode 100644 index 000000000..55b3f252c --- /dev/null +++ b/cpp/testCalibratedCamera.cpp @@ -0,0 +1,89 @@ +/** + * Frank Dellaert + * brief: test CalibratedCamera class + * based on testVSLAMFactor.cpp + */ + +#include + +#include +#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; } +/* ************************************************************************* */ + + diff --git a/cpp/testChordalBayesNet.cpp b/cpp/testChordalBayesNet.cpp new file mode 100644 index 000000000..142d13fa6 --- /dev/null +++ b/cpp/testChordalBayesNet.cpp @@ -0,0 +1,130 @@ +/** + * @file testChordalBayesNet.cpp + * @brief Unit tests for ChordalBayesNet + * @author Frank Dellaert + */ + + +// STL/C++ +#include +#include +#include +#include +#include + +#ifdef HAVE_BOOST_SERIALIZATION +#include +#include +#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 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);} +/* ************************************************************************* */ diff --git a/cpp/testConditionalGaussian.cpp b/cpp/testConditionalGaussian.cpp new file mode 100644 index 000000000..892af2f0d --- /dev/null +++ b/cpp/testConditionalGaussian.cpp @@ -0,0 +1,136 @@ +/** + * @file testConditionalGaussian.cpp + * @brief Unit tests for Conditional gaussian + * @author Christian Potthast + **/ + +/*STL/C++*/ +#include +#include +#include + +#ifdef HAVE_BOOST_SERIALIZATION +#include +#include +#endif //HAVE_BOOST_SERIALIZATION + +#include "Matrix.h" +#include "ConditionalGaussian.h" + + +using namespace gtsam; + +/* ************************************************************************* */ +/* untit test for equals */ +/* ************************************************************************* */ +TEST( ConditionalGaussian, equals ) +{ + // create a conditional gaussion node + Matrix A1(2,2); + A1(0,0) = 1 ; A1(1,0) = 2; + A1(0,1) = 3 ; A1(1,1) = 4; + + Matrix A2(2,2); + A2(0,0) = 6 ; A2(1,0) = 0.2; + A2(0,1) = 8 ; A2(1,1) = 0.4; + + Matrix R(2,2); + R(0,0) = 0.1 ; R(1,0) = 0.3; + R(0,1) = 0.0 ; R(1,1) = 0.34; + + Vector d(2); + d(0) = 0.2; d(1) = 0.5; + + ConditionalGaussian + expected(d, R, "x1", A1, "l1", A2), + actual(d, R, "x1", A1, "l1", A2); + + CHECK( expected.equals(actual) ); + +} + +/* ************************************************************************* */ +/* untit test for solve */ +/* ************************************************************************* */ +TEST( ConditionalGaussian, solve ) +{ + + //expected solution + Vector expected(2); + expected(0) = 15.0471 ; expected(1) = -18.8824; + + // create a conditional gaussion node + Matrix A1 = Matrix_(2,2, 1., 2., + 3., 4.); + + Matrix A2 = Matrix_(2,2, 6., 0.2, + 8., 0.4); + + Matrix R = Matrix_(2,2, 0.1, 0.3, + 0.0, 0.34); + + Vector d(2); + d(0) = 0.2; d(1) = 0.5; + + ConditionalGaussian cg(d, R, "x1", A1, "l1", A2); + + Vector sx1(2); + sx1(0) = 0.2; sx1(1) = 0.5; + + Vector sl1(2); + sl1(0) = 0.5; sl1(1) = 0.8; + + FGConfig solution; + solution.insert("x1", sx1); + solution.insert("l1", sl1); + + Vector result = cg.solve(solution); + + CHECK( equal_with_abs_tol(expected , result, 0.0001)); + +} + +/* ************************************************************************* */ +/* unit test for serialization */ +/* ************************************************************************* */ +#ifdef HAVE_BOOST_SERIALIZATION +TEST( ConditionalGaussian, serialize ) +{ +// // create a conditional gaussion node +// Matrix A1(2,2); +// A1(0,0) = 1 ; A1(1,0) = 2; +// A1(0,1) = 3 ; A1(1,1) = 4; +// +// Matrix A2(2,2); +// A2(0,0) = 6 ; A2(1,0) = 0.2; +// A2(0,1) = 8 ; A2(1,1) = 0.4; +// +// Matrix R(2,2); +// R(0,0) = 0.1 ; R(1,0) = 0.3; +// R(0,1) = 0.0 ; R(1,1) = 0.34; +// +// Vector d(2); +// d(0) = 0.2; d(1) = 0.5; +// +// ConditionalGaussian cg(d, R, "x1", A1, "l1", A2); +// +// //serialize the CG +// std::ostringstream in_archive_stream; +// boost::archive::text_oarchive in_archive(in_archive_stream); +// in_archive << cg; +// std::string serialized = in_archive_stream.str(); +// +// //deserialize the CGg +// std::istringstream out_archive_stream(serialized); +// boost::archive::text_iarchive out_archive(out_archive_stream); +// ConditionalGaussian output; +// out_archive >> output; +// +// //check for equality +// CHECK(cg.equals(output)); + +} +#endif //HAVE_BOOST_SERIALIZATION +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/cpp/testConstrainedChordalBayesNet.cpp b/cpp/testConstrainedChordalBayesNet.cpp new file mode 100644 index 000000000..e10bb681d --- /dev/null +++ b/cpp/testConstrainedChordalBayesNet.cpp @@ -0,0 +1,115 @@ +/* + * testConstrainedChordalBayesNet.cpp + * + * Created on: Aug 11, 2009 + * Author: alexgc + */ + +#include +#include +#include "ConstrainedChordalBayesNet.h" +#include "smallExample.h" + +using namespace gtsam; +using namespace std; + +TEST ( ConstrainedChordalBayesNet, basic ) +{ + ConstrainedChordalBayesNet ccbn = createConstrainedChordalBayesNet(); + FGConfig c = createConstrainedConfig(); + + // get data back out + DeltaFunction::shared_ptr x0 = ccbn.get_delta("x0"); + ConditionalGaussian::shared_ptr x1 = ccbn.get("x1"); + + Matrix R = eye(2); + Vector d = c["x1"]; + double sigma = 0.1; + ConditionalGaussian::shared_ptr f1(new ConditionalGaussian(d/sigma, R/sigma)); + + DeltaFunction::shared_ptr f2(new DeltaFunction(c["x0"], "x0")); + + CHECK(f1->equals(*x1)); + CHECK(f2->equals(*x0)); +} + +TEST ( ConstrainedChordalBayesNet, equals ) +{ + // basic check + ConstrainedChordalBayesNet ccbn1 = createConstrainedChordalBayesNet(); + ConstrainedChordalBayesNet ccbn2 = createConstrainedChordalBayesNet(); + CHECK(ccbn1.equals(ccbn2)); + + // ensure deltas are compared + ConstrainedChordalBayesNet ccbn3; + FGConfig c = createConstrainedConfig(); + Matrix R = eye(2); + Vector d = c["x1"]; + double sigma = 0.1; + ConditionalGaussian::shared_ptr f1(new ConditionalGaussian(d/sigma, R/sigma)); + ccbn3.insert("x1", f1); + + CHECK(!ccbn1.equals(ccbn3)); +} + +TEST ( ConstrainedChordalBayesNet, copy ) +{ + // use copy to allow testing with old example + ChordalBayesNet cbn = createSmallChordalBayesNet(); + ConstrainedChordalBayesNet actual(cbn); + + ConditionalGaussian::shared_ptr x = cbn.get("x"); + ConditionalGaussian::shared_ptr y = cbn.get("y"); + + ConstrainedChordalBayesNet expected; + expected.insert("x",x); + expected.insert("y",y); + + CHECK(assert_equal(actual, expected)); +} + +TEST ( ConstrainedChordalBayesNet, optimize_baseline ) +{ + // optimize simple example + ChordalBayesNet cbn = createSmallChordalBayesNet(); + ConstrainedChordalBayesNet ccbn(cbn); + boost::shared_ptr actual = ccbn.optimize(); + + // create expected + FGConfig expected; + Vector x(1), y(1); x(0)=4.; y(0)=5.; + expected.insert("x", x); + expected.insert("y", y); + + // verify + CHECK(expected.equals(*actual)); +} + +TEST ( ConstrainedChordalBayesNet, optimize ) +{ + ConstrainedChordalBayesNet ccbn = createConstrainedChordalBayesNet(); + FGConfig expected = createConstrainedConfig(); + + // full optimization + boost::shared_ptr actual1 = ccbn.optimize(); + CHECK(expected.equals(*actual1)); + + // plug in a config + boost::shared_ptr c1(new FGConfig); + c1->insert("x0", expected["x0"]); + boost::shared_ptr actual2 = ccbn.optimize(c1); + CHECK(expected.equals(*actual2)); + + // plug in the other value + boost::shared_ptr c2(new FGConfig); + c2->insert("x1", expected["x1"]); + boost::shared_ptr actual3 = ccbn.optimize(c2); + CHECK(expected.equals(*actual3)); +} + + + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ + diff --git a/cpp/testConstrainedLinearFactorGraph.cpp b/cpp/testConstrainedLinearFactorGraph.cpp new file mode 100644 index 000000000..b8dcc5c1e --- /dev/null +++ b/cpp/testConstrainedLinearFactorGraph.cpp @@ -0,0 +1,272 @@ +/* + * testConstrainedLinearFactorGraph.cpp + * + * Created on: Aug 10, 2009 + * Author: Alex Cunningham + */ + + +#include +#include "ConstrainedLinearFactorGraph.h" +#include "LinearFactorGraph.h" +#include "smallExample.h" + +using namespace gtsam; +using namespace std; + +TEST( ConstrainedLinearFactorGraph, basic ) +{ + ConstrainedLinearFactorGraph fg = createConstrainedLinearFactorGraph(); + + // expected equality factor + Vector v1(2); v1(0)=1.;v1(1)=2.; + EqualityFactor::shared_ptr f1(new EqualityFactor(v1, "x0")); + + // expected normal linear factor + 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) = 20 ; b(1) = 30; + + LinearFactor::shared_ptr f2(new LinearFactor("x0", A21, "x1", A22, b)); + + CHECK(f2->equals(*(fg[0]))); + CHECK(f1->equals(*(fg.eq_at(0)))); +} + +TEST ( ConstrainedLinearFactorGraph, copy ) +{ + LinearFactorGraph lfg = createLinearFactorGraph(); + LinearFactor::shared_ptr f1 = lfg[0]; + LinearFactor::shared_ptr f2 = lfg[1]; + LinearFactor::shared_ptr f3 = lfg[2]; + LinearFactor::shared_ptr f4 = lfg[3]; + + ConstrainedLinearFactorGraph actual(lfg); + + ConstrainedLinearFactorGraph expected; + expected.push_back(f1); + expected.push_back(f2); + expected.push_back(f3); + expected.push_back(f4); + + CHECK(actual.equals(expected)); +} + +TEST( ConstrainedLinearFactorGraph, equals ) +{ + // basic equality test + ConstrainedLinearFactorGraph fg = createConstrainedLinearFactorGraph(); + ConstrainedLinearFactorGraph fg2 = createConstrainedLinearFactorGraph(); + CHECK( fg.equals(fg2) ); + + // ensuring that equality factors are compared + LinearFactor::shared_ptr f2 = fg[0]; // get a linear factor from existing graph + ConstrainedLinearFactorGraph fg3; + fg3.push_back(f2); + CHECK( !fg3.equals(fg) ); +} + +TEST( ConstrainedLinearFactorGraph, size ) +{ + LinearFactorGraph lfg = createLinearFactorGraph(); + ConstrainedLinearFactorGraph fg1(lfg); + + CHECK(fg1.size() == lfg.size()); + + ConstrainedLinearFactorGraph fg2 = createConstrainedLinearFactorGraph(); + + CHECK(fg2.size() == 2); +} + +TEST( ConstrainedLinearFactorGraph, involves_equality ) +{ + ConstrainedLinearFactorGraph fg = createConstrainedLinearFactorGraph(); + + CHECK(fg.involves_equality("x0")); + CHECK(!fg.involves_equality("x1")); +} + +TEST( ConstrainedLinearFactorGraph, optimize ) +{ + ConstrainedLinearFactorGraph fg1 = createConstrainedLinearFactorGraph(); + ConstrainedLinearFactorGraph fg2 = createConstrainedLinearFactorGraph(); + + FGConfig expected = createConstrainedConfig(); + + Ordering ord1; + ord1.push_back("x0"); + ord1.push_back("x1"); + + Ordering ord2; + ord2.push_back("x1"); + ord2.push_back("x0"); + + FGConfig actual1 = fg1.optimize(ord1); + FGConfig actual2 = fg2.optimize(ord2); + + CHECK(actual1.equals(expected)); + CHECK(actual1.equals(actual2)); +} + +TEST (ConstrainedLinearFactorGraph, eliminate ) +{ + ConstrainedLinearFactorGraph fg = createConstrainedLinearFactorGraph(); + FGConfig c = createConstrainedConfig(); + + Ordering ord1; + ord1.push_back("x0"); + ord1.push_back("x1"); + + ConstrainedChordalBayesNet::shared_ptr actual = fg.eliminate(ord1); + + // create an expected bayes net + ConstrainedChordalBayesNet::shared_ptr expected(new ConstrainedChordalBayesNet); + + DeltaFunction::shared_ptr d(new DeltaFunction(c["x0"], "x0")); + expected->insert_df("x0", d); + + Matrix A = eye(2); + double sigma = 0.1; + Vector dv = c["x1"]; + ConditionalGaussian::shared_ptr cg(new ConditionalGaussian(dv/sigma, A/sigma)); + expected->insert("x1", cg); + + CHECK(actual->equals(*expected)); +} + +TEST (ConstrainedLinearFactorGraph, baseline_optimize) +{ + // tests performance when there are no equality factors in the graph + LinearFactorGraph lfg = createLinearFactorGraph(); + ConstrainedLinearFactorGraph clfg(lfg); // copy in the linear factor graph + + Ordering ord; + ord.push_back("l1"); + ord.push_back("x1"); + ord.push_back("x2"); + + FGConfig actual = clfg.optimize(ord); + + FGConfig expected = lfg.optimize(ord); // should be identical to regular lfg optimize + + CHECK(actual.equals(expected)); +} + +TEST (ConstrainedLinearFactorGraph, baseline_eliminate_one ) +{ + LinearFactorGraph fg = createLinearFactorGraph(); + ConstrainedLinearFactorGraph cfg(fg); + + ConditionalGaussian::shared_ptr actual = cfg.eliminate_one("x1"); + + // create expected Conditional Gaussian + Matrix R11 = Matrix_(2,2, + 15.0, 00.0, + 00.0, 15.0 + ); + Matrix S12 = Matrix_(2,2, + -1.66667, 0.00, + +0.00,-1.66667 + ); + Matrix S13 = Matrix_(2,2, + -6.66667, 0.00, + +0.00,-6.66667 + ); + Vector d(2); d(0) = -2; d(1) = -1.0/3.0; + ConditionalGaussian expected(d,R11,"l1",S12,"x2",S13); + + CHECK( actual->equals(expected) ); +} + +TEST (ConstrainedLinearFactorGraph, eliminate_one_eq) +{ + ConstrainedLinearFactorGraph fg = createConstrainedLinearFactorGraph(); + DeltaFunction::shared_ptr actual = fg.eliminate_one_eq("x0"); + + FGConfig c = createConstrainedConfig(); + DeltaFunction::shared_ptr expected(new DeltaFunction(c["x0"], "x0")); + + CHECK(assert_equal(*actual, *expected)); // check output for correct delta function + + CHECK(fg.size() == 1); // check size + + ConstrainedLinearFactorGraph::eq_const_iterator eit = fg.eq_begin(); + CHECK(eit == fg.eq_end()); // ensure no remaining equality factors + + // verify the remaining factor - should be a unary factor on x1 + ConstrainedLinearFactorGraph::const_iterator it = fg.begin(); + LinearFactor::shared_ptr factor_actual = *it; + + CHECK(factor_actual->size() == 1); +} + +TEST (ConstrainedLinearFactorGraph, eq_combine_and_eliminate ) +{ + // create a set of factors + ConstrainedLinearFactorGraph fg = createConstrainedLinearFactorGraph(); + EqualityFactor::shared_ptr eq = fg.eq_at(0); + LinearFactor::shared_ptr f1 = fg[0]; + + // make a joint linear factor + set f1_set; + f1_set.insert(f1); + boost::shared_ptr joined(new MutableLinearFactor(f1_set)); + + // create a sample graph + ConstrainedLinearFactorGraph graph; + + // combine linear factor and eliminate + graph.eq_combine_and_eliminate(*eq, *joined); + + // verify structure + CHECK(graph.size() == 1); // will have only one factor + LinearFactor::shared_ptr actual = graph[0]; + CHECK(actual->size() == 1); // remaining factor will be unary + + // verify values + FGConfig c = createConstrainedConfig(); + Vector exp_v = c["x1"]; + Matrix A = actual->get_A("x1"); + Vector b = actual->get_b(); + Vector act_v = backsubstitution(A, b); + CHECK(assert_equal(act_v, exp_v)); +} + +TEST (ConstrainedLinearFactorGraph, extract_eq) +{ + ConstrainedLinearFactorGraph fg = createConstrainedLinearFactorGraph(); + EqualityFactor::shared_ptr actual = fg.extract_eq("x0"); + + Vector v1(2); v1(0)=1.;v1(1)=2.; + EqualityFactor::shared_ptr expected(new EqualityFactor(v1, "x0")); + + // verify output + CHECK(assert_equal(*actual, *expected)); + + // verify removal + ConstrainedLinearFactorGraph::eq_const_iterator it = fg.eq_begin(); + CHECK(it == fg.eq_end()); + + // verify full size + CHECK(fg.size() == 1); +} + +TEST( ConstrainedLinearFactorGraph, GET_ORDERING) +{ + ConstrainedLinearFactorGraph fg = createConstrainedLinearFactorGraph(); + Ordering ord = fg.getOrdering(); + CHECK(ord[0] == string("x0")); + CHECK(ord[1] == string("x1")); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ + diff --git a/cpp/testConstrainedNonlinearFactorGraph.cpp b/cpp/testConstrainedNonlinearFactorGraph.cpp new file mode 100644 index 000000000..eb0e67e16 --- /dev/null +++ b/cpp/testConstrainedNonlinearFactorGraph.cpp @@ -0,0 +1,79 @@ +/* + * testConstrainedNonlinearFactorGraph.cpp + * + * Created on: Aug 10, 2009 + * Author: Alex Cunningham + */ + + +#include +#include "ConstrainedNonlinearFactorGraph.h" +#include "smallExample.h" + +using namespace gtsam; + +typedef boost::shared_ptr shared; + +TEST( ConstrainedNonlinearFactorGraph, equals ) +{ + ConstrainedNonlinearFactorGraph fg = createConstrainedNonlinearFactorGraph(); + ConstrainedNonlinearFactorGraph fg2 = createConstrainedNonlinearFactorGraph(); + CHECK( fg.equals(fg2) ); +} + +TEST( ConstrainedNonlinearFactorGraph, copy ) +{ + NonlinearFactorGraph nfg = createNonlinearFactorGraph(); + ConstrainedNonlinearFactorGraph actual(nfg); + + shared f1 = nfg[0]; + shared f2 = nfg[1]; + shared f3 = nfg[2]; + shared f4 = nfg[3]; + + ConstrainedNonlinearFactorGraph expected; + expected.push_back(f1); + expected.push_back(f2); + expected.push_back(f3); + expected.push_back(f4); + + CHECK(actual.equals(expected)); +} + +TEST( ConstrainedNonlinearFactorGraph, baseline ) +{ + // use existing examples + NonlinearFactorGraph nfg = createNonlinearFactorGraph(); + ConstrainedNonlinearFactorGraph cfg(nfg); + + FGConfig initial = createNoisyConfig(); + ConstrainedLinearFactorGraph linearized = cfg.linearize(initial); + LinearFactorGraph lfg = createLinearFactorGraph(); + ConstrainedLinearFactorGraph expected(lfg); + + CHECK(expected.equals(linearized)); +} + +TEST( ConstrainedNonlinearFactorGraph, convert ) +{ + NonlinearFactorGraph expected = createNonlinearFactorGraph(); + ConstrainedNonlinearFactorGraph cfg(expected); + NonlinearFactorGraph actual = cfg.convert(); + CHECK(actual.equals(expected)); +} + +TEST( ConstrainedNonlinearFactorGraph, linearize_and_solve ) +{ + ConstrainedNonlinearFactorGraph nfg = createConstrainedNonlinearFactorGraph(); + FGConfig lin = createConstrainedLinConfig(); + ConstrainedLinearFactorGraph actual_lfg = nfg.linearize(lin); + FGConfig actual = actual_lfg.optimize(actual_lfg.getOrdering()); + + FGConfig expected = createConstrainedCorrectDelta(); + CHECK(actual.equals(expected)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ + diff --git a/cpp/testDeltaFunction.cpp b/cpp/testDeltaFunction.cpp new file mode 100644 index 000000000..b1e561d83 --- /dev/null +++ b/cpp/testDeltaFunction.cpp @@ -0,0 +1,27 @@ +/* + * testDeltaFunction.cpp + * + * Created on: Aug 11, 2009 + * Author: alexgc + */ + + +#include +#include "DeltaFunction.h" + +using namespace gtsam; + +TEST (DeltaFunction, basic) +{ + Vector v(2); v(0)=1.0; v(1)=2.0; + + DeltaFunction delta(v, "x"); + + CHECK(assert_equal(v, delta.get_value())); +} + + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ + diff --git a/cpp/testEqualityFactor.cpp b/cpp/testEqualityFactor.cpp new file mode 100644 index 000000000..d0fe0c316 --- /dev/null +++ b/cpp/testEqualityFactor.cpp @@ -0,0 +1,62 @@ +/* + * testEqualityFactor.cpp + * + * Created on: Aug 10, 2009 + * Author: Alex Cunningham + */ + + +#include +#include "EqualityFactor.h" +#include "smallExample.h" + +using namespace gtsam; +using namespace std; + +TEST ( EqualityFactor, basic ) +{ + // create an initialized factor + Vector v(2); v(0)=1.2; v(1)=3.4; + string key = "x0"; + EqualityFactor factor(v, key); + + // get the data back out of it + CHECK(assert_equal(v, factor.get_value())); + CHECK(key == factor.get_key()); +} + +TEST ( EqualityFactor, equals ) +{ + Vector v(2); v(0)=1.2; v(1)=3.4; + string key = "x0"; + EqualityFactor factor1(v, key); + EqualityFactor factor2(v, key); + CHECK(factor1.equals(factor2)); +} + +TEST (EqualityFactor, getDeltaFunction ) +{ + Vector v(2); v(0)=1.2; v(1)=3.4; + string key = "x0"; + EqualityFactor factor(v, key); + + DeltaFunction::shared_ptr actual = factor.getDeltaFunction(); + + DeltaFunction::shared_ptr expected(new DeltaFunction(v, key)); + CHECK(assert_equal(*actual, *expected)); +} + +TEST (EqualityFactor, linearize ) +{ + FGConfig c = createConstrainedConfig(); + EqualityFactor init(c["x0"], "x0"); + EqualityFactor::shared_ptr actual = init.linearize(); + EqualityFactor::shared_ptr expected(new EqualityFactor(zero(2), "x0")); + CHECK(assert_equal(*actual, *expected)); +} + + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ + diff --git a/cpp/testFGConfig.cpp b/cpp/testFGConfig.cpp new file mode 100644 index 000000000..a0e96d3cb --- /dev/null +++ b/cpp/testFGConfig.cpp @@ -0,0 +1,99 @@ +/** + * @file testFGConfig.cpp + * @brief Unit tests for Factor Graph Configuration + * @author Carlos Nieto + **/ + +/*STL/C++*/ +#include +#include + +//#include TEST_AC_DEFINE + +#ifdef HAVE_BOOST_SERIALIZATION +#include +#include +#endif //HAVE_BOOST_SERIALIZATION + +#include +#include "Matrix.h" +#include "FGConfig.h" +#include "smallExample.cpp" + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST( FGConfig, equals ) + { + FGConfig expected; + Vector v = Vector_(3, 5.0, 6.0, 7.0); + expected.insert("a",v); + FGConfig actual; + actual.insert("a",v); + CHECK(actual.equals(expected)); + } + +/* ************************************************************************* */ +TEST( FGConfig, contains) +{ + FGConfig fg; + Vector v = Vector_(3, 5.0, 6.0, 7.0); + fg.insert("ali", v); + CHECK(fg.contains("ali")); + CHECK(!fg.contains("gholi")); +} + +/* ************************************************************************* */ +TEST( FGConfig, plus) +{ + FGConfig fg; + Vector vx = Vector_(3, 5.0, 6.0, 7.0), vy = Vector_(2, 8.0, 9.0); + fg.insert("x", vx).insert("y",vy); + + FGConfig delta; + Vector dx = Vector_(3, 1.0, 1.0, 1.0), dy = Vector_(2, -1.0, -1.0); + delta.insert("x", dx).insert("y",dy); + + FGConfig expected; + Vector wx = Vector_(3, 6.0, 7.0, 8.0), wy = Vector_(2, 7.0, 8.0); + expected.insert("x", wx).insert("y",wy); + + // functional + FGConfig actual = fg + delta; + CHECK(actual.equals(expected)); + + // inplace + fg += delta; + CHECK(fg.equals(expected)); +} + +/* ************************************************************************* */ +#ifdef HAVE_BOOST_SERIALIZATION +TEST( FGConfig, serialize) +{ + //DEBUG: + cout << "FGConfig: Running Serialization Test" << endl; + + //create an FGConfig + FGConfig fg = createConfig(); + + //serialize the config + std::ostringstream in_archive_stream; + boost::archive::text_oarchive in_archive(in_archive_stream); + in_archive << fg; + std::string serialized_fgc = in_archive_stream.str(); + + //deserialize the config + std::istringstream out_archive_stream(serialized_fgc); + boost::archive::text_iarchive out_archive(out_archive_stream); + FGConfig output; + out_archive >> output; + + //check for equality + CHECK(fg.equals(output)); +} +#endif //HAVE_BOOST_SERIALIZATION +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/cpp/testFactorgraph.cpp b/cpp/testFactorgraph.cpp new file mode 100644 index 000000000..6e00c1a57 --- /dev/null +++ b/cpp/testFactorgraph.cpp @@ -0,0 +1,17 @@ +/** + * @file testFactorgraph.cpp + * @brief Unit tests for Factor Graphs + * @author Christian Potthast + **/ + +/*STL/C++*/ +#include +#include +using namespace std; + +#include +#include "smallExample.h" + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/cpp/testLinearFactor.cpp b/cpp/testLinearFactor.cpp new file mode 100644 index 000000000..93f73534c --- /dev/null +++ b/cpp/testLinearFactor.cpp @@ -0,0 +1,419 @@ +/** + * @file testLinearFactor.cpp + * @brief Unit tests for Linear Factor + * @author Christian Potthast + **/ + +/*STL/C++*/ +#include +using namespace std; + +#include +#include + +#include "Matrix.h" +#include "smallExample.h" + +using namespace gtsam; + +/* ************************************************************************* */ +TEST( LinearFactor, linearFactor ) +{ + Matrix A1(2,2); + A1(0,0) = -10.0 ; A1(0,1) = 0.0; + A1(1,0) = 0.0 ; A1(1,1) = -10.0; + + Matrix A2(2,2); + A2(0,0) = 10.0 ; A2(0,1) = 0.0; + A2(1,0) = 0.0 ; A2(1,1) = 10.0; + + Vector b(2); + b(0) = 2 ; b(1) = -1; + + LinearFactor expected("x1", A1, "x2", A2, b); + + // create a small linear factor graph + LinearFactorGraph fg = createLinearFactorGraph(); + + // get the factor "f2" from the factor graph + LinearFactor::shared_ptr lf = fg[1]; + + // check if the two factors are the same + CHECK( lf->equals(expected) ); +} + +/* ************************************************************************* */ +TEST( LinearFactor, variables ) +{ + // get the factor "f2" from the small linear factor graph + LinearFactorGraph fg = createLinearFactorGraph(); + LinearFactor::shared_ptr lf = fg[1]; + VariableSet vs = lf->variables(); +} + +/* ************************************************************************* */ + +// NOTE: This test does not pass when running on Linux (Ubuntu 9.04, GCC 4.3.3) +// systems, and appears to be dependent on the ordering of factors in the +// set lfg. Reversing the order in which the matrices are appended will +// make this test work correctly, while the next test will fail. +TEST( LinearFactor, linearFactor2 ) +{ + // create a small linear factor graph + LinearFactorGraph fg = createLinearFactorGraph(); + + // get two factors from it and insert the factors into a set + std::set lfg; + lfg.insert(fg[4 - 1]); + lfg.insert(fg[2 - 1]); + + // combine in a factor + MutableLinearFactor combined(lfg); + + // the expected combined linear factor + Matrix Ax2 = Matrix_(4, 2, // x2 + -5., 0., + +0., -5., + 10., 0., + +0., 10.); + + Matrix Al1 = Matrix_(4, 2, // l1 + 5., 0., + 0., 5., + 0., 0., + 0., 0.); + + Matrix Ax1 = Matrix_(4, 2, // x1 + 0.00, 0., // f4 + 0.00, 0., // f4 + -10., 0., // f2 + 0.00, -10. // f2 + ); + + // the RHS + Vector b2(4); + b2(0) = -1; + b2(1) = 1.5; + b2(2) = 2; + b2(3) = -1; + + LinearFactor expected("x2", Ax2, "l1", Al1, "x1", Ax1, b2); + CHECK(combined.equals(expected)); +} + +/* ************************************************************************* */ +TEST( NonlinearFactorGraph, linearFactor3){ + + Matrix A11(2,2); + A11(0,0) = 10.4545; A11(0,1) = 0; + A11(1,0) = 0; A11(1,1) = 10.4545; + Vector b(2); + b(0) = 2 ; b(1) = -1; + LinearFactor::shared_ptr f1(new LinearFactor("x1", A11, b)); + + A11(0,0) = 2; A11(0,1) = 0; + A11(1,0) = 0; A11(1,1) = -2; + b(0) = 4 ; b(1) = -5; + LinearFactor::shared_ptr f2(new LinearFactor("x1", A11, b)); + + A11(0,0) = 4; A11(0,1) = 0; + A11(1,0) = 0; A11(1,1) = -4; + b(0) = 3 ; b(1) = -88; + LinearFactor::shared_ptr f3(new LinearFactor("x1", A11, b)); + + A11(0,0) = 6; A11(0,1) = 0; + A11(1,0) = 0; A11(1,1) = 7; + b(0) = 5 ; b(1) = -6; + LinearFactor::shared_ptr f4(new LinearFactor("x1", A11, b)); + + std::set lfg; + lfg.insert(f1); + lfg.insert(f2); + lfg.insert(f3); + lfg.insert(f4); + MutableLinearFactor combined(lfg); + + Matrix A22(8,2); + A22(0,0) = 10.4545; A22(0,1) = 0; + A22(1,0) = 0; A22(1,1) = 10.4545; + A22(2,0) = 2; A22(2,1) = 0; + A22(3,0) = 0; A22(3,1) = -2; + A22(4,0) = 4; A22(4,1) = 0; + A22(5,0) = 0; A22(5,1) = -4; + A22(6,0) = 6; A22(6,1) = 0; + A22(7,0) = 0; A22(7,1) = 7; + Vector exb(8); + exb(0) = 2 ; exb(1) = -1; exb(2) = 4 ; exb(3) = -5; + exb(4) = 3 ; exb(5) = -88; exb(6) = 5 ; exb(7) = -6; + LinearFactor::shared_ptr expected(new LinearFactor("x1", A22, exb)); + + CHECK( combined.equals(*expected) ); // currently fails on linux, see previous test's note +} + +/* ************************************************************************* */ +TEST( LinearFactor, error ) +{ + // create a small linear factor graph + LinearFactorGraph fg = createLinearFactorGraph(); + + // get the first factor from the factor graph + LinearFactor::shared_ptr lf = fg[0]; + + // check the error of the first factor with nosiy config + FGConfig cfg = createZeroDelta(); + + // calculate the error from the factor "f1" + // note the error is the same as in testNonlinearFactor + double actual = lf->error(cfg); + DOUBLES_EQUAL( 1.0, actual, 0.00000001 ); +} + +/* ************************************************************************* */ +TEST( LinearFactor, eliminate ) +{ + // the combined linear factor + Matrix Ax2 = Matrix_(4,2, + // x2 + -5., 0., + +0.,-5., + 10., 0., + +0.,10. + ); + + Matrix Al1 = Matrix_(4,2, + // l1 + 5., 0., + 0., 5., + 0., 0., + 0., 0. + ); + + Matrix Ax1 = Matrix_(4,2, + // x1 + 0.00, 0., // f4 + 0.00, 0., // f4 + -10., 0., // f2 + 0.00,-10. // f2 + ); + + // the RHS + Vector b2(4); + b2(0) = -1; + b2(1) = 1.5; + b2(2) = 2; + b2(3) = -1; + + MutableLinearFactor combined("x2", Ax2, "l1", Al1, "x1", Ax1, b2); + + // eliminate the combined factor + + ConditionalGaussian::shared_ptr actualCG; + LinearFactor::shared_ptr actualLF; + boost::tie(actualCG,actualLF) = combined.eliminate("x2"); + + // create expected Conditional Gaussian + Matrix R11 = Matrix_(2,2, + 11.1803, 0.00, + 0.00, 11.1803 + ); + Matrix S12 = Matrix_(2,2, + -2.23607, 0.00, + +0.00,-2.23607 + ); + Matrix S13 = Matrix_(2,2, + -8.94427, 0.00, + +0.00,-8.94427 + ); + Vector d(2); d(0) = 2.23607; d(1) = -1.56525; + ConditionalGaussian expectedCG(d,R11,"l1",S12,"x1",S13); + + // the expected linear factor + Matrix Bl1 = Matrix_(2,2, + // l1 + 4.47214, 0.00, + 0.00, 4.47214 + ); + + Matrix Bx1 = Matrix_(2,2, + // x1 + -4.47214, 0.00, + +0.00, -4.47214 + ); + + // the RHS + Vector b1(2); b1(0) = 0.0; b1(1) = 0.894427; + + LinearFactor expectedLF("l1", Bl1, "x1", Bx1, b1); + + // check if the result matches + CHECK(actualCG->equals(expectedCG)); + CHECK(actualLF->equals(expectedLF,1e-5)); +} + + +/* ************************************************************************* */ +TEST( LinearFactor, eliminate2 ) +{ + // the combined linear factor + Matrix Ax2 = Matrix_(4,2, + // x2 + -5., 0., + +0.,-5., + 10., 0., + +0.,10. + ); + + Matrix Al1x1 = Matrix_(4,4, + // l1 x1 + 5., 0., 0.00, 0., // f4 + 0., 5., 0.00, 0., // f4 + 0., 0., -10., 0., // f2 + 0., 0., 0.00,-10. // f2 + ); + + // the RHS + Vector b2(4); + b2(0) = -1; + b2(1) = 1.5; + b2(2) = 2; + b2(3) = -1; + + MutableLinearFactor combined("x2", Ax2, "l1x1", Al1x1, b2); + + // eliminate the combined factor + ConditionalGaussian::shared_ptr actualCG; + LinearFactor::shared_ptr actualLF; + boost::tie(actualCG,actualLF) = combined.eliminate("x2"); + + // create expected Conditional Gaussian + Matrix R11 = Matrix_(2,2, + 11.1803, 0.00, + 0.00, 11.1803 + ); + Matrix S12 = Matrix_(2,4, + -2.23607, 0.00,-8.94427, 0.00, + +0.00,-2.23607,+0.00,-8.94427 + ); + Vector d(2); d(0) = 2.23607; d(1) = -1.56525; + ConditionalGaussian expectedCG(d,R11,"l1x1",S12); + + // the expected linear factor + Matrix Bl1x1 = Matrix_(2,4, + // l1 x1 + 4.47214, 0.00, -4.47214, 0.00, + 0.00, 4.47214, +0.00, -4.47214 + ); + + // the RHS + Vector b1(2); b1(0) = 0.0; b1(1) = 0.894427; + + LinearFactor expectedLF("l1x1", Bl1x1, b1); + + // check if the result matches + CHECK(actualCG->equals(expectedCG)); + CHECK(actualLF->equals(expectedLF,1e-5)); +} + +//* ************************************************************************* */ +TEST( LinearFactor, default_error ) +{ + MutableLinearFactor f; + FGConfig c; + double actual = f.error(c); + CHECK(actual==0.0); +} + +//* ************************************************************************* */ +TEST( LinearFactor, eliminate_empty ) +{ + // create an empty factor + MutableLinearFactor f; + + // eliminate the empty factor + ConditionalGaussian::shared_ptr actualCG; + LinearFactor::shared_ptr actualLF; + boost::tie(actualCG,actualLF) = f.eliminate("x2"); + + // expected Conditional Gaussian is just a parent-less node with P(x)=1 + ConditionalGaussian expectedCG; + + // expected remaining factor is still empty :-) + MutableLinearFactor expectedLF; + + // check if the result matches + CHECK(actualCG->equals(expectedCG)); + CHECK(actualLF->equals(expectedLF)); +} + +//* ************************************************************************* */ +TEST( LinearFactor, empty ) +{ + // create an empty factor + MutableLinearFactor f; + CHECK(f.empty()==true); +} + +/* ************************************************************************* */ +TEST( LinearFactor, matrix ) +{ + // create a small linear factor graph + LinearFactorGraph fg = createLinearFactorGraph(); + + // get the factor "f2" from the factor graph + LinearFactor::shared_ptr lf = fg[1]; + + // render with a given ordering + Ordering ord; + ord.push_back("x1"); + ord.push_back("x2"); + + Matrix A; Vector b; + boost::tie(A,b) = lf->matrix(ord); + + Matrix A1 = Matrix_(2,4, + -10.0, 0.0, 10.0, 0.0, + 000.0,-10.0, 0.0, 10.0 ); + Vector b1 = Vector_(2, 2.0, -1.0); + + EQUALITY(A,A1); + EQUALITY(b,b1); +} + +/* ************************************************************************* */ +TEST( LinearFactor, size ) +{ + // create a linear factor graph + LinearFactorGraph fg = createLinearFactorGraph(); + + // get some factors from the graph + boost::shared_ptr factor1 = fg[0]; + boost::shared_ptr factor2 = fg[1]; + boost::shared_ptr factor3 = fg[2]; + + CHECK(factor1->size() == 1); + CHECK(factor2->size() == 2); + CHECK(factor3->size() == 2); +} + +/* ************************************************************************* */ +TEST( LinearFactor, CONSTRUCTOR_ConditionalGaussian ) +{ + Matrix R11 = Matrix_(2,2, + 11.1803, 0.00, + 0.00, 11.1803 + ); + Matrix S12 = Matrix_(2,2, + -2.23607, 0.00, + +0.00,-2.23607 + ); + Vector d(2); d(0) = 2.23607; d(1) = -1.56525; + ConditionalGaussian::shared_ptr CG(new ConditionalGaussian(d,R11,"l1x1",S12) ); + LinearFactor actualLF("x2",CG); + // actualLF.print(); + LinearFactor expectedLF("x2",R11,"l1x1",S12,d); + + CHECK(actualLF.equals(expectedLF)); +} +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/cpp/testLinearFactorGraph.cpp b/cpp/testLinearFactorGraph.cpp new file mode 100644 index 000000000..b25e2d701 --- /dev/null +++ b/cpp/testLinearFactorGraph.cpp @@ -0,0 +1,533 @@ +/** + * @file testLinearFactorGraph.cpp + * @brief Unit tests for Linear Factor Graph + * @author Christian Potthast + **/ + +/*STL/C++*/ +#include +using namespace std; + +#include +#include +#include +#include "Matrix.h" +#include "smallExample.h" + +using namespace gtsam; + +/* ************************************************************************* */ +/* unit test for equals (LinearFactorGraph1 == LinearFactorGraph2) */ +/* ************************************************************************* */ +TEST( LinearFactorGraph, equals ){ + + LinearFactorGraph fg = createLinearFactorGraph(); + LinearFactorGraph fg2 = createLinearFactorGraph(); + CHECK( fg.equals(fg2) ); +} + +/* ************************************************************************* */ +TEST( LinearFactorGraph, error ) +{ + LinearFactorGraph fg = createLinearFactorGraph(); + FGConfig cfg = createZeroDelta(); + + // note the error is the same as in testNonlinearFactorGraph as a + // zero delta config in the linear graph is equivalent to noisy in + // non-linear, which is really linear under the hood + double actual = fg.error(cfg); + DOUBLES_EQUAL( 5.625, actual, 1e-9 ); +} + +/* ************************************************************************* */ +/* unit test for find factors */ +/* ************************************************************************* */ +/* +TEST( LinearFactorGraph, find_factors ) +{ + int checksum = 0; + int expected = 16; + + LinearFactorGraph fg = createLinearFactorGraph(); + + // create a shared pointer object to prevent memory leaks + set > factors = fg.find_factors_and_remove("x2"); + + // CHECK whether find the right two factors + + // odometry between x1 and x2 + Matrix A21(2,2); + A21(0,0) = -10.0 ; A21(0,1) = 0.0; + A21(1,0) = 0.0 ; A21(1,1) = -10.0; + + Matrix A22(2,2); + A22(0,0) = 10.0 ; A22(0,1) = 0.0; + A22(1,0) = 0.0 ; A22(1,1) = 10.0; + + Vector b(2); + b(0) = 2 ; b(1) = -1; + + LinearFactor f2 = LinearFactor("x1", A21, "x2", A22, b); + + // measurement between x2 and l1 + Matrix A41(2,2); + A41(0,0) = -5 ; A41(0,1) = 0.0; + A41(1,0) = 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 f4 = LinearFactor("x2", A41, "l1", A42, b); + + set >::iterator it; + for( it = factors.begin() ; it != factors.end() ; it++ ){ + if( strcmp( it->get()->name_.c_str(), "f2") == 0){ + if(it->get()->equals(f2)){ + checksum = checksum + 2; + } + }else if( strcmp( it->get()->name_.c_str(), "f4") == 0){ + if(it->get()->equals(f4)){ + checksum = checksum + 4; + }else{ + } + } + } + + // CHECK if the factors are deleted from the factor graph + + // Create + LinearFactorGraph fg2; + + // prior on x1 + Matrix A11(2,2); + A11(0,0) = 10 ; A11(0,1) = 0; + A11(1,0) = 0 ; A11(1,1) = 10; + + b(0) = -1 ; b(1) = -1; + + LinearFactor *f1 = new LinearFactor("x1", A11, b); + fg2.push_back(f1); + + // 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 *f3 = new LinearFactor("x1", A31, "l1", A32, b); + fg2.push_back(f3); + + if( fg.equals(fg2) ){ + checksum = checksum + 10; + } + + CHECK( checksum == expected ); +} +*/ + +/* ************************************************************************* */ +/* unit test for find seperator */ +/* ************************************************************************* */ +TEST( LinearFactorGraph, find_separator ) +{ + LinearFactorGraph fg = createLinearFactorGraph(); + + set separator = fg.find_separator("x2"); + set expected; + expected.insert("x1"); + expected.insert("l1"); + + CHECK(separator.size()==expected.size()); + set::iterator it1 = separator.begin(), it2 = expected.begin(); + for(; it1!=separator.end(); it1++, it2++) + CHECK(*it1 == *it2); +} + +/* ************************************************************************* */ +// Note: This test does not pass when running on Linux(Ubuntu 9.04, GCC 4.3.3) +// systems. +TEST( LinearFactorGraph, combine_factors_x1 ) +{ + // create a small example for a linear factor graph + LinearFactorGraph fg = createLinearFactorGraph(); + + // combine all factors + LinearFactor::shared_ptr actual = fg.combine_factors("x1"); + + // the expected linear factor + Matrix Al1 = Matrix_(6,2, + 0., 0., + 0., 0., + 0., 0., + 0., 0., + 5., 0., + 0., 5. + ); + + Matrix Ax1 = Matrix_(6,2, + 10., 0., + 0.00, 10., + -10., 0., + 0.00,-10., + -5., 0., + 00., -5. + ); + + Matrix Ax2 = Matrix_(6,2, + 0., 0., + 0., 0., + 10., 0., + +0.,10., + 0., 0., + 0., 0. + ); + + // the expected RHS vector + Vector b(6); + b(0) = -1; + b(1) = -1; + b(2) = 2; + b(3) = -1; + b(4) = 0; + b(5) = 1; + + LinearFactor expected("l1", Al1, "x1", Ax1, "x2", Ax2, b); + + // check if the two factors are the same + CHECK(actual->equals(expected)); //currently fails +} + +/* ************************************************************************* */ +TEST( LinearFactorGraph, combine_factors_x2 ) +{ + // create a small example for a linear factor graph + LinearFactorGraph fg = createLinearFactorGraph(); + + // combine all factors + LinearFactor::shared_ptr actual = fg.combine_factors("x2"); + + // the expected linear factor + Matrix Al1 = Matrix_(4,2, + // l1 + 0., 0., + 0., 0., + 5., 0., + 0., 5. + ); + + Matrix Ax1 = Matrix_(4,2, + // x1 + -10., 0., // f2 + 0.00,-10., // f2 + 0.00, 0., // f4 + 0.00, 0. // f4 + ); + + Matrix Ax2 = Matrix_(4,2, + // x2 + 10., 0., + +0.,10., + -5., 0., + +0.,-5. + ); + + // the expected RHS vector + Vector b(4); + b(0) = 2; + b(1) = -1; + b(2) = -1; + b(3) = 1.5; + + LinearFactor expected("l1", Al1, "x1", Ax1, "x2", Ax2, b); + + // check if the two factors are the same + CHECK(actual->equals(expected)); // currently fails - ordering is different +} + +/* ************************************************************************* */ + +TEST( LinearFactorGraph, eliminate_one_x1 ) +{ + LinearFactorGraph fg = createLinearFactorGraph(); + ConditionalGaussian::shared_ptr actual = fg.eliminate_one("x1"); + + // create expected Conditional Gaussian + Matrix R11 = Matrix_(2,2, + 15.0, 00.0, + 00.0, 15.0 + ); + Matrix S12 = Matrix_(2,2, + -1.66667, 0.00, + +0.00,-1.66667 + ); + Matrix S13 = Matrix_(2,2, + -6.66667, 0.00, + +0.00,-6.66667 + ); + Vector d(2); d(0) = -2; d(1) = -1.0/3.0; + ConditionalGaussian expected(d,R11,"l1",S12,"x2",S13); + + CHECK( actual->equals(expected) ); +} + +/* ************************************************************************* */ + +TEST( LinearFactorGraph, eliminate_one_x2 ) +{ + LinearFactorGraph fg = createLinearFactorGraph(); + ConditionalGaussian::shared_ptr actual = fg.eliminate_one("x2"); + + // create expected Conditional Gaussian + Matrix R11 = Matrix_(2,2, + 11.1803, 0.00, + 0.00, 11.1803 + ); + Matrix S12 = Matrix_(2,2, + -2.23607, 0.00, + +0.00,-2.23607 + ); + Matrix S13 = Matrix_(2,2, + -8.94427, 0.00, + +0.00,-8.94427 + ); + Vector d(2); d(0) = 2.23607; d(1) = -1.56525; + ConditionalGaussian expected(d,R11,"l1",S12,"x1",S13); + + CHECK( actual->equals(expected) ); +} + +/* ************************************************************************* */ +TEST( LinearFactorGraph, eliminate_one_l1 ) +{ + LinearFactorGraph fg = createLinearFactorGraph(); + ConditionalGaussian::shared_ptr actual = fg.eliminate_one("l1"); + + // create expected Conditional Gaussian + Matrix R11 = Matrix_(2,2, + 7.07107, 0.00, + 0.00, 7.07107 + ); + Matrix S12 = Matrix_(2,2, + -3.53553, 0.00, + +0.00,-3.53553 + ); + Matrix S13 = Matrix_(2,2, + -3.53553, 0.00, + +0.00,-3.53553 + ); + Vector d(2); d(0) = -0.707107; d(1) = 1.76777; + ConditionalGaussian expected(d,R11,"x1",S12,"x2",S13); + + CHECK( actual->equals(expected) ); +} + +/* ************************************************************************* */ +TEST( LinearFactorGraph, eliminateAll ) +{ + // create expected Chordal bayes Net + double data1[] = { 10, 0.0, + 0.0, 10}; + Matrix R1 = Matrix_(2,2, data1); + Vector d1(2); d1(0) = -1; d1(1) = -1; + ConditionalGaussian::shared_ptr cg1(new ConditionalGaussian(d1, R1)); + + double data21[] = { 6.7082, 0.0, + 0.0, 6.7082}; + Matrix R2 = Matrix_(2,2, data21); + double data22[] = { -6.7082, 0.0, + 0.0, -6.7082}; + Matrix A1 = Matrix_(2,2, data22); + Vector d2(2); d2(0) = 0.0; d2(1) = 1.34164; + ConditionalGaussian::shared_ptr cg2(new ConditionalGaussian(d2, R2, "x1", A1)); + + double data31[] = { 11.1803, 0.0, + 0.0, 11.1803}; + Matrix R3 = Matrix_(2,2, data31); + double data32[] = { -2.23607, 0.0, + 0.0, -2.23607}; + Matrix A21 = Matrix_(2,2, data32); + double data33[] = { -8.94427, 0.0, + 0.0, -8.94427}; + Matrix A22 = Matrix_(2,2, data33); + + Vector d3(2); d3(0) = 2.23607; d3(1) = -1.56525; + ConditionalGaussian::shared_ptr cg3(new ConditionalGaussian(d3, R3, "l1", A21, "x1", A22)); + + ChordalBayesNet expected; + expected.insert("x1", cg1); + expected.insert("l1", cg2); + expected.insert("x2", cg3); + + // Check one ordering + LinearFactorGraph fg1 = createLinearFactorGraph(); + Ordering ord1; + ord1.push_back("x2"); + ord1.push_back("l1"); + ord1.push_back("x1"); + ChordalBayesNet::shared_ptr actual1 = fg1.eliminate(ord1); + CHECK(actual1->equals(expected)); +} + +/* ************************************************************************* */ +TEST( LinearFactorGraph, add_priors ) +{ + LinearFactorGraph fg = createLinearFactorGraph(); + LinearFactorGraph actual = fg.add_priors(3); + LinearFactorGraph expected = createLinearFactorGraph(); + Matrix A = 3*eye(2); + Vector b = zero(2); + expected.push_back(LinearFactor::shared_ptr(new LinearFactor("l1",A,b))); + expected.push_back(LinearFactor::shared_ptr(new LinearFactor("x1",A,b))); + expected.push_back(LinearFactor::shared_ptr(new LinearFactor("x2",A,b))); + CHECK(actual.equals(expected)); +} + +/* ************************************************************************* */ +TEST( LinearFactorGraph, copying ) +{ + // Create a graph + LinearFactorGraph actual = createLinearFactorGraph(); + + // Copy the graph ! + LinearFactorGraph copy = actual; + + // now eliminate the copy + Ordering ord1; + ord1.push_back("x2"); + ord1.push_back("l1"); + ord1.push_back("x1"); + ChordalBayesNet::shared_ptr actual1 = copy.eliminate(ord1); + + // Create the same graph, but not by copying + LinearFactorGraph expected = createLinearFactorGraph(); + + // and check that original is still the same graph + CHECK(actual.equals(expected)); +} + +/* ************************************************************************* */ +TEST( LinearFactorGraph, matrix ) +{ + // Create a graph + LinearFactorGraph fg = createLinearFactorGraph(); + + // render with a given ordering + Ordering ord; + ord.push_back("x2"); + ord.push_back("l1"); + ord.push_back("x1"); + + Matrix A; Vector b; + boost::tie(A,b) = fg.matrix(ord); + + Matrix A1 = Matrix_(2*4,3*2, + 00.0, 0.0, 0.0, 0.0, 10.0, 0.0, + 00.0, 0.0, 0.0, 0.0, 0.0, 10.0, + 10.0, 0.0, 0.0, 0.0,-10.0, 0.0, + 00.0, 10.0, 0.0, 0.0, 0.0,-10.0, + 00.0, 0.0, 5.0, 0.0, -5.0, 0.0, + 00.0, 0.0, 0.0, 5.0, 0.0, -5.0, + -5.0, 0.0, 5.0, 0.0, 0.0, 0.0, + 00.0, -5.0, 0.0, 5.0, 0.0, 0.0 + ); + Vector b1 = Vector_(8,-1.0, -1.0, 2.0, -1.0, 0.0, 1.0, -1.0, 1.5); + + EQUALITY(A,A1); // currently fails + CHECK(b==b1); // currently fails +} + +/* ************************************************************************* */ +TEST( LinearFactorGraph, CONSTRUCTOR_ChordalBayesNet ) +{ + + LinearFactorGraph fg = createLinearFactorGraph(); + + // render with a given ordering + Ordering ord; + ord.push_back("x2"); + ord.push_back("l1"); + ord.push_back("x1"); + + ChordalBayesNet::shared_ptr CBN = fg.eliminate(ord); + LinearFactorGraph fg2(CBN); + ChordalBayesNet::shared_ptr CBN2 = fg2.eliminate(ord); + + CHECK(CBN->equals(*CBN2)); +} + +/* ************************************************************************* */ +TEST( LinearFactorGraph, GET_ORDERING) +{ + LinearFactorGraph fg = createLinearFactorGraph(); + Ordering ord = fg.getOrdering(); + CHECK(ord[0] == string("l1")); + CHECK(ord[1] == string("x1")); + CHECK(ord[2] == string("x2")); +} + +/* ************************************************************************* */ +TEST( LinearFactorGraph, OPTIMIZE ) +{ + // create a graph + LinearFactorGraph fg = createLinearFactorGraph(); + + // create an ordering + Ordering ord = fg.getOrdering(); + + // optimize the graph + FGConfig actual = fg.optimize(ord); + + // verify + FGConfig expected = createCorrectDelta(); + + CHECK(actual.equals(expected)); +} + +/* ************************************************************************* */ +TEST( LinearFactorGraph, COMBINE_GRAPHS_INPLACE) +{ + // create a test graph + LinearFactorGraph fg1 = createLinearFactorGraph(); + + // create another factor graph + LinearFactorGraph fg2 = createLinearFactorGraph(); + + // get sizes + int size1 = fg1.size(); + int size2 = fg2.size(); + + // combine them + fg1.combine(fg2); + + CHECK(size1+size2 == fg1.size()); +} + +/* ************************************************************************* */ +TEST( LinearFactorGraph, COMBINE_GRAPHS) +{ + // create a test graph + LinearFactorGraph fg1 = createLinearFactorGraph(); + + // create another factor graph + LinearFactorGraph fg2 = createLinearFactorGraph(); + + // get sizes + int size1 = fg1.size(); + int size2 = fg2.size(); + + // combine them + LinearFactorGraph fg3 = LinearFactorGraph::combine2(fg1, fg2); + + CHECK(size1+size2 == fg3.size()); +} +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/cpp/testMatrix.cpp b/cpp/testMatrix.cpp new file mode 100644 index 000000000..82c085e86 --- /dev/null +++ b/cpp/testMatrix.cpp @@ -0,0 +1,458 @@ +/** + * @file testMatrix.cpp + * @brief Unit test for Matrix Library + * @author Christian Potthast + * @author Carlos Nieto + **/ + +#include +#include +#include +#include "Matrix.h" + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST( matrix, constructor_data ) +{ + double data[] = {-5, 3, + 0, -5 }; + Matrix A = Matrix_(2,2,data); + + Matrix B(2,2); + B(0,0) = -5 ; B(0,1) = 3; + B(1,0) = 0 ; B(1,1) = -5; + + EQUALITY(A,B); +} + +/* ************************************************************************* */ + +TEST( matrix, constructor_vector ) +{ + double data[] = {-5, 3, + 0, -5 }; + Matrix A = Matrix_(2,2,data); + Vector v(4); copy(data,data+4,v.begin()); + Matrix B = Matrix_(2,2,v); // this one is column order ! + EQUALITY(A,trans(B)); +} + +/* ************************************************************************* */ +TEST( matrix, Matrix_ ) +{ + Matrix A = Matrix_(2,2, + -5.0 , 3.0, + 00.0, -5.0 ); + Matrix B(2,2); + B(0,0) = -5 ; B(0,1) = 3; + B(1,0) = 0 ; B(1,1) = -5; + + EQUALITY(A,B); + +} + +/* ************************************************************************* */ +TEST( matrix, row_major ) +{ + Matrix A = Matrix_(2,2, + 1.0, 2.0, + 3.0, 4.0 ); + const double * const a = &A(0,0); + CHECK(a[0] == 1); + CHECK(a[1] == 2); + CHECK(a[2] == 3); + CHECK(a[3] == 4); +} + +/* ************************************************************************* */ +TEST( matrix, collect ) +{ + Matrix A = Matrix_(2,2, + -5.0 , 3.0, + 00.0, -5.0 ); + Matrix B = Matrix_(2,3, + -0.5 , 2.1, 1.1, + 3.4 , 2.6 , 7.1); + Matrix AB = collect(2, &A, &B); + Matrix C(2,5); + for(int i = 0; i < 2; i++) for(int j = 0; j < 2; j++) C(i,j) = A(i,j); + for(int i = 0; i < 2; i++) for(int j = 0; j < 3; j++) C(i,j+2) = B(i,j); + + EQUALITY(C,AB); + +} + +/* ************************************************************************* */ +TEST( matrix, stack ) +{ + Matrix A = Matrix_(2,2, + -5.0 , 3.0, + 00.0, -5.0 ); + Matrix B = Matrix_(3,2, + -0.5 , 2.1, + 1.1, 3.4 , + 2.6 , 7.1); + Matrix AB = stack(2, &A, &B); + Matrix C(5,2); + for(int i = 0; i < 2; i++) for(int j = 0; j < 2; j++) C(i,j) = A(i,j); + for(int i = 0; i < 3; i++) for(int j = 0; j < 2; j++) C(i+2,j) = B(i,j); + + EQUALITY(C,AB); +} + +/* ************************************************************************* */ +TEST( matrix, zeros ) +{ + Matrix A(2,3); + A(0,0) = 0 ; A(0,1) = 0; A(0,2) = 0; + A(1,0) = 0 ; A(1,1) = 0; A(1,2) = 0; + + Matrix zero = zeros(2,3); + + EQUALITY(A , zero); +} + +/* ************************************************************************* */ +TEST( matrix, equal ) +{ + Matrix A(4,4); + A(0,0) = -1; A(0,1) = 1; A(0,2)= 2; A(0,3)= 3; + A(1,0) = 1; A(1,1) =-3; A(1,2)= 1; A(1,3)= 3; + A(2,0) = 1; A(2,1) = 2; A(2,2)=-1; A(2,3)= 4; + A(3,0) = 2; A(3,1) = 1; A(3,2)= 2; A(3,3)=-2; + + Matrix A2(A); + + Matrix A3(A); + A3(3,3)=-2.1; + + CHECK(A==A2); + CHECK(A!=A3); +} + +/* ************************************************************************* */ +TEST( matrix, addition ) +{ + Matrix A = Matrix_(2,2, + 1.0, 2.0, + 3.0, 4.0); + Matrix B = Matrix_(2,2, + 4.0, 3.0, + 2.0, 1.0); + Matrix C = Matrix_(2,2, + 5.0, 5.0, + 5.0, 5.0); + EQUALITY(A+B,C); +} + +/* ************************************************************************* */ +TEST( matrix, addition_in_place ) +{ + Matrix A = Matrix_(2,2, + 1.0, 2.0, + 3.0, 4.0); + Matrix B = Matrix_(2,2, + 4.0, 3.0, + 2.0, 1.0); + Matrix C = Matrix_(2,2, + 5.0, 5.0, + 5.0, 5.0); + A += B; + EQUALITY(A,C); +} + +/* ************************************************************************* */ +TEST( matrix, subtraction ) +{ + Matrix A = Matrix_(2,2, + 1.0, 2.0, + 3.0, 4.0); + Matrix B = Matrix_(2,2, + 4.0, 3.0, + 2.0, 1.0); + Matrix C = Matrix_(2,2, + -3.0, -1.0, + 1.0, 3.0); + EQUALITY(A-B,C); +} + +/* ************************************************************************* */ +TEST( matrix, subtraction_in_place ) +{ + Matrix A = Matrix_(2,2, + 1.0, 2.0, + 3.0, 4.0); + Matrix B = Matrix_(2,2, + 4.0, 3.0, + 2.0, 1.0); + Matrix C = Matrix_(2,2, + -3.0, -1.0, + 1.0, 3.0); + A -= B; + EQUALITY(A,C); +} + +/* ************************************************************************* */ +TEST( matrix, multiplication ) +{ + Matrix A(2,2); + A(0,0) = -1; A(1,0) = 1; + A(0,1) = 1; A(1,1) =-3; + + Matrix B(2,1); + B(0,0) = 1.2; + B(1,0) = 3.4; + + Matrix AB(2,1); + AB(0,0) = 2.2; + AB(1,0) = -9.; + + EQUALITY(A*B,AB); +} + +/* ************************************************************************* */ +TEST( matrix, scalar_matrix_multiplication ) +{ + Vector result(2); + + Matrix A(2,2); + A(0,0) = -1; A(1,0) = 1; + A(0,1) = 1; A(1,1) =-3; + + Matrix B(2,2); + B(0,0) = -10; B(1,0) = 10; + B(0,1) = 10; B(1,1) =-30; + + EQUALITY((10*A),B); +} + +/* ************************************************************************* */ +TEST( matrix, matrix_vector_multiplication ) +{ + Vector result(2); + + Matrix A = Matrix_(2,3, + 1.0,2.0,3.0, + 4.0,5.0,6.0 + ); + Vector v(3); + v(0) = 1.0; + v(1) = 2.0; + v(2) = 3.0; + + Vector Av(2); + Av(0) = 14.0; + Av(1) = 32.0; + + EQUALITY(A*v,Av); +} + +/* ************************************************************************* */ +TEST( matrix, nrRowsAndnrCols ) +{ + Matrix A(3,6); + LONGS_EQUAL( A.size1() , 3 ); + LONGS_EQUAL( A.size2() , 6 ); +} + + +/* ************************************************************************* */ +TEST( matrix, scalar_divide ) +{ + Matrix A(2,2); + A(0,0) = 10; A(1,0) = 30; + A(0,1) = 20; A(1,1) = 40; + + Matrix B(2,2); + B(0,0) = 1; B(1,0) = 3; + B(0,1) = 2; B(1,1) = 4; + + EQUALITY(B,A/10); +} + +/* ************************************************************************* */ +TEST( matrix, inverse ) +{ + Matrix A(3,3); + A(0,0)= 1; A(0,1)=2; A(0,2)=3; + A(1,0)= 0; A(1,1)=4; A(1,2)=5; + A(2,0)= 1; A(2,1)=0; A(2,2)=6; + + Matrix Ainv = inverse(A); + + Matrix expected(3,3); + expected(0,0)= 1.0909; expected(0,1)=-0.5454; expected(0,2)=-0.0909; + expected(1,0)= 0.2272; expected(1,1)= 0.1363; expected(1,2)=-0.2272; + expected(2,0)= -0.1818; expected(2,1)= 0.0909; expected(2,2)=0.1818; + + CHECK(assert_equal(expected, Ainv, 1e-4)); +} + +/* ************************************************************************* */ +/* unit test for backsubstitution */ +/* ************************************************************************* */ +TEST( matrix, backsubtitution ) +{ + // TEST ONE 2x2 matrix + Vector expectedA(2); + expectedA(0) = 3.6250 ; expectedA(1) = -0.75; + + // create a 2x2 matrix + double dataA[] = {2, 3, + 0, 4 }; + Matrix A = Matrix_(2,2,dataA); + Vector Ab(2); Ab(0) = 5; Ab(1) = -3; + + CHECK( assert_equal(expectedA , backsubstitution(A, Ab), 0.000001)); + + // TEST TWO 3x3 matrix + Vector expectedB(3); + expectedB(0) = 5.5 ; expectedB(1) = -8.5; expectedB(2) = 5; + + + // create a 3x3 matrix + double dataB[] = { 3, 5, 6, + 0, 2, 3, + 0, 0, 1 }; + Matrix B = Matrix_(3,3,dataB); + + Vector Bb(3); + Bb(0) = 4; Bb(1) = -2; Bb(2) = 5; + + CHECK( assert_equal(expectedB , backsubstitution(B, Bb), 0.000001)); +} + +/* ************************************************************************* */ +// unit tests for housholder transformation +/* ************************************************************************* */ +TEST( matrix, houseHolder ) +{ + double data[] = {-5, 0, 5, 0, 0, 0, -1, + 00, -5, 0, 5, 0, 0, 1.5, + 10, 0, 0, 0,-10, 0, 2, + 00, 10, 0, 0, 0,-10, -1}; + + // check in-place householder, with v vectors below diagonal + double data1[] = {0011.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, + 000000000, 11.1803, 0, -2.2361, 0, -8.9443, -1.565, + -0.618034, 0, 4.4721, 0, -4.4721, 0, 0, + 000000000,-0.618034, 0, 4.4721, 0, -4.4721, 0.894}; + Matrix expected1 = Matrix_(4,7, data1); + Matrix A1 = Matrix_(4, 7, data); + householder_(A1,3); + CHECK(assert_equal(expected1, A1, 1e-3)); + + // in-place, with zeros below diagonal + double data2[] = {0011.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, + 000000000, 11.1803, 0, -2.2361, 0, -8.9443, -1.565, + 000000000, 0, 4.4721, 0, -4.4721, 0, 0, + 000000000, 0, 0, 4.4721, 0, -4.4721, 0.894}; + Matrix expected = Matrix_(4,7, data2); + Matrix A2 = Matrix_(4, 7, data); + householder(A2,3); + CHECK(assert_equal(expected, A2, 1e-3)); +} +/* ************************************************************************* */ +// unit test for qr factorization (and hence householder) +// This behaves the same as QR in matlab: [Q,R] = qr(A), except for signs +/* ************************************************************************* */ +TEST( matrix, qr ) +{ + double data[] = {-5, 0, 5, 0, + 00, -5, 0, 5, + 10, 0, 0, 0, + 00, 10, 0, 0, + 00, 0, 0,-10, + 10, 0,-10, 0}; + Matrix A = Matrix_(6, 4, data); + + double dataQ[] = { + -0.3333, 0, 0.2981, 0, 0, -0.8944, + 0000000, -0.4472, 0, 0.3651, -0.8165, 0, + 00.6667, 0, 0.7454, 0, 0, 0, + 0000000, 0.8944, 0, 0.1826, -0.4082, 0, + 0000000, 0, 0, -0.9129, -0.4082, 0, + 00.6667, 0, -0.5963, 0, 0, -0.4472, + }; + Matrix expectedQ = Matrix_(6,6, dataQ); + + double dataR[] = { + 15, 0, -8.3333, 0, + 00, 11.1803, 0, -2.2361, + 00, 0, 7.4536, 0, + 00, 0, 0, 10.9545, + 00, 0, 0, 0, + 00, 0, 0, 0, + }; + Matrix expectedR = Matrix_(6,4, dataR); + + Matrix Q,R; + boost::tie(Q,R) = qr(A); + CHECK(assert_equal(expectedQ, Q, 1e-4)); + CHECK(assert_equal(expectedR, R, 1e-4)); + CHECK(assert_equal(A, Q*R, 1e-14)); +} + +/* ************************************************************************* */ +TEST( matrix, sub ) +{ + double data1[] = { + -5, 0, 5, 0, 0, 0, + 00, -5, 0, 5, 0, 0, + 10, 0, 0, 0,-10, 0, + 00, 10, 0, 0, 0,-10 + }; + Matrix A = Matrix_(4,6, data1); + Matrix actual = sub(A,1,3,1,5); + + double data2[] = { + -5, 0, 5, 0, + 00, 0, 0,-10, + }; + Matrix expected = Matrix_(2,4, data2); + + EQUALITY(actual,expected); +} + + +/* ************************************************************************* */ +TEST( matrix, trans ) +{ + Matrix A = Matrix_(2,2, + 1.0 ,3.0, + 2.0, 4.0 ); + Matrix B = Matrix_(2,2, + 1.0 ,2.0, + 3.0, 4.0 ); + EQUALITY(trans(A),B); +} + +/* ************************************************************************* */ +TEST( matrix, row_major_access ) +{ + Matrix A = Matrix_(2,2,1.0,2.0,3.0,4.0); + const double* a = &A(0,0); + DOUBLES_EQUAL(3,a[2],1e-9); +} + +/* ************************************************************************* */ + +TEST( matrix, svd ) +{ + double data[] = {2,1,0}; + Vector v(3); copy(data,data+3,v.begin()); + Matrix U1=eye(4,3), S1=diag(v), V1=eye(3,3), A=(U1*S1)*Matrix(trans(V1)); + Matrix U,V; + Vector s; + svd(A,U,s,V); + Matrix S=diag(s); + EQUALITY(U*S*Matrix(trans(V)),A); + EQUALITY(S,S1); +} + + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/cpp/testNonlinearFactor.cpp b/cpp/testNonlinearFactor.cpp new file mode 100644 index 000000000..17a222adc --- /dev/null +++ b/cpp/testNonlinearFactor.cpp @@ -0,0 +1,139 @@ +/** + * @file testNonlinearFactor.cpp + * @brief Unit tests for Non-Linear Factor, + * create a non linear factor graph and a configuration for it and + * calculate the error for the factor. + * @author Christian Potthast + **/ + +/*STL/C++*/ +#include + +#include + +#include "Matrix.h" +#include "smallExample.h" + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST( NonLinearFactor, NonlinearFactor ) +{ + // create a non linear factor graph + NonlinearFactorGraph fg = createNonlinearFactorGraph(); + + // create a configuration for the non linear factor graph + FGConfig cfg = createNoisyConfig(); + + // get the factor "f1" from the factor graph + boost::shared_ptr factor = fg[0]; + + // calculate the error_vector from the factor "f1" + Vector actual_e = factor->error_vector(cfg); + Vector e(2); e(0) = -0.1; e(1) = -0.1; + CHECK(assert_equal(e,actual_e)); + + // the expected value for the error from the factor + // error_vector / sigma = [0.1 0.1]/0.1 = [1;1] + // error = 0.5 * [1 1] * [1;1] = 1 + double expected = 1.0; + + // calculate the error from the factor "f1" + double actual = factor->error(cfg); + DOUBLES_EQUAL(expected,actual,0.00000001); +} + +/* ************************************************************************* */ +TEST( NonLinearFactor, linearize_f1 ) +{ + // Grab a non-linear factor + NonlinearFactorGraph nfg = createNonlinearFactorGraph(); + boost::shared_ptr nlf = + boost::static_pointer_cast(nfg[0]); + + // We linearize at noisy config from SmallExample + FGConfig c = createNoisyConfig(); + LinearFactor::shared_ptr actual = nlf->linearize(c); + + LinearFactorGraph lfg = createLinearFactorGraph(); + LinearFactor::shared_ptr expected = lfg[0]; + + CHECK(expected->equals(*actual)); +} + +/* ************************************************************************* */ +TEST( NonLinearFactor, linearize_f2 ) +{ + // Grab a non-linear factor + NonlinearFactorGraph nfg = createNonlinearFactorGraph(); + boost::shared_ptr nlf = + boost::static_pointer_cast(nfg[1]); + + // We linearize at noisy config from SmallExample + FGConfig c = createNoisyConfig(); + LinearFactor::shared_ptr actual = nlf->linearize(c); + + LinearFactorGraph lfg = createLinearFactorGraph(); + LinearFactor::shared_ptr expected = lfg[1]; + + CHECK(expected->equals(*actual)); +} + +/* ************************************************************************* */ +TEST( NonLinearFactor, linearize_f3 ) +{ + // Grab a non-linear factor + NonlinearFactorGraph nfg = createNonlinearFactorGraph(); + boost::shared_ptr nlf = + boost::static_pointer_cast(nfg[2]); + + // We linearize at noisy config from SmallExample + FGConfig c = createNoisyConfig(); + LinearFactor::shared_ptr actual = nlf->linearize(c); + + LinearFactorGraph lfg = createLinearFactorGraph(); + LinearFactor::shared_ptr expected = lfg[2]; + + CHECK(expected->equals(*actual)); +} + +/* ************************************************************************* */ +TEST( NonLinearFactor, linearize_f4 ) +{ + // Grab a non-linear factor + NonlinearFactorGraph nfg = createNonlinearFactorGraph(); + boost::shared_ptr nlf = + boost::static_pointer_cast(nfg[3]); + + // We linearize at noisy config from SmallExample + FGConfig c = createNoisyConfig(); + LinearFactor::shared_ptr actual = nlf->linearize(c); + + LinearFactorGraph lfg = createLinearFactorGraph(); + LinearFactor::shared_ptr expected = lfg[3]; + + CHECK(expected->equals(*actual)); +} + +/* ************************************************************************* */ +TEST( NonLinearFactor, size ) +{ + // create a non linear factor graph + NonlinearFactorGraph fg = createNonlinearFactorGraph(); + + // create a configuration for the non linear factor graph + FGConfig cfg = createNoisyConfig(); + + // get some factors from the graph + boost::shared_ptr factor1 = fg[0]; + boost::shared_ptr factor2 = fg[1]; + boost::shared_ptr factor3 = fg[2]; + + CHECK(factor1->size() == 1); + CHECK(factor2->size() == 2); + CHECK(factor3->size() == 2); +} +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/cpp/testNonlinearFactorGraph.cpp b/cpp/testNonlinearFactorGraph.cpp new file mode 100644 index 000000000..912cbe9f4 --- /dev/null +++ b/cpp/testNonlinearFactorGraph.cpp @@ -0,0 +1,170 @@ +/** + * @file testNonlinearFactorGraph.cpp + * @brief Unit tests for Non-Linear Factor Graph + * @brief testNonlinearFactorGraph + * @author Carlos Nieto + * @author Christian Potthast + */ + + +/*STL/C++*/ +#include +using namespace std; + +#include + +#include "Matrix.h" +#include "smallExample.h" + +using namespace gtsam; + +/* ************************************************************************* */ +TEST( NonlinearFactorGraph, equals ){ + + NonlinearFactorGraph fg = createNonlinearFactorGraph(); + NonlinearFactorGraph fg2 = createNonlinearFactorGraph(); + CHECK( fg.equals(fg2) ); +} + +/* ************************************************************************* */ +TEST( NonlinearFactorGraph, error ) +{ + NonlinearFactorGraph fg = createNonlinearFactorGraph(); + + FGConfig c1 = createConfig(); + double actual1 = fg.error(c1); + DOUBLES_EQUAL( 0.0, actual1, 1e-9 ); + + FGConfig c2 = createNoisyConfig(); + double actual2 = fg.error(c2); + DOUBLES_EQUAL( 5.625, actual2, 1e-9 ); +} + +/* ************************************************************************* */ +TEST( NonlinearFactorGraph, probPrime ) +{ + NonlinearFactorGraph fg = createNonlinearFactorGraph(); + FGConfig cfg = createConfig(); + + // evaluate the probability of the factor graph + double actual = fg.probPrime(cfg); + double expected = 1.0; + DOUBLES_EQUAL(expected,actual,0); +} + +/* ************************************************************************* */ +TEST( NonlinearFactorGraph, linearize ) +{ + NonlinearFactorGraph fg = createNonlinearFactorGraph(); + FGConfig initial = createNoisyConfig(); + LinearFactorGraph linearized = fg.linearize(initial); + LinearFactorGraph expected = createLinearFactorGraph(); + CHECK(expected.equals(linearized)); +} + +/* ************************************************************************* */ +TEST( NonlinearFactorGraph, iterate ) +{ + NonlinearFactorGraph fg = createNonlinearFactorGraph(); + FGConfig initial = createNoisyConfig(); + + // Expected configuration is the difference between the noisy config + // and the ground-truth config. One step only because it's linear ! + FGConfig expected; + Vector dl1(2); dl1(0)=-0.1; dl1(1)= 0.1; expected.insert("l1",dl1); + Vector dx1(2); dx1(0)=-0.1; dx1(1)=-0.1; expected.insert("x1",dx1); + Vector dx2(2); dx2(0)= 0.1; dx2(1)=-0.2; expected.insert("x2",dx2); + + // Check one ordering + Ordering ord1; + ord1.push_back("x2"); + ord1.push_back("l1"); + ord1.push_back("x1"); + FGConfig actual1 = fg.iterate(initial, ord1); + CHECK(actual1.equals(expected)); + + // Check another + Ordering ord2; + ord2.push_back("x1"); + ord2.push_back("x2"); + ord2.push_back("l1"); + FGConfig actual2 = fg.iterate(initial, ord2); + CHECK(actual2.equals(expected)); + + // And yet another... + Ordering ord3; + ord3.push_back("l1"); + ord3.push_back("x1"); + ord3.push_back("x2"); + FGConfig actual3 = fg.iterate(initial, ord3); + CHECK(actual3.equals(expected)); + +} + +/* ************************************************************************* */ +TEST( NonlinearFactorGraph, optimize ) { + + NonlinearFactorGraph fg = createReallyNonlinearFactorGraph(); + + // test error at minimum + Vector xstar = Vector_(1,0.0); + FGConfig cstar; + cstar.insert("x",xstar); + DOUBLES_EQUAL(0.0,fg.error(cstar),0.0); + + // test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 = + Vector x0 = Vector_(1,3.0); + FGConfig c0; + c0.insert("x",x0); + DOUBLES_EQUAL(199.0,fg.error(c0),1e-3); + + // optimize parameters + Ordering ord; + ord.push_back("x"); + double relativeErrorTreshold = 1e-5; + double absoluteErrorTreshold = 1e-5; + + // Gauss-Newton + FGConfig actual = c0; + fg.optimize(actual,ord,relativeErrorTreshold,absoluteErrorTreshold); + CHECK(actual.equals(cstar)); + + // Levenberg-Marquardt + FGConfig actual2 = c0; + double lambda0 = 1000, lambdaFactor = 10; + fg.optimizeLM(actual2,ord,relativeErrorTreshold,absoluteErrorTreshold,0,lambda0,lambdaFactor); + CHECK(actual2.equals(cstar)); +} + +/* ************************************************************************* */ +TEST( NonlinearFactorGraph, iterateLM ) { + + // really non-linear factor graph + NonlinearFactorGraph fg = createReallyNonlinearFactorGraph(); + + // config far from minimum + Vector x0 = Vector_(1,3.0); + FGConfig config; + config.insert("x",x0); + + // ordering + Ordering ord; + ord.push_back("x"); + + // normal iterate + int verbosity = 0; + FGConfig actual1 = config; + fg.iterate(actual1,ord,verbosity); + + // LM iterate with lambda 0 should be the same + FGConfig actual2 = config; + double lambda0 = 0, lambdaFactor = 10; + double currentError = fg.error(actual2); + fg.iterateLM(actual2, currentError, lambda0, lambdaFactor, ord, verbosity); + + CHECK(actual1.equals(actual2)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/cpp/testPoint2.cpp b/cpp/testPoint2.cpp new file mode 100644 index 000000000..580b63401 --- /dev/null +++ b/cpp/testPoint2.cpp @@ -0,0 +1,32 @@ +/** + * @file testPoint2.cpp + * @brief Unit tests for Point2 class + * @author Frank Dellaert + **/ + +#include +#include "Point2.h" + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST( Point2, exmap) { + Vector d(2);d(0)=1;d(1)=-1; + Point2 a(4,5), b=a.exmap(d),c(5,4); + CHECK(assert_equal(b,c)); +} + +/* ************************************************************************* */ +TEST( Point2, add) { + CHECK(assert_equal( Point2(4,5)+Point2(1,1), Point2(5,6) )); +} + +/* ************************************************************************* */ +TEST( Point2, subtract) { + CHECK(assert_equal( Point2(4,5)-Point2(1,1), Point2(3,4) )); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/cpp/testPoint3.cpp b/cpp/testPoint3.cpp new file mode 100644 index 000000000..999f089c8 --- /dev/null +++ b/cpp/testPoint3.cpp @@ -0,0 +1,30 @@ +/** + * @file testPoint3.cpp + * @brief Unit tests for Point3 class + */ + +#include +#include "Point3.h" + +using namespace gtsam; + +Point3 P(0.2,0.7,-2); + +/* ************************************************************************* */ +TEST( Point3, scalar_multiplication) +{ + CHECK(P*3==3*P); +} + +/* ************************************************************************* */ +TEST( Point3, equals) +{ + CHECK(P.equals(P)); + Point3 Q; + CHECK(!P.equals(Q)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ + diff --git a/cpp/testPose3.cpp b/cpp/testPose3.cpp new file mode 100644 index 000000000..4190288ed --- /dev/null +++ b/cpp/testPose3.cpp @@ -0,0 +1,206 @@ +/** + * @file testPose3.cpp + * @brief Unit tests for Pose3 class + */ + + +#include +#include "numericalDerivative.h" +#include "Pose3.h" + +using namespace gtsam; + +Rot3 R = rodriguez(0.3,0,0); +Point3 t(3.5,-8.2,4.2); +Pose3 T(R,t); +Point3 P(0.2,0.7,-2); +Rot3 r1 = rodriguez(-90, 0, 0); +Pose3 pose1(r1, Point3(1, 2, 3)); +double error = 1e-9; + +#define PI 3.14159265358979323846 + + +/* ************************************************************************* */ +TEST( Pose3, equals) +{ + Pose3 pose2 = pose1; + CHECK(pose1.equals(pose2)); + Pose3 origin; + CHECK(!pose1.equals(origin)); +} + +/* ************************************************************************* */ +TEST( Pose3, exmap) +{ + Pose3 id; + Vector v(6); + fill(v.begin(), v.end(), 0); + v(0) = 0.3; + CHECK(assert_equal(id.exmap(v), Pose3(R, Point3()))); + v(3)=0.2;v(4)=0.7;v(5)=-2; + CHECK(assert_equal(id.exmap(v), Pose3(R, P))); +} + +/* ************************************************************************* */ +TEST( Pose3, compose) +{ + Matrix actual = (T*T).matrix(); + Matrix expected = T.matrix()*T.matrix(); + CHECK(assert_equal(actual,expected,error)); +} + +/* ************************************************************************* */ +TEST( Pose3, inverse) +{ + Matrix actual = T.inverse().matrix(); + Matrix expected = inverse(T.matrix()); + CHECK(assert_equal(actual,expected,error)); +} + +/* ************************************************************************* */ +TEST( Pose3, compose_inverse) +{ + Matrix actual = (T*T.inverse()).matrix(); + Matrix expected = eye(4,4); + CHECK(assert_equal(actual,expected,error)); +} + +/* ************************************************************************* */ +// transform derivatives + +TEST( Pose3, Dtransform_from1) +{ + Matrix computed = Dtransform_from1(T, P); + Matrix numerical = numericalDerivative21(transform_from,T,P); + CHECK(assert_equal(numerical,computed,error)); +} + + +TEST( Pose3, Dtransform_from2) +{ + Matrix computed = Dtransform_from2(T); + Matrix numerical = numericalDerivative22(transform_from,T,P); + CHECK(assert_equal(numerical,computed,error)); +} + +TEST( Pose3, Dtransform_to1) +{ + Matrix computed = Dtransform_to1(T, P); + Matrix numerical = numericalDerivative21(transform_to,T,P); + CHECK(assert_equal(numerical,computed,error)); +} + +TEST( Pose3, Dtransform_to2) +{ + Matrix computed = Dtransform_to2(T); + Matrix numerical = numericalDerivative22(transform_to,T,P); + CHECK(assert_equal(numerical,computed,error)); +} +/* ************************************************************************* */ +TEST( Pose3, transform_to_translate) +{ + Point3 actual = transform_to(Pose3(Rot3(), Point3(1, 2, 3)), Point3(10.,20.,30.)); + Point3 expected(9.,18.,27.); + CHECK(assert_equal(expected, actual)); +} +TEST( Pose3, transform_to_rotate) +{ + Pose3 transform(rodriguez(0,0,-1.570796), Point3()); + Point3 actual = transform_to(transform, Point3(2,1,10)); + Point3 expected(-1,2,10); + CHECK(assert_equal(expected, actual, 0.001)); +} +TEST( Pose3, transform_to) +{ + Pose3 transform(rodriguez(0,0,-1.570796), Point3(2,4, 0)); + Point3 actual = transform_to(transform, Point3(3,2,10)); + Point3 expected(2,1,10); + CHECK(assert_equal(expected, actual, 0.001)); +} + +/* ************************************************************************* */ +TEST( Pose3, transform_from) +{ + Point3 actual = transform_from(pose1, Point3()); + Point3 expected = Point3(1.,2.,3.); + CHECK(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST( Pose3, transform_roundtrip) +{ + Point3 actual = transform_from(pose1, transform_to(pose1, Point3(12., -0.11,7.0))); + Point3 expected(12., -0.11,7.0); + CHECK(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST( Pose3, transformPose_to_origin) +{ + // transform to origin + Pose3 actual = pose1.transformPose_to(Pose3()); + CHECK(assert_equal(pose1, actual, error)); +} +TEST( Pose3, transformPose_to_itself) +{ + // transform to itself + Pose3 actual = pose1.transformPose_to(pose1); + CHECK(assert_equal(Pose3(), actual, error)); +} +TEST( Pose3, transformPose_to_translation) +{ + // transform translation only + Rot3 r = rodriguez(-1.570796,0,0); + Pose3 pose2(r, Point3(21.,32.,13.)); + Pose3 actual = pose2.transformPose_to(Pose3(Rot3(), Point3(1,2,3))); + Pose3 expected(r, Point3(20.,30.,10.)); + CHECK(assert_equal(expected, actual, error)); +} +TEST( Pose3, transformPose_to_simple_rotate) +{ + // transform translation only + Rot3 r = rodriguez(0,0,-1.570796); + Pose3 pose2(r, Point3(21.,32.,13.)); + Pose3 transform(r, Point3(1,2,3)); + Pose3 actual = pose2.transformPose_to(transform); + Pose3 expected(Rot3(), Point3(-30.,20.,10.)); + CHECK(assert_equal(expected, actual, 0.001)); +} +TEST( Pose3, transformPose_to) +{ + // transform to + Rot3 r = rodriguez(0,0,-1.570796); //-90 degree yaw + Rot3 r2 = rodriguez(0,0,0.698131701); //40 degree yaw + Pose3 pose2(r2, Point3(21.,32.,13.)); + Pose3 transform(r, Point3(1,2,3)); + Pose3 actual = pose2.transformPose_to(transform); + Pose3 expected(rodriguez(0,0,2.26892803), Point3(-30.,20.,10.)); + CHECK(assert_equal(expected, actual, 0.001)); +} + +/* ************************************************************************* */ +TEST( Pose3, composeTransform) +{ + // known transform + Rot3 r = rodriguez(0,0,-1.570796); + Pose3 exp_transform(r, Point3(1,2,3)); + + // current + Rot3 r2 = rodriguez(0,0,0.698131701); + Pose3 current(r2, Point3(21.,32.,13.)); + + // target + Pose3 target(rodriguez(0,0,2.26892803), Point3(-30.,20.,10.)); + + // calculate transform + Pose3 actual = composeTransform(current, target); + + //verify + CHECK(assert_equal(exp_transform, actual, 0.001)); +} + +/* ************************************************************************* */ +int main(){ TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ + diff --git a/cpp/testRot3.cpp b/cpp/testRot3.cpp new file mode 100644 index 000000000..466c54d95 --- /dev/null +++ b/cpp/testRot3.cpp @@ -0,0 +1,110 @@ +/** + * @file testRot3.cpp + * @brief Unit tests for Rot3 class + * @author Alireza Fathi + */ + +#include +#include "numericalDerivative.h" +#include "Rot3.h" + +using namespace gtsam; + +Rot3 R = rodriguez(0.1,0.4,0.2); +Point3 P(0.2,0.7,-2.0); +double error = 1e-9, epsilon=0.001; + +/* ************************************************************************* */ +TEST( Rot3, constructor) { + Rot3 expected(eye(3,3)); + Vector r1(3), r2(3), r3(3); + r1(0)=1;r1(1)=0;r1(2)=0; + r2(0)=0;r2(1)=1;r2(2)=0; + r3(0)=0;r3(1)=0;r3(2)=1; + Rot3 actual(r1,r2,r3); + CHECK(assert_equal(actual,expected)); +} + +/* ************************************************************************* */ +TEST( Rot3, constructor2) { + Matrix R = Matrix_(3,3, + 11.,12.,13., + 21.,22.,23., + 31.,32.,33.); + Rot3 actual(R); + Rot3 expected(11,12,13, + 21,22,23, + 31,32,33); + CHECK(assert_equal(actual,expected)); +} + +/* ************************************************************************* */ +TEST( Rot3, equals) { + CHECK(R.equals(R)); + Rot3 zero; + CHECK(!R.equals(zero)); +} + +/* ************************************************************************* */ +TEST( Rot3, rodriguez) { + Rot3 rd1 = rodriguez(epsilon, 0, 0); + Vector temp(3); temp(0) = epsilon; temp(1) = 0; temp(2) = 0; + Rot3 rd2 = rodriguez(temp); + CHECK(assert_equal(rd1,rd2)); +} + +/* ************************************************************************* */ +TEST( Rot3, exmap) +{ + Vector v(3); + fill(v.begin(), v.end(), 0); + CHECK(assert_equal(R.exmap(v), R)); +} + +/* ************************************************************************* */ +// rotate derivatives + +TEST( Rot3, Drotate1) +{ + Matrix computed = Drotate1(R, P); + Matrix numerical = numericalDerivative21(rotate,R,P); + CHECK(assert_equal(numerical,computed,error)); +} + +TEST( Rot3, Drotate2_DNrotate2) +{ + Matrix computed = Drotate2(R); + Matrix numerical = numericalDerivative22(rotate,R,P); + CHECK(assert_equal(numerical,computed,error)); +} + +/* ************************************************************************* */ +// unrotate + +TEST( Rot3, unrotate) +{ + Point3 w = R*P; + CHECK(assert_equal(unrotate(R,w),P)); +} + +/* ************************************************************************* */ +// unrotate derivatives + +TEST( Rot3, Dunrotate1) +{ + Matrix computed = Dunrotate1(R, P); + Matrix numerical = numericalDerivative21(unrotate,R,P); + CHECK(assert_equal(numerical,computed,error)); +} + +TEST( Rot3, Dunrotate2_DNunrotate2) +{ + Matrix computed = Dunrotate2(R); + Matrix numerical = numericalDerivative22(unrotate,R,P); + CHECK(assert_equal(numerical,computed,error)); +} + +/* ************************************************************************* */ +int main(){ TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ + diff --git a/cpp/testSimpleCamera.cpp b/cpp/testSimpleCamera.cpp new file mode 100644 index 000000000..816e83734 --- /dev/null +++ b/cpp/testSimpleCamera.cpp @@ -0,0 +1,83 @@ +/** + * Frank Dellaert + * brief: test SimpleCamera class + * based on testVSLAMFactor.cpp + */ + +#include + +#include +#include "numericalDerivative.h" +#include "SimpleCamera.h" + +using namespace std; +using namespace gtsam; + +const Cal3_S2 K(625, 625, 0, 0, 0); + +const Pose3 pose1(Matrix_(3,3, + 1., 0., 0., + 0.,-1., 0., + 0., 0.,-1. + ), + Point3(0,0,500)); + +const SimpleCamera camera(K, 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( SimpleCamera, constructor) +{ + CHECK(assert_equal( camera.calibration(), K)); + CHECK(assert_equal( camera.pose(), pose1)); +} + +/* ************************************************************************* */ +TEST( SimpleCamera, project) +{ + CHECK(assert_equal( camera.project(point1), Point2(-100, 100) )); + CHECK(assert_equal( camera.project(point2), Point2(-100, -100) )); + CHECK(assert_equal( camera.project(point3), Point2( 100, -100) )); + CHECK(assert_equal( camera.project(point4), Point2( 100, 100) )); +} + +/* ************************************************************************* */ +Point2 project2(const Pose3& pose, const Point3& point) { + return project(SimpleCamera(K,pose), point); +} + +TEST( SimpleCamera, Dproject_pose) +{ + Matrix computed = Dproject_pose(camera, point1); + Matrix numerical = numericalDerivative21(project2, pose1, point1); + CHECK(assert_equal(computed, numerical,1e-7)); +} + +TEST( SimpleCamera, Dproject_point) +{ + Matrix computed = Dproject_point(camera, point1); + Matrix numerical = numericalDerivative22(project2, pose1, point1); + CHECK(assert_equal(computed, numerical,1e-7)); +} + +TEST( SimpleCamera, 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(-100, 100) )); + 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; } +/* ************************************************************************* */ + + diff --git a/cpp/testSimulated2D.cpp b/cpp/testSimulated2D.cpp new file mode 100644 index 000000000..d248c6197 --- /dev/null +++ b/cpp/testSimulated2D.cpp @@ -0,0 +1,48 @@ +/** + * @file testSimulated2D.cpp + * @brief Unit tests for simulated 2D measurement functions + * @author Christian Potthast + * @author Carlos Nieto + **/ + +#include +#include + +#include "numericalDerivative.h" +#include "simulated2D.h" + +using namespace gtsam; +using namespace std; + +/* ************************************************************************* */ +TEST( simulated2D, Dprior ) +{ + Vector x(2);x(0)=1;x(1)=-9; + Matrix numerical = NumericalDerivative11(prior,x); + Matrix computed = Dprior(x); + CHECK(assert_equal(numerical,computed,1e-9)); +} + +/* ************************************************************************* */ + TEST( simulated2D, DOdo1 ) +{ + Vector x1(2);x1(0)= 1;x1(1)=-9; + Vector x2(2);x2(0)=-5;x2(1)= 6; + Matrix numerical = NumericalDerivative21(odo,x1,x2); + Matrix computed = Dodo1(x1,x2); + CHECK(assert_equal(numerical,computed,1e-9)); +} + +/* ************************************************************************* */ + TEST( simulated2D, DOdo2 ) +{ + Vector x1(2);x1(0)= 1;x1(1)=-9; + Vector x2(2);x2(0)=-5;x2(1)= 6; + Matrix numerical = NumericalDerivative22(odo,x1,x2); + Matrix computed = Dodo2(x1,x2); + CHECK(assert_equal(numerical,computed,1e-9)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/cpp/testSimulated3D.cpp b/cpp/testSimulated3D.cpp new file mode 100644 index 000000000..9d83bba07 --- /dev/null +++ b/cpp/testSimulated3D.cpp @@ -0,0 +1,53 @@ +/** + * @file testSimulated3D.cpp + * @brief Unit tests for simulated 3D measurement functions + * @author Alex Cunningham + **/ + +#include +#include + +#include "Pose3.h" +#include "numericalDerivative.h" +#include "Simulated3D.h" + +using namespace gtsam; + +/* ************************************************************************* */ +TEST( simulated3D, Dprior_3D ) +{ + Pose3 x1(rodriguez(0, 0, 1.57), Point3(1, 5, 0)); + Vector v = x1.vector(); + Matrix numerical = NumericalDerivative11(prior_3D,v); + Matrix computed = Dprior_3D(v); + CHECK(assert_equal(numerical,computed,1e-9)); +} + +/* ************************************************************************* */ +TEST( simulated3D, DOdo1 ) +{ + Pose3 x1(rodriguez(0, 0, 1.57), Point3(1, 5, 0)); + Vector v1 = x1.vector(); + Pose3 x2(rodriguez(0, 0, 0), Point3(2, 3, 0)); + Vector v2 = x2.vector(); + Matrix numerical = NumericalDerivative21(odo_3D,v1,v2); + Matrix computed = Dodo1_3D(v1,v2); + CHECK(assert_equal(numerical,computed,1e-9)); +} + +/* ************************************************************************* */ +TEST( simulated3D, DOdo2 ) +{ + Pose3 x1(rodriguez(0, 0, 1.57), Point3(1, 5, 0)); + Vector v1 = x1.vector(); + Pose3 x2(rodriguez(0, 0, 0), Point3(2, 3, 0)); + Vector v2 = x2.vector(); + Matrix numerical = NumericalDerivative22(odo_3D,v1,v2); + Matrix computed = Dodo2_3D(v1,v2); + CHECK(assert_equal(numerical,computed,1e-9)); +} + + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/cpp/testVSLAMFactor.cpp b/cpp/testVSLAMFactor.cpp new file mode 100644 index 000000000..15f4b6d68 --- /dev/null +++ b/cpp/testVSLAMFactor.cpp @@ -0,0 +1,23 @@ +/********************************************************** +Written by Alireza Fathi, 2nd of April 2009 +**********************************************************/ + +#include +#include +#include "numericalDerivative.h" +#include "NonlinearFactorGraph.h" +#include "VSLAMFactor.h" + +using namespace std; +using namespace gtsam; + +// TODO: test VSLAMFactor !!! + +/* ************************************************************************* */ +int main() { + TestResult tr; + TestRegistry::runAllTests(tr); + return 0; +} +/* ************************************************************************* */ + diff --git a/cpp/testVector.cpp b/cpp/testVector.cpp new file mode 100644 index 000000000..383e80d3b --- /dev/null +++ b/cpp/testVector.cpp @@ -0,0 +1,131 @@ +/** + * @file testVector.cpp + * @brief Unit tests for Vector class + * @author Frank Dellaert + **/ + +#include +#include +#include +#include "Vector.h" + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST( TestVector, Vector_variants ) +{ + Vector a = Vector_(2,10.0,20.0); + double data[] = {10,20}; + Vector b = Vector_(2,data); + CHECK(a==b); +} + +/* ************************************************************************* */ +TEST( TestVector, copy ) +{ + Vector a(2); a(0) = 10; a(1) = 20; + double data[] = {10,20}; + Vector b(2); copy(data,data+2,b.begin()); + CHECK(a==b); +} + +/* ************************************************************************* */ +TEST( TestVector, zero1 ) +{ + Vector v(2,0.0); + CHECK(zero(v)==true); +} + +/* ************************************************************************* */ +TEST( TestVector, zero2 ) +{ + Vector a = zero(2); + Vector b(2,0.0); + CHECK(a==b); +} + +/* ************************************************************************* */ +TEST( TestVector, scalar_multiply ) +{ + Vector a(2); a(0) = 10; a(1) = 20; + Vector b(2); b(0) = 1; b(1) = 2; + CHECK(a==b*10.0); +} + +/* ************************************************************************* */ +TEST( TestVector, scalar_divide ) +{ + Vector a(2); a(0) = 10; a(1) = 20; + Vector b(2); b(0) = 1; b(1) = 2; + CHECK(b==a/10.0); +} + +/* ************************************************************************* */ +TEST( TestVector, negate ) +{ + Vector a(2); a(0) = 10; a(1) = 20; + Vector b(2); b(0) = -10; b(1) = -20; + CHECK(b==-a); +} + +/* ************************************************************************* */ +TEST( TestVector, sub ) +{ + Vector a(6); + a(0) = 10; a(1) = 20; a(2) = 3; + a(3) = 34; a(4) = 11; a(5) = 2; + + Vector result(sub(a,2,5)); + + Vector b(3); + b(0) = 3; b(1) = 34; b(2) =11; + + CHECK(b==result); +} + +/* ************************************************************************* */ +TEST( TestVector, householder ) +{ + Vector x(4); + x(0) = 3; x(1) = 1; x(2) = 5; x(3) = 1; + + Vector expected(4); + expected(0) = 1.0; expected(1) = -0.333333; expected(2) = -1.66667; expected(3) = -0.333333; + + pair result = house(x); + + CHECK(result.first==0.5); + CHECK(equal_with_abs_tol(expected,result.second,1e-5)); +} + +/* ************************************************************************* */ +TEST( TestVector, zeros ) +{ + Vector a(2); a(0) = 0; a(1) = 0; + Vector b(2,0.0); + CHECK(b==a); +} + +/* ************************************************************************* */ +TEST( TestVector, concatVectors) +{ + Vector A(2); + for(int i = 0; i < 2; i++) + A(i) = i; + Vector B(5); + for(int i = 0; i < 5; i++) + B(i) = i; + + Vector C(7); + for(int i = 0; i < 2; i++) C(i) = A(i); + for(int i = 0; i < 5; i++) C(i+2) = B(i); + + Vector AB = concatVectors(2, &A, &B); + + CHECK(AB == C); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/cpp/timeLinearFactor.cpp b/cpp/timeLinearFactor.cpp new file mode 100644 index 000000000..64edf8c57 --- /dev/null +++ b/cpp/timeLinearFactor.cpp @@ -0,0 +1,83 @@ +/** + * @file timeLinearFactor.cpp + * @brief time LinearFactor.eliminate + * @author Alireza Fathi + */ + +#include + +/*STL/C++*/ +#include +using namespace std; + +#include +#include "Matrix.h" +#include "LinearFactor.h" + +using namespace gtsam; + +int main() +{ + // create a linear factor + Matrix Ax2 = Matrix_(8,2, + // x2 + -5., 0., + +0.,-5., + 10., 0., + +0.,10., + -5., 0., + +0.,-5., + 10., 0., + +0.,10. + ); + + Matrix Al1 = Matrix_(8,10, + // l1 + 5., 0.,1.,2.,3.,4.,5.,6.,7.,8., + 0., 5.,1.,2.,3.,4.,5.,6.,7.,8., + 0., 0.,1.,2.,3.,4.,5.,6.,7.,8., + 0., 0.,1.,2.,3.,4.,5.,6.,7.,8., + 5., 0.,1.,2.,3.,4.,5.,6.,7.,8., + 0., 5.,1.,2.,3.,4.,5.,6.,7.,8., + 0., 0.,1.,2.,3.,4.,5.,6.,7.,8., + 0., 0.,1.,2.,3.,4.,5.,6.,7.,8. + ); + + Matrix Ax1 = Matrix_(8,2, + // x1 + 0.00, 0.,1.,2.,3.,4.,5.,6.,7.,8., + 0.00, 0.,1.,2.,3.,4.,5.,6.,7.,8., + -10., 0.,1.,2.,3.,4.,5.,6.,7.,8., + 0.00,-10.,1.,2.,3.,4.,5.,6.,7.,8., + 0.00, 0.,1.,2.,3.,4.,5.,6.,7.,8., + 0.00, 0.,1.,2.,3.,4.,5.,6.,7.,8., + -10., 0.,1.,2.,3.,4.,5.,6.,7.,8., + 0.00,-10.,1.,2.,3.,4.,5.,6.,7.,8. + ); + + // and a RHS + Vector b2(8); + b2(0) = -1; + b2(1) = 1.5; + b2(2) = 2; + b2(3) = -1; + b2(4) = -1; + b2(5) = 1.5; + b2(6) = 2; + b2(7) = -1; + + LinearFactor combined("x2", Ax2, "l1", Al1, "x1", Ax1, b2); + long timeLog = clock(); + int n = 1000000; + ConditionalGaussian::shared_ptr conditional; + LinearFactor::shared_ptr factor; + + for(int i = 0; i < n; i++) + boost::tie(conditional,factor) = combined.eliminate("x2"); + + long timeLog2 = clock(); + double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC; + cout << seconds << " seconds" << endl; + cout << ((double)n/seconds) << " calls/second" << endl; + return 0; +} diff --git a/m4/ax_boost_base.m4 b/m4/ax_boost_base.m4 new file mode 100644 index 000000000..2e5afd091 --- /dev/null +++ b/m4/ax_boost_base.m4 @@ -0,0 +1,223 @@ +# =========================================================================== +# http://autoconf-archive.cryp.to/ax_boost_base.html +# =========================================================================== +# +# SYNOPSIS +# +# AX_BOOST_BASE([MINIMUM-VERSION]) +# +# DESCRIPTION +# +# Test for the Boost C++ libraries of a particular version (or newer) +# +# If no path to the installed boost library is given the macro searchs +# under /usr, /usr/local, /opt and /opt/local and evaluates the +# $BOOST_ROOT environment variable. Further documentation is available at +# . +# +# This macro calls: +# +# AC_SUBST(BOOST_CPPFLAGS) / AC_SUBST(BOOST_LDFLAGS) +# +# And sets: +# +# HAVE_BOOST +# +# LAST MODIFICATION +# +# 2008-04-12 +# +# COPYLEFT +# +# Copyright (c) 2008 Thomas Porschberg +# +# Copying and distribution of this file, with or without modification, are +# permitted in any medium without royalty provided the copyright notice +# and this notice are preserved. + +AC_DEFUN([AX_BOOST_BASE], +[ +AC_ARG_WITH([boost], + AS_HELP_STRING([--with-boost@<:@=DIR@:>@], [use boost (default is yes) - it is possible to specify the root directory for boost (optional)]), + [ + if test "$withval" = "no"; then + want_boost="no" + elif test "$withval" = "yes"; then + want_boost="yes" + ac_boost_path="" + else + want_boost="yes" + ac_boost_path="$withval" + fi + ], + [want_boost="yes"]) + + +AC_ARG_WITH([boost-libdir], + AS_HELP_STRING([--with-boost-libdir=LIB_DIR], + [Force given directory for boost libraries. Note that this will overwrite library path detection, so use this parameter only if default library detection fails and you know exactly where your boost libraries are located.]), + [ + if test -d $withval + then + ac_boost_lib_path="$withval" + else + AC_MSG_ERROR(--with-boost-libdir expected directory name) + fi + ], + [ac_boost_lib_path=""] +) + +if test "x$want_boost" = "xyes"; then + boost_lib_version_req=ifelse([$1], ,1.20.0,$1) + boost_lib_version_req_shorten=`expr $boost_lib_version_req : '\([[0-9]]*\.[[0-9]]*\)'` + boost_lib_version_req_major=`expr $boost_lib_version_req : '\([[0-9]]*\)'` + boost_lib_version_req_minor=`expr $boost_lib_version_req : '[[0-9]]*\.\([[0-9]]*\)'` + boost_lib_version_req_sub_minor=`expr $boost_lib_version_req : '[[0-9]]*\.[[0-9]]*\.\([[0-9]]*\)'` + if test "x$boost_lib_version_req_sub_minor" = "x" ; then + boost_lib_version_req_sub_minor="0" + fi + WANT_BOOST_VERSION=`expr $boost_lib_version_req_major \* 100000 \+ $boost_lib_version_req_minor \* 100 \+ $boost_lib_version_req_sub_minor` + AC_MSG_CHECKING(for boostlib >= $boost_lib_version_req) + succeeded=no + + dnl first we check the system location for boost libraries + dnl this location ist chosen if boost libraries are installed with the --layout=system option + dnl or if you install boost with RPM + if test "$ac_boost_path" != ""; then + BOOST_LDFLAGS="-L$ac_boost_path/lib" + BOOST_CPPFLAGS="-I$ac_boost_path/include" + else + for ac_boost_path_tmp in /usr /usr/local /opt /opt/local ; do + if test -d "$ac_boost_path_tmp/include/boost" && test -r "$ac_boost_path_tmp/include/boost"; then + BOOST_LDFLAGS="-L$ac_boost_path_tmp/lib" + BOOST_CPPFLAGS="-I$ac_boost_path_tmp/include" + break; + fi + done + fi + + dnl overwrite ld flags if we have required special directory with + dnl --with-boost-libdir parameter + if test "$ac_boost_lib_path" != ""; then + BOOST_LDFLAGS="-L$ac_boost_lib_path" + fi + + CPPFLAGS_SAVED="$CPPFLAGS" + CPPFLAGS="$CPPFLAGS $BOOST_CPPFLAGS" + export CPPFLAGS + + LDFLAGS_SAVED="$LDFLAGS" + LDFLAGS="$LDFLAGS $BOOST_LDFLAGS" + export LDFLAGS + + AC_LANG_PUSH(C++) + AC_COMPILE_IFELSE([AC_LANG_PROGRAM([[ + @%:@include + ]], [[ + #if BOOST_VERSION >= $WANT_BOOST_VERSION + // Everything is okay + #else + # error Boost version is too old + #endif + ]])],[ + AC_MSG_RESULT(yes) + succeeded=yes + found_system=yes + ],[ + ]) + AC_LANG_POP([C++]) + + + + dnl if we found no boost with system layout we search for boost libraries + dnl built and installed without the --layout=system option or for a staged(not installed) version + if test "x$succeeded" != "xyes"; then + _version=0 + if test "$ac_boost_path" != ""; then + if test -d "$ac_boost_path" && test -r "$ac_boost_path"; then + for i in `ls -d $ac_boost_path/include/boost-* 2>/dev/null`; do + _version_tmp=`echo $i | sed "s#$ac_boost_path##" | sed 's/\/include\/boost-//' | sed 's/_/./'` + V_CHECK=`expr $_version_tmp \> $_version` + if test "$V_CHECK" = "1" ; then + _version=$_version_tmp + fi + VERSION_UNDERSCORE=`echo $_version | sed 's/\./_/'` + BOOST_CPPFLAGS="-I$ac_boost_path/include/boost-$VERSION_UNDERSCORE" + done + fi + else + for ac_boost_path in /usr /usr/local /opt /opt/local ; do + if test -d "$ac_boost_path" && test -r "$ac_boost_path"; then + for i in `ls -d $ac_boost_path/include/boost-* 2>/dev/null`; do + _version_tmp=`echo $i | sed "s#$ac_boost_path##" | sed 's/\/include\/boost-//' | sed 's/_/./'` + V_CHECK=`expr $_version_tmp \> $_version` + if test "$V_CHECK" = "1" ; then + _version=$_version_tmp + best_path=$ac_boost_path + fi + done + fi + done + + VERSION_UNDERSCORE=`echo $_version | sed 's/\./_/'` + BOOST_CPPFLAGS="-I$best_path/include/boost-$VERSION_UNDERSCORE" + if test "$ac_boost_lib_path" = "" + then + BOOST_LDFLAGS="-L$best_path/lib" + fi + + if test "x$BOOST_ROOT" != "x"; then + if test -d "$BOOST_ROOT" && test -r "$BOOST_ROOT" && test -d "$BOOST_ROOT/stage/lib" && test -r "$BOOST_ROOT/stage/lib"; then + version_dir=`expr //$BOOST_ROOT : '.*/\(.*\)'` + stage_version=`echo $version_dir | sed 's/boost_//' | sed 's/_/./g'` + stage_version_shorten=`expr $stage_version : '\([[0-9]]*\.[[0-9]]*\)'` + V_CHECK=`expr $stage_version_shorten \>\= $_version` + if test "$V_CHECK" = "1" -a "$ac_boost_lib_path" = "" ; then + AC_MSG_NOTICE(We will use a staged boost library from $BOOST_ROOT) + BOOST_CPPFLAGS="-I$BOOST_ROOT" + BOOST_LDFLAGS="-L$BOOST_ROOT/stage/lib" + fi + fi + fi + fi + + CPPFLAGS="$CPPFLAGS $BOOST_CPPFLAGS" + export CPPFLAGS + LDFLAGS="$LDFLAGS $BOOST_LDFLAGS" + export LDFLAGS + + AC_LANG_PUSH(C++) + AC_COMPILE_IFELSE([AC_LANG_PROGRAM([[ + @%:@include + ]], [[ + #if BOOST_VERSION >= $WANT_BOOST_VERSION + // Everything is okay + #else + # error Boost version is too old + #endif + ]])],[ + AC_MSG_RESULT(yes) + succeeded=yes + found_system=yes + ],[ + ]) + AC_LANG_POP([C++]) + fi + + if test "$succeeded" != "yes" ; then + if test "$_version" = "0" ; then + AC_MSG_ERROR([[We could not detect the boost libraries (version $boost_lib_version_req_shorten or higher). If you have a staged boost library (still not installed) please specify \$BOOST_ROOT in your environment and do not give a PATH to --with-boost option. If you are sure you have boost installed, then check your version number looking in . See http://randspringer.de/boost for more documentation.]]) + else + AC_MSG_NOTICE([Your boost libraries seems to old (version $_version).]) + fi + else + AC_SUBST(BOOST_CPPFLAGS) + AC_SUBST(BOOST_LDFLAGS) + AC_DEFINE(HAVE_BOOST,,[define if the Boost library is available]) + fi + + CPPFLAGS="$CPPFLAGS_SAVED" + LDFLAGS="$LDFLAGS_SAVED" +fi + +]) diff --git a/m4/ax_boost_serialization.m4 b/m4/ax_boost_serialization.m4 new file mode 100644 index 000000000..86cb0bf2d --- /dev/null +++ b/m4/ax_boost_serialization.m4 @@ -0,0 +1,111 @@ +# =========================================================================== +# http://www.nongnu.org/autoconf-archive/ax_boost_serialization.html +# =========================================================================== +# +# SYNOPSIS +# +# AX_BOOST_SERIALIZATION +# +# DESCRIPTION +# +# Test for Serialization library from the Boost C++ libraries. The macro +# requires a preceding call to AX_BOOST_BASE. Further documentation is +# available at . +# +# This macro calls: +# +# AC_SUBST(BOOST_SERIALIZATION_LIB) +# +# And sets: +# +# HAVE_BOOST_SERIALIZATION +# +# LICENSE +# +# Copyright (c) 2008 Thomas Porschberg +# +# Copying and distribution of this file, with or without modification, are +# permitted in any medium without royalty provided the copyright notice +# and this notice are preserved. + +AC_DEFUN([AX_BOOST_SERIALIZATION], +[ + AC_ARG_WITH([boost-serialization], + AS_HELP_STRING([--with-boost-serialization@<:@=special-lib@:>@], + [use the Serialization library from boost - it is possible to specify a certain library for the linker + e.g. --with-boost-serialization=boost_serialization-gcc-mt-d-1_33_1 ]), + [ + if test "$withval" = "no"; then + want_boost="no" + elif test "$withval" = "yes"; then + want_boost="yes" + ax_boost_user_serialization_lib="" + else + want_boost="yes" + ax_boost_user_serialization_lib="$withval" + fi + ], + [want_boost="yes"] + ) + + if test "x$want_boost" = "xyes"; then + AC_REQUIRE([AC_PROG_CC]) + CPPFLAGS_SAVED="$CPPFLAGS" + CPPFLAGS="$CPPFLAGS $BOOST_CPPFLAGS" + AC_MSG_WARN(BOOST_CPPFLAGS $BOOST_CPPFLAGS) + export CPPFLAGS + + LDFLAGS_SAVED="$LDFLAGS" + LDFLAGS="$LDFLAGS $BOOST_LDFLAGS" + export LDFLAGS + + AC_CACHE_CHECK(whether the Boost::Serialization library is available, + ax_cv_boost_serialization, + [AC_LANG_PUSH([C++]) + AC_COMPILE_IFELSE(AC_LANG_PROGRAM([[@%:@include + @%:@include + @%:@include + ]], + [[std::ofstream ofs("filename"); + boost::archive::text_oarchive oa(ofs); + return 0; + ]]), + ax_cv_boost_serialization=yes, ax_cv_boost_serialization=no) + AC_LANG_POP([C++]) + ]) + if test "x$ax_cv_boost_serialization" = "xyes"; then + AC_DEFINE(HAVE_BOOST_SERIALIZATION,,[define if the Boost::Serialization library is available]) + BOOSTLIBDIR=`echo $BOOST_LDFLAGS | sed -e 's/@<:@^\/@:>@*//'` + if test "x$ax_boost_user_serialization_lib" = "x"; then + for libextension in `ls $BOOSTLIBDIR/libboost_serialization*.{so,a}* 2>/dev/null | sed 's,.*/,,' | sed -e 's;^lib\(boost_serialization.*\)\.so.*$;\1;' -e 's;^lib\(boost_serialization.*\)\.a*$;\1;'` ; do + ax_lib=${libextension} + AC_CHECK_LIB($ax_lib, exit, + [BOOST_SERIALIZATION_LIB="-l$ax_lib"; AC_SUBST(BOOST_SERIALIZATION_LIB) link_serialization="yes"; break], + [link_serialization="no"]) + done + if test "x$link_serialization" != "xyes"; then + for libextension in `ls $BOOSTLIBDIR/boost_serialization*.{dll,a}* 2>/dev/null | sed 's,.*/,,' | sed -e 's;^\(boost_serialization.*\)\.dll.*$;\1;' -e 's;^\(boost_serialization.*\)\.a*$;\1;'` ; do + ax_lib=${libextension} + AC_CHECK_LIB($ax_lib, exit, + [BOOST_SERIALIZATION_LIB="-l$ax_lib"; AC_SUBST(BOOST_SERIALIZATION_LIB) link_serialization="yes"; break], + [link_serialization="no"]) + done + fi + + else + for ax_lib in $ax_boost_user_serialization_lib boost_serialization-$ax_boost_user_serialization_lib; do + AC_CHECK_LIB($ax_lib, main, + [BOOST_SERIALIZATION_LIB="-l$ax_lib"; AC_SUBST(BOOST_SERIALIZATION_LIB) link_serialization="yes"; break], + [link_serialization="no"]) + done + + fi + if test "x$link_serialization" != "xyes"; then + AC_MSG_ERROR(Could not link against $ax_lib !) + fi + fi + + CPPFLAGS="$CPPFLAGS_SAVED" + LDFLAGS="$LDFLAGS_SAVED" + fi +]) diff --git a/matlab/CHECK.m b/matlab/CHECK.m new file mode 100644 index 000000000..15f8c2b2b --- /dev/null +++ b/matlab/CHECK.m @@ -0,0 +1,5 @@ +function CEHCK(name,assertion) + +if (assertion~=1) + error(['CHECK ' name ' fails']); +end diff --git a/matlab/DOUBLES_EQUAL.m b/matlab/DOUBLES_EQUAL.m new file mode 100644 index 000000000..35b0d68fa --- /dev/null +++ b/matlab/DOUBLES_EQUAL.m @@ -0,0 +1,5 @@ +function DOUBLES_EQUAL(expected,actual,atol) + +if abs(expected-actual)>atol + error(sprintf('DOUBLES_EQUAL fails: expected %g but got %g',expected,actual)); +end diff --git a/matlab/createLinearFactorGraph.m b/matlab/createLinearFactorGraph.m new file mode 100644 index 000000000..abd406598 --- /dev/null +++ b/matlab/createLinearFactorGraph.m @@ -0,0 +1,41 @@ +% create a linear factor graph +% The non-linear graph above evaluated at NoisyConfig +function fg = createLinearFactorGraph() + +c = createNoisyConfig(); + +% Create +fg = LinearFactorGraph; + +% prior on x1 +A11=[10 0; 0 10]; +b = - c.get('x1')/0.1; + +f1 = LinearFactor('x1', A11, b); +fg.push_back(f1); + +% odometry between x1 and x2 +A21=[-10 0; 0 -10]; +A22=[ 10 0; 0 10]; +b = [2;-1]; + +f2 = LinearFactor('x1', A21, 'x2', A22, b); +fg.push_back(f2); + +% measurement between x1 and l1 +A31=[-5 0; 0 -5]; +A32=[ 5 0; 0 5]; +b = [0;1]; + +f3 = LinearFactor('x1', A31, 'l1', A32, b); +fg.push_back(f3); + +% measurement between x2 and l1 +A41=[-5 0; 0 -5]; +A42=[ 5 0; 0 5]; +b = [-1;1.5]; + +f4 = LinearFactor('x2', A41, 'l1', A42, b); +fg.push_back(f4); + +end \ No newline at end of file diff --git a/matlab/createNoisyConfig.m b/matlab/createNoisyConfig.m new file mode 100644 index 000000000..92b2d917d --- /dev/null +++ b/matlab/createNoisyConfig.m @@ -0,0 +1,10 @@ +% A noise configuration +function c = createNoisyConfig() +v_x1 = [0.1; 0.1]; +v_x2 = [1.4; 0.2]; +v_l1 = [0.1; -1.1]; +c = FGConfig(); +c.insert('x1', v_x1); +c.insert('x2', v_x2); +c.insert('l1', v_l1); +end \ No newline at end of file diff --git a/matlab/createZeroDelta.m b/matlab/createZeroDelta.m new file mode 100644 index 000000000..221d480c5 --- /dev/null +++ b/matlab/createZeroDelta.m @@ -0,0 +1,10 @@ +% A zero delta configuration +function c = createZeroDelta() +v_x1 = [0; 0]; +v_x2 = [0; 0]; +v_l1 = [0; 0]; +c = FGConfig(); +c.insert('x1', v_x1); +c.insert('x2', v_x2); +c.insert('l1', v_l1); +end \ No newline at end of file diff --git a/matlab/create_config.m b/matlab/create_config.m new file mode 100644 index 000000000..4e0da5cf5 --- /dev/null +++ b/matlab/create_config.m @@ -0,0 +1,14 @@ +% Christian Potthast +% create a configuration + +function config = create_config(n,m) + +config = FGConfig(); + +for j = 1:n + config.insert(sprintf('m%d',j), [0;0]); +end + +for i = 1:m + config.insert(sprintf('x%d',i), [0;0]); +end \ No newline at end of file diff --git a/matlab/create_linear_factor_graph.m b/matlab/create_linear_factor_graph.m new file mode 100644 index 000000000..523ffdb1b --- /dev/null +++ b/matlab/create_linear_factor_graph.m @@ -0,0 +1,42 @@ +% Christan Potthast +% create linear factor graph + +function lfg = create_linear_factor_graph(config, measurements, odometry, measurement_sigma, odo_sigma, n) + +m = size(measurements,2); + +% create linear factor graph +lfg = LinearFactorGraph(); + +% create prior for initial robot pose +prior = Point2Prior([0;0],0.2,'x1'); +lf = prior.linearize(config); +lfg.push_back(lf); + +% add prior for landmarks +for j = 1:n + key = sprintf('m%d',j); + prior = Point2Prior([0;0],1000,key); + lf = prior.linearize(config); + lfg.push_back(lf); +end + +% add measurement factors +for k = 1 : size(measurements,2) + measurement = measurements{k}; + i = sprintf('x%d',measurement.i); + j = sprintf('m%d',measurement.j); + nlf = Simulated2DMeasurement(measurement.z, measurement_sigma, i, j); + lf = nlf.linearize(config); + lfg.push_back(lf); +end + +% add odometry factors +for i = 2 : size(odometry,2) + odo = odometry{i}; + p = sprintf('x%d',i-1); + c = sprintf('x%d',i); + nlf = Simulated2DOdometry(odo, odo_sigma, p, c); + lf = nlf.linearize(config); + lfg.push_back(lf); +end \ No newline at end of file diff --git a/matlab/create_ordering.m b/matlab/create_ordering.m new file mode 100644 index 000000000..7a6829314 --- /dev/null +++ b/matlab/create_ordering.m @@ -0,0 +1,14 @@ +% Christian Potthast +% create an elimination ordering + +function ord = create_ordering(n,m) + +ord = Ordering(); + +for j = 1:n + ord.push_back(sprintf('m%d',j)); +end + +for i = 1:m + ord.push_back(sprintf('x%d',i)); +end \ No newline at end of file diff --git a/matlab/create_random_landmarks.m b/matlab/create_random_landmarks.m new file mode 100644 index 000000000..60894aa9f --- /dev/null +++ b/matlab/create_random_landmarks.m @@ -0,0 +1,6 @@ +% Christian Potthast +% Create a map with random landmarks + +function map = create_random_landmarks(numberOfLandmarks) + +map = rand(2,numberOfLandmarks)*100; diff --git a/matlab/create_visibility.m b/matlab/create_visibility.m new file mode 100644 index 000000000..91a43d87c --- /dev/null +++ b/matlab/create_visibility.m @@ -0,0 +1,20 @@ +% Christian Potthast +% create visibility matrix + +function V = create_visibility(map,pose,threshold) + +n = size(map,2); +m = size(pose,2); +V = sparse([],[],[],n+m,n+m); + +for t = 1:m + % find measurements within Manhattan range + js = find(2==sum(abs(map-pose(:,t)*ones(1,n))<[threshold;threshold]*ones(1,n))); + for j = js + V(j,t+n)=1; + end + % add in odometry links + if t>1 + V((t+n)-1,t+n)=1; + end +end \ No newline at end of file diff --git a/matlab/example.m b/matlab/example.m new file mode 100644 index 000000000..511db8280 --- /dev/null +++ b/matlab/example.m @@ -0,0 +1,60 @@ +% Set up a small SLAM example in MATLAB +% Authors: Christian Potthast, Frank Dellaert + +clear; + +n = 100; +m = 20; + +% Set up the map +map = create_random_landmarks(n); +figure(1);clf; +plot(map(1,:), map(2,:),'g.'); hold on; + +% have the robot move in this world +trajectory = random_walk([0.1,0.1],5,m); +plot(trajectory(1,:),trajectory(2,:),'b+'); +axis([0 100 0 100]);axis square; + +% Check visibility and plot this on the problem figure +visibility = create_visibility(map, trajectory,10); +gplot(visibility,[map trajectory]'); +figure(2);clf; +spy(visibility) + +% simulate the measurements +measurement_sigma = 1; +odo_sigma = 0.1; +[measurements, odometry] = simulate_measurements(map, trajectory, visibility, measurement_sigma, odo_sigma); + +% create a configuration of all zeroes +config = create_config(n,m); + +% create the factor graph +linearFactorGraph = create_linear_factor_graph(config, measurements, odometry, measurement_sigma, odo_sigma, n); + +% create an ordering +ord = create_ordering(n,m); + +% show the matrix +figure(3); clf; +[A,b] = linearFactorGraph.matrix(ord); +spy(A); + +% eliminate with that ordering +BayesNet = linearFactorGraph.eliminate(ord); + +% show the eliminated matrix +figure(4); clf; +[R,d] = BayesNet.matrix(); +spy(R); + +% optimize in the BayesNet +optimal = BayesNet.optimize; + +% plot the solution +figure(5);clf; +plot_config(optimal,n,m);hold on +plot(trajectory(1,:),trajectory(2,:),'b+'); +plot(map(1,:), map(2,:),'g.'); +axis([0 100 0 100]);axis square; diff --git a/matlab/plot_config.m b/matlab/plot_config.m new file mode 100644 index 000000000..5068d8bc5 --- /dev/null +++ b/matlab/plot_config.m @@ -0,0 +1,19 @@ +% Christian Potthast +% plot a configuration + +function plot_config(config,n,m) + +hold on; + +for j = 1:n + key = sprintf('m%d',j); + mj = config.get(key); + plot(mj(1), mj(2),'r*'); +end + +for i = 1:m + key = sprintf('x%d',i); + xi = config.get(key); + plot(xi(1), xi(2),'rx'); +end + diff --git a/matlab/random_walk.m b/matlab/random_walk.m new file mode 100644 index 000000000..a72a567e0 --- /dev/null +++ b/matlab/random_walk.m @@ -0,0 +1,20 @@ +% Christian Potthast +% random walk from a robot + +function pose = random_walk(initial, velocity, steps) + +pose(:,1) = initial; +bearing = 1; + + +for step = 2:steps + + bearing = bearing + 0.05*randn(); + + pose(1,step) = pose(1,step-1) + sin(bearing) * velocity; + pose(2,step) = pose(2,step-1) + cos(bearing) * velocity; + +end + + + diff --git a/matlab/run_tests.m b/matlab/run_tests.m new file mode 100644 index 000000000..86b66640a --- /dev/null +++ b/matlab/run_tests.m @@ -0,0 +1,4 @@ +% run all matlab unit tests +testLinearFactor +testConditionalGaussian +testLinearFactorGraph diff --git a/matlab/simulate_measurements.m b/matlab/simulate_measurements.m new file mode 100644 index 000000000..31ef3555c --- /dev/null +++ b/matlab/simulate_measurements.m @@ -0,0 +1,23 @@ +% Christian Potthast +% simulate measurements + +function [measurements,odometry] = simulate_measurements(map, pose, visibility, noise_sigma, odo_sigma) +m = size(pose,2); +n = size(map,2); +measurements = {}; +odometry = {}; +k =1; +for i = 1:m + js = find(visibility(1:n,i+n)); + if size(js ,1) > 0 + for j = js' + z = map(:,j)-pose(:,i)+randn(2,1)*noise_sigma; + measurement = struct('z',z,'i',i,'j',j); + measurements{k}=measurement; + k = k+1; + end + end + if i>1 + odometry{i}= pose(:,i)-pose(:,i-1)+randn(2,1)*odo_sigma; + end +end \ No newline at end of file diff --git a/matlab/testConditionalGaussian.m b/matlab/testConditionalGaussian.m new file mode 100644 index 000000000..b4a7c63b8 --- /dev/null +++ b/matlab/testConditionalGaussian.m @@ -0,0 +1,26 @@ +%----------------------------------------------------------------------- +% solve + +expected = [15.0471 ; -18.8824]; + +% create a conditional gaussion node +A1 =[1 2; 3 4]; +A2 = [6 0.2;8 0.4]; +R = [0.1 0.3; 0.0 0.34]; +d=[0.2;0.5]; + +cg = ConditionalGaussian(d, R, 'x1', A1, 'l1', A2); + +sx1 = [0.2;0.5]; +sl1 = [0.5;0.8]; + +solution = FGConfig; +solution.insert('x1', sx1); +solution.insert('l1', sl1); + +result = cg.solve(solution); + +if(~all( abs(expected - result) < 0.0001 )) warning('solve failed'); end + +%----------------------------------------------------------------------- + diff --git a/matlab/testLinearFactor.m b/matlab/testLinearFactor.m new file mode 100644 index 000000000..9f82721dd --- /dev/null +++ b/matlab/testLinearFactor.m @@ -0,0 +1,74 @@ +%----------------------------------------------------------------------- +% eliminate + +% the combined linear factor +Ax2 = [ +-5., 0. ++0.,-5. +10., 0. ++0.,10. +]; + +Al1 = [ +5., 0. +0., 5. +0., 0. +0., 0. +]; + +Ax1 = [ +0.00, 0. % f4 +0.00, 0. % f4 +-10., 0. % f2 +0.00,-10. % f2 +]; + +% the RHS +b2=[-1;1.5;2;-1]; + +combined = LinearFactor('x2', Ax2, 'l1', Al1, 'x1', Ax1, b2); + +% eliminate the combined factor +% NOT WORKING +% this is not working because there is no elimination function for a linear +% factor. Just for the MutableLinearFactor +%[actualCG,actualLF] = combined.eliminate('x2'); + +% create expected Conditional Gaussian +R11 = [ +11.1803, 0.00 +0.00, 11.1803 +]; +S12 = [ +-2.23607, 0.00 ++0.00,-2.23607 +]; +S13 = [ +-8.94427, 0.00 ++0.00,-8.94427 +]; +d=[2.23607;-1.56525]; +expectedCG = ConditionalGaussian(d,R11,'l1',S12,'x1',S13); + +% the expected linear factor +Bl1 = [ +4.47214, 0.00 +0.00, 4.47214 +]; + +Bx1 = [ +% x1 +-4.47214, 0.00 ++0.00, -4.47214 +]; + +% the RHS +b1= [0.0;0.894427]; + +expectedLF = LinearFactor('l1', Bl1, 'x1', Bx1, b1); + +% check if the result matches +% NOT WORKING +% because can not be computed with LinearFactor.eliminate +%if(~actualCG.equals(expectedCG)), warning('GTSAM:unit','~actualCG.equals(expectedCG)'); end +%if(~actualLF.equals(expectedLF,1e-5)), warning('GTSAM:unit','~actualLF.equals(expectedLF,1e-5)');end diff --git a/matlab/testLinearFactorGraph.m b/matlab/testLinearFactorGraph.m new file mode 100644 index 000000000..f48c01ea0 --- /dev/null +++ b/matlab/testLinearFactorGraph.m @@ -0,0 +1,112 @@ +%----------------------------------------------------------------------- +% equals +fg = createLinearFactorGraph(); +fg2 = createLinearFactorGraph(); +CHECK('equals',fg.equals(fg2)); + +%----------------------------------------------------------------------- +% error +cfg = createZeroDelta(); +actual = fg.error(cfg); +DOUBLES_EQUAL( 5.625, actual, 1e-9 ); + +%----------------------------------------------------------------------- +% combine_factors_x1 +fg = createLinearFactorGraph(); +actual = fg.combine_factors('x1'); +Al1 = [ + 0., 0. + 0., 0. + 0., 0. + 0., 0. + 5., 0. + 0., 5. + ]; + +Ax1 = [ + 10., 0. + 0.00, 10. + -10., 0. + 0.00,-10. + -5., 0. + 00., -5. + ]; + +Ax2 = [ + 0., 0. + 0., 0. + 10., 0. + +0.,10. + 0., 0. + 0., 0. + ]; + +b=[-1;-1;2;-1;0;1]; + +expected = LinearFactor('l1',Al1,'x1',Ax1,'x2',Ax2,b); +CHECK('combine_factors_x1', actual.equals(expected,1e-9)); + +%----------------------------------------------------------------------- +% combine_factors_x2 +fg = createLinearFactorGraph(); +actual = fg.combine_factors('x2'); + +%----------------------------------------------------------------------- +% eliminate_x1 +fg = createLinearFactorGraph(); +actual = fg.eliminate_one('x1'); + +%----------------------------------------------------------------------- +% eliminate_x2 +fg = createLinearFactorGraph(); +actual = fg.eliminate_one('x2'); + +%----------------------------------------------------------------------- +% eliminateAll + +R1 = [10, 0.0 + 0.0, 10]; +d1=[-1;-1]; +cg1 = ConditionalGaussian(d1, R1); + +R2 = [6.7082, 0.0 + 0.0, 6.7082]; +A1= [ -6.7082, 0.0 + 0.0, -6.7082]; +d2=[0;1.34164]; +cg2 = ConditionalGaussian(d2, R2, 'x1', A1); + +R3 = [11.1803, 0.0 + 0.0, 11.1803]; +A21 = [ -2.23607, 0.0 + 0.0, -2.23607]; +A22 = [-8.94427, 0.0 + 0.0, -8.94427]; +d3 =[2.23607; -1.56525]; +cg3 = ConditionalGaussian(d3, R3, 'l1', A21, 'x1', A22); + +expected = ChordalBayesNet; +expected.insert('x1', cg1); +expected.insert('l1', cg2); +expected.insert('x2', cg3); + +% Check one ordering +fg1 = createLinearFactorGraph(); +ord1 = Ordering; +ord1.push_back('x2'); +ord1.push_back('l1'); +ord1.push_back('x1'); +actual1 = fg1.eliminate(ord1); +CHECK('eliminateAll', actual1.equals(expected)); + +%----------------------------------------------------------------------- +% matrix + +fg = createLinearFactorGraph(); +ord = Ordering; +ord.push_back('x2'); +ord.push_back('l1'); +ord.push_back('x1'); + +A = fg.matrix(ord); + diff --git a/myconfigure b/myconfigure new file mode 100755 index 000000000..8b40b3a2f --- /dev/null +++ b/myconfigure @@ -0,0 +1 @@ +./configure --prefix=$HOME --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ diff --git a/wrap/.cvsignore b/wrap/.cvsignore new file mode 100644 index 000000000..e9c87a8fc --- /dev/null +++ b/wrap/.cvsignore @@ -0,0 +1,5 @@ +Makefile +Makefile.in +wrap +.deps +.DS_Store diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp new file mode 100644 index 000000000..2f2e31617 --- /dev/null +++ b/wrap/Argument.cpp @@ -0,0 +1,74 @@ +/** + * file: Argument.ccp + * Author: Frank Dellaert + **/ + +#include +#include +#include +#include + +#include "Argument.h" + +using namespace std; + +/* ************************************************************************* */ +void Argument::matlab_unwrap(ofstream& ofs, + const string& matlabName) +{ + // the templated unwrap function returns a pointer + // example: double tol = unwrap< double >(in[2]); + ofs << " "; + + if (is_ptr) + ofs << "shared_ptr<" << type << "> " << name << " = unwrap_shared_ptr< "; + else if (is_ref) + ofs << type << "& " << name << " = *unwrap_shared_ptr< "; + else + ofs << type << " " << name << " = unwrap< "; + + ofs << type << " >(" << matlabName; + if (is_ptr || is_ref) ofs << ", \"" << type << "\""; + ofs << ");" << endl; +} + +/* ************************************************************************* */ +string ArgumentList::types() { + string str; + bool first=true; + BOOST_FOREACH(Argument arg, *this) { + if (!first) str += ","; str += arg.type; first=false; + } + return str; +} + +/* ************************************************************************* */ +string ArgumentList::signature() { + string str; + BOOST_FOREACH(Argument arg, *this) + str += arg.type[0]; + return str; +} + +/* ************************************************************************* */ +string ArgumentList::names() { + string str; + bool first=true; + BOOST_FOREACH(Argument arg, *this) { + if (!first) str += ","; str += arg.name; first=false; + } + return str; +} + +/* ************************************************************************* */ +void ArgumentList::matlab_unwrap(ofstream& ofs, int start) { + int index = start; + BOOST_FOREACH(Argument arg, *this) { + stringstream buf; + buf << "in[" << index << "]"; + arg.matlab_unwrap(ofs,buf.str()); + index++; + } +} + +/* ************************************************************************* */ diff --git a/wrap/Argument.h b/wrap/Argument.h new file mode 100644 index 000000000..bdb86aa54 --- /dev/null +++ b/wrap/Argument.h @@ -0,0 +1,40 @@ +/** + * file: Argument.h + * brief: arguments to constructors and methods + * Author: Frank Dellaert + **/ + +#pragma once + +#include +#include + +// Argument class +struct Argument { + bool is_const, is_ref, is_ptr; + std::string type; + std::string name; +Argument() : is_const(false), is_ref(false), is_ptr(false) {} + + // MATLAB code generation: + void matlab_unwrap(std::ofstream& ofs, + const std::string& matlabName); // MATLAB to C++ +}; + +// Argument list +struct ArgumentList : public std::list { + std::list args; + std::string types (); + std::string signature(); + std::string names (); + + // MATLAB code generation: + + /** + * emit code to unwrap arguments + * @param ofs output stream + * @param start initial index for input array, set to 1 for method + */ + void matlab_unwrap(std::ofstream& ofs, int start=0); // MATLAB to C++ +}; + diff --git a/wrap/Class.cpp b/wrap/Class.cpp new file mode 100644 index 000000000..508dfebbf --- /dev/null +++ b/wrap/Class.cpp @@ -0,0 +1,74 @@ +/** + * file: Class.ccp + * Author: Frank Dellaert + **/ + +#include +#include + +#include + +#include "Class.h" +#include "utilities.h" + +using namespace std; + +/* ************************************************************************* */ +void Class::matlab_proxy(const string& classFile) { + // open destination classFile + ofstream ofs(classFile.c_str()); + if(!ofs) throw CantOpenFile(classFile); + cerr << "generating " << classFile << endl; + + // emit class proxy code + emit_header_comment(ofs,"%"); + ofs << "classdef " << name << endl; + ofs << " properties" << endl; + ofs << " self = 0" << endl; + ofs << " end" << endl; + ofs << " methods" << endl; + ofs << " function obj = " << name << "(varargin)" << endl; + BOOST_FOREACH(Constructor c, constructors) + c.matlab_proxy_fragment(ofs,name); + ofs << " if nargin ~= 13 && obj.self == 0, error('" << name << " constructor failed'); end" << endl; + ofs << " end" << endl; + ofs << " function display(obj), obj.print; end" << endl; + ofs << " function disp(obj), obj.display; end" << endl; + ofs << " end" << endl; + ofs << "end" << endl; + + // close file + ofs.close(); +} + +/* ************************************************************************* */ +void Class::matlab_constructors(const string& toolboxPath,const string& nameSpace) { + BOOST_FOREACH(Constructor c, constructors) { + c.matlab_mfile (toolboxPath, name); + c.matlab_wrapper(toolboxPath, name, nameSpace); + } +} + +/* ************************************************************************* */ +void Class::matlab_methods(const string& classPath, const string& nameSpace) { + BOOST_FOREACH(Method m, methods) { + m.matlab_mfile (classPath); + m.matlab_wrapper(classPath, name, nameSpace); + } +} + +/* ************************************************************************* */ +void Class::matlab_make_fragment(ofstream& ofs, + const string& toolboxPath, + const string& mexFlags) +{ + string mex = "mex " + mexFlags + " "; + BOOST_FOREACH(Constructor c, constructors) + ofs << mex << c.matlab_wrapper_name(name) << ".cpp" << endl; + ofs << endl << "cd @" << name << endl; + BOOST_FOREACH(Method m, methods) + ofs << mex << m.name << ".cpp" << endl; + ofs << endl; +} + +/* ************************************************************************* */ diff --git a/wrap/Class.h b/wrap/Class.h new file mode 100644 index 000000000..4aac1c0a4 --- /dev/null +++ b/wrap/Class.h @@ -0,0 +1,31 @@ +/** + * file: Class.h + * brief: describe the C++ class that is being wrapped + * Author: Frank Dellaert + **/ + +#pragma once + +#include +#include + +#include "Constructor.h" +#include "Method.h" + +// Class has name, constructors, methods +struct Class { + std::string name; + std::list constructors; + std::list methods; + + // MATLAB code generation: + void matlab_proxy(const std::string& classFile); // proxy class + void matlab_constructors(const std::string& toolboxPath, + const std::string& nameSpace); // constructor wrappers + void matlab_methods(const std::string& classPath, + const std::string& nameSpace); // method wrappers + void matlab_make_fragment(std::ofstream& ofs, + const std::string& toolboxPath, + const std::string& mexFlags); // make fragment +}; + diff --git a/wrap/Constructor.cpp b/wrap/Constructor.cpp new file mode 100644 index 000000000..e4b54f680 --- /dev/null +++ b/wrap/Constructor.cpp @@ -0,0 +1,89 @@ +/** + * file: Constructor.ccp + * Author: Frank Dellaert + **/ + +#include +#include + +#include + +#include "utilities.h" +#include "Constructor.h" + +using namespace std; + +/* ************************************************************************* */ +string Constructor::matlab_wrapper_name(const string& className) { + string str = "new_" + className + "_" + args.signature(); + return str; +} + +/* ************************************************************************* */ +void Constructor::matlab_proxy_fragment(ofstream& ofs, const string& className) { + ofs << " if nargin == " << args.size() << ", obj.self = " + << matlab_wrapper_name(className) << "("; + bool first = true; + for(size_t i=0;i" << endl; + ofs << "#include <" << className << ".h>" << endl; + if (!nameSpace.empty()) ofs << "using namespace " << nameSpace << ";" << endl; + ofs << "void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])" << endl; + ofs << "{" << endl; + ofs << " checkArguments(\"" << name << "\",nargout,nargin," << args.size() << ");" << endl; + args.matlab_unwrap(ofs); // unwrap arguments + ofs << " " << className << "* self = new " << className << "(" << args.names() << ");" << endl; + ofs << " out[0] = wrap_constructed(self,\"" << className << "\");" << endl; + ofs << "}" << endl; + + // close file + ofs.close(); +} + +/* ************************************************************************* */ diff --git a/wrap/Constructor.h b/wrap/Constructor.h new file mode 100644 index 000000000..1a7236868 --- /dev/null +++ b/wrap/Constructor.h @@ -0,0 +1,29 @@ +/** + * file: Constructor.h + * brief: class describing a constructor + code generation + * Author: Frank Dellaert + **/ + +#pragma once + +#include +#include + +#include "Argument.h" + +// Constructor class +struct Constructor { + ArgumentList args; + + // MATLAB code generation + // toolboxPath is main toolbox directory, e.g., ../matlab + // classFile is class proxy file, e.g., ../matlab/@Point2/Point2.m + + std::string matlab_wrapper_name(const std::string& className); // wrapper name + void matlab_proxy_fragment(std::ofstream& ofs, const std::string& className); // proxy class fragment + void matlab_mfile (const std::string& toolboxPath, const std::string& className); // m-file + void matlab_wrapper(const std::string& toolboxPath, + const std::string& className, + const std::string& nameSpace); // wrapper +}; + diff --git a/wrap/Makefile.am b/wrap/Makefile.am new file mode 100644 index 000000000..d25a8a74e --- /dev/null +++ b/wrap/Makefile.am @@ -0,0 +1,25 @@ +INSTALL = install + +AM_CXXFLAGS = -MMD -I$(boost) -I.. -O5 + +noinst_PROGRAMS = wrap +wrap_SOURCES = utilities.cpp Argument.cpp Constructor.cpp Method.cpp Class.cpp Module.cpp wrap.cpp + +# generate local toolbox dir +interfacePath = ../cpp +moduleName = gtsam +toolboxpath = ../toolbox +nameSpace = "gtsam" +mexFlags = "-I${boost} -I${prefix}/include -I${prefix}/include/gtsam -L${exec_prefix}/lib -lgtsam" +all: + ./wrap ${interfacePath} ${moduleName} ${toolboxpath} ${nameSpace} ${mexFlags} + +# install the header files +include_HEADERS = utilities.h Argument.h Constructor.h Method.h Class.h Module.h wrap-matlab.h + +install: all + +# clean local toolbox dir +clean: + @test -z "wrap" || rm -f wrap + @test -z "../toolbox" || rm -rf ../toolbox diff --git a/wrap/Method.cpp b/wrap/Method.cpp new file mode 100644 index 000000000..469634381 --- /dev/null +++ b/wrap/Method.cpp @@ -0,0 +1,128 @@ +/** + * file: Method.ccp + * Author: Frank Dellaert + **/ + +#include +#include + +#include + +#include "Method.h" +#include "utilities.h" + +using namespace std; + +/* ************************************************************************* */ +// auxiliary function to wrap an argument into a shared_ptr template +/* ************************************************************************* */ +string maybe_shared_ptr(bool add, const string& type) { + string str = add? "shared_ptr<" : ""; + str += type; + if (add) str += ">"; + return str; +} + +/* ************************************************************************* */ +string Method::return_type(bool add_ptr, pairing p) { + if (p==pair && returns_pair) { + string str = "pair< " + + maybe_shared_ptr(add_ptr && returns_ptr, returns ) + ", " + + maybe_shared_ptr(add_ptr && returns_ptr, returns2) + " >"; + return str; + } else + return maybe_shared_ptr(add_ptr && returns_ptr, (p==arg2)? returns2 : returns); +} + +/* ************************************************************************* */ +void Method::matlab_mfile(const string& classPath) { + + // open destination m-file + string wrapperFile = classPath + "/" + name + ".m"; + ofstream ofs(wrapperFile.c_str()); + if(!ofs) throw CantOpenFile(wrapperFile); + cerr << "generating " << wrapperFile << endl; + + // generate code + emit_header_comment(ofs, "%"); + ofs << "% usage: obj." << name << "(" << args.names() << ")" << endl; + string returnType = returns_pair? "[first,second]" : "result"; + ofs << "function " << returnType << " = " << name << "(obj"; + if (args.size()) ofs << "," << args.names(); + ofs << ")" << endl; + ofs << " error('need to compile " << name << ".cpp');" << endl; + ofs << "end" << endl; + + // close file + ofs.close(); +} + +/* ************************************************************************* */ +void Method::matlab_wrapper(const string& classPath, + const string& className, + const string& nameSpace) +{ + // open destination wrapperFile + string wrapperFile = classPath + "/" + name + ".cpp"; + ofstream ofs(wrapperFile.c_str()); + if(!ofs) throw CantOpenFile(wrapperFile); + cerr << "generating " << wrapperFile << endl; + + // generate code + + // header + emit_header_comment(ofs, "//"); + ofs << "#include \n"; + ofs << "#include <" << className << ".h>\n"; + if (!nameSpace.empty()) ofs << "using namespace " << nameSpace << ";" << endl; + + // call + ofs << "void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; + // start + ofs << "{\n"; + + // check arguments + // extra argument obj -> nargin-1 is passed ! + // example: checkArguments("equals",nargout,nargin-1,2); + ofs << " checkArguments(\"" << name << "\",nargout,nargin-1," << args.size() << ");\n"; + + // get class pointer + // example: shared_ptr = unwrap_shared_ptr< Test >(in[0], "Test"); + ofs << " shared_ptr<" << className << "> self = unwrap_shared_ptr< " << className + << " >(in[0],\"" << className << "\");" << endl; + + // unwrap arguments, see Argument.cpp + args.matlab_unwrap(ofs,1); + + // call method + // example: bool result = self->return_field(t); + ofs << " "; + if (returns!="void") + ofs << return_type(true,pair) << " result = "; + ofs << "self->" << name << "(" << args.names() << ");\n"; + + // wrap result + // example: out[0]=wrap(result); + if (returns_pair) { + if (returns_ptr) + ofs << " out[0] = wrap_shared_ptr(result.first,\"" << returns << "\");\n"; + else + ofs << " out[0] = wrap< " << return_type(true,arg1) << " >(result.first);\n"; + if (returns_ptr2) + ofs << " out[1] = wrap_shared_ptr(result.second,\"" << returns2 << "\");\n"; + else + ofs << " out[1] = wrap< " << return_type(true,arg2) << " >(result.second);\n"; + } + else if (returns_ptr) + ofs << " out[0] = wrap_shared_ptr(result,\"" << returns << "\");\n"; + else if (returns!="void") + ofs << " out[0] = wrap< " << return_type(true,arg1) << " >(result);\n"; + + // finish + ofs << "}\n"; + + // close file + ofs.close(); +} + +/* ************************************************************************* */ diff --git a/wrap/Method.h b/wrap/Method.h new file mode 100644 index 000000000..590feadbf --- /dev/null +++ b/wrap/Method.h @@ -0,0 +1,34 @@ +/** + * file: Method.h + * brief: describes and generates code for methods + * Author: Frank Dellaert + **/ + +#pragma once + +#include +#include + +#include "Argument.h" + +// Method class +struct Method { + bool is_const; + ArgumentList args; + std::string returns, returns2, name; + bool returns_ptr, returns_ptr2, returns_pair; + +Method() : returns_ptr(false), returns_ptr2(false), returns_pair(false) {} + + enum pairing {arg1, arg2, pair}; + std::string return_type(bool add_ptr, pairing p); + + // MATLAB code generation + // classPath is class directory, e.g., ../matlab/@Point2 + + void matlab_mfile (const std::string& classPath); // m-file + void matlab_wrapper(const std::string& classPath, + const std::string& className, + const std::string& nameSpace); // wrapper +}; + diff --git a/wrap/Module.cpp b/wrap/Module.cpp new file mode 100644 index 000000000..4f042df55 --- /dev/null +++ b/wrap/Module.cpp @@ -0,0 +1,217 @@ +/** + * file: Module.ccp + * Author: Frank Dellaert + **/ + +#include +#include + +//#define BOOST_SPIRIT_DEBUG +#include +#include + +#include "Module.h" +#include "utilities.h" + +using namespace std; +using namespace BOOST_SPIRIT_CLASSIC_NS; + +typedef rule Rule; + +/* ************************************************************************* */ +// We parse an interface file into a Module object. +// The grammar is defined using the boost/spirit combinatorial parser. +// For example, str_p("const") parses the string "const", and the >> +// operator creates a sequence parser. The grammar below, composed of rules +// and with start rule [class_p], doubles as the specs for our interface files. +/* ************************************************************************* */ + +Module::Module(const string& interfacePath, + const string& moduleName) : name(moduleName) +{ + // these variables will be imperatively updated to gradually build [cls] + // The one with postfix 0 are used to reset the variables after parse. + Argument arg0, arg; + ArgumentList args0, args; + Constructor constructor0, constructor; + Method method0, method; + Class cls0,cls; + + //---------------------------------------------------------------------------- + // Grammar with actions that build the Class object. Actions are + // defined within the square brackets [] and are executed whenever a + // rule is successfully parsed. Define BOOST_SPIRIT_DEBUG to debug. + // The grammar is allows a very restricted C++ header: + // - No comments allowed. + // -Only types allowed are string, bool, size_t int, double, Vector, and Matrix + // as well as class names that start with an uppercase letter + // - The types unsigned int and bool should be specified as int. + // ---------------------------------------------------------------------------- + + // lexeme_d turns off white space skipping + // http://www.boost.org/doc/libs/1_37_0/libs/spirit/classic/doc/directives.html + + Rule className_p = lexeme_d[upper_p >> *(alnum_p | '_')]; + + Rule classPtr_p = + className_p [assign_a(arg.type)] >> + ch_p('*') [assign_a(arg.is_ptr,true)]; + + Rule classRef_p = + !str_p("const") [assign_a(arg.is_const,true)] >> + className_p [assign_a(arg.type)] >> + ch_p('&') [assign_a(arg.is_ref,true)]; + + Rule basisType_p = + (str_p("string") | "bool" | "size_t" | "int" | "double"); + + Rule ublasType = + (str_p("Vector") | "Matrix")[assign_a(arg.type)] >> + !ch_p('*')[assign_a(arg.is_ptr,true)]; + + Rule name_p = lexeme_d[alpha_p >> *(alnum_p | '_')]; + + Rule argument_p = + ((basisType_p[assign_a(arg.type)] | ublasType | classPtr_p | classRef_p) >> name_p[assign_a(arg.name)]) + [push_back_a(args, arg)] + [assign_a(arg,arg0)]; + + Rule argumentList_p = !argument_p >> * (',' >> argument_p); + + Rule constructor_p = + (className_p >> '(' >> argumentList_p >> ')' >> ';') + [assign_a(constructor.args,args)] + [assign_a(args,args0)] + [push_back_a(cls.constructors, constructor)] + [assign_a(constructor,constructor0)]; + + Rule returnType1_p = + basisType_p[assign_a(method.returns)] | + ((str_p("Vector") | "Matrix" | className_p)[assign_a(method.returns)] >> + !ch_p('*') [assign_a(method.returns_ptr,true)]); + + Rule returnType2_p = + basisType_p[assign_a(method.returns2)] | + ((str_p("Vector") | "Matrix" | className_p)[assign_a(method.returns2)] >> + !ch_p('*') [assign_a(method.returns_ptr2,true)]); + + Rule pair_p = + (str_p("pair") >> '<' >> returnType1_p >> ',' >> returnType2_p >> '>') + [assign_a(method.returns_pair,true)]; + + Rule void_p = str_p("void")[assign_a(method.returns)]; + + Rule returnType_p = void_p | returnType1_p | pair_p; + + Rule methodName_p = lexeme_d[lower_p >> *(alnum_p | '_')]; + + Rule method_p = + (returnType_p >> methodName_p[assign_a(method.name)] >> + '(' >> argumentList_p >> ')' >> + !str_p("const")[assign_a(method.is_const,true)] >> ';') + [assign_a(method.args,args)] + [assign_a(args,args0)] + [push_back_a(cls.methods, method)] + [assign_a(method,method0)]; + + Rule class_p = str_p("class") >> className_p[assign_a(cls.name)] >> '{' >> + *constructor_p >> + *method_p >> + '}' >> ";"; + + Rule module_p = +class_p + [push_back_a(classes,cls)] + [assign_a(cls,cls0)] + >> !end_p; + + //---------------------------------------------------------------------------- + // for debugging, define BOOST_SPIRIT_DEBUG +# ifdef BOOST_SPIRIT_DEBUG + BOOST_SPIRIT_DEBUG_NODE(className_p); + BOOST_SPIRIT_DEBUG_NODE(classPtr_p); + BOOST_SPIRIT_DEBUG_NODE(classRef_p); + BOOST_SPIRIT_DEBUG_NODE(basisType_p); + BOOST_SPIRIT_DEBUG_NODE(name_p); + BOOST_SPIRIT_DEBUG_NODE(argument_p); + BOOST_SPIRIT_DEBUG_NODE(argumentList_p); + BOOST_SPIRIT_DEBUG_NODE(constructor_p); + BOOST_SPIRIT_DEBUG_NODE(returnType1_p); + BOOST_SPIRIT_DEBUG_NODE(returnType2_p); + BOOST_SPIRIT_DEBUG_NODE(pair_p); + BOOST_SPIRIT_DEBUG_NODE(void_p); + BOOST_SPIRIT_DEBUG_NODE(returnType_p); + BOOST_SPIRIT_DEBUG_NODE(methodName_p); + BOOST_SPIRIT_DEBUG_NODE(method_p); + BOOST_SPIRIT_DEBUG_NODE(class_p); + BOOST_SPIRIT_DEBUG_NODE(module_p); +# endif + //---------------------------------------------------------------------------- + + // read interface file + string interfaceFile = interfacePath + "/" + moduleName + ".h"; + string contents = file_contents(interfaceFile); + + // Comment parser : does not work for some reason + rule<> comment_p = str_p("/*") >> +anychar_p >> "*/"; + rule<> skip_p = space_p; // | comment_p; + + // and parse contents + parse_info info = parse(contents.c_str(), module_p, skip_p); + if(!info.full) { + printf("parsing stopped at \n%.20s\n",info.stop); + throw ParseFailed(info.length); + } +} + +/* ************************************************************************* */ +void Module::matlab_code(const string& toolboxPath, + const string& nameSpace, + const string& mexFlags) +{ + try { + string installCmd = "install -d " + toolboxPath; + system(installCmd.c_str()); + + // create make m-file + string makeFile = toolboxPath + "/make_" + name + ".m"; + ofstream ofs(makeFile.c_str()); + if(!ofs) throw CantOpenFile(makeFile); + + cerr << "generating " << makeFile << endl; + emit_header_comment(ofs,"%"); + ofs << "echo on" << endl << endl; + ofs << "toolboxpath = pwd" << endl; + ofs << "addpath(toolboxpath);" << endl << endl; + + // generate proxy classes and wrappers + BOOST_FOREACH(Class cls, classes) { + // create directory if needed + string classPath = toolboxPath + "/@" + cls.name; + string installCmd = "install -d " + classPath; + system(installCmd.c_str()); + + // create proxy class + string classFile = classPath + "/" + cls.name + ".m"; + cls.matlab_proxy(classFile); + + // create constructor and method wrappers + cls.matlab_constructors(toolboxPath,nameSpace); + cls.matlab_methods(classPath,nameSpace); + + // add lines to make m-file + ofs << "cd(toolboxpath)" << endl; + cls.matlab_make_fragment(ofs, toolboxPath, mexFlags); + } + + // finish make m-file + ofs << "cd(toolboxpath)" << endl << endl; + ofs << "echo off" << endl; + ofs.close(); + } + catch(exception &e) { + cerr << "generate_matlab_toolbox failed because " << e.what() << endl; + } + +} + +/* ************************************************************************* */ diff --git a/wrap/Module.h b/wrap/Module.h new file mode 100644 index 000000000..292da4ed6 --- /dev/null +++ b/wrap/Module.h @@ -0,0 +1,32 @@ +/** + * file: Module.h + * brief: describes module to be wrapped + * Author: Frank Dellaert + **/ + +#pragma once + +#include +#include + +#include "Class.h" + +// A module has classes +struct Module { + std::string name; + std::list classes; + + /** + * constructor that parses interface file + */ + Module(const std::string& interfacePath, + const std::string& moduleName); + + /** + * MATLAB code generation: + */ + void matlab_code(const std::string& path, + const std::string& nameSpace, + const std::string& mexFlags); +}; + diff --git a/wrap/hello.cpp b/wrap/hello.cpp new file mode 100644 index 000000000..e6401bfc7 --- /dev/null +++ b/wrap/hello.cpp @@ -0,0 +1,15 @@ +// example for wrapping python with boost +// from http://www.boost.org/doc/libs/1_37_0/libs/python/doc/tutorial/doc/html/index.html + +char const* greet() +{ + return "hello, world"; +} + +#include + +BOOST_PYTHON_MODULE(hello_ext) +{ + using namespace boost::python; + def("greet", greet); +} diff --git a/wrap/testSpirit.cpp b/wrap/testSpirit.cpp new file mode 100644 index 000000000..307cb452d --- /dev/null +++ b/wrap/testSpirit.cpp @@ -0,0 +1,94 @@ +/** + * Unit test for Boost's awesome Spirit parser + * Author: Frank Dellaert + **/ + +#include +#include +#include + +using namespace std; +using namespace BOOST_SPIRIT_CLASSIC_NS; + +typedef rule Rule; + +/* ************************************************************************* */ +// lexeme_d turns off white space skipping +// http://www.boost.org/doc/libs/1_37_0/libs/spirit/classic/doc/directives.html +Rule name_p = lexeme_d[alpha_p >> *(alnum_p | '_')]; +Rule className_p = lexeme_d[upper_p >> *(alnum_p | '_')]; +Rule methodName_p = lexeme_d[lower_p >> *(alnum_p | '_')]; + +Rule basisType_p = (str_p("string") | "bool" | "size_t" | "int" | "double" | "Vector" | "Matrix"); + +/* ************************************************************************* */ +TEST( spirit, real ) { + // check if we can parse 8.99 as a real + CHECK(parse("8.99", real_p, space_p).full); + // make sure parsing fails on this one + CHECK(!parse("zztop", real_p, space_p).full); +} + +/* ************************************************************************* */ +TEST( spirit, string ) { + // check if we can parse a string + CHECK(parse("double", str_p("double"), space_p).full); +} + +/* ************************************************************************* */ +TEST( spirit, sequence ) { + // check that we skip white space + CHECK(parse("int int", str_p("int") >> *str_p("int"), space_p).full); + CHECK(parse("int --- - -- -", str_p("int") >> *ch_p('-'), space_p).full); + CHECK(parse("const \t string", str_p("const") >> str_p("string"), space_p).full); + + // not that (see spirit FAQ) the vanilla rule<> does not deal with whitespace + rule<>vanilla_p = str_p("const") >> str_p("string"); + CHECK(!parse("const \t string", vanilla_p, space_p).full); + + // to fix it, we need to use + rulephrase_level_p = str_p("const") >> str_p("string"); + CHECK(parse("const \t string", phrase_level_p, space_p).full); +} + +/* ************************************************************************* */ +// parser for interface files + +// const string reference reference +Rule constStringRef_p = + str_p("const") >> "string" >> '&'; + +// class reference +Rule classRef_p = className_p >> '&'; + +// const class reference +Rule constClassRef_p = str_p("const") >> classRef_p; + +// method parsers +Rule constMethod_p = basisType_p >> methodName_p >> '(' >> ')' >> "const" >> ';'; + +/* ************************************************************************* */ +TEST( spirit, basisType_p ) { + CHECK(!parse("Point3", basisType_p, space_p).full); + CHECK(parse("string", basisType_p, space_p).full); +} + +/* ************************************************************************* */ +TEST( spirit, className_p ) { + CHECK(parse("Point3", className_p, space_p).full); +} + +/* ************************************************************************* */ +TEST( spirit, classRef_p ) { + CHECK(parse("Point3 &", classRef_p, space_p).full); + CHECK(parse("Point3&", classRef_p, space_p).full); +} + +/* ************************************************************************* */ +TEST( spirit, constMethod_p ) { + CHECK(parse("double norm() const;", constMethod_p, space_p).full); +} + +/* ************************************************************************* */ +int main() { TestResult tr; TestRegistry::runAllTests(tr); return 0; } +/* ************************************************************************* */ diff --git a/wrap/testWrap.cpp b/wrap/testWrap.cpp new file mode 100644 index 000000000..44e7d7ceb --- /dev/null +++ b/wrap/testWrap.cpp @@ -0,0 +1,91 @@ +/** + * Unit test for wrap.c + * Author: Frank Dellaert + **/ + +#include +#include +#include +#include + +#include "utilities.h" +#include "Module.h" + +using namespace std; + +/* ************************************************************************* */ +TEST( wrap, ArgumentList ) { + ArgumentList args; + Argument arg; arg.type = "double"; arg.name = "x"; + args.push_back(arg); + args.push_back(arg); + args.push_back(arg); + CHECK(args.signature()=="ddd"); + CHECK(args.types()=="double,double,double"); + CHECK(args.names()=="x,x,x"); +} + +/* ************************************************************************* */ +TEST( wrap, parse ) { + Module module("../interfaces", "geometry"); + CHECK(module.classes.size()==3); + + // check second class, Point3 + Class cls = *(++module.classes.begin()); + CHECK(cls.name=="Point3"); + CHECK(cls.constructors.size()==1); + CHECK(cls.methods.size()==1); + + // first constructor takes 3 doubles + Constructor c1 = cls.constructors.front(); + CHECK(c1.args.size()==3); + + // check first double argument + Argument a1 = c1.args.front(); + CHECK(!a1.is_const); + CHECK(a1.type=="double"); + CHECK(!a1.is_ref); + CHECK(a1.name=="x"); + + // check method + Method m1 = cls.methods.front(); + CHECK(m1.returns=="double"); + CHECK(m1.name=="norm"); + CHECK(m1.args.size()==0); + CHECK(m1.is_const); +} + +/* ************************************************************************* */ +TEST( wrap, matlab_code ) { + // Parse into class object + Module module("../interfaces","geometry"); + + // emit MATLAB code + // make_geometry will not compile, use make testwrap to generate real make + module.matlab_code("/Users/dellaert/projects/gtsam/matlab", "", "-O5"); + + CHECK(files_equal("../matlab/@Point2/Point2.m" , "../matlab/@Point2/Point2-expected.m" )); + CHECK(files_equal("../matlab/@Point2/x.cpp" , "../matlab/@Point2/x-expected.cpp" )); + + CHECK(files_equal("../matlab/@Point3/Point3.m" , "../matlab/@Point3/Point3-expected.m" )); + CHECK(files_equal("../matlab/new_Point3_ddd.m" , "../matlab/new_Point3_ddd-expected.m" )); + CHECK(files_equal("../matlab/new_Point3_ddd.cpp", "../matlab/new_Point3_ddd-expected.cpp")); + CHECK(files_equal("../matlab/@Point3/norm.m" , "../matlab/@Point3/norm-expected.m" )); + CHECK(files_equal("../matlab/@Point3/norm.cpp" , "../matlab/@Point3/norm-expected.cpp" )); + + CHECK(files_equal("../matlab/new_Test_.cpp" , "../matlab/new_Test_-expected.cpp" )); + CHECK(files_equal("../matlab/@Test/Test.m" , "../matlab/@Test/Test-expected.m" )); + CHECK(files_equal("../matlab/@Test/return_string.cpp" , "../matlab/@Test/return_string-expected.cpp" )); + CHECK(files_equal("../matlab/@Test/return_pair.cpp" , "../matlab/@Test/return_pair-expected.cpp" )); + CHECK(files_equal("../matlab/@Test/return_field.cpp" , "../matlab/@Test/return_field-expected.cpp" )); + CHECK(files_equal("../matlab/@Test/return_TestPtr.cpp", "../matlab/@Test/return_TestPtr-expected.cpp")); + CHECK(files_equal("../matlab/@Test/return_ptrs.cpp" , "../matlab/@Test/return_ptrs-expected.cpp" )); + CHECK(files_equal("../matlab/@Test/print.m" , "../matlab/@Test/print-expected.m" )); + CHECK(files_equal("../matlab/@Test/print.cpp" , "../matlab/@Test/print-expected.cpp" )); + + CHECK(files_equal("../matlab/make_geometry.m" , "../matlab/make_geometry_expected.m" )); +} + +/* ************************************************************************* */ +int main() { TestResult tr; TestRegistry::runAllTests(tr); return 0; } +/* ************************************************************************* */ diff --git a/wrap/utilities.cpp b/wrap/utilities.cpp new file mode 100644 index 000000000..c41b0c06e --- /dev/null +++ b/wrap/utilities.cpp @@ -0,0 +1,57 @@ +/** + * file: utilities.ccp + * Author: Frank Dellaert + **/ + +#include +#include + +#include + +#include "utilities.h" + +using namespace std; +using namespace boost::gregorian; + +/* ************************************************************************* */ +string file_contents(const string& filename, bool skipheader) { + ifstream ifs(filename.c_str()); + if(!ifs) throw CantOpenFile(filename); + + // read file into stringstream + stringstream ss; + if (skipheader) ifs.ignore(256,'\n'); + ss << ifs.rdbuf(); + ifs.close(); + + // return string + return ss.str(); +} + +/* ************************************************************************* */ +bool files_equal(const string& actual, const string& expected, bool skipheader) { + try { + string actual_contents = file_contents(actual, skipheader); + string expected_contents = file_contents(expected); + bool equal = actual_contents == expected_contents; + if (!equal) { + stringstream command; + command << "diff " << actual << " " << expected << endl; + system(command.str().c_str()); + } + return equal; + } + catch (const string& reason) { + cerr << "expection: " << reason << endl; + return false; + } + return true; +} + +/* ************************************************************************* */ +void emit_header_comment(ofstream& ofs, const string& delimiter) { + date today = day_clock::local_day(); + ofs << delimiter << " automatically generated by wrap on " << today << endl; +} + +/* ************************************************************************* */ diff --git a/wrap/utilities.h b/wrap/utilities.h new file mode 100644 index 000000000..be7da3c23 --- /dev/null +++ b/wrap/utilities.h @@ -0,0 +1,49 @@ +/** + * file: utilities.ccp + * Author: Frank Dellaert + **/ + +#pragma once + +#include +#include + +class CantOpenFile : public std::exception { + private: + std::string filename_; + public: + CantOpenFile(const std::string& filename) : filename_(filename) {} + ~CantOpenFile() throw() {} + virtual const char* what() const throw() { + return ("Can't open file " + filename_).c_str(); + } +}; + +class ParseFailed : public std::exception { + private: + int length_; + public: + ParseFailed(int length) : length_(length) {} + ~ParseFailed() throw() {} + virtual const char* what() const throw() { + std::stringstream buf; + buf << "Parse failed at character " << (length_+1); + return buf.str().c_str(); + } +}; + +/** + * read contents of a file into a std::string + */ +std::string file_contents(const std::string& filename, bool skipheader=false); + +/** + * Check whether two files are equal + * By default, skips the first line of actual so header is not generated + */ +bool files_equal(const std::string& actual, const std::string& expected, bool skipheader=true); + +/** + * emit a header at the top of generated files + */ +void emit_header_comment(std::ofstream& ofs, const std::string& delimiter); diff --git a/wrap/wrap-matlab.h b/wrap/wrap-matlab.h new file mode 100644 index 000000000..28d428ecb --- /dev/null +++ b/wrap/wrap-matlab.h @@ -0,0 +1,420 @@ +// header file to be included in MATLAB wrappers +// Copyright (c) 2008 Frank Dellaert, All Rights reserved +// wrapping and unwrapping is done using specialized templates, see +// http://www.cplusplus.com/doc/tutorial/templates.html + +extern "C" { +#include +} + +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace boost; // not usual, but for consiseness of generated code + +typedef numeric::ublas::vector Vector; +typedef numeric::ublas::matrix Matrix; + +//***************************************************************************** +// Utilities +//***************************************************************************** + +void error(const char* str) { + mexErrMsgIdAndTxt("wrap:error", str); +} + +mxArray *scalar(mxClassID classid) { + mwSize dims[1]; dims[0]=1; + return mxCreateNumericArray(1, dims, classid, mxREAL); +} + +mxArray *vector(int m, mxClassID classid) { + mwSize dims[1]; dims[0]=m; + return mxCreateNumericArray(1, dims, classid, mxREAL); +} + +mxArray *matrix(int m, int n, mxClassID classid) { + mwSize dims[2]; dims[0]=m; dims[1]=n; + return mxCreateNumericArray(2, dims, mxUINT32_CLASS, mxREAL); +} + +//***************************************************************************** +// Check arguments +//***************************************************************************** + +void checkArguments(const string& name, int nargout, int nargin, int expected) { + stringstream err; + err << name << " expects " << expected << " arguments, not " << nargin; + if (nargin!=expected) + error(err.str().c_str()); +} + +//***************************************************************************** +// wrapping C++ basis types in MATLAB arrays +//***************************************************************************** + +// default wrapping throws an error: only basis types are allowed in wrap +template +mxArray* wrap(Class& value) { + error("wrap internal error: attempted wrap of invalid type"); +} + +// specialization to string +// wraps into a character array +template<> +mxArray* wrap(string& value) { + return mxCreateString(value.c_str()); +} + +// specialization to bool -> uint32 +// Warning: relies on sizeof(UINT32_T)==sizeof(bool) +template<> +mxArray* wrap(bool& value) { + mxArray *result = scalar(mxUINT32_CLASS); + *(bool*)mxGetData(result) = value; + return result; +} + +// specialization to size_t -> uint32 +// Warning: relies on sizeof(UINT32_T)==sizeof(size_t) +template<> +mxArray* wrap(size_t& value) { + mxArray *result = scalar(mxUINT32_CLASS); + *(size_t*)mxGetData(result) = value; + return result; +} + +// specialization to int -> uint32 +// Warning: relies on sizeof(INT32_T)==sizeof(int) +template<> +mxArray* wrap(int& value) { + mxArray *result = scalar(mxINT32_CLASS); + *(int*)mxGetData(result) = value; + return result; +} + +// specialization to double -> just double +template<> +mxArray* wrap(double& value) { + return mxCreateDoubleScalar(value); +} + +// wrap a const BOOST vector into a double vector +mxArray* wrap_Vector(const Vector& v) { + int m = v.size(); + mxArray *result = mxCreateDoubleMatrix(m, 1, mxREAL); + double *data = mxGetPr(result); + for (int i=0;i double vector +template<> +mxArray* wrap(Vector& v) { + return wrap_Vector(v); +} + +// const version +template<> +mxArray* wrap(const Vector& v) { + return wrap_Vector(v); +} + +// wrap a const BOOST MATRIX into a double matrix +mxArray* wrap_Matrix(const Matrix& A) { + int m = A.size1(), n = A.size2(); + mxArray *result = mxCreateDoubleMatrix(m, n, mxREAL); + double *data = mxGetPr(result); + // converts from column-major to row-major + for (int j=0;j double matrix +template<> +mxArray* wrap(Matrix& A) { + return wrap_Matrix(A); +} + +// const version +template<> +mxArray* wrap(const Matrix& A) { + return wrap_Matrix(A); +} + +//***************************************************************************** +// unwrapping MATLAB arrays into C++ basis types +//***************************************************************************** + +// default unwrapping throws an error +// as wrap only supports passing a reference or one of the basic types +template +T unwrap(const mxArray* array) { + error("wrap internal error: attempted unwrap of invalid type"); +} + +// specialization to string +// expects a character array +// Warning: relies on mxChar==char +template<> +string unwrap(const mxArray* array) { + char *data = mxArrayToString(array); + if (data==NULL) error("unwrap: not a character array"); + string str(data); + mxFree(data); + return str; +} + +// specialization to bool +// returns a pointer to the array data itself +// Warning: relies on sizeof(UINT32_T)==sizeof(bool) +template<> +bool unwrap(const mxArray* array) { + return *(bool*)mxGetData(array); +} + +// specialization to size_t +// returns a pointer to the array data itself +// Warning: relies on sizeof(UINT32_T)==sizeof(size_t) +template<> +size_t unwrap(const mxArray* array) { + return *(size_t*)mxGetData(array); +} + +// specialization to int +// returns a pointer to the array data itself +// Warning: relies on sizeof(INT32_T)==sizeof(int) +template<> +int unwrap(const mxArray* array) { + return *(int*)mxGetData(array); +} + +// specialization to double +// returns a pointer to the array data itself +template<> +double unwrap(const mxArray* array) { + return *(double*)mxGetData(array); +} + +// specialization to BOOST vector +template<> +Vector unwrap< Vector >(const mxArray* array) { + int m = mxGetM(array), n = mxGetN(array); + if (mxIsDouble(array)==false || n!=1) error("unwrap: not a vector"); + double* data = (double*)mxGetData(array); + Vector v(m); + copy(data,data+m,v.begin()); + return v; +} + +// specialization to BOOST matrix +template<> +Matrix unwrap< Matrix >(const mxArray* array) { + if (mxIsDouble(array)==false) error("unwrap: not a matrix"); + int m = mxGetM(array), n = mxGetN(array); + double* data = (double*)mxGetData(array); + Matrix A(m,n); + // converts from row-major to column-major + for (int j=0;j class Collector; + +template +class ObjectHandle { +private: + ObjectHandle* signature; // use 'this' as a unique object signature + const std::type_info* type; // type checking information + shared_ptr t; // object pointer + +public: + // Constructor for free-store allocated objects. + // Creates shared pointer, will delete if is last one to hold pointer +ObjectHandle(T* ptr) : type(&typeid(T)), t(shared_ptr(ptr)) { + signature = this; + Collector::register_handle(this); + } + + // Constructor for shared pointers + // Creates shared pointer, will delete if is last one to hold pointer +ObjectHandle(shared_ptr ptr) : type(&typeid(T)), t(ptr) { + signature= this; + } + + ~ObjectHandle() { + // object is in shared_ptr, will be automatically deleted + signature= 0; // destroy signature + } + + // Get the actual object contained by handle + shared_ptr get_object() const { return t; } + + // Print the mexhandle for debugging + void print(const char* str) { + mexPrintf("mexhandle %s:\n", str); + mexPrintf(" signature = %d:\n", signature); + mexPrintf(" pointer = %d:\n", t.get()); + } + + // Convert ObjectHandle to a mxArray handle (to pass back from mex-function). + // Create a numeric array as handle for an ObjectHandle. + // We ASSUME we can store object pointer in the mxUINT32 element of mxArray. + mxArray* to_mex_handle() + { + mxArray* handle = mxCreateNumericMatrix(1, 1, mxUINT32_CLASS, mxREAL); + *reinterpret_cast**>(mxGetPr(handle)) = this; + return handle; + } + + string type_name() const {return type->name();} + + // Convert mxArray (passed to mex-function) to an ObjectHandle. + // Import a handle from MatLab as a mxArray of UINT32. Check that + // it is actually a pointer to an ObjectHandle. + static ObjectHandle* from_mex_handle(const mxArray* handle) + { + if (mxGetClassID(handle) != mxUINT32_CLASS + || mxIsComplex(handle) || mxGetM(handle)!=1 || mxGetN(handle)!=1) + error("Parameter is not an ObjectHandle type."); + + // We *assume* we can store ObjectHandle pointer in the mxUINT32 of handle + ObjectHandle* obj = *reinterpret_cast(mxGetPr(handle)); + + if (!obj) // gross check to see we don't have an invalid pointer + error("Parameter is NULL. It does not represent an ObjectHandle object."); + // TODO: change this for max-min check for pointer values + + if (obj->signature != obj) // check memory has correct signature + error("Parameter does not represent an ObjectHandle object."); + +/* + if (*(obj->type) != typeid(T)) { // check type + mexPrintf("Given: <%s>, Required: <%s>.\n", obj->type_name(), typeid(T).name()); + error("Given ObjectHandle does not represent the correct type."); + } +*/ + + return obj; + } + + friend class Collector; // allow Collector access to signature +}; + +// --------------------------------------------------------- +// ------------------ Garbage Collection ------------------- +// --------------------------------------------------------- + +// Garbage collection singleton (one collector object for each type T). +// Ensures that registered handles are deleted when the dll is released (they +// may also be deleted previously without problem). +// The Collector provides protection against resource leaks in the case +// where 'clear all' is called in MatLab. (This is because MatLab will call +// the destructors of statically allocated objects but not free-store allocated +// objects.) +template +class Collector { + typedef ObjectHandle Handle; + typedef std::list< Handle* > ObjList; + typedef typename ObjList::iterator iterator; + ObjList objlist; +public: + ~Collector() { + for (iterator i= objlist.begin(); i!=objlist.end(); ++i) { + if ((*i)->signature == *i) // check for valid signature + delete *i; + } + } + + static void register_handle (Handle* obj) { + static Collector singleton; + singleton.objlist.push_back(obj); + } + +private: // prevent construction + Collector() {} + Collector(const Collector&); +}; + +//***************************************************************************** +// wrapping C++ objects in a MATLAB proxy class +//***************************************************************************** + +/* + For every C++ class Class, a matlab proxy class @Class/Class.m object + is created. Its constructor will check which of the C++ constructors + needs to be called, based on nr of arguments. It then calls the + corresponding mex function new_Class_signature, which will create a + C++ object using new, and pass the pointer to wrap_constructed + (below). This creates a mexhandle and returns it to the proxy class + constructor, which assigns it to self. Matlab owns this handle now. +*/ +template +mxArray* wrap_constructed(Class* pointer, const char *classname) { + ObjectHandle* handle = new ObjectHandle(pointer); + return handle->to_mex_handle(); +} + +/* + [create_object] creates a MATLAB proxy class object with a mexhandle + in the self property. Matlab does not allow the creation of matlab + objects from within mex files, hence we resort to an ugly trick: we + invoke the proxy class constructor by calling MATLAB, and pass 13 + dummy arguments to let the constructor know we want an object without + the self property initialized. We then assign the mexhandle to self. +*/ +// TODO: think about memory +mxArray* create_object(const char *classname, mxArray* h) { + mxArray *result; + mxArray* dummy[13] = {h,h,h,h,h, h,h,h,h,h, h,h,h}; + mexCallMATLAB(1,&result,13,dummy,classname); + mxSetProperty(result, 0, "self", h); + return result; +} + +/* + When the user calls a method that returns a shared pointer, we create + an ObjectHandle from the shared_pointer and return it as a proxy + class to matlab. +*/ +template +mxArray* wrap_shared_ptr(shared_ptr< Class > shared_ptr, const char *classname) { + ObjectHandle* handle = new ObjectHandle(shared_ptr); + return create_object(classname,handle->to_mex_handle()); +} + +//***************************************************************************** +// unwrapping a MATLAB proxy class to a C++ object reference +//***************************************************************************** + +/* + Besides the basis types, the only other argument type allowed is a + shared pointer to a C++ object. In this case, matlab needs to pass a + proxy class object to the mex function. [unwrap_shared_ptr] extracts + the ObjectHandle from the self property, and returns a shared pointer + to the object. +*/ +template +shared_ptr unwrap_shared_ptr(const mxArray* obj, const string& className) { + bool isClass = mxIsClass(obj, className.c_str()); + if (!isClass) { + mexPrintf("Expected %s, got %s\n", className.c_str(), mxGetClassName(obj)); + error("Argument has wrong type."); + } + mxArray* mxh = mxGetProperty(obj,0,"self"); + if (mxh==NULL) error("unwrap_reference: invalid wrap object"); + ObjectHandle* handle = ObjectHandle::from_mex_handle(mxh); + return handle->get_object(); +} + +//***************************************************************************** diff --git a/wrap/wrap.cpp b/wrap/wrap.cpp new file mode 100644 index 000000000..07da297ec --- /dev/null +++ b/wrap/wrap.cpp @@ -0,0 +1,47 @@ +/** + * file: wrap.ccp + * brief: wraps functions + * Author: Frank Dellaert + **/ + +#include +#include + +#include "Module.h" + +using namespace std; + +/* ************************************************************************* */ +/** + * main function to wrap a module + */ +void generate_matlab_toolbox(const string& interfacePath, + const string& moduleName, + const string& toolboxPath, + const string& nameSpace, + const string& mexFlags) +{ + // Parse into class object + Module module(interfacePath, moduleName); + + // emit MATLAB code + module.matlab_code(toolboxPath,nameSpace,mexFlags); +} + +/* ************************************************************************* */ + +int main(int argc, const char* argv[]) { + if (argc<5 || argc>6) { + cerr << "wrap parses an interface file and produces a MATLAB toolbox" << endl; + cerr << "usage: wrap interfacePath moduleName toolboxPath" << endl; + cerr << " interfacePath : *absolute* path to directory of module interface file" << endl; + cerr << " moduleName : the name of the module, interface file must be called moduleName.h" << endl; + cerr << " toolboxPath : the directory in which to generate the wrappers" << endl; + cerr << " nameSpace : namespace to use, pass empty string if none" << endl; + cerr << " [mexFlags] : extra flags for the mex command" << endl; + } + else + generate_matlab_toolbox(argv[1],argv[2],argv[3],argv[4],argc==5 ? " " : argv[5]); +} + +/* ************************************************************************* */