Vector-like operators

cpp/VectorConfig.cpp

@@ -14,7 +14,8 @@
 #define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) 
 
 using namespace std;
-using namespace gtsam;
+
+namespace gtsam {
 
 /* ************************************************************************* */
 void check_size(const string& key, const Vector & vj, const Vector & dj) {
@@ -27,14 +28,65 @@ void check_size(const string& key, const Vector & vj, const Vector & dj) {
 }
 
 /* ************************************************************************* */
-VectorConfig VectorConfig::scale(double gain) const {
+std::vector<std::string> VectorConfig::get_names() const {
+  std::vector<std::string> names;
+  for(const_iterator it=values.begin(); it!=values.end(); it++)
+    names.push_back(it->first);
+  return names;
+}
+
+/* ************************************************************************* */
+VectorConfig& VectorConfig::insert(const std::string& name, const Vector& val) {
+  values.insert(std::make_pair(name,val));
+  return *this;
+}
+
+/* ************************************************************************* */
+void VectorConfig::add(const std::string& j, const Vector& a) {
+	Vector& vj = values[j];
+	if (vj.size()==0) vj = a; else vj += a;
+}
+
+/* ************************************************************************* */
+size_t VectorConfig::dim() const {
+	size_t result=0;
+	string key; Vector v;
+	FOREACH_PAIR(key, v, values) result += v.size();
+	return result;
+}
+
+/* ************************************************************************* */
+VectorConfig VectorConfig::scale(double s) const {
 	VectorConfig scaled;
 	string key; Vector val;
 	FOREACH_PAIR(key, val, values)
-		scaled.insert(key, gain*val);
+		scaled.insert(key, s*val);
 	return scaled;
 }
 
+/* ************************************************************************* */
+VectorConfig VectorConfig::operator*(double s) const {
+	return scale(s);
+}
+
+/* ************************************************************************* */
+VectorConfig VectorConfig::operator+(const VectorConfig& b) const {
+	VectorConfig result;
+	string key; Vector v;
+	FOREACH_PAIR(key, v, values)
+		result.insert(key, v + b.get(key));
+	return result;
+}
+
+/* ************************************************************************* */
+VectorConfig VectorConfig::operator-(const VectorConfig& b) const {
+	VectorConfig result;
+	string key; Vector v;
+	FOREACH_PAIR(key, v, values)
+		result.insert(key, v - b.get(key));
+	return result;
+}
+
 /* ************************************************************************* */
 VectorConfig VectorConfig::exmap(const VectorConfig & delta) const
 {
@@ -90,4 +142,16 @@ bool VectorConfig::equals(const VectorConfig& expected, double tol) const {
 }
 
 /* ************************************************************************* */
+double VectorConfig::dot(const VectorConfig& b) const {
+	string j; Vector v; double result = 0.0;
+	FOREACH_PAIR(j, v, values) result += gtsam::dot(v,b.get(j));
+	return result;
+}
 
+/* ************************************************************************* */
+double dot(const VectorConfig& a, const VectorConfig& b) {
+	return a.dot(b);
+}
+/* ************************************************************************* */
+
+} // gtsam

cpp/VectorConfig.h

@@ -34,24 +34,13 @@ namespace gtsam {
     virtual ~VectorConfig() {}
 
     /** return all the nodes in the graph **/
-    std::vector<std::string> get_names() const {
-      std::vector<std::string> names;
-      for(const_iterator it=values.begin(); it!=values.end(); it++)
-        names.push_back(it->first);
-      return names;
-    }
+    std::vector<std::string> get_names() const;
 
     /** Insert a value into the configuration with a given index */
-    VectorConfig& insert(const std::string& name, const Vector& val) {
-      values.insert(std::make_pair(name,val));
-      return *this;
-    }
+    VectorConfig& insert(const std::string& name, const Vector& val);
 
     /** Add to vector at position j */
-    void add(const std::string& j, const Vector& a) {
-    	Vector& vj = values[j];
-    	if (vj.size()==0) vj = a; else vj += a;
-    }
+    void add(const std::string& j, const Vector& a);
 
     /**
      * Add a delta config, needed for use in NonlinearOptimizer
@@ -59,9 +48,6 @@ namespace gtsam {
      */
     VectorConfig exmap(const VectorConfig & delta) const;
 
-    /** Scales the configuration by a gain */
-    VectorConfig scale(double gain) const;
- 
     const_iterator begin() const {return values.begin();}
     const_iterator end()   const {return values.end();}
 
@@ -73,15 +59,24 @@ namespace gtsam {
 
     bool contains(const std::string& name) const {
       const_iterator it = values.find(name);
-      if (it==values.end())
-        return false;
-      return true;
+      return (it!=values.end());
     }
 
-    /** size of the configurations */
-    size_t size() const {
-      return values.size();
-    }
+    /** Nr of vectors */
+    size_t size() const { return values.size();}
+
+    /** Total dimensionality */
+    size_t dim() const;
+
+    /** Scale */
+    VectorConfig scale(double s) const;
+    VectorConfig operator*(double s) const;
+
+    /** Add */
+    VectorConfig operator+(const VectorConfig &b) const;
+
+    /** Subtract */
+    VectorConfig operator-(const VectorConfig &b) const;
 
     /** print */
     void print(const std::string& name = "") const;
@@ -91,6 +86,8 @@ namespace gtsam {
 
     void clear() {values.clear();}
     
+    /** Dot product */
+    double dot(const VectorConfig& b) const;
     
   private:
     /** Serialization function */
@@ -100,5 +97,9 @@ namespace gtsam {
     {
       ar & BOOST_SERIALIZATION_NVP(values);
     }
-  };
-}
+  }; // VectorConfig
+
+  /** Dot product */
+  double dot(const VectorConfig&, const VectorConfig&);
+
+} // gtsam

cpp/testVectorConfig.cpp

@@ -122,6 +122,28 @@ TEST( VectorConfig, update_with_large_delta) {
 	CHECK(assert_equal(actual, expected));
 }
 
+/* ************************************************************************* */
+TEST( VectorConfig, dot) {
+	VectorConfig c = createConfig();
+	DOUBLES_EQUAL(3.25,dot(c,c),1e-9);
+}
+
+/* ************************************************************************* */
+TEST( VectorConfig, dim) {
+	VectorConfig c = createConfig();
+	LONGS_EQUAL(6,c.dim());
+}
+
+/* ************************************************************************* */
+TEST( VectorConfig, operators) {
+	VectorConfig c; c.insert("x", Vector_(2, 1.1, 2.2));
+	VectorConfig expected1; expected1.insert("x", Vector_(2, 2.2, 4.4));
+	CHECK(assert_equal(expected1,c*2));
+	CHECK(assert_equal(expected1,c+c));
+	VectorConfig expected2; expected2.insert("x", Vector_(2, 0.0, 0.0));
+	CHECK(assert_equal(expected2,c-c));
+}
+
 /* ************************************************************************* */
 #ifdef HAVE_BOOST_SERIALIZATION
 TEST( VectorConfig, serialize)