created some unit tests to help viorela to debug

.cproject

@@ -317,7 +317,6 @@
 </target>
 <target name="clean" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>clean</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -477,6 +476,7 @@
 </target>
 <target name="testBayesTree.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testBayesTree.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>false</useDefaultCommand>
@@ -484,7 +484,6 @@
 </target>
 <target name="testSymbolicBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testSymbolicBayesNet.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>false</useDefaultCommand>
@@ -492,6 +491,7 @@
 </target>
 <target name="testSymbolicFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testSymbolicFactorGraph.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>false</useDefaultCommand>
@@ -683,6 +683,7 @@
 </target>
 <target name="testGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testGraph.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>false</useDefaultCommand>
@@ -738,7 +739,6 @@
 </target>
 <target name="testSimulated2D.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testSimulated2D.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>false</useDefaultCommand>
@@ -786,6 +786,7 @@
 </target>
 <target name="testErrors.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testErrors.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>false</useDefaultCommand>
@@ -793,6 +794,7 @@
 </target>
 <target name="testDSF.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testDSF.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -808,6 +810,7 @@
 </target>
 <target name="testConstraintOptimizer.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testConstraintOptimizer.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -815,12 +818,19 @@
 </target>
 <target name="testBTree.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testBTree.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
 <runAllBuilders>true</runAllBuilders>
 </target>
+<target name="testSimulated2DOriented.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+<buildCommand>make</buildCommand>
+<buildArguments/>
+<buildTarget>testSimulated2DOriented.run</buildTarget>
+<stopOnError>true</stopOnError>
+<useDefaultCommand>false</useDefaultCommand>
+<runAllBuilders>true</runAllBuilders>
+</target>
 <target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
 <buildArguments>-j2</buildArguments>

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 

cpp/simulated2DOriented.cpp

@@ -28,9 +28,7 @@ namespace gtsam {
 		/* ************************************************************************* */
 		Pose2 odo(const Pose2& x1, const Pose2& x2, boost::optional<Matrix&> H1,
 				boost::optional<Matrix&> H2) {
-			if (H1) *H1 = -I;
+			return between(x1, x2, H1, H2);
-			if (H2) *H2 = I;
-			return between(x1, x2);
 		}
 
 	/* ************************************************************************* */

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

cpp/testSimulated2DOriented.cpp

@@ -0,0 +1,67 @@
+/*
+ * testSimulated2DOriented.cpp
+ *
+ *   Created on: Jun 10, 2010
+ *       Author: nikai
+ *  Description: unit tests for simulated2DOriented
+ */
+
+#include <iostream>
+#include <CppUnitLite/TestHarness.h>
+
+#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<GaussianFactor> lf = factor.linearize(config);
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
+/* ************************************************************************* */