diff --git a/.cproject b/.cproject index 05e61f875..050d377b8 100644 --- a/.cproject +++ b/.cproject @@ -617,22 +617,6 @@ true true - -make --j2 -testUrbanGraph.run -true -true -true - - -make --j2 -testUrbanConfig.run -true -true -true - make -j2 @@ -729,10 +713,10 @@ true true - + make -j2 -testBearingFactor.run +testPlanarSLAM.run true true true @@ -753,10 +737,18 @@ true true - + make -j2 -testRangeFactor.run +testTupleConfig.run +true +true +true + + +make +-j2 +testPose2Prior.run true true true diff --git a/cpp/Makefile.am b/cpp/Makefile.am index 5680b7e9d..886538b81 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -193,13 +193,15 @@ headers += BetweenFactor.h LieConfig.h LieConfig-inl.h TupleConfig.h TupleConfig # 2D Pose constraints headers += Pose2Prior.h sources += Pose2Graph.cpp -check_PROGRAMS += testPose2Factor testPose2Config testPose2Graph +check_PROGRAMS += testPose2Factor testPose2Config testPose2Graph testPose2Prior testPose2Factor_SOURCES = $(example) testPose2Factor.cpp testPose2Factor_LDADD = libgtsam.la testPose2Config_SOURCES = $(example) testPose2Config.cpp testPose2Config_LDADD = libgtsam.la testPose2Graph_SOURCES = $(example) testPose2Graph.cpp testPose2Graph_LDADD = libgtsam.la +testPose2Prior_SOURCES = $(example) testPose2Prior.cpp +testPose2Prior_LDADD = libgtsam.la # 2D SLAM using Bearing and Range headers += diff --git a/cpp/Pose2Prior.h b/cpp/Pose2Prior.h index 9124c7f03..b777fcd83 100644 --- a/cpp/Pose2Prior.h +++ b/cpp/Pose2Prior.h @@ -25,11 +25,10 @@ private: public: - typedef NonlinearFactor Base; typedef boost::shared_ptr shared_ptr; // shorthand for a smart pointer to a factor Pose2Prior(const Key& key, const Pose2& measured, const Matrix& measurement_covariance) : - key_(key),measured_(measured),Base(1.0) { + key_(key),measured_(measured) { square_root_inverse_covariance_ = inverse_square_root(measurement_covariance); keys_.push_back(key); } diff --git a/cpp/testPose2Prior b/cpp/testPose2Prior new file mode 100755 index 000000000..b4dd9129a --- /dev/null +++ b/cpp/testPose2Prior @@ -0,0 +1,130 @@ +#! /bin/sh + +# testPose2Prior - temporary wrapper script for .libs/testPose2Prior +# Generated by ltmain.sh (GNU libtool) 2.2.6 +# +# The testPose2Prior 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.6' + 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="/bin/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 '\t'; } 2>/dev/null`" = 'X\t'; 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='testPose2Prior' + progdir="$thisdir/.libs" + + + if test -f "$progdir/$program"; then + # Add our own library path to DYLD_LIBRARY_PATH + DYLD_LIBRARY_PATH="/Users/dellaert/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 + /bin/echo "See the libtool documentation for more information." 1>&2 + exit 1 + fi +fi diff --git a/cpp/testPose2Prior.cpp b/cpp/testPose2Prior.cpp new file mode 100644 index 000000000..a4dffc5dc --- /dev/null +++ b/cpp/testPose2Prior.cpp @@ -0,0 +1,85 @@ +/** + * @file testPose2Prior.cpp + * @brief Unit tests for Pose2Prior Class + * @authors Frank Dellaert, Viorela Ila + **/ + +#include +#include "numericalDerivative.h" +#include "Pose2Prior.h" + +using namespace std; +using namespace gtsam; + +// Common measurement covariance +static double sx=0.5, sy=0.5,st=0.1; +static Matrix covariance = Matrix_(3,3, + sx*sx, 0.0, 0.0, + 0.0, sy*sy, 0.0, + 0.0, 0.0, st*st + ); + +/* ************************************************************************* */ +// Very simple test establishing Ax-b \approx z-h(x) +TEST( Pose2Prior, error ) +{ + // Choose a linearization point + Pose2 p1(1, 0, 0); // robot at (1,0) + Pose2Config x0; + x0.insert(1, p1); + + // Create factor + Pose2Prior factor(1, p1, covariance); + + // Actual linearization + boost::shared_ptr linear = factor.linearize(x0); + + // Check error at x0, i.e. delta = zero ! + VectorConfig delta; + delta.insert("x1", zero(3)); + Vector error_at_zero = Vector_(3,0.0,0.0,0.0); + CHECK(assert_equal(error_at_zero,factor.error_vector(x0))); + CHECK(assert_equal(-error_at_zero,linear->error_vector(delta))); + + // Check error after increasing p2 + VectorConfig plus = delta + VectorConfig("x1", Vector_(3, 0.1, 0.0, 0.0)); + Pose2Config x1 = expmap(x0, plus); + Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 ! + CHECK(assert_equal(error_at_plus,factor.error_vector(x1))); + CHECK(assert_equal(error_at_plus,linear->error_vector(plus))); +} + +/* ************************************************************************* */ +// common Pose2Prior for tests below +static Pose2 prior(2,2,M_PI_2); +static Pose2Prior factor(1,prior, covariance); + +/* ************************************************************************* * +// The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector +// Hence i.e., b = approximates z-h(x0) = error_vector(x0) +Vector h(const Pose2& p1) { + return factor.evaluateError(p1); +} + +/* ************************************************************************* * +TEST( Pose2Prior, linearize ) +{ + // Choose a linearization point at ground truth + Pose2Config x0; + x0.insert(1,prior); + + // Actual linearization + boost::shared_ptr actual = factor.linearize(x0); + CHECK(assert_equal(expected,*actual)); + + // Numerical do not work out because BetweenFactor is approximate ? + Matrix numericalH = numericalDerivative11(h, prior, 1e-5); + CHECK(assert_equal(expectedH,numericalH)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */