diff --git a/gtsam/linear/VectorValuesUnordered.cpp b/gtsam/linear/VectorValuesUnordered.cpp index 0d853a178..b000c2914 100644 --- a/gtsam/linear/VectorValuesUnordered.cpp +++ b/gtsam/linear/VectorValuesUnordered.cpp @@ -34,167 +34,184 @@ namespace gtsam { using boost::accumulate; /* ************************************************************************* */ -namespace internal -{ - bool structureCompareOp(const boost::tuple& vv) + namespace internal { - return vv.get<0>().first == vv.get<1>().first && vv.get<0>().second.size() == vv.get<1>().second.size(); + bool structureCompareOp(const boost::tuple& vv) + { + return vv.get<0>().first == vv.get<1>().first && vv.get<0>().second.size() == vv.get<1>().second.size(); + } + + bool hasSameStructure(const VectorValuesUnordered& vv1, const VectorValuesUnordered& vv2) + { + return accumulate(combine(vv1, vv2) | transformed(structureCompareOp), true, std::logical_and()); + } } - bool hasSameStructure(const VectorValuesUnordered& vv1, const VectorValuesUnordered& vv2) + /* ************************************************************************* */ + VectorValuesUnordered::VectorValuesUnordered(const VectorValuesUnordered& first, const VectorValuesUnordered& second) { - return accumulate(combine(vv1, vv2) | transformed(structureCompareOp), true, std::logical_and()); + std::merge(first.begin(), first.end(), second.begin(), second.end(), std::inserter(values_, values_.end())); + if(size() != first.size() + second.size()) + throw std::invalid_argument("Requested to merge two VectorValues that have one or more variables in common."); } -} -/* ************************************************************************* */ -void VectorValuesUnordered::print(const std::string& str, const KeyFormatter& formatter) const { - std::cout << str << ": " << size() << " elements\n"; - BOOST_FOREACH(const value_type& key_value, *this) - std::cout << " " << formatter(key_value.first) << ": \n" << key_value.second.transpose() << "\n"; - std::cout.flush(); -} + /* ************************************************************************* */ + void VectorValuesUnordered::insert(const VectorValuesUnordered& values) + { + 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."); + } -/* ************************************************************************* */ -bool VectorValuesUnordered::equals(const VectorValuesUnordered& x, double tol) const { - if(this->size() != x.size()) - return false; - typedef boost::tuple ValuePair; - BOOST_FOREACH(const ValuePair& values, boost::combine(*this, x)) { - if(values.get<0>().first != values.get<1>().first || - !equal_with_abs_tol(values.get<0>().second, values.get<1>().second, tol)) + /* ************************************************************************* */ + void VectorValuesUnordered::print(const std::string& str, const KeyFormatter& formatter) const { + std::cout << str << ": " << size() << " elements\n"; + BOOST_FOREACH(const value_type& key_value, *this) + std::cout << " " << formatter(key_value.first) << ": \n" << key_value.second.transpose() << "\n"; + std::cout.flush(); + } + + /* ************************************************************************* */ + bool VectorValuesUnordered::equals(const VectorValuesUnordered& x, double tol) const { + if(this->size() != x.size()) return false; - } - return true; -} - -/* ************************************************************************* */ -const Vector VectorValuesUnordered::asVector() const -{ - // Count dimensions - DenseIndex totalDim = 0; - BOOST_FOREACH(const value_type& v, *this) - totalDim += v.second.size(); - - // Copy vectors - Vector result; - DenseIndex pos = 0; - BOOST_FOREACH(const Vector& v, *this | map_values) { - result.segment(pos, v.size()) = v; - pos += v.size(); + typedef boost::tuple ValuePair; + BOOST_FOREACH(const ValuePair& values, boost::combine(*this, x)) { + if(values.get<0>().first != values.get<1>().first || + !equal_with_abs_tol(values.get<0>().second, values.get<1>().second, tol)) + return false; + } + return true; } - return result; -} + /* ************************************************************************* */ + const Vector VectorValuesUnordered::asVector() const + { + // Count dimensions + DenseIndex totalDim = 0; + BOOST_FOREACH(const value_type& v, *this) + totalDim += v.second.size(); -/* ************************************************************************* */ -const Vector VectorValuesUnordered::vector(const std::vector& keys) const -{ - // Count dimensions and collect pointers to avoid double lookups - DenseIndex totalDim = 0; - std::vector items(keys.size()); - for(size_t i = 0; i < keys.size(); ++i) { - items[i] = &at(i); - totalDim += items[i]->size(); + // Copy vectors + Vector result; + DenseIndex pos = 0; + BOOST_FOREACH(const Vector& v, *this | map_values) { + result.segment(pos, v.size()) = v; + pos += v.size(); + } + + return result; } - // Copy vectors - Vector result(totalDim); - DenseIndex pos = 0; - BOOST_FOREACH(const Vector *v, items) { - result.segment(pos, v->size()) = *v; - pos += v->size(); + /* ************************************************************************* */ + const Vector VectorValuesUnordered::vector(const std::vector& keys) const + { + // Count dimensions and collect pointers to avoid double lookups + DenseIndex totalDim = 0; + std::vector items(keys.size()); + for(size_t i = 0; i < keys.size(); ++i) { + items[i] = &at(i); + totalDim += items[i]->size(); + } + + // Copy vectors + Vector result(totalDim); + DenseIndex pos = 0; + BOOST_FOREACH(const Vector *v, items) { + result.segment(pos, v->size()) = *v; + pos += v->size(); + } + + return result; } - return result; -} - -/* ************************************************************************* */ -void VectorValuesUnordered::swap(VectorValuesUnordered& other) { - this->values_.swap(other.values_); -} - -/* ************************************************************************* */ -double VectorValuesUnordered::dot(const VectorValuesUnordered& v) const -{ - if(this->size() != v.size()) - throw invalid_argument("VectorValues::dot called with a VectorValues of different structure"); - double result = 0.0; - typedef boost::tuple ValuePair; - 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")); - assert_throw(values.get<0>().second.size() == values.get<1>().second.size(), - std::invalid_argument("VectorValues::dot called with a VectorValues of different structure")); - result += values.get<0>().second.dot(values.get<1>().second); + /* ************************************************************************* */ + void VectorValuesUnordered::swap(VectorValuesUnordered& other) { + this->values_.swap(other.values_); } - return result; -} -/* ************************************************************************* */ -double VectorValuesUnordered::norm() const { - return std::sqrt(this->squaredNorm()); -} + /* ************************************************************************* */ + double VectorValuesUnordered::dot(const VectorValuesUnordered& v) const + { + if(this->size() != v.size()) + throw invalid_argument("VectorValues::dot called with a VectorValues of different structure"); + double result = 0.0; + typedef boost::tuple ValuePair; + 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")); + assert_throw(values.get<0>().second.size() == values.get<1>().second.size(), + std::invalid_argument("VectorValues::dot called with a VectorValues of different structure")); + result += values.get<0>().second.dot(values.get<1>().second); + } + return result; + } -/* ************************************************************************* */ -double VectorValuesUnordered::squaredNorm() const { - double sumSquares = 0.0; - using boost::adaptors::map_values; - BOOST_FOREACH(const Vector& v, *this | map_values) - sumSquares += v.squaredNorm(); - return sumSquares; -} + /* ************************************************************************* */ + double VectorValuesUnordered::norm() const { + return std::sqrt(this->squaredNorm()); + } -/* ************************************************************************* */ -VectorValuesUnordered VectorValuesUnordered::operator+(const VectorValuesUnordered& c) const -{ - if(this->size() != c.size()) - throw invalid_argument("VectorValues::operator+ called with different vector sizes"); - assert_throw(internal::hasSameStructure(*this, c), - invalid_argument("VectorValues::operator+ called with different vector sizes")); + /* ************************************************************************* */ + double VectorValuesUnordered::squaredNorm() const { + double sumSquares = 0.0; + using boost::adaptors::map_values; + BOOST_FOREACH(const Vector& v, *this | map_values) + sumSquares += v.squaredNorm(); + return sumSquares; + } - VectorValuesUnordered 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)); + /* ************************************************************************* */ + VectorValuesUnordered VectorValuesUnordered::operator+(const VectorValuesUnordered& c) const + { + if(this->size() != c.size()) + throw invalid_argument("VectorValues::operator+ called with different vector sizes"); + assert_throw(internal::hasSameStructure(*this, c), + invalid_argument("VectorValues::operator+ called with different vector sizes")); - return result; -} + VectorValuesUnordered 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)); -/* ************************************************************************* */ -VectorValuesUnordered VectorValuesUnordered::operator-(const VectorValuesUnordered& c) const -{ - if(this->size() != c.size()) - throw invalid_argument("VectorValues::operator- called with different vector sizes"); - assert_throw(internal::hasSameStructure(*this, c), - invalid_argument("VectorValues::operator- called with different vector sizes")); + return result; + } - VectorValuesUnordered 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)); + /* ************************************************************************* */ + VectorValuesUnordered VectorValuesUnordered::operator-(const VectorValuesUnordered& c) const + { + if(this->size() != c.size()) + throw invalid_argument("VectorValues::operator- called with different vector sizes"); + assert_throw(internal::hasSameStructure(*this, c), + invalid_argument("VectorValues::operator- called with different vector sizes")); - return result; -} + VectorValuesUnordered 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)); -/* ************************************************************************* */ -VectorValuesUnordered& VectorValuesUnordered::operator+=(const VectorValuesUnordered& c) -{ - if(this->size() != c.size()) - throw invalid_argument("VectorValues::operator+= called with different vector sizes"); - assert_throw(internal::hasSameStructure(*this, c), - invalid_argument("VectorValues::operator+= called with different vector sizes")); + return result; + } - iterator j1 = begin(); - const_iterator j2 = begin(); - // The result.end() hint here should result in constant-time inserts - for(; j1 != end(); ++j1, ++j2) - j1->second += j2->second; + /* ************************************************************************* */ + VectorValuesUnordered& VectorValuesUnordered::operator+=(const VectorValuesUnordered& c) + { + if(this->size() != c.size()) + throw invalid_argument("VectorValues::operator+= called with different vector sizes"); + assert_throw(internal::hasSameStructure(*this, c), + invalid_argument("VectorValues::operator+= called with different vector sizes")); - return *this; -} + iterator j1 = begin(); + const_iterator j2 = begin(); + // The result.end() hint here should result in constant-time inserts + for(; j1 != end(); ++j1, ++j2) + j1->second += j2->second; -/* ************************************************************************* */ + return *this; + } + + /* ************************************************************************* */ } // \namespace gtsam diff --git a/gtsam/linear/VectorValuesUnordered.h b/gtsam/linear/VectorValuesUnordered.h index 52379e13e..6497ce1f9 100644 --- a/gtsam/linear/VectorValuesUnordered.h +++ b/gtsam/linear/VectorValuesUnordered.h @@ -106,6 +106,9 @@ namespace gtsam { */ VectorValuesUnordered() {} + /** Merge two VectorValues into one, this is more efficient than inserting elements one by one. */ + VectorValuesUnordered(const VectorValuesUnordered& first, const VectorValuesUnordered& second); + /// @} /// @name Standard Interface /// @{ @@ -155,6 +158,10 @@ namespace gtsam { "Requested to insert variable '" + DefaultKeyFormatter(j) + "' already in this VectorValues."); } + /** Insert all values from \c values. Throws an invalid_argument exception if any keys to be + * inserted are already used. */ + void insert(const VectorValuesUnordered& values); + iterator begin() { return values_.begin(); } ///< Iterator over variables const_iterator begin() const { return values_.begin(); } ///< Iterator over variables iterator end() { return values_.end(); } ///< Iterator over variables diff --git a/gtsam/linear/tests/testVectorValuesUnordered.cpp b/gtsam/linear/tests/testVectorValuesUnordered.cpp new file mode 100644 index 000000000..a2054f4f0 --- /dev/null +++ b/gtsam/linear/tests/testVectorValuesUnordered.cpp @@ -0,0 +1,104 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testVectorValues.cpp + * @author Richard Roberts + * @date Sep 16, 2010 + */ + +#include + +#include +#include + +#include + +using namespace std; +using namespace boost::assign; +using namespace gtsam; + +/* ************************************************************************* */ +TEST(VectorValues, insert) { + + // insert + VectorValuesUnordered actual; + actual.insert(0, Vector_(1, 1.0)); + actual.insert(1, Vector_(2, 2.0, 3.0)); + actual.insert(5, Vector_(2, 6.0, 7.0)); + actual.insert(2, Vector_(2, 4.0, 5.0)); + + // Check dimensions + LONGS_EQUAL(6, actual.size()); + LONGS_EQUAL(1, actual.dim(0)); + LONGS_EQUAL(2, actual.dim(1)); + LONGS_EQUAL(2, actual.dim(2)); + LONGS_EQUAL(2, actual.dim(5)); + + // Logic + EXPECT(actual.exists(0)); + EXPECT(actual.exists(1)); + EXPECT(actual.exists(2)); + EXPECT(!actual.exists(3)); + EXPECT(!actual.exists(4)); + EXPECT(actual.exists(5)); + EXPECT(!actual.exists(6)); + + // Check values + EXPECT(assert_equal(Vector_(1, 1.0), actual[0])); + EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1])); + EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2])); + EXPECT(assert_equal(Vector_(2, 6.0, 7.0), actual[5])); + EXPECT(assert_equal(Vector_(7, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), actual.asVector())); + + // Check exceptions + CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument); + CHECK_EXCEPTION(actual.dim(3), out_of_range); +} + +/* ************************************************************************* */ +TEST(VectorValues, combine) { + VectorValuesUnordered expected; + expected.insert(0, Vector_(1, 1.0)); + expected.insert(1, Vector_(2, 2.0, 3.0)); + expected.insert(5, Vector_(2, 6.0, 7.0)); + expected.insert(2, Vector_(2, 4.0, 5.0)); + + VectorValuesUnordered first; + first.insert(0, Vector_(1, 1.0)); + first.insert(1, Vector_(2, 2.0, 3.0)); + + VectorValuesUnordered second; + second.insert(5, Vector_(2, 6.0, 7.0)); + second.insert(2, Vector_(2, 4.0, 5.0)); + + VectorValuesUnordered actual(first, second); + + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST(VectorValues, subvector) { + VectorValuesUnordered init; + init.insert(0, Vector_(1, 1.0)); + init.insert(1, Vector_(2, 2.0, 3.0)); + init.insert(2, Vector_(2, 4.0, 5.0)); + init.insert(3, Vector_(2, 6.0, 7.0)); + + std::vector keys; + keys += 0, 2, 3; + Vector expSubVector = Vector_(5, 1.0, 4.0, 5.0, 6.0, 7.0); + EXPECT(assert_equal(expSubVector, init.vector(keys))); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */