diff --git a/.cproject b/.cproject
index 706f18e26..f41da3e32 100644
--- a/.cproject
+++ b/.cproject
@@ -317,7 +317,6 @@
make
-
clean
true
true
@@ -477,6 +476,7 @@
make
+
testBayesTree.run
true
false
@@ -484,7 +484,6 @@
make
-
testSymbolicBayesNet.run
true
false
@@ -492,6 +491,7 @@
make
+
testSymbolicFactorGraph.run
true
false
@@ -683,6 +683,7 @@
make
+
testGraph.run
true
false
@@ -738,7 +739,6 @@
make
-
testSimulated2D.run
true
false
@@ -786,6 +786,7 @@
make
+
testErrors.run
true
false
@@ -793,6 +794,7 @@
make
+
testDSF.run
true
true
@@ -808,6 +810,7 @@
make
+
testConstraintOptimizer.run
true
true
@@ -815,12 +818,19 @@
make
-
testBTree.run
true
true
true
+
+make
+
+testSimulated2DOriented.run
+true
+false
+true
+
make
-j2
diff --git a/cpp/Makefile.am b/cpp/Makefile.am
index fd170310a..143a97729 100644
--- a/cpp/Makefile.am
+++ b/cpp/Makefile.am
@@ -191,7 +191,9 @@ headers += Simulated2DOdometry.h Simulated2DMeasurement.h Simulated2DOrientedOdo
sources += simulated2D.cpp simulated2DOriented.cpp
testSimulated2D_SOURCES = testSimulated2D.cpp
testSimulated2D_LDADD = libgtsam.la
-check_PROGRAMS += testSimulated2D
+testSimulated2DOriented_SOURCES = testSimulated2DOriented.cpp
+testSimulated2DOriented_LDADD = libgtsam.la
+check_PROGRAMS += testSimulated2D testSimulated2DOriented
# simulated3D example
sources += Simulated3D.cpp
diff --git a/cpp/simulated2DOriented.cpp b/cpp/simulated2DOriented.cpp
index 602870315..662866db2 100644
--- a/cpp/simulated2DOriented.cpp
+++ b/cpp/simulated2DOriented.cpp
@@ -28,9 +28,7 @@ namespace gtsam {
/* ************************************************************************* */
Pose2 odo(const Pose2& x1, const Pose2& x2, boost::optional H1,
boost::optional H2) {
- if (H1) *H1 = -I;
- if (H2) *H2 = I;
- return between(x1, x2);
+ return between(x1, x2, H1, H2);
}
/* ************************************************************************* */
diff --git a/cpp/testSimulated2DOriented b/cpp/testSimulated2DOriented
new file mode 100755
index 000000000..482c9d47b
--- /dev/null
+++ b/cpp/testSimulated2DOriented
@@ -0,0 +1,130 @@
+#! /bin/sh
+
+# testSimulated2DOriented - temporary wrapper script for .libs/testSimulated2DOriented
+# Generated by ltmain.sh (GNU libtool) 2.2.6b
+#
+# The testSimulated2DOriented program cannot be directly executed until all the libtool
+# libraries that it depends on are installed.
+#
+# This wrapper script should never be moved out of the build directory.
+# If it is, it will not operate correctly.
+
+# Sed substitution that helps us do robust quoting. It backslashifies
+# metacharacters that are still active within double-quoted strings.
+Xsed='/usr/bin/sed -e 1s/^X//'
+sed_quote_subst='s/\([`"$\]\)/\\1/g'
+
+# Be Bourne compatible
+if test -n "${ZSH_VERSION+set}" && (emulate sh) >/dev/null 2>&1; then
+ emulate sh
+ NULLCMD=:
+ # Zsh 3.x and 4.x performs word splitting on ${1+"$@"}, which
+ # is contrary to our usage. Disable this feature.
+ alias -g '${1+"$@"}'='"$@"'
+ setopt NO_GLOB_SUBST
+else
+ case `(set -o) 2>/dev/null` in *posix*) set -o posix;; esac
+fi
+BIN_SH=xpg4; export BIN_SH # for Tru64
+DUALCASE=1; export DUALCASE # for MKS sh
+
+# The HP-UX ksh and POSIX shell print the target directory to stdout
+# if CDPATH is set.
+(unset CDPATH) >/dev/null 2>&1 && unset CDPATH
+
+relink_command=""
+
+# This environment variable determines our operation mode.
+if test "$libtool_install_magic" = "%%%MAGIC variable%%%"; then
+ # install mode needs the following variables:
+ generated_by_libtool_version='2.2.6b'
+ notinst_deplibs=' libgtsam.la'
+else
+ # When we are sourced in execute mode, $file and $ECHO are already set.
+ if test "$libtool_execute_magic" != "%%%MAGIC variable%%%"; then
+ ECHO="echo"
+ file="$0"
+ # Make sure echo works.
+ if test "X$1" = X--no-reexec; then
+ # Discard the --no-reexec flag, and continue.
+ shift
+ elif test "X`{ $ECHO ' '; } 2>/dev/null`" = 'X '; then
+ # Yippee, $ECHO works!
+ :
+ else
+ # Restart under the correct shell, and then maybe $ECHO will work.
+ exec /bin/sh "$0" --no-reexec ${1+"$@"}
+ fi
+ fi
+
+ # Find the directory that this script lives in.
+ thisdir=`$ECHO "X$file" | $Xsed -e 's%/[^/]*$%%'`
+ test "x$thisdir" = "x$file" && thisdir=.
+
+ # Follow symbolic links until we get to the real thisdir.
+ file=`ls -ld "$file" | /usr/bin/sed -n 's/.*-> //p'`
+ while test -n "$file"; do
+ destdir=`$ECHO "X$file" | $Xsed -e 's%/[^/]*$%%'`
+
+ # If there was a directory component, then change thisdir.
+ if test "x$destdir" != "x$file"; then
+ case "$destdir" in
+ [\/]* | [A-Za-z]:[\/]*) thisdir="$destdir" ;;
+ *) thisdir="$thisdir/$destdir" ;;
+ esac
+ fi
+
+ file=`$ECHO "X$file" | $Xsed -e 's%^.*/%%'`
+ file=`ls -ld "$thisdir/$file" | /usr/bin/sed -n 's/.*-> //p'`
+ done
+
+
+ # Usually 'no', except on cygwin/mingw when embedded into
+ # the cwrapper.
+ WRAPPER_SCRIPT_BELONGS_IN_OBJDIR=no
+ if test "$WRAPPER_SCRIPT_BELONGS_IN_OBJDIR" = "yes"; then
+ # special case for '.'
+ if test "$thisdir" = "."; then
+ thisdir=`pwd`
+ fi
+ # remove .libs from thisdir
+ case "$thisdir" in
+ *[\/].libs ) thisdir=`$ECHO "X$thisdir" | $Xsed -e 's%[\/][^\/]*$%%'` ;;
+ .libs ) thisdir=. ;;
+ esac
+ fi
+
+ # Try to get the absolute directory name.
+ absdir=`cd "$thisdir" && pwd`
+ test -n "$absdir" && thisdir="$absdir"
+
+ program='testSimulated2DOriented'
+ progdir="$thisdir/.libs"
+
+
+ if test -f "$progdir/$program"; then
+ # Add our own library path to DYLD_LIBRARY_PATH
+ DYLD_LIBRARY_PATH="/Users/nikai/borg/gtsam/cpp/.libs:$DYLD_LIBRARY_PATH"
+
+ # Some systems cannot cope with colon-terminated DYLD_LIBRARY_PATH
+ # The second colon is a workaround for a bug in BeOS R4 sed
+ DYLD_LIBRARY_PATH=`$ECHO "X$DYLD_LIBRARY_PATH" | $Xsed -e 's/::*$//'`
+
+ export DYLD_LIBRARY_PATH
+
+ if test "$libtool_execute_magic" != "%%%MAGIC variable%%%"; then
+ # Run the actual program with our arguments.
+
+ exec "$progdir/$program" ${1+"$@"}
+
+ $ECHO "$0: cannot exec $program $*" 1>&2
+ exit 1
+ fi
+ else
+ # The program doesn't exist.
+ $ECHO "$0: error: \`$progdir/$program' does not exist" 1>&2
+ $ECHO "This script is just a wrapper for $program." 1>&2
+ echo "See the libtool documentation for more information." 1>&2
+ exit 1
+ fi
+fi
diff --git a/cpp/testSimulated2DOriented.cpp b/cpp/testSimulated2DOriented.cpp
new file mode 100644
index 000000000..23cb32452
--- /dev/null
+++ b/cpp/testSimulated2DOriented.cpp
@@ -0,0 +1,67 @@
+/*
+ * testSimulated2DOriented.cpp
+ *
+ * Created on: Jun 10, 2010
+ * Author: nikai
+ * Description: unit tests for simulated2DOriented
+ */
+
+#include
+#include
+
+#include "numericalDerivative.h"
+#include "Simulated2DConfig.h"
+#include "simulated2DOriented.h"
+#include "Simulated2DOrientedOdometry.h"
+
+using namespace gtsam;
+using namespace std;
+using namespace simulated2DOriented;
+
+/* ************************************************************************* */
+TEST( simulated2DOriented, Simulated2DConfig )
+{
+ Simulated2DConfig actual;
+ actual.insertPose(1,Point2(1,1));
+ actual.insertPoint(2,Point2(2,2));
+ CHECK(assert_equal(actual,actual,1e-9));
+}
+
+/* ************************************************************************* */
+TEST( simulated2DOriented, Dprior )
+{
+ Pose2 x(1,-9, 0.1);
+ Matrix numerical = numericalDerivative11(prior,x);
+ Matrix computed;
+ prior(x,computed);
+ CHECK(assert_equal(numerical,computed,1e-9));
+}
+
+/* ************************************************************************* */
+ TEST( simulated2DOriented, DOdo )
+{
+ Pose2 x1(1,-9,0.1),x2(-5,6,0.2);
+ Matrix H1,H2;
+ odo(x1,x2,H1,H2);
+ Matrix A1 = numericalDerivative21(odo,x1,x2);
+ CHECK(assert_equal(A1,H1,1e-9));
+ Matrix A2 = numericalDerivative22(odo,x1,x2);
+ CHECK(assert_equal(A2,H2,1e-9));
+}
+
+/* ************************************************************************* */
+TEST( simulated2DOriented, constructor )
+{
+ Pose2 measurement(0.2, 0.3, 0.1);
+ SharedDiagonal model(Vector_(3, 1., 1., 1.));
+ Simulated2DOrientedOdometry factor(measurement, model, PoseKey(1), PoseKey(2));
+
+ Config config;
+ config.insert(PoseKey(1), Pose2(1., 0., 0.2));
+ config.insert(PoseKey(2), Pose2(2., 0., 0.1));
+ boost::shared_ptr lf = factor.linearize(config);
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
+/* ************************************************************************* */