3D Pose SLAM + removed obsolete Urban files that somehow re-appeared in CitySLAM

.cproject

@@ -600,10 +600,10 @@
 <useDefaultCommand>true</useDefaultCommand>
 <runAllBuilders>true</runAllBuilders>
 </target>
-<target name="testPose3Factor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+<target name="testPose3Config.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
 <buildArguments>-j2</buildArguments>
-<buildTarget>testPose3Factor.run</buildTarget>
+<buildTarget>testPose3Config.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
 <runAllBuilders>true</runAllBuilders>
@@ -719,10 +719,10 @@
 <useDefaultCommand>false</useDefaultCommand>
 <runAllBuilders>true</runAllBuilders>
 </target>
-<target name="testPose3Graph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+<target name="testPose3SLAM.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
 <buildArguments>-j2</buildArguments>
-<buildTarget>testPose3Graph.run</buildTarget>
+<buildTarget>testPose3SLAM.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
 <runAllBuilders>true</runAllBuilders>

cpp/Makefile.am

@@ -214,15 +214,15 @@ testPlanarSLAM_SOURCES = $(example) testPlanarSLAM.cpp
 testPlanarSLAM_LDADD   = libgtsam.la
 
 # 3D Pose constraints
-headers += Pose3Factor.h  
+headers +=   
-sources += Pose3Config.cpp Pose3Graph.cpp 
+sources += pose3SLAM.cpp 
-check_PROGRAMS += testPose3Factor testPose3Config testPose3Graph 
+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
+testPose3SLAM_SOURCES  = $(example) testPose3SLAM.cpp
-testPose3Graph_LDADD    = libgtsam.la
+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)
+
+
+
+	

cpp/Pose3Config.cpp

@@ -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<Symbol<Pose3,'x'>,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

cpp/Pose3Config.h

@@ -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<Symbol<Pose3,'x'>,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

cpp/Pose3Factor.h

@@ -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<Pose3Config,Pose3Config::Key,Pose3> Pose3Factor;
-
-} /// namespace gtsam

cpp/Pose3Graph.cpp

@@ -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<NonlinearFactor<gtsam::Pose3Config> > ;
-	template class NonlinearFactorGraph<Pose3Config> ;
-	template class NonlinearEquality<Pose3Config, Pose3Config::Key, Pose3> ;
-	template class NonlinearOptimizer<Pose3Graph, Pose3Config> ;
-
-	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<Pose3Config, Pose3Config::Key,
-				Pose3> (key, pose)));
-	}
-
-} // namespace gtsam

cpp/Pose3Graph.h

@@ -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<Pose3Config> {
-
-	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<class Archive>
-		void serialize(Archive & ar, const unsigned int version) {
-		}
-	};
-
-} // namespace gtsam

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) {
+		void Graph::addConstraint(const Key& i, const Key& j, const Pose2& z, const Matrix& cov) {
-			sharedFactor factor(new Odometry(i, j, z, 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

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<Config, Key, Pose2> Prior;
-		typedef BetweenFactor<Config, Key, Pose2> Odometry;
+		typedef BetweenFactor<Config, Key, Pose2> Constraint;
-		typedef NonlinearEquality<Config, Key, Pose2> Constraint;
+		typedef NonlinearEquality<Config, Key, Pose2> HardConstraint;
 
 		// Graph
 		struct Graph: public NonlinearFactorGraph<Config> {
-			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

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

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<Pose3, 'x'> Key;
+		typedef LieConfig<Key, Pose3> 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<Config, Key, Pose3> Prior;
+		typedef BetweenFactor<Config, Key, Pose3> Constraint;
+		typedef NonlinearEquality<Config, Key, Pose3> HardConstraint;
+
+		// Graph
+		struct Graph: public NonlinearFactorGraph<Config> {
+			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<Graph, Config> Optimizer;
+
+	} // pose3SLAM
+
+	/**
+	 * Backwards compatibility
+	 */
+	typedef pose3SLAM::Config Pose3Config;
+	typedef pose3SLAM::Prior Pose3Prior;
+	typedef pose3SLAM::Constraint Pose3Factor;
+	typedef pose3SLAM::Graph Pose3Graph;
+
+} // namespace gtsam
+

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.addConstraint(1,2, Pose2(2.0, 0.0, 0.0), cov);
-	graph.add(2,3, Pose2(3.0, 0.0, 0.0), cov);
+	graph.addConstraint(2,3, Pose2(3.0, 0.0, 0.0), cov);
 
 	PredecessorMap<Pose2Config::Key> tree;
 	tree.insert(1,2);

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));

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

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

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<Pose2Graph> fg(new Pose2Graph);
-  fg->addConstraint(0, Pose2(0,0,0));
+  fg->addHardConstraint(0, Pose2(0,0,0));
-  fg->add(0, 1, Pose2(1,2,M_PI_2), covariance);
+  fg->addConstraint(0, 1, Pose2(1,2,M_PI_2), covariance);
 
   // Create initial config
   boost::shared_ptr<Pose2Config> initial(new Pose2Config());
@@ -120,14 +120,14 @@ TEST(Pose2Graph, optimizeCircle) {
 
 	// create a Pose graph with one equality constraint and one measurement
   shared_ptr<Pose2Graph> fg(new Pose2Graph);
-  fg->addConstraint(0, p0);
+  fg->addHardConstraint(0, p0);
   Pose2 delta = between(p0,p1);
-  fg->add(0, 1, delta, covariance);
+  fg->addConstraint(0, 1, delta, covariance);
-  fg->add(1,2, delta, covariance);
+  fg->addConstraint(1,2, delta, covariance);
-  fg->add(2,3, delta, covariance);
+  fg->addConstraint(2,3, delta, covariance);
-  fg->add(3,4, delta, covariance);
+  fg->addConstraint(3,4, delta, covariance);
-  fg->add(4,5, delta, covariance);
+  fg->addConstraint(4,5, delta, covariance);
-  fg->add(5, 0, delta, covariance);
+  fg->addConstraint(5, 0, delta, covariance);
 
   // Create initial config
   boost::shared_ptr<Pose2Config> 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.addConstraint(1, 2, Pose2(0.,0.,0.), cov);
-	G.push_back(Pose2Graph::sharedFactor(new Pose2Factor(Key(1), Key(3), Pose2(0.,0.,0.), cov)));
+	G.addConstraint(1, 3, Pose2(0.,0.,0.), cov);
-	G.push_back(Pose2Graph::sharedFactor(new Pose2Factor(Key(2), Key(3), Pose2(0.,0.,0.), cov)));
+	G.addConstraint(2, 3, Pose2(0.,0.,0.), cov);
 
-	PredecessorMap<Key> tree = G.findMinimumSpanningTree<Key, Pose2Factor>();
+	PredecessorMap<pose2SLAM::Key> tree =
-	CHECK(tree[Key(1)] == Key(1));
+			G.findMinimumSpanningTree<pose2SLAM::Key, Pose2Factor>();
-	CHECK(tree[Key(2)] == Key(1));
+	CHECK(tree[1] == 1);
-	CHECK(tree[Key(3)] == Key(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.addConstraint(1, 2, Pose2(0.,0.,0.), cov);
-	G.push_back(Pose2Graph::sharedFactor(new Pose2Factor(Key(1), Key(3), Pose2(0.,0.,0.), cov)));
+	G.addConstraint(1, 3, Pose2(0.,0.,0.), cov);
-	G.push_back(Pose2Graph::sharedFactor(new Pose2Factor(Key(2), Key(3), Pose2(0.,0.,0.), cov)));
+	G.addConstraint(2, 3, Pose2(0.,0.,0.), cov);
 
-	PredecessorMap<Key> tree;
+	PredecessorMap<pose2SLAM::Key> tree;
-	tree.insert(Key(1),Key(2));
+	tree.insert(1,2);
-	tree.insert(Key(2),Key(2));
+	tree.insert(2,2);
-	tree.insert(Key(3),Key(2));
+	tree.insert(3,2);
 
-	G.split<Key, Pose2Factor>(tree, T, C);
+	G.split<pose2SLAM::Key, Pose2Factor>(tree, T, C);
 	LONGS_EQUAL(2, T.size());
 	LONGS_EQUAL(1, C.size());
 }

cpp/testPose3Config.cpp

@@ -6,7 +6,7 @@
 #include <iostream>
 
 #include <CppUnitLite/TestHarness.h>
-#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));
 }
 

cpp/testPose3Factor.cpp

@@ -9,8 +9,7 @@
 using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
-#include "Pose3Factor.h"
+#include "pose3SLAM.h"
-#include "LieConfig-inl.h"
 
 using namespace std;
 using namespace gtsam;

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

cpp/testPose3SLAM.cpp

@@ -13,9 +13,7 @@ using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
 
-#include "Pose3Graph.h"
+#include "pose3SLAM.h"
-#include "NonlinearOptimizer-inl.h"
-#include "NonlinearEquality.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<Pose3Graph> fg(new Pose3Graph);
-  fg->addConstraint(0, p0);
+  fg->addHardConstraint(0, p0);
   Pose3 delta = between(p0,p1);
-  fg->add(0,1, delta, covariance);
+  fg->addConstraint(0,1, delta, covariance);
-  fg->add(1,2, delta, covariance);
+  fg->addConstraint(1,2, delta, covariance);
-  fg->add(2,3, delta, covariance);
+  fg->addConstraint(2,3, delta, covariance);
-  fg->add(3,4, delta, covariance);
+  fg->addConstraint(3,4, delta, covariance);
-  fg->add(4,5, delta, covariance);
+  fg->addConstraint(4,5, delta, covariance);
-  fg->add(5,0, delta, covariance);
+  fg->addConstraint(5,0, delta, covariance);
 
   // Create initial config
   boost::shared_ptr<Pose3Config> initial(new Pose3Config());

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
+#./configure --prefix=$HOME --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CXXFLAGS=" -g -O3 -march=nocona -mtune=generic -DNDEBUG" --disable-static