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

Constructor that converts from Vector

release/4.3a0
Pablo Fernandez Alcantarilla 2013-10-25 00:54:05 +00:00
parent 42538c0252
commit d03eafb6c0
3 changed files with 86 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
gtsam/linear/VectorValues.cpp
View File

@ -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<Key>::operator(), std::less<Key>(), 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<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 {
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<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;

4
gtsam/linear/VectorValues.h
View File

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

90
gtsam/linear/tests/testVectorValuesUnordered.cpp
View File

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