diff --git a/.cproject b/.cproject index 19fa7e086..32ddccfa1 100644 --- a/.cproject +++ b/.cproject @@ -600,10 +600,10 @@ true true - + make -j2 -testPose3Factor.run +testPose3Config.run true true true @@ -719,10 +719,10 @@ false true - + make -j2 -testPose3Graph.run +testPose3SLAM.run true true true diff --git a/cpp/Makefile.am b/cpp/Makefile.am index 06371c0cb..d90a5f098 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -214,15 +214,15 @@ testPlanarSLAM_SOURCES = $(example) testPlanarSLAM.cpp testPlanarSLAM_LDADD = libgtsam.la # 3D Pose constraints -headers += Pose3Factor.h -sources += Pose3Config.cpp Pose3Graph.cpp -check_PROGRAMS += testPose3Factor testPose3Config testPose3Graph +headers += +sources += pose3SLAM.cpp +check_PROGRAMS += testPose3Factor testPose3Config testPose3SLAM testPose3Factor_SOURCES = $(example) testPose3Factor.cpp testPose3Factor_LDADD = libgtsam.la testPose3Config_SOURCES = $(example) testPose3Config.cpp testPose3Config_LDADD = libgtsam.la -testPose3Graph_SOURCES = $(example) testPose3Graph.cpp -testPose3Graph_LDADD = libgtsam.la +testPose3SLAM_SOURCES = $(example) testPose3SLAM.cpp +testPose3SLAM_LDADD = libgtsam.la # Cameras sources += CalibratedCamera.cpp SimpleCamera.cpp @@ -280,3 +280,12 @@ CXXLINK = $(LIBTOOL) $(AM_LIBTOOLFLAGS) $(LIBTOOLFLAGS) --mode=link \ # rule to run an executable %.run: % libgtsam.la ./$^ + +# to compile colamd and cppunitlite first when make all and make install. +# The part after external_libs is copied from automatically generated Makefile when without the following line +libgtsam.la: external_libs $(libgtsam_la_OBJECTS) $(libgtsam_la_DEPENDENCIES) + $(libgtsam_la_LINK) -rpath $(libdir) $(libgtsam_la_OBJECTS) $(libgtsam_la_LIBADD) $(LIBS) + + + + \ No newline at end of file diff --git a/cpp/Pose3Config.cpp b/cpp/Pose3Config.cpp deleted file mode 100644 index 574bf88fd..000000000 --- a/cpp/Pose3Config.cpp +++ /dev/null @@ -1,40 +0,0 @@ -/** - * @file Pose3Config.cpp - * @brief Configuration of 3D poses - * @author Frank Dellaert - */ - -#include "Pose3Config.h" -#include "LieConfig-inl.h" - -using namespace std; - -namespace gtsam { - - /** Explicit instantiation of templated methods and functions */ - template class LieConfig,Pose3>; - template size_t dim(const Pose3Config& c); - template Pose3Config expmap(const Pose3Config& c, const Vector& delta); - template Pose3Config expmap(const Pose3Config& c, const VectorConfig& delta); - - /* ************************************************************************* */ - // TODO: local version, should probably defined in LieConfig - static string symbol(char c, int index) { - stringstream ss; - ss << c << index; - return ss.str(); - } - - /* ************************************************************************* */ - Pose3Config pose3Circle(size_t n, double R) { - Pose3Config x; - double theta = 0, dtheta = 2*M_PI/n; - // Vehicle at p0 is looking towards y axis - Rot3 R0(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); - for (size_t i = 0; i < n; i++, theta += dtheta) - x.insert(i, Pose3(R0 * Rot3::yaw(-theta), Point3(cos(theta),sin(theta),0))); - return x; - } - - /* ************************************************************************* */ -} // namespace diff --git a/cpp/Pose3Config.h b/cpp/Pose3Config.h deleted file mode 100644 index 16757c871..000000000 --- a/cpp/Pose3Config.h +++ /dev/null @@ -1,30 +0,0 @@ -/** - * @file Pose3Config.cpp - * @brief Configuration of 3D poses - * @author Frank Dellaert - */ - -#pragma once - -#include "Pose3.h" -#include "LieConfig.h" -#include "Key.h" - -namespace gtsam { - - /** - * Pose3Config is now simply a typedef - */ - typedef LieConfig,Pose3> Pose3Config; - - /** - * Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0,0) - * The convention used is the Navlab/Aerospace convention: X-forward,Y-right,Z-down - * @param n number of poses - * @param R radius of circle - * @param c character to use for keys - * @return circle of n 2D poses - */ - Pose3Config pose3Circle(size_t n, double R); - -} // namespace diff --git a/cpp/Pose3Factor.h b/cpp/Pose3Factor.h deleted file mode 100644 index 600c43e4b..000000000 --- a/cpp/Pose3Factor.h +++ /dev/null @@ -1,19 +0,0 @@ -/** - * @file Pose3Factor.H - * @authors Frank Dellaert, Viorela Ila - **/ - -#pragma once - -#include "Pose3Config.h" -#include "BetweenFactor.h" - -namespace gtsam { - - /** - * A Factor for 3D pose measurements - * This is just a typedef now - */ - typedef BetweenFactor Pose3Factor; - -} /// namespace gtsam diff --git a/cpp/Pose3Graph.cpp b/cpp/Pose3Graph.cpp deleted file mode 100644 index 1879dac9b..000000000 --- a/cpp/Pose3Graph.cpp +++ /dev/null @@ -1,36 +0,0 @@ -/** - * @file Pose3Graph.cpp - * @brief A factor graph for the 2D PoseSLAM problem - * @authors Frank Dellaert, Viorela Ila - */ - -#include "Pose3Graph.h" -#include "FactorGraph-inl.h" -#include "NonlinearFactorGraph-inl.h" -#include "NonlinearOptimizer-inl.h" -#include "NonlinearEquality.h" -#include "graph-inl.h" - -namespace gtsam { - - // explicit instantiation so all the code is there and we can link with it - template class FactorGraph > ; - template class NonlinearFactorGraph ; - template class NonlinearEquality ; - template class NonlinearOptimizer ; - - bool Pose3Graph::equals(const Pose3Graph& p, double tol) const { - return false; - } - - /** - * Add an equality constraint on a pose - * @param key of pose - * @param pose which pose to constrain it to - */ - void Pose3Graph::addConstraint(const Pose3Config::Key& key, const Pose3& pose) { - push_back(sharedFactor(new NonlinearEquality (key, pose))); - } - -} // namespace gtsam diff --git a/cpp/Pose3Graph.h b/cpp/Pose3Graph.h deleted file mode 100644 index 8bfce2353..000000000 --- a/cpp/Pose3Graph.h +++ /dev/null @@ -1,54 +0,0 @@ -/** - * @file Pose3Graph.h - * @brief A factor graph for the 3D PoseSLAM problem - * @author Frank Dellaert - * @author Viorela Ila - */ - -#pragma once - -#include "Pose3Factor.h" -#include "NonlinearFactorGraph.h" - -namespace gtsam { - - /** - * Non-linear factor graph for visual SLAM - */ - class Pose3Graph: public gtsam::NonlinearFactorGraph { - - public: - - /** default constructor is empty graph */ - Pose3Graph() { - } - - /** - * equals - */ - bool equals(const Pose3Graph& p, double tol = 1e-9) const; - - /** - * Add a factor without having to do shared factor dance - */ - inline void add(const Pose3Config::Key& key1, const Pose3Config::Key& key2, - const Pose3& measured, const Matrix& covariance) { - push_back(sharedFactor(new Pose3Factor(key1, key2, measured, covariance))); - } - - /** - * Add an equality constraint on a pose - * @param key of pose - * @param pose which pose to constrain it to - */ - void addConstraint(const Pose3Config::Key& key, const Pose3& pose =Pose3()); - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { - } - }; - -} // namespace gtsam diff --git a/cpp/pose2SLAM.cpp b/cpp/pose2SLAM.cpp index 1944dc715..0b5f78941 100644 --- a/cpp/pose2SLAM.cpp +++ b/cpp/pose2SLAM.cpp @@ -29,20 +29,21 @@ namespace gtsam { } /* ************************************************************************* */ - void Graph::addConstraint(const Key& i, const Pose2& p) { - sharedFactor factor(new Constraint(i, p)); - push_back(factor); - } - void Graph::addPrior(const Key& i, const Pose2& p, const Matrix& cov) { sharedFactor factor(new Prior(i, p, cov)); push_back(factor); } - void Graph::add(const Key& i, const Key& j, const Pose2& z, const Matrix& cov) { - sharedFactor factor(new Odometry(i, j, z, cov)); + void Graph::addConstraint(const Key& i, const Key& j, const Pose2& z, const Matrix& cov) { + sharedFactor factor(new Constraint(i, j, z, cov)); push_back(factor); } + + void Graph::addHardConstraint(const Key& i, const Pose2& p) { + sharedFactor factor(new HardConstraint(i, p)); + push_back(factor); + } + /* ************************************************************************* */ } // pose2SLAM diff --git a/cpp/pose2SLAM.h b/cpp/pose2SLAM.h index c1cbdf310..9498cde4d 100644 --- a/cpp/pose2SLAM.h +++ b/cpp/pose2SLAM.h @@ -1,6 +1,6 @@ /** * @file pose2SLAM.h - * @brief: bearing/range measurements in 2D plane + * @brief: 2D Pose SLAM * @authors Frank Dellaert **/ @@ -35,14 +35,14 @@ namespace gtsam { // Factors typedef PriorFactor Prior; - typedef BetweenFactor Odometry; - typedef NonlinearEquality Constraint; + typedef BetweenFactor Constraint; + typedef NonlinearEquality HardConstraint; // Graph struct Graph: public NonlinearFactorGraph { - void addConstraint(const Key& i, const Pose2& p); void addPrior(const Key& i, const Pose2& p, const Matrix& cov); - void add(const Key& i, const Key& j, const Pose2& z, const Matrix& cov); + void addConstraint(const Key& i, const Key& j, const Pose2& z, const Matrix& cov); + void addHardConstraint(const Key& i, const Pose2& p); }; // Optimizer @@ -53,10 +53,9 @@ namespace gtsam { /** * Backwards compatibility */ - typedef pose2SLAM::Prior Pose2Prior; - typedef pose2SLAM::Odometry Pose2Factor; - typedef pose2SLAM::Constraint Pose2Constraint; typedef pose2SLAM::Config Pose2Config; + typedef pose2SLAM::Prior Pose2Prior; + typedef pose2SLAM::Constraint Pose2Factor; typedef pose2SLAM::Graph Pose2Graph; } // namespace gtsam diff --git a/cpp/pose3SLAM.cpp b/cpp/pose3SLAM.cpp new file mode 100644 index 000000000..e11fe82aa --- /dev/null +++ b/cpp/pose3SLAM.cpp @@ -0,0 +1,53 @@ +/** + * @file pose3SLAM.cpp + * @brief: bearing/range measurements in 2D plane + * @authors Frank Dellaert + **/ + +#include "pose3SLAM.h" +#include "LieConfig-inl.h" +#include "NonlinearFactorGraph-inl.h" +#include "NonlinearOptimizer-inl.h" + +// Use pose3SLAM namespace for specific SLAM instance +namespace gtsam { + + using namespace pose3SLAM; + INSTANTIATE_LIE_CONFIG(Key, Pose3) + INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config) + INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config) + + namespace pose3SLAM { + + /* ************************************************************************* */ + Config circle(size_t n, double R) { + Config x; + double theta = 0, dtheta = 2*M_PI/n; + // Vehicle at p0 is looking towards y axis + Rot3 R0(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); + for (size_t i = 0; i < n; i++, theta += dtheta) + x.insert(i, Pose3(R0 * Rot3::yaw(-theta), Point3(cos(theta),sin(theta),0))); + return x; + } + + /* ************************************************************************* */ + void Graph::addPrior(const Key& i, const Pose3& p, const Matrix& cov) { + sharedFactor factor(new Prior(i, p, cov)); + push_back(factor); + } + + void Graph::addConstraint(const Key& i, const Key& j, const Pose3& z, const Matrix& cov) { + sharedFactor factor(new Constraint(i, j, z, cov)); + push_back(factor); + } + + void Graph::addHardConstraint(const Key& i, const Pose3& p) { + sharedFactor factor(new HardConstraint(i, p)); + push_back(factor); + } + + /* ************************************************************************* */ + + } // pose3SLAM + +} // gtsam diff --git a/cpp/pose3SLAM.h b/cpp/pose3SLAM.h new file mode 100644 index 000000000..b74a7cf65 --- /dev/null +++ b/cpp/pose3SLAM.h @@ -0,0 +1,62 @@ +/** + * @file pose3SLAM.h + * @brief: 3D Pose SLAM + * @authors Frank Dellaert + **/ + +#pragma once + +#include "Key.h" +#include "Pose3.h" +#include "LieConfig.h" +#include "PriorFactor.h" +#include "BetweenFactor.h" +#include "NonlinearEquality.h" +#include "NonlinearFactorGraph.h" +#include "NonlinearOptimizer.h" + +namespace gtsam { + + // Use pose3SLAM namespace for specific SLAM instance + namespace pose3SLAM { + + // Keys and Config + typedef Symbol Key; + typedef LieConfig Config; + + /** + * Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0) + * @param n number of poses + * @param R radius of circle + * @param c character to use for keys + * @return circle of n 3D poses + */ + Config circle(size_t n, double R); + + // Factors + typedef PriorFactor Prior; + typedef BetweenFactor Constraint; + typedef NonlinearEquality HardConstraint; + + // Graph + struct Graph: public NonlinearFactorGraph { + void addPrior(const Key& i, const Pose3& p, const Matrix& cov); + void addConstraint(const Key& i, const Key& j, const Pose3& z, const Matrix& cov); + void addHardConstraint(const Key& i, const Pose3& p); + }; + + // Optimizer + typedef NonlinearOptimizer Optimizer; + + } // pose3SLAM + + /** + * Backwards compatibility + */ + typedef pose3SLAM::Config Pose3Config; + typedef pose3SLAM::Prior Pose3Prior; + typedef pose3SLAM::Constraint Pose3Factor; + typedef pose3SLAM::Graph Pose3Graph; + +} // namespace gtsam + diff --git a/cpp/testGraph.cpp b/cpp/testGraph.cpp index b481c9a34..edda723e1 100644 --- a/cpp/testGraph.cpp +++ b/cpp/testGraph.cpp @@ -43,8 +43,8 @@ TEST( Graph, composePoses ) { Pose2Graph graph; Matrix cov = eye(3); - graph.add(1,2, Pose2(2.0, 0.0, 0.0), cov); - graph.add(2,3, Pose2(3.0, 0.0, 0.0), cov); + graph.addConstraint(1,2, Pose2(2.0, 0.0, 0.0), cov); + graph.addConstraint(2,3, Pose2(3.0, 0.0, 0.0), cov); PredecessorMap tree; tree.insert(1,2); diff --git a/cpp/testIterative.cpp b/cpp/testIterative.cpp index d8dfe6805..04468bfd4 100644 --- a/cpp/testIterative.cpp +++ b/cpp/testIterative.cpp @@ -107,8 +107,8 @@ TEST( Iterative, conjugateGradientDescent_soft_constraint ) Pose2Graph graph; Matrix cov = eye(3); Matrix cov2 = eye(3) * 1e-10; - graph.add(1,2, Pose2(1.,0.,0.), cov); graph.addPrior(1, Pose2(0.,0.,0.), cov2); + graph.addConstraint(1,2, Pose2(1.,0.,0.), cov); VectorConfig zeros; zeros.insert("x1",zero(3)); diff --git a/cpp/testNoiseModel b/cpp/testNoiseModel new file mode 100755 index 000000000..fda2f5ab9 --- /dev/null +++ b/cpp/testNoiseModel @@ -0,0 +1,130 @@ +#! /bin/sh + +# testNoiseModel - temporary wrapper script for .libs/testNoiseModel +# Generated by ltmain.sh (GNU libtool) 2.2.6 +# +# The testNoiseModel 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='testNoiseModel' + 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 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/testPose2SLAM.cpp b/cpp/testPose2SLAM.cpp index f9d22c2d4..6db1e58ae 100644 --- a/cpp/testPose2SLAM.cpp +++ b/cpp/testPose2SLAM.cpp @@ -34,7 +34,7 @@ TEST( Pose2Graph, constructor ) Pose2 measured(2,2,M_PI_2); Pose2Factor constraint(1,2,measured, covariance); Pose2Graph graph; - graph.add(1,2,measured, covariance); + graph.addConstraint(1,2,measured, covariance); // get the size of the graph size_t actual = graph.size(); // verify @@ -50,7 +50,7 @@ TEST( Pose2Graph, linerization ) Pose2 measured(2,2,M_PI_2); Pose2Factor constraint(1,2,measured, covariance); Pose2Graph graph; - graph.add(1,2,measured, covariance); + graph.addConstraint(1,2,measured, covariance); // Choose a linearization point Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2) @@ -86,8 +86,8 @@ TEST(Pose2Graph, optimize) { // create a Pose graph with one equality constraint and one measurement shared_ptr fg(new Pose2Graph); - fg->addConstraint(0, Pose2(0,0,0)); - fg->add(0, 1, Pose2(1,2,M_PI_2), covariance); + fg->addHardConstraint(0, Pose2(0,0,0)); + fg->addConstraint(0, 1, Pose2(1,2,M_PI_2), covariance); // Create initial config boost::shared_ptr initial(new Pose2Config()); @@ -120,14 +120,14 @@ TEST(Pose2Graph, optimizeCircle) { // create a Pose graph with one equality constraint and one measurement shared_ptr fg(new Pose2Graph); - fg->addConstraint(0, p0); + fg->addHardConstraint(0, p0); Pose2 delta = between(p0,p1); - fg->add(0, 1, delta, covariance); - fg->add(1,2, delta, covariance); - fg->add(2,3, delta, covariance); - fg->add(3,4, delta, covariance); - fg->add(4,5, delta, covariance); - fg->add(5, 0, delta, covariance); + fg->addConstraint(0, 1, delta, covariance); + fg->addConstraint(1,2, delta, covariance); + fg->addConstraint(2,3, delta, covariance); + fg->addConstraint(3,4, delta, covariance); + fg->addConstraint(4,5, delta, covariance); + fg->addConstraint(5, 0, delta, covariance); // Create initial config boost::shared_ptr initial(new Pose2Config()); @@ -159,37 +159,34 @@ TEST(Pose2Graph, optimizeCircle) { /* ************************************************************************* */ // test optimization with 6 poses arranged in a hexagon and a loop closure TEST(Pose2Graph, findMinimumSpanningTree) { - typedef Pose2Config::Key Key; - Pose2Graph G, T, C; Matrix cov = eye(3); - G.push_back(Pose2Graph::sharedFactor(new Pose2Factor(Key(1), Key(2), Pose2(0.,0.,0.), cov))); - G.push_back(Pose2Graph::sharedFactor(new Pose2Factor(Key(1), Key(3), Pose2(0.,0.,0.), cov))); - G.push_back(Pose2Graph::sharedFactor(new Pose2Factor(Key(2), Key(3), Pose2(0.,0.,0.), cov))); + G.addConstraint(1, 2, Pose2(0.,0.,0.), cov); + G.addConstraint(1, 3, Pose2(0.,0.,0.), cov); + G.addConstraint(2, 3, Pose2(0.,0.,0.), cov); - PredecessorMap tree = G.findMinimumSpanningTree(); - CHECK(tree[Key(1)] == Key(1)); - CHECK(tree[Key(2)] == Key(1)); - CHECK(tree[Key(3)] == Key(1)); + PredecessorMap tree = + G.findMinimumSpanningTree(); + CHECK(tree[1] == 1); + CHECK(tree[2] == 1); + CHECK(tree[3] == 1); } /* ************************************************************************* */ // test optimization with 6 poses arranged in a hexagon and a loop closure TEST(Pose2Graph, split) { - typedef Pose2Config::Key Key; - Pose2Graph G, T, C; Matrix cov = eye(3); - G.push_back(Pose2Graph::sharedFactor(new Pose2Factor(Key(1), Key(2), Pose2(0.,0.,0.), cov))); - G.push_back(Pose2Graph::sharedFactor(new Pose2Factor(Key(1), Key(3), Pose2(0.,0.,0.), cov))); - G.push_back(Pose2Graph::sharedFactor(new Pose2Factor(Key(2), Key(3), Pose2(0.,0.,0.), cov))); + G.addConstraint(1, 2, Pose2(0.,0.,0.), cov); + G.addConstraint(1, 3, Pose2(0.,0.,0.), cov); + G.addConstraint(2, 3, Pose2(0.,0.,0.), cov); - PredecessorMap tree; - tree.insert(Key(1),Key(2)); - tree.insert(Key(2),Key(2)); - tree.insert(Key(3),Key(2)); + PredecessorMap tree; + tree.insert(1,2); + tree.insert(2,2); + tree.insert(3,2); - G.split(tree, T, C); + G.split(tree, T, C); LONGS_EQUAL(2, T.size()); LONGS_EQUAL(1, C.size()); } diff --git a/cpp/testPose3Config.cpp b/cpp/testPose3Config.cpp index f3d989124..a98423e4e 100644 --- a/cpp/testPose3Config.cpp +++ b/cpp/testPose3Config.cpp @@ -6,7 +6,7 @@ #include #include -#include "Pose3Config.h" +#include "pose3SLAM.h" using namespace std; using namespace gtsam; @@ -28,7 +28,7 @@ TEST( Pose3Config, pose3Circle ) expected.insert(2, Pose3(R3, Point3(-1, 0, 0))); expected.insert(3, Pose3(R4, Point3( 0,-1, 0))); - Pose3Config actual = pose3Circle(4,1.0); + Pose3Config actual = pose3SLAM::circle(4,1.0); CHECK(assert_equal(expected,actual)); } @@ -49,7 +49,7 @@ TEST( Pose3Config, expmap ) 0.0,0.0,0.0, 0.1, 0.0, 0.0, 0.0,0.0,0.0, 0.1, 0.0, 0.0, 0.0,0.0,0.0, 0.1, 0.0, 0.0); - Pose3Config actual = expmap(pose3Circle(4,1.0),delta); + Pose3Config actual = expmap(pose3SLAM::circle(4,1.0),delta); CHECK(assert_equal(expected,actual)); } diff --git a/cpp/testPose3Factor.cpp b/cpp/testPose3Factor.cpp index 3314ceddd..f0ef81dc4 100644 --- a/cpp/testPose3Factor.cpp +++ b/cpp/testPose3Factor.cpp @@ -9,8 +9,7 @@ using namespace boost::assign; #include -#include "Pose3Factor.h" -#include "LieConfig-inl.h" +#include "pose3SLAM.h" using namespace std; using namespace gtsam; diff --git a/cpp/testPose3SLAM b/cpp/testPose3SLAM new file mode 100755 index 000000000..e4d8736bc --- /dev/null +++ b/cpp/testPose3SLAM @@ -0,0 +1,130 @@ +#! /bin/sh + +# testPose3SLAM - temporary wrapper script for .libs/testPose3SLAM +# Generated by ltmain.sh (GNU libtool) 2.2.6 +# +# The testPose3SLAM 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='testPose3SLAM' + 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/testPose3Graph.cpp b/cpp/testPose3SLAM.cpp similarity index 85% rename from cpp/testPose3Graph.cpp rename to cpp/testPose3SLAM.cpp index cfad6d797..13f45c4ca 100644 --- a/cpp/testPose3Graph.cpp +++ b/cpp/testPose3SLAM.cpp @@ -13,9 +13,7 @@ using namespace boost::assign; #include -#include "Pose3Graph.h" -#include "NonlinearOptimizer-inl.h" -#include "NonlinearEquality.h" +#include "pose3SLAM.h" #include "Ordering.h" using namespace std; @@ -29,19 +27,19 @@ static Matrix covariance = eye(6); TEST(Pose3Graph, optimizeCircle) { // Create a hexagon of poses - Pose3Config hexagon = pose3Circle(6,1.0); + Pose3Config hexagon = pose3SLAM::circle(6,1.0); Pose3 p0 = hexagon[0], p1 = hexagon[1]; // create a Pose graph with one equality constraint and one measurement shared_ptr fg(new Pose3Graph); - fg->addConstraint(0, p0); + fg->addHardConstraint(0, p0); Pose3 delta = between(p0,p1); - fg->add(0,1, delta, covariance); - fg->add(1,2, delta, covariance); - fg->add(2,3, delta, covariance); - fg->add(3,4, delta, covariance); - fg->add(4,5, delta, covariance); - fg->add(5,0, delta, covariance); + fg->addConstraint(0,1, delta, covariance); + fg->addConstraint(1,2, delta, covariance); + fg->addConstraint(2,3, delta, covariance); + fg->addConstraint(3,4, delta, covariance); + fg->addConstraint(4,5, delta, covariance); + fg->addConstraint(5,0, delta, covariance); // Create initial config boost::shared_ptr initial(new Pose3Config()); diff --git a/myconfigure b/myconfigure index 12658b16c..e3433e6cc 100755 --- a/myconfigure +++ b/myconfigure @@ -1,4 +1,4 @@ ./configure --prefix=$HOME --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ # for maximum performance on Intel Core2 platform: -#./configure --prefix=$HOME --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CXXFLAGS=" -g -ftree-vectorize O3 -march=nocona -mtune=generic -DNDEBUG" --disable-static \ No newline at end of file +#./configure --prefix=$HOME --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CXXFLAGS=" -g -O3 -march=nocona -mtune=generic -DNDEBUG" --disable-static \ No newline at end of file