Constructor that converts from Vector

gtsam/linear/VectorValues.cpp

@@ -38,10 +38,23 @@ namespace gtsam {
   VectorValues::VectorValues(const VectorValues& first, const VectorValues& second)
   {
     // Merge using predicate for comparing first of pair
-    std::merge(first.begin(), first.end(), second.begin(), second.end(), std::inserter(values_, values_.end()),
+    merge(first.begin(), first.end(), second.begin(), second.end(), inserter(values_, values_.end()),
-      boost::bind(&std::less<Key>::operator(), std::less<Key>(), boost::bind(&KeyValuePair::first, _1), boost::bind(&KeyValuePair::first, _2)));
+      boost::bind(&less<Key>::operator(), less<Key>(), boost::bind(&KeyValuePair::first, _1), boost::bind(&KeyValuePair::first, _2)));
     if(size() != first.size() + second.size())
-      throw std::invalid_argument("Requested to merge two VectorValues that have one or more variables in common.");
+      throw invalid_argument("Requested to merge two VectorValues that have one or more variables in common.");
+  }
+
+  /* ************************************************************************* */
+  VectorValues::VectorValues(const Vector& x, const map<Key, size_t>& dims) {
+    typedef pair<Key, size_t> Pair;
+    size_t j = 0;
+    BOOST_FOREACH(const Pair& v, dims) {
+      Key key;
+      size_t n;
+      boost::tie(key, n) = v;
+      values_.insert(make_pair(key, sub(x, j, j + n)));
+      j += n;
+    }
   }
 
   /* ************************************************************************* */
@@ -64,7 +77,7 @@ namespace gtsam {
       hint = values_.insert(hint, key_value);
       if(values_.size() > oldSize) {
         values_.unsafe_erase(hint);
-        throw std::out_of_range("Requested to update a VectorValues with another VectorValues that contains keys not present in the first.");
+        throw out_of_range("Requested to update a VectorValues with another VectorValues that contains keys not present in the first.");
       } else {
         hint->second = key_value.second;
       }
@@ -77,7 +90,7 @@ namespace gtsam {
     size_t originalSize = size();
     values_.insert(values.begin(), values.end());
     if(size() != originalSize + values.size())
-      throw std::invalid_argument("Requested to insert a VectorValues into another VectorValues that already contains one or more of its keys.");
+      throw invalid_argument("Requested to insert a VectorValues into another VectorValues that already contains one or more of its keys.");
   }
 
   /* ************************************************************************* */
@@ -88,11 +101,11 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  void VectorValues::print(const std::string& str, const KeyFormatter& formatter) const {
+  void VectorValues::print(const string& str, const KeyFormatter& formatter) const {
-    std::cout << str << ": " << size() << " elements\n";
+    cout << str << ": " << size() << " elements\n";
     BOOST_FOREACH(const value_type& key_value, *this)
-      std::cout << "  " << formatter(key_value.first) << ": " << key_value.second.transpose() << "\n";
+      cout << "  " << formatter(key_value.first) << ": " << key_value.second.transpose() << "\n";
-    std::cout.flush();
+    cout.flush();
   }
 
   /* ************************************************************************* */
@@ -169,7 +182,7 @@ namespace gtsam {
   bool VectorValues::hasSameStructure(const VectorValues other) const
   {
     return accumulate(combine(*this, other)
-      | transformed(internal::structureCompareOp), true, std::logical_and<bool>());
+      | transformed(internal::structureCompareOp), true, logical_and<bool>());
   }
 
   /* ************************************************************************* */
@@ -182,9 +195,9 @@ namespace gtsam {
     using boost::adaptors::map_values;
     BOOST_FOREACH(const ValuePair& values, boost::combine(*this, v)) {
       assert_throw(values.get<0>().first == values.get<1>().first,
-        std::invalid_argument("VectorValues::dot called with a VectorValues of different structure"));
+        invalid_argument("VectorValues::dot called with a VectorValues of different structure"));
       assert_throw(values.get<0>().second.size() == values.get<1>().second.size(),
-        std::invalid_argument("VectorValues::dot called with a VectorValues of different structure"));
+        invalid_argument("VectorValues::dot called with a VectorValues of different structure"));
       result += values.get<0>().second.dot(values.get<1>().second);
     }
     return result;

gtsam/linear/VectorValues.h

@@ -21,6 +21,7 @@
 #include <gtsam/base/ConcurrentMap.h>
 #include <gtsam/base/FastVector.h>
 #include <gtsam/global_includes.h>
+#include <gtsam/inference/Ordering.h>
 
 #include <boost/shared_ptr.hpp>
 
@@ -122,6 +123,9 @@ namespace gtsam {
     template<typename ITERATOR>
     VectorValues(ITERATOR first, ITERATOR last) : values_(first, last) {}
 
+    /** Constructor from Vector. */
+    VectorValues(const Vector& c, const std::map<Key,size_t>& dims);
+
     /** Create a VectorValues with the same structure as \c other, but filled with zeros. */
     static VectorValues Zero(const VectorValues& other);
 

gtsam/linear/tests/testVectorValuesUnordered.cpp

@@ -15,14 +15,14 @@
  * @date    Sep 16, 2010
  */
 
-#include <boost/assign/std/vector.hpp>
-#include <boost/assign/list_of.hpp>
-
 #include <gtsam/base/Testable.h>
 #include <gtsam/linear/VectorValues.h>
 
 #include <CppUnitLite/TestHarness.h>
 
+#include <boost/assign/std/vector.hpp>
+#include <boost/assign/list_of.hpp>
+
 using namespace std;
 using namespace boost::assign;
 using namespace gtsam;
@@ -34,10 +34,10 @@ TEST(VectorValues, basics)
 
   // insert
   VectorValues actual;
-  actual.insert(0, (Vec(1) << 1.0));
+  actual.insert(0, (Vec(1) << 1));
-  actual.insert(1, (Vec(2) << 2.0, 3.0));
+  actual.insert(1, (Vec(2) << 2, 3));
-  actual.insert(5, (Vec(2) << 6.0, 7.0));
+  actual.insert(5, (Vec(2) << 6, 7));
-  actual.insert(2, (Vec(2) << 4.0, 5.0));
+  actual.insert(2, (Vec(2) << 4, 5));
 
   // Check dimensions
   LONGS_EQUAL(4, actual.size());
@@ -56,12 +56,12 @@ TEST(VectorValues, basics)
   EXPECT(!actual.exists(6));
 
   // Check values
-  EXPECT(assert_equal((Vec(1) << 1.0), actual[0]));
+  EXPECT(assert_equal((Vec(1) << 1), actual[0]));
-  EXPECT(assert_equal((Vec(2) << 2.0, 3.0), actual[1]));
+  EXPECT(assert_equal((Vec(2) << 2, 3), actual[1]));
-  EXPECT(assert_equal((Vec(2) << 4.0, 5.0), actual[2]));
+  EXPECT(assert_equal((Vec(2) << 4, 5), actual[2]));
-  EXPECT(assert_equal((Vec(2) << 6.0, 7.0), actual[5]));
+  EXPECT(assert_equal((Vec(2) << 6, 7), actual[5]));
   FastVector<Key> keys = list_of(0)(1)(2)(5);
-  EXPECT(assert_equal((Vec(7) << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), actual.vector(keys)));
+  EXPECT(assert_equal((Vec(7) << 1, 2, 3, 4, 5, 6, 7), actual.vector(keys)));
 
   // Check exceptions
   CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
@@ -72,18 +72,18 @@ TEST(VectorValues, basics)
 TEST(VectorValues, combine)
 {
   VectorValues expected;
-  expected.insert(0, (Vec(1) << 1.0));
+  expected.insert(0, (Vec(1) << 1));
-  expected.insert(1, (Vec(2) << 2.0, 3.0));
+  expected.insert(1, (Vec(2) << 2, 3));
-  expected.insert(5, (Vec(2) << 6.0, 7.0));
+  expected.insert(5, (Vec(2) << 6, 7));
-  expected.insert(2, (Vec(2) << 4.0, 5.0));
+  expected.insert(2, (Vec(2) << 4, 5));
 
   VectorValues first;
-  first.insert(0, (Vec(1) << 1.0));
+  first.insert(0, (Vec(1) << 1));
-  first.insert(1, (Vec(2) << 2.0, 3.0));
+  first.insert(1, (Vec(2) << 2, 3));
 
   VectorValues second;
-  second.insert(5, (Vec(2) << 6.0, 7.0));
+  second.insert(5, (Vec(2) << 6, 7));
-  second.insert(2, (Vec(2) << 4.0, 5.0));
+  second.insert(2, (Vec(2) << 4, 5));
 
   VectorValues actual(first, second);
 
@@ -94,14 +94,14 @@ TEST(VectorValues, combine)
 TEST(VectorValues, subvector)
 {
   VectorValues init;
-  init.insert(10, (Vec(1) << 1.0));
+  init.insert(10, (Vec(1) << 1));
-  init.insert(11, (Vec(2) << 2.0, 3.0));
+  init.insert(11, (Vec(2) << 2, 3));
-  init.insert(12, (Vec(2) << 4.0, 5.0));
+  init.insert(12, (Vec(2) << 4, 5));
-  init.insert(13, (Vec(2) << 6.0, 7.0));
+  init.insert(13, (Vec(2) << 6, 7));
 
   std::vector<Key> keys;
   keys += 10, 12, 13;
-  Vector expSubVector = (Vec(5) << 1.0, 4.0, 5.0, 6.0, 7.0);
+  Vector expSubVector = (Vec(5) << 1, 4, 5, 6, 7);
   EXPECT(assert_equal(expSubVector, init.vector(keys)));
 }
 
@@ -109,16 +109,16 @@ TEST(VectorValues, subvector)
 TEST(VectorValues, LinearAlgebra)
 {
   VectorValues test1;
-  test1.insert(0, (Vec(1) << 1.0));
+  test1.insert(0, (Vec(1) << 1));
-  test1.insert(1, (Vec(2) << 2.0, 3.0));
+  test1.insert(1, (Vec(2) << 2, 3));
-  test1.insert(5, (Vec(2) << 6.0, 7.0));
+  test1.insert(5, (Vec(2) << 6, 7));
-  test1.insert(2, (Vec(2) << 4.0, 5.0));
+  test1.insert(2, (Vec(2) << 4, 5));
 
   VectorValues test2;
-  test2.insert(0, (Vec(1) << 6.0));
+  test2.insert(0, (Vec(1) << 6));
-  test2.insert(1, (Vec(2) << 1.0, 6.0));
+  test2.insert(1, (Vec(2) << 1, 6));
-  test2.insert(5, (Vec(2) << 4.0, 3.0));
+  test2.insert(5, (Vec(2) << 4, 3));
-  test2.insert(2, (Vec(2) << 1.0, 8.0));
+  test2.insert(2, (Vec(2) << 1, 8));
 
   // Dot product
   double dotExpected = test1.vector().dot(test2.vector());
@@ -160,6 +160,30 @@ TEST(VectorValues, LinearAlgebra)
   EXPECT(assert_equal(scalExpected, scal2Actual.vector()));
 }
 
+/* ************************************************************************* */
+TEST(VectorValues, convert)
+{
+  Vector x(7);
+  x << 1, 2, 3, 4, 5, 6, 7;
+
+  VectorValues expected;
+  expected.insert(0, (Vec(1) << 1));
+  expected.insert(1, (Vec(2) << 2, 3));
+  expected.insert(2, (Vec(2) << 4, 5));
+  expected.insert(5, (Vec(2) << 6, 7));
+
+  std::map<Key,size_t> dims;
+  dims.insert(make_pair(0,1));
+  dims.insert(make_pair(1,2));
+  dims.insert(make_pair(2,2));
+  dims.insert(make_pair(5,2));
+  VectorValues actual(x,dims);
+  EXPECT(assert_equal(expected, actual));
+
+  // Test other direction
+  EXPECT(assert_equal(x, actual.vector()));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */