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

103
gtsam/linear/VectorValuesUnordered.cpp
View File

@ -34,8 +34,8 @@ namespace gtsam {
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();
@ -45,18 +45,35 @@ namespace internal
{
return accumulate(combine(vv1, vv2) | transformed(structureCompareOp), true, std::logical_and<bool>());
}
}
}
/* ************************************************************************* */
void VectorValuesUnordered::print(const std::string& str, const KeyFormatter& formatter) const {
/* ************************************************************************* */
VectorValuesUnordered::VectorValuesUnordered(const VectorValuesUnordered& first, const VectorValuesUnordered& second)
{
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::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.");
}
/* ************************************************************************* */
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 {
/* ************************************************************************* */
bool VectorValuesUnordered::equals(const VectorValuesUnordered& x, double tol) const {
if(this->size() != x.size())
return false;
typedef boost::tuple<value_type, value_type> ValuePair;
@ -66,11 +83,11 @@ bool VectorValuesUnordered::equals(const VectorValuesUnordered& x, double tol) c
return false;
}
return true;
}
}
/* ************************************************************************* */
const Vector VectorValuesUnordered::asVector() const
{
/* ************************************************************************* */
const Vector VectorValuesUnordered::asVector() const
{
// Count dimensions
DenseIndex totalDim = 0;
BOOST_FOREACH(const value_type& v, *this)
@ -85,11 +102,11 @@ const Vector VectorValuesUnordered::asVector() const
}
return result;
}
}
/* ************************************************************************* */
const Vector VectorValuesUnordered::vector(const std::vector<Key>& keys) const
{
/* ************************************************************************* */
const Vector VectorValuesUnordered::vector(const std::vector<Key>& keys) const
{
// Count dimensions and collect pointers to avoid double lookups
DenseIndex totalDim = 0;
std::vector<const Vector*> items(keys.size());
@ -107,16 +124,16 @@ const Vector VectorValuesUnordered::vector(const std::vector<Key>& keys) const
}
return result;
}
}
/* ************************************************************************* */
void VectorValuesUnordered::swap(VectorValuesUnordered& other) {
/* ************************************************************************* */
void VectorValuesUnordered::swap(VectorValuesUnordered& other) {
this->values_.swap(other.values_);
}
}
/* ************************************************************************* */
double VectorValuesUnordered::dot(const VectorValuesUnordered& v) const
{
/* ************************************************************************* */
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;
@ -130,25 +147,25 @@ double VectorValuesUnordered::dot(const VectorValuesUnordered& v) const
result += values.get<0>().second.dot(values.get<1>().second);
}
return result;
}
}
/* ************************************************************************* */
double VectorValuesUnordered::norm() const {
/* ************************************************************************* */
double VectorValuesUnordered::norm() const {
return std::sqrt(this->squaredNorm());
}
}
/* ************************************************************************* */
double VectorValuesUnordered::squaredNorm() const {
/* ************************************************************************* */
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 VectorValuesUnordered::operator+(const VectorValuesUnordered& c) const
{
/* ************************************************************************* */
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),
@ -160,11 +177,11 @@ VectorValuesUnordered VectorValuesUnordered::operator+(const VectorValuesUnorder
result.values_.insert(result.end(), make_pair(j1->first, j1->second + j2->second));
return result;
}
}
/* ************************************************************************* */
VectorValuesUnordered VectorValuesUnordered::operator-(const VectorValuesUnordered& c) const
{
/* ************************************************************************* */
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),
@ -176,11 +193,11 @@ VectorValuesUnordered VectorValuesUnordered::operator-(const VectorValuesUnorder
result.values_.insert(result.end(), make_pair(j1->first, j1->second - j2->second));
return result;
}
}
/* ************************************************************************* */
VectorValuesUnordered& VectorValuesUnordered::operator+=(const VectorValuesUnordered& c)
{
/* ************************************************************************* */
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),
@ -193,8 +210,8 @@ VectorValuesUnordered& VectorValuesUnordered::operator+=(const VectorValuesUnord
j1->second += j2->second;
return *this;
}
}
/* ************************************************************************* */
/* ************************************************************************* */
} // \namespace gtsam

7
gtsam/linear/VectorValuesUnordered.h
View File

@ -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

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); }
/* ************************************************************************* */