removed dim_ (which was buggy) and added zero

cpp/LieConfig-inl.h

@@ -54,17 +54,33 @@ namespace gtsam {
     else return it->second;
   }
 
+  template<class J, class T>
+  size_t LieConfig<J,T>::dim() const {
+  	size_t n = 0;
+  	typedef pair<J,T> Value;
+  	BOOST_FOREACH(const Value& value, values_)
+  		n += gtsam::dim(value.second);
+  	return n;
+  }
+
+  template<class J, class T>
+  VectorConfig LieConfig<J,T>::zero() const {
+  	VectorConfig z;
+  	typedef pair<J,T> Value;
+  	BOOST_FOREACH(const Value& value, values_)
+  		z.insert(value.first,gtsam::zero(gtsam::dim(value.second)));
+  	return z;
+  }
+
   template<class J, class T>
   void LieConfig<J,T>::insert(const J& name, const T& val) {
     values_.insert(make_pair(name, val));
-    dim_ += gtsam::dim(val);
   }
 
   template<class J, class T>
   void LieConfig<J,T>::insert(const LieConfig<J,T>& cfg) {
 	  BOOST_FOREACH(const typename Values::value_type& v, cfg.values_)
 		 insert(v.first, v.second);
-	  dim_ += cfg.dim_;
   }
 
   template<class J, class T>
@@ -86,7 +102,6 @@ namespace gtsam {
     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);
   }
 
@@ -111,8 +126,10 @@ namespace gtsam {
   // todo: insert for every element is inefficient
   template<class J, class T>
   LieConfig<J,T> expmap(const LieConfig<J,T>& c, const Vector& delta) {
-    if(delta.size() != dim(c))
+    if(delta.size() != dim(c)) {
+    	cout << "LieConfig::dim (" << dim(c) << ") <> delta.size (" << delta.size() << ")" << endl;
       throw invalid_argument("Delta vector length does not match config dimensionality.");
+    }
     LieConfig<J,T> newConfig;
     int delta_offset = 0;
 		typedef pair<J,T> Value;
@@ -139,6 +156,7 @@ namespace gtsam {
   	}
   	return delta;
   }
+
 }
 
 

cpp/LieConfig.h

@@ -41,13 +41,12 @@ namespace gtsam {
   private:
 
     Values values_;
-    size_t dim_;
 
   public:
 
-    LieConfig() : dim_(0) {}
+    LieConfig() {}
     LieConfig(const LieConfig& config) :
-      values_(config.values_), dim_(config.dim_) {}
+      values_(config.values_) {}
     virtual ~LieConfig() {}
 
     /** print */
@@ -71,10 +70,11 @@ namespace gtsam {
     /** whether the config is empty */
     bool empty() const { return values_.empty(); }
 
-    /**
-     * The dimensionality of the tangent space
-     */
-    size_t dim() const { return dim_; }
+    /** The dimensionality of the tangent space */
+    size_t dim() const;
+
+    /** Get a zero Vectorconfig of the correct structure */
+    VectorConfig zero() const;
 
     const_iterator begin() const { return values_.begin(); }
     const_iterator end() const { return values_.end(); }
@@ -106,14 +106,12 @@ namespace gtsam {
     /** Replace all keys and variables */
     LieConfig& operator=(const LieConfig& rhs) {
       values_ = rhs.values_;
-      dim_ = rhs.dim_;
       return (*this);
     }
 
     /** Remove all variables from the config */
     void clear() {
       values_.clear();
-      dim_ = 0;
     }
 
   };

cpp/testLieConfig.cpp

@@ -7,19 +7,19 @@
 
 #include <CppUnitLite/TestHarness.h>
 #include <stdexcept>
-
+#include <limits>
 #include <boost/assign/std/list.hpp> // for operator +=
 using namespace boost::assign;
 
 #define GTSAM_MAGIC_KEY
 
-#include <Pose2.h>
-
+#include "Pose2.h"
 #include "LieConfig-inl.h"
 #include "Vector.h"
 
 using namespace gtsam;
 using namespace std;
+static double inf = std::numeric_limits<double>::infinity();
 
 /* ************************************************************************* */
 TEST( LieConfig, equals1 )
@@ -49,7 +49,7 @@ TEST( LieConfig, equals_nan )
 {
   LieConfig<string,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);
+  Vector v2 = Vector_(3, inf, inf, inf);
   cfg1.insert("x", v1);
   cfg2.insert("x", v2);
   CHECK(!cfg1.equals(cfg2));
@@ -79,6 +79,20 @@ TEST( LieConfig, insert_config )
   CHECK(assert_equal(cfg1, expected));
 }
 
+/* ************************************************************************* */
+TEST(LieConfig, dim_zero)
+{
+  LieConfig<string,Vector> config0;
+  config0.insert("v1", Vector_(2, 2.0, 3.0));
+  config0.insert("v2", Vector_(3, 5.0, 6.0, 7.0));
+  LONGS_EQUAL(5,config0.dim());
+
+  VectorConfig expected;
+  expected.insert("v1", zero(2));
+  expected.insert("v2", zero(3));
+  CHECK(assert_equal(expected, config0.zero()));
+}
+
 /* ************************************************************************* */
 TEST(LieConfig, expmap_a)
 {
@@ -132,7 +146,7 @@ TEST(LieConfig, expmap_c)
   CHECK(assert_equal(expected, expmap(config0, increment)));
 }
 
-/* ************************************************************************* */
+/* ************************************************************************* *
 TEST(LieConfig, expmap_d)
 {
   LieConfig<string,Vector> config0;
@@ -150,7 +164,7 @@ TEST(LieConfig, expmap_d)
   CHECK(config0.equals(config0));
 }
 
-/* ************************************************************************* */
+/* ************************************************************************* *
 TEST(LieConfig, extract_keys)
 {
 	typedef TypedSymbol<Pose2, 'x'> PoseKey;