From d03eafb6c0b68fe26fbce858a89bc60632237d45 Mon Sep 17 00:00:00 2001 From: Pablo Fernandez Alcantarilla Date: Fri, 25 Oct 2013 00:54:05 +0000 Subject: [PATCH] Constructor that converts from Vector --- gtsam/linear/VectorValues.cpp | 37 +++++--- gtsam/linear/VectorValues.h | 4 + .../tests/testVectorValuesUnordered.cpp | 90 ++++++++++++------- 3 files changed, 86 insertions(+), 45 deletions(-) diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 11967e858..eecf3ce96 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/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()), - boost::bind(&std::less::operator(), std::less(), boost::bind(&KeyValuePair::first, _1), boost::bind(&KeyValuePair::first, _2))); + merge(first.begin(), first.end(), second.begin(), second.end(), inserter(values_, values_.end()), + boost::bind(&less::operator(), less(), 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& dims) { + typedef pair 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 { - std::cout << str << ": " << size() << " elements\n"; + void VectorValues::print(const string& str, const KeyFormatter& formatter) const { + cout << str << ": " << size() << " elements\n"; BOOST_FOREACH(const value_type& key_value, *this) - std::cout << " " << formatter(key_value.first) << ": " << key_value.second.transpose() << "\n"; - std::cout.flush(); + cout << " " << formatter(key_value.first) << ": " << key_value.second.transpose() << "\n"; + 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()); + | transformed(internal::structureCompareOp), true, logical_and()); } /* ************************************************************************* */ @@ -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; diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 192d66acc..d27db427f 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -21,6 +21,7 @@ #include #include #include +#include #include @@ -122,6 +123,9 @@ namespace gtsam { template VectorValues(ITERATOR first, ITERATOR last) : values_(first, last) {} + /** Constructor from Vector. */ + VectorValues(const Vector& c, const std::map& dims); + /** Create a VectorValues with the same structure as \c other, but filled with zeros. */ static VectorValues Zero(const VectorValues& other); diff --git a/gtsam/linear/tests/testVectorValuesUnordered.cpp b/gtsam/linear/tests/testVectorValuesUnordered.cpp index 307b8de32..e446e2ddf 100644 --- a/gtsam/linear/tests/testVectorValuesUnordered.cpp +++ b/gtsam/linear/tests/testVectorValuesUnordered.cpp @@ -15,14 +15,14 @@ * @date Sep 16, 2010 */ -#include -#include - #include #include #include +#include +#include + 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(1, (Vec(2) << 2.0, 3.0)); - actual.insert(5, (Vec(2) << 6.0, 7.0)); - actual.insert(2, (Vec(2) << 4.0, 5.0)); + actual.insert(0, (Vec(1) << 1)); + actual.insert(1, (Vec(2) << 2, 3)); + actual.insert(5, (Vec(2) << 6, 7)); + 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(2) << 2.0, 3.0), actual[1])); - EXPECT(assert_equal((Vec(2) << 4.0, 5.0), actual[2])); - EXPECT(assert_equal((Vec(2) << 6.0, 7.0), actual[5])); + EXPECT(assert_equal((Vec(1) << 1), actual[0])); + EXPECT(assert_equal((Vec(2) << 2, 3), actual[1])); + EXPECT(assert_equal((Vec(2) << 4, 5), actual[2])); + EXPECT(assert_equal((Vec(2) << 6, 7), actual[5])); FastVector 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(1, (Vec(2) << 2.0, 3.0)); - expected.insert(5, (Vec(2) << 6.0, 7.0)); - expected.insert(2, (Vec(2) << 4.0, 5.0)); + expected.insert(0, (Vec(1) << 1)); + expected.insert(1, (Vec(2) << 2, 3)); + expected.insert(5, (Vec(2) << 6, 7)); + expected.insert(2, (Vec(2) << 4, 5)); VectorValues first; - first.insert(0, (Vec(1) << 1.0)); - first.insert(1, (Vec(2) << 2.0, 3.0)); + first.insert(0, (Vec(1) << 1)); + first.insert(1, (Vec(2) << 2, 3)); VectorValues second; - second.insert(5, (Vec(2) << 6.0, 7.0)); - second.insert(2, (Vec(2) << 4.0, 5.0)); + second.insert(5, (Vec(2) << 6, 7)); + 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(11, (Vec(2) << 2.0, 3.0)); - init.insert(12, (Vec(2) << 4.0, 5.0)); - init.insert(13, (Vec(2) << 6.0, 7.0)); + init.insert(10, (Vec(1) << 1)); + init.insert(11, (Vec(2) << 2, 3)); + init.insert(12, (Vec(2) << 4, 5)); + init.insert(13, (Vec(2) << 6, 7)); std::vector 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(1, (Vec(2) << 2.0, 3.0)); - test1.insert(5, (Vec(2) << 6.0, 7.0)); - test1.insert(2, (Vec(2) << 4.0, 5.0)); + test1.insert(0, (Vec(1) << 1)); + test1.insert(1, (Vec(2) << 2, 3)); + test1.insert(5, (Vec(2) << 6, 7)); + test1.insert(2, (Vec(2) << 4, 5)); VectorValues test2; - test2.insert(0, (Vec(1) << 6.0)); - test2.insert(1, (Vec(2) << 1.0, 6.0)); - test2.insert(5, (Vec(2) << 4.0, 3.0)); - test2.insert(2, (Vec(2) << 1.0, 8.0)); + test2.insert(0, (Vec(1) << 6)); + test2.insert(1, (Vec(2) << 1, 6)); + test2.insert(5, (Vec(2) << 4, 3)); + 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 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); } /* ************************************************************************* */