Merging lieconfig branch - LieConfig works but currently requires Lie objects to have a global print function.

.cproject

@@ -300,6 +300,7 @@
 <buildTargets>
 <target name="install" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>install</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -307,6 +308,7 @@
 </target>
 <target name="check" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>check</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -322,6 +324,7 @@
 </target>
 <target name="testSimpleCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testSimpleCamera.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -337,7 +340,6 @@
 </target>
 <target name="testVSLAMFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-						<buildArguments/>
 <buildTarget>testVSLAMFactor.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -345,6 +347,7 @@
 </target>
 <target name="testCalibratedCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testCalibratedCamera.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -352,7 +355,6 @@
 </target>
 <target name="testGaussianConditional.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-						<buildArguments/>
 <buildTarget>testGaussianConditional.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -360,6 +362,7 @@
 </target>
 <target name="testPose2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testPose2.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -367,6 +370,7 @@
 </target>
 <target name="testRot3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testRot3.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -374,7 +378,6 @@
 </target>
 <target name="testNonlinearOptimizer.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-						<buildArguments/>
 <buildTarget>testNonlinearOptimizer.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -382,6 +385,7 @@
 </target>
 <target name="testGaussianFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testGaussianFactor.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -389,6 +393,7 @@
 </target>
 <target name="testGaussianFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testGaussianFactorGraph.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -396,7 +401,6 @@
 </target>
 <target name="testNonlinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-						<buildArguments/>
 <buildTarget>testNonlinearFactorGraph.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -404,6 +408,7 @@
 </target>
 <target name="testPose3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testPose3.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -411,7 +416,6 @@
 </target>
 <target name="testVectorConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-						<buildArguments/>
 <buildTarget>testVectorConfig.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -419,7 +423,6 @@
 </target>
 <target name="testPoint2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-						<buildArguments/>
 <buildTarget>testPoint2.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -427,6 +430,7 @@
 </target>
 <target name="testNonlinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testNonlinearFactor.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -434,6 +438,7 @@
 </target>
 <target name="timeGaussianFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>timeGaussianFactor.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -441,6 +446,7 @@
 </target>
 <target name="timeGaussianFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>timeGaussianFactorGraph.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -448,6 +454,7 @@
 </target>
 <target name="testGaussianBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testGaussianBayesNet.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -455,7 +462,6 @@
 </target>
 <target name="testBayesTree.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-						<buildArguments/>
 <buildTarget>testBayesTree.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>false</useDefaultCommand>
@@ -463,6 +469,7 @@
 </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>
@@ -470,7 +477,6 @@
 </target>
 <target name="testSymbolicFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-						<buildArguments/>
 <buildTarget>testSymbolicFactorGraph.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>false</useDefaultCommand>
@@ -478,6 +484,7 @@
 </target>
 <target name="testVector.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testVector.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -485,6 +492,7 @@
 </target>
 <target name="testMatrix.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testMatrix.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -492,7 +500,6 @@
 </target>
 <target name="testVSLAMGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-						<buildArguments/>
 <buildTarget>testVSLAMGraph.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -500,7 +507,6 @@
 </target>
 <target name="testNonlinearEquality.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-						<buildArguments/>
 <buildTarget>testNonlinearEquality.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -508,6 +514,7 @@
 </target>
 <target name="testSQP.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testSQP.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -515,7 +522,6 @@
 </target>
 <target name="testNonlinearConstraint.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-						<buildArguments/>
 <buildTarget>testNonlinearConstraint.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -523,6 +529,7 @@
 </target>
 <target name="testSQPOptimizer.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testSQPOptimizer.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -530,6 +537,7 @@
 </target>
 <target name="testVSLAMConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testVSLAMConfig.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -537,6 +545,7 @@
 </target>
 <target name="testControlConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testControlConfig.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -544,6 +553,7 @@
 </target>
 <target name="testControlPoint.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testControlPoint.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -551,7 +561,6 @@
 </target>
 <target name="testControlGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-						<buildArguments/>
 <buildTarget>testControlGraph.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -559,6 +568,7 @@
 </target>
 <target name="testOrdering.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testOrdering.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -566,7 +576,6 @@
 </target>
 <target name="testPose2Constraint.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-						<buildArguments/>
 <buildTarget>testPose2Constraint.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -574,6 +583,7 @@
 </target>
 <target name="testRot2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testRot2.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -581,6 +591,7 @@
 </target>
 <target name="testGaussianBayesTree.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testGaussianBayesTree.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -588,7 +599,6 @@
 </target>
 <target name="testPose3Factor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-						<buildArguments/>
 <buildTarget>testPose3Factor.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -596,7 +606,6 @@
 </target>
 <target name="testUrbanGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-						<buildArguments/>
 <buildTarget>testUrbanGraph.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -604,7 +613,6 @@
 </target>
 <target name="testUrbanConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-						<buildArguments/>
 <buildTarget>testUrbanConfig.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -612,7 +620,6 @@
 </target>
 <target name="testUrbanMeasurement.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-						<buildArguments/>
 <buildTarget>testUrbanMeasurement.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -620,6 +627,7 @@
 </target>
 <target name="testUrbanOdometry.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testUrbanOdometry.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -627,7 +635,6 @@
 </target>
 <target name="testIterative.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-						<buildArguments/>
 <buildTarget>testIterative.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -635,7 +642,6 @@
 </target>
 <target name="testGaussianISAM2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-						<buildArguments/>
 <buildTarget>testGaussianISAM2.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -643,6 +649,7 @@
 </target>
 <target name="testSubgraphPreconditioner.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testSubgraphPreconditioner.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -650,6 +657,7 @@
 </target>
 <target name="testBayesNetPreconditioner.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testBayesNetPreconditioner.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -657,7 +665,6 @@
 </target>
 <target name="testPose2Factor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-						<buildArguments/>
 <buildTarget>testPose2Factor.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -665,7 +672,6 @@
 </target>
 <target name="timeRot3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-						<buildArguments/>
 <buildTarget>timeRot3.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -673,15 +679,21 @@
 </target>
 <target name="install" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-						<buildArguments/>
 <buildTarget>install</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
 <runAllBuilders>true</runAllBuilders>
 </target>
+<target name="testLieConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+<buildCommand>make</buildCommand>
+<buildArguments>-j2</buildArguments>
+<buildTarget>testLieConfig.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/>
 <buildTarget>install</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -689,7 +701,6 @@
 </target>
 <target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-						<buildArguments/>
 <buildTarget>clean</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -697,7 +708,6 @@
 </target>
 <target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-						<buildArguments/>
 <buildTarget>check</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>

cpp/Lie.h

@@ -8,10 +8,8 @@
 #ifndef LIE_H_
 #define LIE_H_
 
+#include <string>
 #include "Vector.h"
-#include "VectorConfig.h"
-#include "Matrix.h"
-
 
 namespace gtsam {
 
@@ -61,6 +59,25 @@ namespace gtsam {
     Vector logmap(const T& lp) const;
 
   };
+  
+  /** Call print on the object */
+  template<class T>
+  inline void print_(const T& object, const char *s = "") {
+    object.print(s);
+  }
+
+  /** Call equal on the object */
+  template<class T>
+  inline bool equal(const T& obj1, const T& obj2, double tol) {
+    return obj1.equals(obj2, tol);
+  }
+
+  /** Call equal on the object without tolerance (use default tolerance) */
+  template<class T>
+  inline bool equal(const T& obj1, const T& obj2) {
+    return obj1.equal(obj2);
+  }
+
 }
 
 

cpp/LieConfig-inl.h

@@ -0,0 +1,68 @@
+/*
+ * LieConfig.cpp
+ *
+ *  Created on: Jan 8, 2010
+ *      Author: richard
+ */
+
+#include <boost/foreach.hpp>
+#include <boost/tuple/tuple.hpp>
+#include "LieConfig.h"
+
+using namespace std;
+
+#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
+
+namespace gtsam {
+
+  template<class T>
+  void LieConfig<T>::print(const std::string &s) const {
+       std::cout << "LieConfig " << s << ", size " << values_.size() << "\n";
+       std::pair<std::string, T> v;
+       BOOST_FOREACH(v, values_)
+         ::gtsam::print(v.second, v.first + ": ");
+     }
+
+  template<class T>
+  bool LieConfig<T>::equals(const LieConfig<T>& expected, double tol) const {
+    if (values_.size() != expected.values_.size()) return false;
+    std::pair<std::string, T> v;
+    BOOST_FOREACH(v, values_) {
+      boost::optional<const T&> expval = expected.gettry(v.first);
+      if(!expval || !::gtsam::equal(v.second, *expval, tol))
+        return false;
+    }
+    return true;
+  }
+
+  template<class T>
+  LieConfig<T> expmap(const LieConfig<T>& c, const VectorConfig& delta) {
+    LieConfig<T> newConfig;
+    std::string j; T pj;
+    FOREACH_PAIR(j, pj, c.values_) {
+        if (delta.contains(j)) {
+            const Vector& dj = delta[j];
+            //check_size(j,vj,dj);
+            newConfig.insert(j, expmap(pj,dj));
+        } else
+            newConfig.insert(j, pj);
+    }
+    return newConfig;
+  }
+
+  template<class T>
+  LieConfig<T> expmap(const LieConfig<T>& c, const Vector& delta) {
+    LieConfig<T> newConfig;
+    std::pair<std::string, Vector> value;
+    int delta_offset = 0;
+    BOOST_FOREACH(value, c) {
+      int cur_dim = dim(value.second);
+      newConfig.insert(value.first,
+          expmap(value.second,
+          sub(delta, delta_offset, delta_offset+cur_dim)));
+      delta_offset += cur_dim;
+    }
+    return newConfig;
+  }
+
+}

cpp/LieConfig.h

@@ -0,0 +1,115 @@
+/*
+ * LieConfig.h
+ *
+ *  Created on: Jan 5, 2010
+ *      Author: Richard Roberts
+ *
+ *  A templated config for Lie-group elements
+ */
+
+#pragma once
+
+
+#include <map>
+#include <string>
+#include <boost/foreach.hpp>
+#include <utility>
+#include <iostream>
+#include <stdexcept>
+#include <boost/optional.hpp>
+
+#include "Testable.h"
+#include "VectorConfig.h"
+#include "Vector.h"
+#include "Lie.h"
+
+
+namespace gtsam {
+
+  template<class T> class LieConfig;
+
+  /** Dimensionality of the tangent space */
+  template<class T>
+  size_t dim(const LieConfig<T>& c);
+
+  /** Add a delta config */
+  template<class T>
+  LieConfig<T> expmap(const LieConfig<T>& c, const VectorConfig& delta);
+
+  /** Add a delta vector, uses iterator order */
+  template<class T>
+  LieConfig<T> expmap(const LieConfig<T>& c, const Vector& delta);
+
+
+  template<class T>
+  class LieConfig : public Testable<LieConfig<T> > {
+  private:
+    std::map<std::string, T> values_;
+    size_t dim_;
+
+  public:
+    typedef typename std::map<std::string, T>::const_iterator iterator;
+    typedef iterator const_iterator;
+
+    LieConfig() : dim_(0) {}
+    LieConfig(const LieConfig& config) :
+      values_(config.values_), dim_(dim(config)) {}
+    virtual ~LieConfig() {}
+
+    /** Retrieve a variable by key, throws std::invalid_argument if not found */
+    const T& get(std::string& key) const {
+      iterator it = values_.find(key);
+      if (it == values_.end()) throw std::invalid_argument("invalid key");
+      else return it->second;
+    }
+
+    /** Retrieve a variable by key, returns nothing if not found */
+    boost::optional<const T&> gettry(std::string& key) const {
+      const_iterator it = values_.find(key);
+      if (it == values_.end()) return boost::optional<const T&>();
+      else return it->second;
+    }
+
+    /** Add a variable with the given key */
+    void insert(const std::string& name, const T& val) {
+      values_.insert(make_pair(name, val));
+      dim_ += dim(val);
+    }
+
+    /** Replace all keys and variables */
+    LieConfig& operator=(const LieConfig& rhs) {
+      values_ = rhs.values_;
+      dim_ = dim(rhs);
+      return (*this);
+    }
+
+    /** Remove all variables from the config */
+    void clear() {
+      values_.clear();
+      dim_ = 0;
+    }
+
+    /** The number of variables in this config */
+    int size() { return values_.size(); }
+
+    /** Test whether configs are identical in keys and values */
+    bool equals(const LieConfig& expected, double tol=1e-9) const;
+
+    void print(const std::string &s) const;
+
+    const_iterator begin() const { return values_.begin(); }
+    const_iterator end() const { return values_.end(); }
+    iterator begin() { return values_.begin(); }
+    iterator end() { return values_.end(); }
+
+    friend LieConfig<T> expmap<T>(const LieConfig<T>& c, const VectorConfig& delta);
+    friend LieConfig<T> expmap<T>(const LieConfig<T>& c, const Vector& delta);
+    friend size_t dim<T>(const LieConfig<T>& c);
+
+  };
+
+  /** Dimensionality of the tangent space */
+  template<class T>
+  size_t dim(const LieConfig<T>& c) { return c.dim_; }
+}
+

cpp/Makefile.am

@@ -159,7 +159,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
+check_PROGRAMS += testPoint2 testRot2 testPose2 testPoint3 testRot3 testPose3 testCal3_S2 testLieConfig
 testPoint2_SOURCES  = testPoint2.cpp
 testRot2_SOURCES    = testRot2.cpp
 testPose2_SOURCES   = testPose2.cpp
@@ -167,6 +167,7 @@ testPoint3_SOURCES  = testPoint3.cpp
 testRot3_SOURCES    = testRot3.cpp
 testPose3_SOURCES   = testPose3.cpp
 testCal3_S2_SOURCES = testCal3_S2.cpp
+testLieConfig_SOURCES = testLieConfig.cpp               
 
 testPoint2_LDADD    = libgtsam.la
 testRot2_LDADD      = libgtsam.la
@@ -175,6 +176,7 @@ testPoint3_LDADD    = libgtsam.la
 testRot3_LDADD      = libgtsam.la
 testPose3_LDADD     = libgtsam.la
 testCal3_S2_LDADD   = libgtsam.la
+testLieConfig_LDADD = libgtsam.la
 
 noinst_PROGRAMS = timeRot3
 timeRot3_SOURCES = timeRot3.cpp

cpp/Point2.h

@@ -8,6 +8,7 @@
 
 #include <boost/serialization/nvp.hpp>
 #include "Vector.h"
+#include "Matrix.h"
 #include "Testable.h"
 #include "Lie.h"
 

cpp/Pose2.h

@@ -81,6 +81,8 @@ namespace gtsam {
   /** Return the x,y,theta of this pose */
   inline Vector logmap(const Pose2& p) { return Vector_(3, p.x(), p.y(), p.theta()); }
 
+  /** print using member print function, currently used by LieConfig */
+  inline void print(const Pose2& obj, const std::string& str = "") { obj.print(str); }
 
   /** Return point coordinates in pose coordinate frame */
   inline Point2 transform_to(const Pose2& pose, const Point2& point) {

cpp/Vector.h

@@ -19,6 +19,8 @@
 typedef boost::numeric::ublas::vector<double> Vector;
 #endif
 
+#include "Lie.h"
+
 namespace gtsam {
 
 /**
@@ -107,6 +109,20 @@ bool greaterThanOrEqual(const Vector& v1, const Vector& v2);
  */
 bool equal_with_abs_tol(const Vector& vec1, const Vector& vec2, double tol=1e-9);
 
+/**
+ * Override of equal in Lie.h
+ */
+inline bool equal(const Vector& vec1, const Vector& vec2, double tol) {
+  return equal_with_abs_tol(vec1, vec2, tol);
+}
+
+/**
+ * Override of equal in Lie.h
+ */
+inline bool equal(const Vector& vec1, const Vector& vec2) {
+  return equal_with_abs_tol(vec1, vec2);
+}
+
 /**
  * Same, prints if error
  * @param vec1 Vector
@@ -193,6 +209,21 @@ Vector concatVectors(size_t nrVectors, ...);
  */
 Vector rand_vector_norm(size_t dim, double mean = 0, double sigma = 1);
 
+/**
+ * Exponential map, just returns the vector itself
+ */
+template<> inline Vector expmap(const Vector& v) { return v; }
+
+/**
+ * Compose, adds vectors
+ */
+inline Vector compose(const Vector& v0, const Vector& v1) { return v0+v1; }
+
+/**
+ * Inverse, negates the vector
+ */
+inline Vector inverse(const Vector& v) { return -v; }
+
 } // namespace gtsam
 
 static boost::minstd_rand generator(42u);

cpp/testLieConfig.cpp

@@ -0,0 +1,122 @@
+/*
+ * testLieConfig.cpp
+ *
+ *  Created on: Jan 5, 2010
+ *      Author: richard
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <stdexcept>
+#include <Pose2.h>
+
+#include "LieConfig-inl.h"
+#include "Vector.h"
+
+using namespace gtsam;
+using namespace std;
+
+/* ************************************************************************* */
+TEST( LieConfig, equals1 )
+{
+  LieConfig<Vector> expected;
+  Vector v = Vector_(3, 5.0, 6.0, 7.0);
+  expected.insert("a",v);
+  LieConfig<Vector> actual;
+  actual.insert("a",v);
+  CHECK(assert_equal(expected,actual));
+}
+
+/* ************************************************************************* */
+TEST( LieConfig, equals2 )
+{
+  LieConfig<Vector> cfg1, cfg2;
+  Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
+  Vector v2 = Vector_(3, 5.0, 6.0, 8.0);
+  cfg1.insert("x", v1);
+  cfg2.insert("x", v2);
+  CHECK(!cfg1.equals(cfg2));
+  CHECK(!cfg2.equals(cfg1));
+}
+
+/* ************************************************************************* */
+TEST( LieConfig, equals_nan )
+{
+  LieConfig<Vector> cfg1, cfg2;
+  Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
+  Vector v2 = Vector_(3, 0.0/0.0, 0.0/0.0, 0.0/0.0);
+  cfg1.insert("x", v1);
+  cfg2.insert("x", v2);
+  CHECK(!cfg1.equals(cfg2));
+  CHECK(!cfg2.equals(cfg1));
+}
+
+/* ************************************************************************* */
+TEST(LieConfig, expmap_a)
+{
+  LieConfig<Vector> config0;
+  config0.insert("v1", Vector_(3, 1.0, 2.0, 3.0));
+  config0.insert("v2", Vector_(3, 5.0, 6.0, 7.0));
+
+  VectorConfig increment;
+  increment.insert("v1", Vector_(3, 1.0, 1.1, 1.2));
+  increment.insert("v2", Vector_(3, 1.3, 1.4, 1.5));
+
+  LieConfig<Vector> expected;
+  expected.insert("v1", Vector_(3, 2.0, 3.1, 4.2));
+  expected.insert("v2", Vector_(3, 6.3, 7.4, 8.5));
+
+  CHECK(assert_equal(expected, expmap(config0, increment)));
+}
+
+/* ************************************************************************* */
+TEST(LieConfig, expmap_b)
+{
+  LieConfig<Vector> config0;
+  config0.insert("v1", Vector_(3, 1.0, 2.0, 3.0));
+  config0.insert("v2", Vector_(3, 5.0, 6.0, 7.0));
+
+  VectorConfig increment;
+  increment.insert("v2", Vector_(3, 1.3, 1.4, 1.5));
+
+  LieConfig<Vector> expected;
+  expected.insert("v1", Vector_(3, 1.0, 2.0, 3.0));
+  expected.insert("v2", Vector_(3, 6.3, 7.4, 8.5));
+
+  CHECK(assert_equal(expected, expmap(config0, increment)));
+}
+
+/* ************************************************************************* */
+TEST(LieConfig, expmap_c)
+{
+  LieConfig<Vector> config0;
+  config0.insert("v1", Vector_(3, 1.0, 2.0, 3.0));
+  config0.insert("v2", Vector_(3, 5.0, 6.0, 7.0));
+
+  Vector increment = Vector_(6,
+      1.0, 1.1, 1.2,
+      1.3, 1.4, 1.5);
+
+  LieConfig<Vector> expected;
+  expected.insert("v1", Vector_(3, 2.0, 3.1, 4.2));
+  expected.insert("v2", Vector_(3, 6.3, 7.4, 8.5));
+
+  CHECK(assert_equal(expected, expmap(config0, increment)));
+}
+
+/* ************************************************************************* */
+TEST(LieConfig, expmap_d)
+{
+  LieConfig<Vector> config0;
+  config0.insert("v1", Vector_(3, 1.0, 2.0, 3.0));
+  config0.insert("v2", Vector_(3, 5.0, 6.0, 7.0));
+  config0.print("config0");
+
+  LieConfig<Pose2> poseconfig;
+  poseconfig.insert("p1", Pose2(1,2,3));
+  poseconfig.insert("p2", Pose2(0.3, 0.4, 0.5));
+  poseconfig.print("poseconfig");
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */