PairConfig is implemented, VSLAMConfig is now a typedef!

.cproject

@@ -1,4 +1,4 @@
-<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<?xml version="1.0" encoding="UTF-8"?>
 <?fileVersion 4.0.0?>
 
 <cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
@@ -298,22 +298,6 @@
 			</storageModule>
 			<storageModule moduleId="org.eclipse.cdt.make.core.buildtargets">
 <buildTargets>
-<target name="install" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-<buildCommand>make</buildCommand>
-<buildArguments>-j2</buildArguments>
-<buildTarget>install</buildTarget>
-<stopOnError>true</stopOnError>
-<useDefaultCommand>true</useDefaultCommand>
-<runAllBuilders>true</runAllBuilders>
-</target>
-<target name="check" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-<buildCommand>make</buildCommand>
-<buildArguments>-j2</buildArguments>
-<buildTarget>check</buildTarget>
-<stopOnError>true</stopOnError>
-<useDefaultCommand>true</useDefaultCommand>
-<runAllBuilders>true</runAllBuilders>
-</target>
 <target name="check" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
 <buildArguments>-k</buildArguments>
@@ -468,6 +452,7 @@
 </target>
 <target name="testBayesTree.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments></buildArguments>
 <buildTarget>testBayesTree.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>false</useDefaultCommand>
@@ -475,7 +460,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>
@@ -483,6 +467,7 @@
 </target>
 <target name="testSymbolicFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments></buildArguments>
 <buildTarget>testSymbolicFactorGraph.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>false</useDefaultCommand>
@@ -738,6 +723,7 @@
 </target>
 <target name="testGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments></buildArguments>
 <buildTarget>testGraph.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>false</useDefaultCommand>
@@ -751,6 +737,14 @@
 <useDefaultCommand>true</useDefaultCommand>
 <runAllBuilders>true</runAllBuilders>
 </target>
+<target name="testTupleConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+<buildCommand>make</buildCommand>
+<buildArguments>-j2</buildArguments>
+<buildTarget>testTupleConfig.run</buildTarget>
+<stopOnError>true</stopOnError>
+<useDefaultCommand>true</useDefaultCommand>
+<runAllBuilders>true</runAllBuilders>
+</target>
 <target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
 <buildArguments>-j2</buildArguments>
@@ -775,6 +769,22 @@
 <useDefaultCommand>true</useDefaultCommand>
 <runAllBuilders>true</runAllBuilders>
 </target>
+<target name="install" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+<buildCommand>make</buildCommand>
+<buildArguments>-j2</buildArguments>
+<buildTarget>install</buildTarget>
+<stopOnError>true</stopOnError>
+<useDefaultCommand>true</useDefaultCommand>
+<runAllBuilders>true</runAllBuilders>
+</target>
+<target name="check" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+<buildCommand>make</buildCommand>
+<buildArguments>-j2</buildArguments>
+<buildTarget>check</buildTarget>
+<stopOnError>true</stopOnError>
+<useDefaultCommand>true</useDefaultCommand>
+<runAllBuilders>true</runAllBuilders>
+</target>
 </buildTargets>
 </storageModule>
 </cconfiguration>

cpp/LieConfig-inl.h

@@ -38,22 +38,30 @@ namespace gtsam {
   }
 
   template<class J, class T>
-  const T& LieConfig<J,T>::at(const Key& key) const {
+  const T& LieConfig<J,T>::at(const J& j) const {
-    const_iterator it = values_.find(key);
+    const_iterator it = values_.find(j);
-    if (it == values_.end()) throw std::invalid_argument("invalid key: " + (string)key);
+    if (it == values_.end()) throw std::invalid_argument("invalid j: " + (string)j);
     else return it->second;
   }
 
   template<class J, class T>
-  void LieConfig<J,T>::insert(const Key& name, const T& val) {
+  void LieConfig<J,T>::insert(const J& name, const T& val) {
     values_.insert(make_pair(name, val));
     dim_ += dim(val);
   }
 
   template<class J, class T>
-  void LieConfig<J,T>::erase(const Key& key) {
+  void LieConfig<J,T>::erase(const J& j) {
-    iterator it = values_.find(key);
+    size_t dim; // unused
-    if (it == values_.end()) throw std::invalid_argument("invalid key: " + (string)key);
+    erase(j, dim);
+  }
+
+  template<class J, class T>
+  void LieConfig<J,T>::erase(const J& j, size_t& dim) {
+    iterator it = values_.find(j);
+    if (it == values_.end()) throw std::invalid_argument("invalid j: " + (string)j);
+    dim = gtsam::dim(it->second);
+    dim_ -= dim;
     values_.erase(it);
   }
 
@@ -61,13 +69,13 @@ namespace gtsam {
   template<class J, class T>
   LieConfig<J,T> expmap(const LieConfig<J,T>& c, const VectorConfig& delta) {
 		LieConfig<J,T> newConfig;
-		typedef pair<typename LieConfig<J,T>::Key,T> Value;
+		typedef pair<J,T> Value;
 		BOOST_FOREACH(const Value& value, c) {
-			const typename LieConfig<J,T>::Key& j = value.first;
+			const J& j = value.first;
 			const T& pj = value.second;
-			string key = (string)j;
+			string jstr = (string)j;
-			if (delta.contains(key)) {
+			if (delta.contains(jstr)) {
-				const Vector& dj = delta[key];
+				const Vector& dj = delta[jstr];
 				newConfig.insert(j, expmap(pj,dj));
 			} else
 			newConfig.insert(j, pj);
@@ -82,9 +90,9 @@ namespace gtsam {
       throw invalid_argument("Delta vector length does not match config dimensionality.");
     LieConfig<J,T> newConfig;
     int delta_offset = 0;
-		typedef pair<typename LieConfig<J,T>::Key,T> Value;
+		typedef pair<J,T> Value;
 		BOOST_FOREACH(const Value& value, c) {
-			const typename LieConfig<J,T>::Key& j = value.first;
+			const J& j = value.first;
 			const T& pj = value.second;
       int cur_dim = dim(pj);
       newConfig.insert(j,expmap(pj,sub(delta, delta_offset, delta_offset+cur_dim)));

cpp/LieConfig.h

@@ -48,8 +48,9 @@ namespace gtsam {
     /**
      * Typedefs
      */
-  	typedef J Key;
+    typedef J Key;
-    typedef std::map<Key, T> Values;
+    typedef T Value;
+    typedef std::map<J, T> Values;
     typedef typename Values::iterator iterator;
     typedef typename Values::const_iterator const_iterator;
 
@@ -71,17 +72,17 @@ namespace gtsam {
     /** Test whether configs are identical in keys and values */
     bool equals(const LieConfig& expected, double tol=1e-9) const;
 
-    /** Retrieve a variable by key, throws std::invalid_argument if not found */
+    /** Retrieve a variable by j, throws std::invalid_argument if not found */
-    const T& at(const Key& key) const;
+    const T& at(const J& j) const;
 
     /** operator[] syntax for get */
-		inline const T& operator[](const Key& key) const { return at(key);}
+		const T& operator[](const J& j) const { return at(j); }
 
 	  /** Check if a variable exists */
-	  bool exists(const Key& i) const {return values_.find(i)!=values_.end();}
+	  bool exists(const J& i) const { return values_.find(i)!=values_.end(); }
 
     /** The number of variables in this config */
-    int size() const { return values_.size(); }
+    size_t size() const { return values_.size(); }
 
     const_iterator begin() const { return values_.begin(); }
     const_iterator end() const { return values_.end(); }
@@ -90,11 +91,16 @@ namespace gtsam {
 
     // imperative methods:
 
-    /** Add a variable with the given key */
+    /** Add a variable with the given j */
-    void insert(const Key& key, const T& val);
+    void insert(const J& j, const T& val);
 
     /** Remove a variable from the config */
-    void erase(const Key& key) ;
+    void erase(const J& j);
+
+    /** Remove a variable from the config while returning the dimensionality of
+     * the removed element (normally not needed by user code).
+     */
+    void erase(const J& j, size_t& dim);
 
     /** Replace all keys and variables */
     LieConfig& operator=(const LieConfig& rhs) {

cpp/Makefile.am

@@ -162,7 +162,7 @@ testSQPOptimizer_LDADD     		  = libgtsam.la
 # geometry
 headers += Lie.h Lie-inl.h
 sources += Point2.cpp Rot2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp
-check_PROGRAMS += testPoint2 testRot2 testPose2 testPoint3 testRot3 testPose3 testCal3_S2 testLieConfig
+check_PROGRAMS += testPoint2 testRot2 testPose2 testPoint3 testRot3 testPose3 testCal3_S2 testLieConfig testTupleConfig
 testPoint2_SOURCES  = testPoint2.cpp
 testRot2_SOURCES    = testRot2.cpp
 testPose2_SOURCES   = testPose2.cpp
@@ -171,6 +171,7 @@ testRot3_SOURCES    = testRot3.cpp
 testPose3_SOURCES   = testPose3.cpp
 testCal3_S2_SOURCES = testCal3_S2.cpp
 testLieConfig_SOURCES = testLieConfig.cpp               
+testTupleConfig_SOURCES = testTupleConfig.cpp               
 
 testPoint2_LDADD    = libgtsam.la
 testRot2_LDADD      = libgtsam.la
@@ -180,6 +181,7 @@ testRot3_LDADD      = libgtsam.la
 testPose3_LDADD     = libgtsam.la
 testCal3_S2_LDADD   = libgtsam.la
 testLieConfig_LDADD = libgtsam.la
+testTupleConfig_LDADD = libgtsam.la
 
 noinst_PROGRAMS = timeRot3
 timeRot3_SOURCES = timeRot3.cpp
@@ -238,7 +240,7 @@ testSimpleCamera_SOURCES = testSimpleCamera.cpp
 testSimpleCamera_LDADD = libgtsam.la
 
 # Visual SLAM
-sources += VSLAMConfig.cpp VSLAMGraph.cpp VSLAMFactor.cpp
+sources += VSLAMGraph.cpp VSLAMFactor.cpp
 check_PROGRAMS += testVSLAMFactor testVSLAMGraph testVSLAMConfig
 testVSLAMFactor_SOURCES = testVSLAMFactor.cpp
 testVSLAMFactor_LDADD = libgtsam.la

cpp/TupleConfig.h

@@ -0,0 +1,134 @@
+/*
+ * TupleConfig.h
+ *
+ *  Created on: Jan 13, 2010
+ *      Author: Richard Roberts and Manohar Paluri
+ */
+
+#include <iostream>
+
+#include "LieConfig-inl.h"
+
+#pragma once
+
+namespace gtsam {
+
+  /**
+   * PairConfig:  a config that holds two data types.
+   */
+  template<class J1, class X1, class J2, class X2>
+  class PairConfig : public Testable<PairConfig<J1, X1, J2, X2> > {
+  public:
+//    typedef J1 Key1;
+//    typedef J2 Key2;
+//    typedef X1 Value1;
+//    typedef X2 Value2;
+    typedef LieConfig<J1, X1> Config1;
+    typedef LieConfig<J2, X2> Config2;
+
+  private:
+    LieConfig<J1, X1> config1_;
+    LieConfig<J2, X2> config2_;
+    size_t size_;
+    size_t dim_;
+
+    PairConfig(const LieConfig<J1,X1>& config1, const LieConfig<J2,X2>& config2) :
+      config1_(config1), config2_(config2),
+      size_(config1.size()+config2.size()), dim_(gtsam::dim(config1)+gtsam::dim(config2)) {}
+
+  public:
+
+    /**
+     * Default constructor creates an empty config.
+     */
+    PairConfig(): size_(0), dim_(0) {}
+
+    /**
+     * Copy constructor
+     */
+    PairConfig(const PairConfig<J1, X1, J2, X2>& c):
+      config1_(c.config1_), config2_(c.config2_), size_(c.size_), dim_(c.dim_) {}
+
+    /**
+     * Print
+     */
+    void print(const std::string& s = "") const {
+      std::cout << "TupleConfig " << s << ", size " << size_ << "\n";
+      config1_.print(s + "Config1: ");
+      config2_.print(s + "Config2: ");
+    }
+
+    /**
+     * Test for equality in keys and values
+     */
+    bool equals(const PairConfig<J1, X1, J2, X2>& c, double tol=1e-9) const {
+      return config1_.equals(c.config1_, tol) && config2_.equals(c.config2_, tol); }
+
+    /**
+     * operator[] syntax to get a value by j, throws invalid_argument if
+     * value with specified j is not present.  Will generate compile-time
+     * errors if j type does not match that on which the Config is templated.
+     */
+    const X1& operator[](const J1& j) const { return config1_[j]; }
+    const X2& operator[](const J2& j) const { return config2_[j]; }
+
+    /**
+     * size is the total number of variables in this config.
+     */
+    size_t size() const { return size_; }
+
+    /**
+     * dim is the dimensionality of the tangent space
+     */
+    size_t dim() const { return dim_; }
+
+  private:
+    template<class Config, class Key, class Value>
+    void insert_helper(Config& config, const Key& j, const Value& value) {
+      config.insert(j, value);
+      size_ ++;
+      dim_ += gtsam::dim(value);
+    }
+
+    template<class Config, class Key>
+    void erase_helper(Config& config, const Key& j) {
+      size_t dim;
+      config.erase(j, dim);
+      dim_ -= dim;
+      size_ --;
+    }
+
+  public:
+    /**
+     * expmap each element
+     */
+    PairConfig<J1,X1,J2,X2> expmap(const VectorConfig& delta) {
+      return PairConfig(gtsam::expmap(config1_, delta), gtsam::expmap(config2_, delta)); }
+
+    /**
+     * Insert a variable with the given j
+     */
+    void insert(const J1& j, const X1& value) { insert_helper(config1_, j, value); }
+    void insert(const J2& j, const X2& value) { insert_helper(config2_, j, value); }
+
+    /**
+     * Remove the variable with the given j.  Throws invalid_argument if the
+     * j is not present in the config.
+     */
+    void erase(const J1& j) { erase_helper(config1_, j); }
+    void erase(const J2& j) { erase_helper(config2_, j); }
+
+    /**
+     * Check if a variable exists
+     */
+    bool exists(const J1& j) const { return config1_.exists(j); }
+    bool exists(const J2& j) const { return config2_.exists(j); }
+
+
+  };
+
+  template<class J1, class X1, class J2, class X2>
+  inline PairConfig<J1,X1,J2,X2> expmap(PairConfig<J1,X1,J2,X2> c, const VectorConfig& delta) { return c.expmap(delta); }
+
+
+}

cpp/VSLAMConfig.cpp

@@ -1,44 +0,0 @@
-/**
- * @file   VSLAMConfig.cpp
- * @brief  The Config
- * @author Alireza Fathi
- * @author Carlos Nieto
- */
-
-#include <iostream>
-
-#include <boost/foreach.hpp>
-#include <boost/tuple/tuple.hpp>
-
-#include "VSLAMConfig.h"
-#include "LieConfig-inl.h"
-
-using namespace std;
-
-namespace gtsam {
-
-	/* ************************************************************************* */
-	// Exponential map
-	VSLAMConfig expmap(const VSLAMConfig& x0, const VectorConfig & delta) {
-		VSLAMConfig x;
-		x.poses_ = expmap(x0.poses_, delta);
-		x.points_ = expmap(x0.points_, delta);
-		return x;
-	}
-
-	/* ************************************************************************* */
-	void VSLAMConfig::print(const std::string& s) const {
-		printf("%s:\n", s.c_str());
-		poses_.print("Poses");
-		points_.print("Points");
-	}
-
-	/* ************************************************************************* */
-	bool VSLAMConfig::equals(const VSLAMConfig& c, double tol) const {
-		return poses_.equals(c.poses_, tol) && points_.equals(c.points_, tol);
-	}
-
-/* ************************************************************************* */
-
-} // namespace gtsam
-

cpp/VSLAMConfig.h

@@ -5,85 +5,16 @@
  * @author Carlos Nieto
  */
 
-#include <string>
+#include "Pose3.h"
-#include <map>
+#include "Point3.h"
-#include <vector>
+#include "TupleConfig.h"
-#include <fstream>
-
-#include "Pose3Config.h"
 
 #pragma once
 
 namespace gtsam{
 
-/**
+  typedef Symbol<Pose3,'x'> VSLAMPoseKey;
- * Config that knows about points and poses
+  typedef Symbol<Point3,'l'> VSLAMPointKey;
- */
+  typedef PairConfig<VSLAMPoseKey, Pose3, VSLAMPointKey, Point3> VSLAMConfig;
-class VSLAMConfig : Testable<VSLAMConfig> {
-
-public:
-
- typedef Symbol<Pose3,'x'> PoseKey;
- typedef Symbol<Point3,'l'> PointKey;
-
-private:
-
-  LieConfig<PoseKey,  Pose3>  poses_;
-	LieConfig<PointKey, Point3> points_;
-
-public:
-
-  /**
-   * default constructor
-   */
-  VSLAMConfig() {}
-
-  /**
-   * print
-   */
-  void print(const std::string& s = "") const;
-
-  /**
-   * check whether two configs are equal
-   */
-  bool equals(const VSLAMConfig& c, double tol=1e-6) const;
-
-  /**
-   * Get Poses or Points
-   */
-  inline const Pose3&  operator[](const PoseKey&  key) const {return poses_[key];}
-  inline const Point3& operator[](const PointKey& key) const {return points_[key];}
-
-  // (Awkwardly named) backwards compatibility:
-
-  inline bool cameraPoseExists   (const PoseKey&  key) const {return  poses_.exists(key);}
-  inline bool landmarkPointExists(const PointKey& key) const {return points_.exists(key);}
-
-  inline Pose3  cameraPose   (const PoseKey&  key) const {return  poses_[key];}
-  inline Point3 landmarkPoint(const PointKey& key) const {return points_[key];}
-
-  inline size_t size() const {return points_.size() + poses_.size();}
-  inline size_t dim() const {return gtsam::dim(points_) + gtsam::dim(poses_);}
-
-  // Imperative functions:
-
-  inline void addCameraPose(const PoseKey& key, Pose3 cp) {poses_.insert(key,cp);}
-  inline void addLandmarkPoint(const PointKey& key, Point3 lp) {points_.insert(key,lp);}
-
-  inline void removeCameraPose(const PoseKey& key) { poses_.erase(key);}
-  inline void removeLandmarkPose(const PointKey& key) { points_.erase(key);}
-
-  inline void clear() {points_.clear(); poses_.clear();}
-
-  friend VSLAMConfig expmap(const VSLAMConfig& c, const VectorConfig & delta);
-};
-
-
-/**
- * Exponential map: takes 6D vectors in VectorConfig
- * and applies them to the poses in the VSLAMConfig.
- * Needed for use in nonlinear optimization
- */
-VSLAMConfig expmap(const VSLAMConfig& c, const VectorConfig & delta);
-} // namespace gtsam
 
+}

cpp/VSLAMFactor.h

@@ -15,8 +15,8 @@
 
 namespace gtsam {
 
-	typedef NonlinearFactor2<VSLAMConfig, VSLAMConfig::PoseKey,
+	typedef NonlinearFactor2<VSLAMConfig,
-	Pose3, VSLAMConfig::PointKey, Point3> VSLAMFactorBase;
+	VSLAMPoseKey,	Pose3, VSLAMPointKey, Point3> VSLAMFactorBase;
 
 	/**
 	 * Non-linear factor for a constraint derived from a 2D measurement,
@@ -81,8 +81,8 @@ namespace gtsam {
 		friend class boost::serialization::access;
 		template<class Archive>
 		void serialize(Archive & ar, const unsigned int version) {
-			ar & BOOST_SERIALIZATION_NVP(key1_);
+			//ar & BOOST_SERIALIZATION_NVP(key1_);
-			ar & BOOST_SERIALIZATION_NVP(key2_);
+			//ar & BOOST_SERIALIZATION_NVP(key2_);
 			ar & BOOST_SERIALIZATION_NVP(z_);
 			ar & BOOST_SERIALIZATION_NVP(K_);
 		}

cpp/VSLAMGraph.cpp

@@ -28,12 +28,12 @@ bool compareLandmark(const std::string& key,
 					const VSLAMConfig& feasible,
 					const VSLAMConfig& input) {
 	int j = atoi(key.substr(1, key.size() - 1).c_str());
-	return feasible.landmarkPoint(j).equals(input.landmarkPoint(j));
+	return feasible[VSLAMPointKey(j)].equals(input[VSLAMPointKey(j)]);
 }
 
 /* ************************************************************************* */
 void VSLAMGraph::addLandmarkConstraint(int j, const gtsam::Point3& p) {
-  typedef NonlinearEquality<VSLAMConfig,VSLAMConfig::PointKey,Point3> NLE;
+  typedef NonlinearEquality<VSLAMConfig,VSLAMPointKey,Point3> NLE;
   boost::shared_ptr<NLE> factor(new NLE(j, p));
   push_back(factor);
 }
@@ -43,12 +43,12 @@ bool compareCamera(const std::string& key,
 					const VSLAMConfig& feasible,
 					const VSLAMConfig& input) {
 	int j = atoi(key.substr(1, key.size() - 1).c_str());
-	return feasible.cameraPose(j).equals(input.cameraPose(j));
+	return feasible[VSLAMPoseKey(j)].equals(input[VSLAMPoseKey(j)]);
 }
 
 /* ************************************************************************* */
 void VSLAMGraph::addCameraConstraint(int j, const gtsam::Pose3& p) {
-  typedef NonlinearEquality<VSLAMConfig,VSLAMConfig::PoseKey,Pose3> NLE;
+  typedef NonlinearEquality<VSLAMConfig,VSLAMPoseKey,Pose3> NLE;
   boost::shared_ptr<NLE> factor(new NLE(j,p));
   push_back(factor);
 }

cpp/testSQP.cpp

@@ -493,9 +493,9 @@ TEST (SQP, stereo_truth ) {
 
 	// create truth config
 	boost::shared_ptr<VSLAMConfig> truthConfig(new VSLAMConfig);
-	truthConfig->addCameraPose(1, camera1.pose());
+	truthConfig->insert(1, camera1.pose());
-	truthConfig->addCameraPose(2, camera2.pose());
+	truthConfig->insert(2, camera2.pose());
-	truthConfig->addLandmarkPoint(1, landmark);
+	truthConfig->insert(1, landmark);
 
 	// create graph
 	shared_ptr<VSLAMGraph> graph(new VSLAMGraph());
@@ -560,15 +560,15 @@ TEST (SQP, stereo_truth_noisy ) {
 
 	// create truth config
 	boost::shared_ptr<VSLAMConfig> truthConfig(new VSLAMConfig);
-	truthConfig->addCameraPose(1, camera1.pose());
+	truthConfig->insert(1, camera1.pose());
-	truthConfig->addCameraPose(2, camera2.pose());
+	truthConfig->insert(2, camera2.pose());
-	truthConfig->addLandmarkPoint(1, landmark);
+	truthConfig->insert(1, landmark);
 
 	// create config
 	boost::shared_ptr<VSLAMConfig> noisyConfig(new VSLAMConfig);
-	noisyConfig->addCameraPose(1, camera1.pose());
+	noisyConfig->insert(1, camera1.pose());
-	noisyConfig->addCameraPose(2, camera2.pose());
+	noisyConfig->insert(2, camera2.pose());
-	noisyConfig->addLandmarkPoint(1, landmarkNoisy);
+	noisyConfig->insert(1, landmarkNoisy);
 
 	// create graph
 	shared_ptr<VSLAMGraph> graph(new VSLAMGraph());
@@ -629,8 +629,8 @@ namespace sqp_stereo {
 	// binary constraint between landmarks
 	/** g(x) = x-y = 0 */
 	Vector g(const VSLAMConfig& config, const list<string>& keys) {
-		return config.landmarkPoint(getNum(keys.front())).vector()
+		return config[VSLAMPointKey(getNum(keys.front()))].vector()
-				- config.landmarkPoint(getNum(keys.back())).vector();
+				- config[VSLAMPointKey(getNum(keys.back()))].vector();
 	}
 
 	/** jacobian at l1 */
@@ -705,10 +705,10 @@ boost::shared_ptr<VSLAMConfig> stereoExampleTruthConfig() {
 
 	// create config
 	boost::shared_ptr<VSLAMConfig> truthConfig(new VSLAMConfig);
-	truthConfig->addCameraPose(1, camera1.pose());
+	truthConfig->insert(1, camera1.pose());
-	truthConfig->addCameraPose(2, camera2.pose());
+	truthConfig->insert(2, camera2.pose());
-	truthConfig->addLandmarkPoint(1, landmark1);
+	truthConfig->insert(1, landmark1);
-	truthConfig->addLandmarkPoint(2, landmark2); // create two landmarks in same place
+	truthConfig->insert(2, landmark2); // create two landmarks in same place
 
 	return truthConfig;
 }
@@ -763,10 +763,10 @@ TEST (SQP, stereo_sqp_noisy ) {
 
 	// noisy config
 	boost::shared_ptr<VSLAMConfig> initConfig(new VSLAMConfig);
-	initConfig->addCameraPose(1, pose1);
+	initConfig->insert(1, pose1);
-	initConfig->addCameraPose(2, pose2);
+	initConfig->insert(2, pose2);
-	initConfig->addLandmarkPoint(1, landmark1);
+	initConfig->insert(1, landmark1);
-	initConfig->addLandmarkPoint(2, landmark2); // create two landmarks in same place
+	initConfig->insert(2, landmark2); // create two landmarks in same place
 
 	// create ordering
 	Ordering ord;
@@ -828,10 +828,10 @@ TEST (SQP, stereo_sqp_noisy_manualLagrange ) {
 
 	// noisy config
 	boost::shared_ptr<VSLAMConfig> initConfig(new VSLAMConfig);
-	initConfig->addCameraPose(1, pose1);
+	initConfig->insert(1, pose1);
-	initConfig->addCameraPose(2, pose2);
+	initConfig->insert(2, pose2);
-	initConfig->addLandmarkPoint(1, landmark1);
+	initConfig->insert(1, landmark1);
-	initConfig->addLandmarkPoint(2, landmark2); // create two landmarks in same place
+	initConfig->insert(2, landmark2); // create two landmarks in same place
 
 	// create ordering with lagrange multiplier included
 	Ordering ord;

cpp/testTupleConfig.cpp

@@ -0,0 +1,166 @@
+/*
+ * testTupleConfig.cpp
+ *
+ *  Created on: Jan 13, 2010
+ *      Author: richard
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <stdexcept>
+#include <Pose2.h>
+#include <Point2.h>
+
+#include "TupleConfig.h"
+#include "Vector.h"
+
+using namespace gtsam;
+using namespace std;
+
+typedef Symbol<Pose2, 'x'> PoseKey;
+typedef Symbol<Point2, 'l'> PointKey;
+typedef PairConfig<PoseKey, Pose2, PointKey, Point2> Config;
+
+/* ************************************************************************* */
+TEST( PairConfig, insert_equals1 )
+{
+  Pose2 x1(1,2,3), x2(6,7,8);
+  Point2 l1(4,5), l2(9,10);
+
+  Config expected;
+  expected.insert(1, x1);
+  expected.insert(2, x2);
+  expected.insert(1, l1);
+  expected.insert(2, l2);
+
+  Config actual;
+  actual.insert(1, x1);
+  actual.insert(2, x2);
+  actual.insert(1, l1);
+  actual.insert(2, l2);
+
+  CHECK(assert_equal(expected,actual));
+}
+
+/* ************************************************************************* */
+TEST( PairConfig, insert_equals2 )
+{
+  Pose2 x1(1,2,3), x2(6,7,8);
+  Point2 l1(4,5), l2(9,10);
+
+  Config cfg1;
+  cfg1.insert(1, x1);
+  cfg1.insert(2, x2);
+  cfg1.insert(1, l1);
+  cfg1.insert(2, l2);
+
+  Config cfg2;
+  cfg2.insert(1, x1);
+  cfg2.insert(2, x2);
+  cfg2.insert(1, l1);
+
+  CHECK(!cfg1.equals(cfg2));
+
+  cfg2.insert(2, Point2(9,11));
+
+  CHECK(!cfg1.equals(cfg2));
+}
+
+///* ************************************************************************* */
+//TEST( PairConfig, insert_duplicate )
+//{
+//  Pose2 x1(1,2,3), x2(6,7,8);
+//  Point2 l1(4,5), l2(9,10);
+//
+//  Config cfg1;
+//  cfg1.insert(1, x1);
+//  cfg1.insert(2, x2);
+//  cfg1.insert(1, l1);
+//  cfg1.insert(2, l2);
+//  cfg1.insert(2, l1);
+//
+//  CHECK(assert_equal(l2, cfg1[PointKey(2)]));
+//  CHECK(cfg1.size() == 4);
+//  CHECK(cfg1.dim() == 10);
+//}
+
+
+/* ************************************************************************* */
+TEST( PairConfig, size_dim )
+{
+  Pose2 x1(1,2,3), x2(6,7,8);
+  Point2 l1(4,5), l2(9,10);
+
+  Config cfg1;
+  cfg1.insert(1, x1);
+  cfg1.insert(2, x2);
+  cfg1.insert(1, l1);
+  cfg1.insert(2, l2);
+
+  CHECK(cfg1.size() == 4);
+  CHECK(cfg1.dim() == 10);
+}
+
+/* ************************************************************************* */
+TEST(PairConfig, at)
+{
+  Pose2 x1(1,2,3), x2(6,7,8);
+  Point2 l1(4,5), l2(9,10);
+
+  Config cfg1;
+  cfg1.insert(1, x1);
+  cfg1.insert(2, x2);
+  cfg1.insert(1, l1);
+  cfg1.insert(2, l2);
+
+  CHECK(assert_equal(x1, cfg1[PoseKey(1)]));
+  CHECK(assert_equal(x2, cfg1[PoseKey(2)]));
+  CHECK(assert_equal(l1, cfg1[PointKey(1)]));
+  CHECK(assert_equal(l2, cfg1[PointKey(2)]));
+
+  bool caught = false;
+  try {
+    cfg1[PoseKey(3)];
+  } catch(invalid_argument e) {
+    caught = true;
+  }
+  CHECK(caught);
+
+  caught = false;
+  try {
+    cfg1[PointKey(3)];
+  } catch(invalid_argument e) {
+    caught = true;
+  }
+  CHECK(caught);
+}
+
+/* ************************************************************************* */
+TEST(PairConfig, expmap)
+{
+  Pose2 x1(1,2,3), x2(6,7,8);
+  Point2 l1(4,5), l2(9,10);
+
+  Config cfg1;
+  cfg1.insert(1, x1);
+  cfg1.insert(2, x2);
+  cfg1.insert(1, l1);
+  cfg1.insert(2, l2);
+
+  VectorConfig increment;
+  increment.insert("x1", Vector_(3, 1.0, 1.1, 1.2));
+  increment.insert("x2", Vector_(3, 1.3, 1.4, 1.5));
+  increment.insert("l1", Vector_(2, 1.0, 1.1));
+  increment.insert("l2", Vector_(2, 1.3, 1.4));
+
+  Config expected;
+  expected.insert(1, expmap(x1, Vector_(3, 1.0, 1.1, 1.2)));
+  expected.insert(2, expmap(x2, Vector_(3, 1.3, 1.4, 1.5)));
+  expected.insert(1, Point2(5.0, 6.1));
+  expected.insert(2, Point2(10.3, 11.4));
+
+  CHECK(assert_equal(expected, expmap(cfg1, increment)));
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */

cpp/testVSLAMConfig.cpp

@@ -16,12 +16,12 @@ TEST( VSLAMConfig, update_with_large_delta) {
 	// this test ensures that if the update for delta is larger than
 	// the size of the config, it only updates existing variables
 	VSLAMConfig init;
-	init.addCameraPose(1, Pose3());
+	init.insert(1, Pose3());
-	init.addLandmarkPoint(1, Point3(1.0, 2.0, 3.0));
+	init.insert(1, Point3(1.0, 2.0, 3.0));
 
 	VSLAMConfig expected;
-	expected.addCameraPose(1, Pose3(Rot3(), Point3(0.1, 0.1, 0.1)));
+	expected.insert(1, Pose3(Rot3(), Point3(0.1, 0.1, 0.1)));
-	expected.addLandmarkPoint(1, Point3(1.1, 2.1, 3.1));
+	expected.insert(1, Point3(1.1, 2.1, 3.1));
 
 	VectorConfig delta;
 	delta.insert("x1", Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1));

cpp/testVSLAMFactor.cpp

@@ -36,8 +36,8 @@ TEST( VSLAMFactor, error )
 
   // For the following configuration, the factor predicts 320,240
   VSLAMConfig config;
-  Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.addCameraPose(1, x1);
+  Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.insert(1, x1);
-  Point3 l1;  config.addLandmarkPoint(1, l1);
+  Point3 l1;  config.insert(1, l1);
   CHECK(assert_equal(Point2(320.,240.),factor->predict(x1,l1)));
 
   // Which yields an error of 3^2/2 = 4.5
@@ -65,8 +65,8 @@ TEST( VSLAMFactor, error )
 	delta.insert("l1",Vector_(3, 1.,2.,3.));
 	VSLAMConfig actual_config = expmap(config, delta);
   VSLAMConfig expected_config;
-  Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.addCameraPose(1, x2);
+  Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.insert(1, x2);
-  Point3 l2(1,2,3); expected_config.addLandmarkPoint(1, l2);
+  Point3 l2(1,2,3); expected_config.insert(1, l2);
   CHECK(assert_equal(expected_config,actual_config,1e-9));
 }
 

cpp/testVSLAMGraph.cpp

@@ -76,12 +76,12 @@ TEST( VSLAMGraph, optimizeLM)
 
   // Create an initial configuration corresponding to the ground truth
   boost::shared_ptr<VSLAMConfig> initialEstimate(new VSLAMConfig);
-  initialEstimate->addCameraPose(1, camera1);
+  initialEstimate->insert(1, camera1);
-  initialEstimate->addCameraPose(2, camera2);
+  initialEstimate->insert(2, camera2);
-  initialEstimate->addLandmarkPoint(1, landmark1);
+  initialEstimate->insert(1, landmark1);
-  initialEstimate->addLandmarkPoint(2, landmark2);
+  initialEstimate->insert(2, landmark2);
-  initialEstimate->addLandmarkPoint(3, landmark3);
+  initialEstimate->insert(3, landmark3);
-  initialEstimate->addLandmarkPoint(4, landmark4);
+  initialEstimate->insert(4, landmark4);
 
   // Create an ordering of the variables
   list<string> keys;
@@ -119,12 +119,12 @@ TEST( VSLAMGraph, optimizeLM2)
 
   // Create an initial configuration corresponding to the ground truth
   boost::shared_ptr<VSLAMConfig> initialEstimate(new VSLAMConfig);
-  initialEstimate->addCameraPose(1, camera1);
+  initialEstimate->insert(1, camera1);
-  initialEstimate->addCameraPose(2, camera2);
+  initialEstimate->insert(2, camera2);
-  initialEstimate->addLandmarkPoint(1, landmark1);
+  initialEstimate->insert(1, landmark1);
-  initialEstimate->addLandmarkPoint(2, landmark2);
+  initialEstimate->insert(2, landmark2);
-  initialEstimate->addLandmarkPoint(3, landmark3);
+  initialEstimate->insert(3, landmark3);
-  initialEstimate->addLandmarkPoint(4, landmark4);
+  initialEstimate->insert(4, landmark4);
 
   // Create an ordering of the variables
   list<string> keys;
@@ -163,12 +163,12 @@ TEST( VSLAMGraph, CHECK_ORDERING)
 
   // Create an initial configuration corresponding to the ground truth
   boost::shared_ptr<VSLAMConfig> initialEstimate(new VSLAMConfig);
-  initialEstimate->addCameraPose(1, camera1);
+  initialEstimate->insert(1, camera1);
-  initialEstimate->addCameraPose(2, camera2);
+  initialEstimate->insert(2, camera2);
-  initialEstimate->addLandmarkPoint(1, landmark1);
+  initialEstimate->insert(1, landmark1);
-  initialEstimate->addLandmarkPoint(2, landmark2);
+  initialEstimate->insert(2, landmark2);
-  initialEstimate->addLandmarkPoint(3, landmark3);
+  initialEstimate->insert(3, landmark3);
-  initialEstimate->addLandmarkPoint(4, landmark4);
+  initialEstimate->insert(4, landmark4);
 
   shared_ptr<Ordering> ordering(new Ordering(graph->getOrdering()));