Pose2Prior test
Frank Dellaert
parent
2605d18152
commit
4914091c87
32
.cproject
32
.cproject
|
|
@ -617,22 +617,6 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testUrbanGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testUrbanGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testUrbanConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testUrbanConfig.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testUrbanMeasurement.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -729,10 +713,10 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testBearingFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testPlanarSLAM.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testBearingFactor.run</buildTarget>
|
||||
<buildTarget>testPlanarSLAM.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
@ -753,10 +737,18 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testRangeFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testTupleConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testRangeFactor.run</buildTarget>
|
||||
<buildTarget>testTupleConfig.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testPose2Prior.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testPose2Prior.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
|
|||
|
|
@ -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 +=
|
||||
|
|
|
|||
|
|
@ -25,11 +25,10 @@ private:
|
|||
|
||||
public:
|
||||
|
||||
typedef NonlinearFactor<Pose2Config> Base;
|
||||
typedef boost::shared_ptr<Pose2Prior> 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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -0,0 +1,85 @@
|
|||
/**
|
||||
* @file testPose2Prior.cpp
|
||||
* @brief Unit tests for Pose2Prior Class
|
||||
* @authors Frank Dellaert, Viorela Ila
|
||||
**/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#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<GaussianFactor> 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<GaussianFactor> 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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue