From bbb76aab9527f4f153607af365cbb42af4dea99d Mon Sep 17 00:00:00 2001
From: thduynguyen <thduynguyen@gmail.com>
Date: Tue, 29 Apr 2014 16:18:23 -0400
Subject: [PATCH] create VectorValues with all 1.0

---
 gtsam/linear/VectorValues.cpp | 25 +++++++++++++++++++++++++
 gtsam/linear/VectorValues.h   |  7 +++++++
 2 files changed, 32 insertions(+)

diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp
index 664fcf3b7..ab607875d 100644
--- a/gtsam/linear/VectorValues.cpp
+++ b/gtsam/linear/VectorValues.cpp
@@ -66,6 +66,15 @@ namespace gtsam {
     return result;
   }
 
+  /* ************************************************************************* */
+  VectorValues VectorValues::One(const VectorValues& other)
+  {
+    VectorValues result;
+    BOOST_FOREACH(const KeyValuePair& v, other)
+      result.values_.insert(make_pair(v.first, Vector::Ones(v.second.size())));
+    return result;
+  }
+
   /* ************************************************************************* */
   void VectorValues::update(const VectorValues& values)
   {
@@ -307,6 +316,22 @@ namespace gtsam {
     return result;
   }
 
+  /* ************************************************************************* */
+  VectorValues VectorValues::operator*(const VectorValues& c) const
+  {
+    if(this->size() != c.size())
+      throw invalid_argument("VectorValues::operator* called with different vector sizes");
+    assert_throw(hasSameStructure(c),
+      invalid_argument("VectorValues::operator* called with different vector sizes"));
+
+    VectorValues result;
+    // The result.end() hint here should result in constant-time inserts
+    for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2)
+      result.values_.insert(result.end(), make_pair(j1->first, j1->second * j2->second));
+
+    return result;
+  }
+
   /* ************************************************************************* */
   VectorValues VectorValues::subtract(const VectorValues& c) const
   {
diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h
index 7f7943341..542d24736 100644
--- a/gtsam/linear/VectorValues.h
+++ b/gtsam/linear/VectorValues.h
@@ -130,6 +130,9 @@ namespace gtsam {
     /** Create a VectorValues with the same structure as \c other, but filled with zeros. */
     static VectorValues Zero(const VectorValues& other);
 
+    /** Create a VectorValues with the same structure as \c other, but filled with a constant. */
+    static VectorValues One(const VectorValues& other);
+
     /// @}
     /// @name Standard Interface
     /// @{
@@ -302,6 +305,10 @@ namespace gtsam {
      *  structure (checked when NDEBUG is not defined). */
     VectorValues subtract(const VectorValues& c) const;
 
+    /** Element-wise multiplication.  Both VectorValues must have the same structure
+     *  (checked when NDEBUG is not defined). */
+    VectorValues operator*(const VectorValues& c) const;
+
     /** Element-wise scaling by a constant. */
     friend GTSAM_EXPORT VectorValues operator*(const double a, const VectorValues &v);