12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Working on unordered VectorValues.

release/4.3a0
Richard Roberts 2013-07-05 20:45:55 +00:00
parent 4d6647ba01
commit 748406d73f
3 changed files with 262 additions and 134 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

285
gtsam/linear/VectorValuesUnordered.cpp
View File

@ -34,167 +34,184 @@ namespace gtsam {
using boost::accumulate; using boost::accumulate;
/* ************************************************************************* */ /* ************************************************************************* */
namespace internal namespace internal
{
bool structureCompareOp(const boost::tuple<VectorValuesUnordered::value_type, VectorValuesUnordered::value_type>& vv)
{ {
return vv.get<0>().first == vv.get<1>().first && vv.get<0>().second.size() == vv.get<1>().second.size(); bool structureCompareOp(const boost::tuple<VectorValuesUnordered::value_type, VectorValuesUnordered::value_type>& 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>());
}
} }
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<bool>()); 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 { void VectorValuesUnordered::insert(const VectorValuesUnordered& values)
std::cout << str << ": " << size() << " elements\n"; {
BOOST_FOREACH(const value_type& key_value, *this) size_t originalSize = size();
std::cout << " " << formatter(key_value.first) << ": \n" << key_value.second.transpose() << "\n"; values_.insert(values.begin(), values.end());
std::cout.flush(); 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 { void VectorValuesUnordered::print(const std::string& str, const KeyFormatter& formatter) const {
if(this->size() != x.size()) std::cout << str << ": " << size() << " elements\n";
return false; BOOST_FOREACH(const value_type& key_value, *this)
typedef boost::tuple<value_type, value_type> ValuePair; std::cout << " " << formatter(key_value.first) << ": \n" << key_value.second.transpose() << "\n";
BOOST_FOREACH(const ValuePair& values, boost::combine(*this, x)) { std::cout.flush();
if(values.get<0>().first != values.get<1>().first || }
!equal_with_abs_tol(values.get<0>().second, values.get<1>().second, tol))
/* ************************************************************************* */
bool VectorValuesUnordered::equals(const VectorValuesUnordered& x, double tol) const {
if(this->size() != x.size())
return false; return false;
} typedef boost::tuple<value_type, value_type> ValuePair;
return true; 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;
const Vector VectorValuesUnordered::asVector() const }
{ return true;
// 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();
} }
return result; /* ************************************************************************* */
} const Vector VectorValuesUnordered::asVector() const
{
// Count dimensions
DenseIndex totalDim = 0;
BOOST_FOREACH(const value_type& v, *this)
totalDim += v.second.size();
/* ************************************************************************* */ // Copy vectors
const Vector VectorValuesUnordered::vector(const std::vector<Key>& keys) const Vector result;
{ DenseIndex pos = 0;
// Count dimensions and collect pointers to avoid double lookups BOOST_FOREACH(const Vector& v, *this | map_values) {
DenseIndex totalDim = 0; result.segment(pos, v.size()) = v;
std::vector<const Vector*> items(keys.size()); pos += v.size();
for(size_t i = 0; i < keys.size(); ++i) { }
items[i] = &at(i);
totalDim += items[i]->size(); return result;
} }
// Copy vectors /* ************************************************************************* */
Vector result(totalDim); const Vector VectorValuesUnordered::vector(const std::vector<Key>& keys) const
DenseIndex pos = 0; {
BOOST_FOREACH(const Vector *v, items) { // Count dimensions and collect pointers to avoid double lookups
result.segment(pos, v->size()) = *v; DenseIndex totalDim = 0;
pos += v->size(); std::vector<const 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_);
/* ************************************************************************* */
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<value_type, value_type> 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::norm() const { double VectorValuesUnordered::dot(const VectorValuesUnordered& v) const
return std::sqrt(this->squaredNorm()); {
} if(this->size() != v.size())
throw invalid_argument("VectorValues::dot called with a VectorValues of different structure");
double result = 0.0;
typedef boost::tuple<value_type, value_type> 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 VectorValuesUnordered::norm() const {
double sumSquares = 0.0; return std::sqrt(this->squaredNorm());
using boost::adaptors::map_values; }
BOOST_FOREACH(const Vector& v, *this | map_values)
sumSquares += v.squaredNorm();
return sumSquares;
}
/* ************************************************************************* */ /* ************************************************************************* */
VectorValuesUnordered VectorValuesUnordered::operator+(const VectorValuesUnordered& c) const double VectorValuesUnordered::squaredNorm() const {
{ double sumSquares = 0.0;
if(this->size() != c.size()) using boost::adaptors::map_values;
throw invalid_argument("VectorValues::operator+ called with different vector sizes"); BOOST_FOREACH(const Vector& v, *this | map_values)
assert_throw(internal::hasSameStructure(*this, c), sumSquares += v.squaredNorm();
invalid_argument("VectorValues::operator+ called with different vector sizes")); return sumSquares;
}
VectorValuesUnordered result; /* ************************************************************************* */
// The result.end() hint here should result in constant-time inserts VectorValuesUnordered VectorValuesUnordered::operator+(const VectorValuesUnordered& c) const
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)); 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));
/* ************************************************************************* */ return result;
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"));
VectorValuesUnordered result; /* ************************************************************************* */
// The result.end() hint here should result in constant-time inserts VectorValuesUnordered VectorValuesUnordered::operator-(const VectorValuesUnordered& c) const
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)); 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));
/* ************************************************************************* */ return result;
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"));
iterator j1 = begin(); /* ************************************************************************* */
const_iterator j2 = begin(); VectorValuesUnordered& VectorValuesUnordered::operator+=(const VectorValuesUnordered& c)
// The result.end() hint here should result in constant-time inserts {
for(; j1 != end(); ++j1, ++j2) if(this->size() != c.size())
j1->second += j2->second; 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 } // \namespace gtsam

7
gtsam/linear/VectorValuesUnordered.h
View File

@ -106,6 +106,9 @@ namespace gtsam {
*/ */
VectorValuesUnordered() {} 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 /// @name Standard Interface
/// @{ /// @{
@ -155,6 +158,10 @@ namespace gtsam {
"Requested to insert variable '" + DefaultKeyFormatter(j) + "' already in this VectorValues."); "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 iterator begin() { return values_.begin(); } ///< Iterator over variables
const_iterator begin() const { return values_.begin(); } ///< Iterator over variables const_iterator begin() const { return values_.begin(); } ///< Iterator over variables
iterator end() { return values_.end(); } ///< Iterator over variables iterator end() { return values_.end(); } ///< Iterator over variables

104
gtsam/linear/tests/testVectorValuesUnordered.cpp Normal file
View File

@ -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 <boost/assign/std/vector.hpp>
#include <gtsam/base/Testable.h>
#include <gtsam/linear/VectorValuesUnordered.h>
#include <CppUnitLite/TestHarness.h>
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<Key> 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); }
/* ************************************************************************* */