297 lines
8.5 KiB
C++
297 lines
8.5 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* 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 <gtsam/base/Testable.h>
|
|
#include <gtsam/linear/VectorValues.h>
|
|
#include <gtsam/inference/Symbol.h>
|
|
|
|
#include <CppUnitLite/TestHarness.h>
|
|
|
|
#include <boost/assign/std/vector.hpp>
|
|
#include <boost/assign/list_of.hpp>
|
|
#include <boost/range/adaptor/map.hpp>
|
|
|
|
#include <sstream>
|
|
|
|
using namespace std;
|
|
using namespace boost::assign;
|
|
using boost::adaptors::map_keys;
|
|
using namespace gtsam;
|
|
|
|
/* ************************************************************************* */
|
|
TEST(VectorValues, basics)
|
|
{
|
|
// Tests insert(), size(), dim(), exists(), vector()
|
|
|
|
// insert
|
|
VectorValues actual;
|
|
actual.insert(0, (Vector(1) << 1).finished());
|
|
actual.insert(1, Vector2(2, 3));
|
|
actual.insert(5, Vector2(6, 7));
|
|
actual.insert(2, Vector2(4, 5));
|
|
|
|
// Check dimensions
|
|
LONGS_EQUAL(4, 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).finished(), actual[0]));
|
|
EXPECT(assert_equal(Vector2(2, 3), actual[1]));
|
|
EXPECT(assert_equal(Vector2(4, 5), actual[2]));
|
|
EXPECT(assert_equal(Vector2(6, 7), actual[5]));
|
|
KeyVector keys {0, 1, 2, 5};
|
|
EXPECT(assert_equal((Vector(7) << 1, 2, 3, 4, 5, 6, 7).finished(), actual.vector(keys)));
|
|
|
|
// Check exceptions
|
|
CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
|
|
CHECK_EXCEPTION(actual.dim(3), out_of_range);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
|
|
static const VectorValues kExample = {{99, Vector2(2, 3)}};
|
|
|
|
// Check insert
|
|
TEST(VectorValues, Insert) {
|
|
VectorValues actual;
|
|
EXPECT(assert_equal(kExample, actual.insert(kExample)));
|
|
}
|
|
|
|
// Check update.
|
|
TEST(VectorValues, Update) {
|
|
VectorValues actual(kExample);
|
|
EXPECT(assert_equal(kExample, actual.update(kExample)));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST(VectorValues, combine)
|
|
{
|
|
VectorValues expected;
|
|
expected.insert(0, (Vector(1) << 1).finished());
|
|
expected.insert(1, Vector2(2, 3));
|
|
expected.insert(5, Vector2(6, 7));
|
|
expected.insert(2, Vector2(4, 5));
|
|
|
|
VectorValues first;
|
|
first.insert(0, (Vector(1) << 1).finished());
|
|
first.insert(1, Vector2(2, 3));
|
|
|
|
VectorValues second;
|
|
second.insert(5, Vector2(6, 7));
|
|
second.insert(2, Vector2(4, 5));
|
|
|
|
VectorValues actual(first, second);
|
|
|
|
EXPECT(assert_equal(expected, actual));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST(VectorValues, subvector)
|
|
{
|
|
VectorValues init;
|
|
init.insert(10, (Vector(1) << 1).finished());
|
|
init.insert(11, Vector2(2, 3));
|
|
init.insert(12, Vector2(4, 5));
|
|
init.insert(13, Vector2(6, 7));
|
|
|
|
KeyVector keys {10, 12, 13};
|
|
Vector expSubVector = (Vector(5) << 1, 4, 5, 6, 7).finished();
|
|
EXPECT(assert_equal(expSubVector, init.vector(keys)));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST(VectorValues, LinearAlgebra)
|
|
{
|
|
VectorValues test1;
|
|
test1.insert(0, (Vector(1) << 1).finished());
|
|
test1.insert(1, Vector2(2, 3));
|
|
test1.insert(5, Vector2(6, 7));
|
|
test1.insert(2, Vector2(4, 5));
|
|
|
|
VectorValues test2;
|
|
test2.insert(0, (Vector(1) << 6).finished());
|
|
test2.insert(1, Vector2(1, 6));
|
|
test2.insert(5, Vector2(4, 3));
|
|
test2.insert(2, Vector2(1, 8));
|
|
|
|
// Dot product
|
|
double dotExpected = test1.vector().dot(test2.vector());
|
|
double dotActual = test1.dot(test2);
|
|
DOUBLES_EQUAL(dotExpected, dotActual, 1e-10);
|
|
|
|
// Norm
|
|
double normExpected = test1.vector().norm();
|
|
double normActual = test1.norm();
|
|
DOUBLES_EQUAL(normExpected, normActual, 1e-10);
|
|
|
|
// Squared norm
|
|
double sqNormExpected = test1.vector().norm();
|
|
double sqNormActual = test1.norm();
|
|
DOUBLES_EQUAL(sqNormExpected, sqNormActual, 1e-10);
|
|
|
|
// Addition
|
|
Vector sumExpected = test1.vector() + test2.vector();
|
|
VectorValues sumActual = test1 + test2;
|
|
EXPECT(sumActual.hasSameStructure(test1));
|
|
EXPECT(assert_equal(sumExpected, sumActual.vector()));
|
|
Vector sum2Expected = sumActual.vector() + test1.vector();
|
|
VectorValues sum2Actual = sumActual;
|
|
sum2Actual += test1;
|
|
EXPECT(assert_equal(sum2Expected, sum2Actual.vector()));
|
|
|
|
// Add to empty
|
|
VectorValues sumActual3;
|
|
sumActual3.addInPlace_(test1);
|
|
sumActual3.addInPlace_(test2);
|
|
EXPECT(assert_equal(sumExpected, sumActual3.vector()));
|
|
|
|
// Subtraction
|
|
Vector difExpected = test1.vector() - test2.vector();
|
|
VectorValues difActual = test1 - test2;
|
|
EXPECT(difActual.hasSameStructure(test1));
|
|
EXPECT(assert_equal(difExpected, difActual.vector()));
|
|
|
|
// Scaling
|
|
Vector scalExpected = test1.vector() * 5.0;
|
|
VectorValues scalActual = test1;
|
|
scalActual *= 5.0;
|
|
EXPECT(assert_equal(scalExpected, scalActual.vector()));
|
|
VectorValues scal2Actual = 5.0 * test1;
|
|
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, (Vector(1) << 1).finished());
|
|
expected.insert(1, Vector2(2, 3));
|
|
expected.insert(2, Vector2(4, 5));
|
|
expected.insert(5, Vector2(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));
|
|
|
|
Scatter scatter;
|
|
scatter.emplace_back(0,1);
|
|
scatter.emplace_back(1,2);
|
|
scatter.emplace_back(2,2);
|
|
scatter.emplace_back(5,2);
|
|
VectorValues actual2(x,scatter);
|
|
EXPECT(assert_equal(expected, actual2));
|
|
|
|
// Test other direction, note vector() is not guaranteed to give right result
|
|
KeyVector keys {0, 1, 2, 5};
|
|
EXPECT(assert_equal(x, actual.vector(keys)));
|
|
|
|
// Test version with dims argument
|
|
EXPECT(assert_equal(x, actual.vector(dims)));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST(VectorValues, vector_sub)
|
|
{
|
|
VectorValues vv;
|
|
vv.insert(0, (Vector(1) << 1).finished());
|
|
vv.insert(1, Vector2(2, 3));
|
|
vv.insert(2, Vector2(4, 5));
|
|
vv.insert(5, Vector2(6, 7));
|
|
vv.insert(7, Vector2(8, 9));
|
|
|
|
std::map<Key,size_t> dims;
|
|
dims.insert(make_pair(0,1));
|
|
dims.insert(make_pair(5,2));
|
|
|
|
Vector expected(3);
|
|
expected << 1, 6, 7;
|
|
|
|
// Test FastVector version
|
|
KeyVector keys {0, 5};
|
|
EXPECT(assert_equal(expected, vv.vector(keys)));
|
|
|
|
// Test version with dims argument
|
|
EXPECT(assert_equal(expected, vv.vector(dims)));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST(VectorValues, print)
|
|
{
|
|
VectorValues vv;
|
|
vv.insert(0, (Vector(1) << 1).finished());
|
|
vv.insert(1, Vector2(2, 3));
|
|
vv.insert(2, Vector2(4, 5));
|
|
vv.insert(5, Vector2(6, 7));
|
|
vv.insert(7, Vector2(8, 9));
|
|
|
|
string expected =
|
|
" 0: 1\n 1: 2 3\n 2: 4 5\n 5: 6 7\n 7: 8 9\n";
|
|
stringstream actual;
|
|
actual << vv;
|
|
EXPECT(expected == actual.str());
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
// Check html representation.
|
|
TEST(VectorValues, html) {
|
|
VectorValues vv;
|
|
using symbol_shorthand::X;
|
|
vv.insert(X(1), Vector2(2, 3.1));
|
|
vv.insert(X(2), Vector2(4, 5.2));
|
|
vv.insert(X(5), Vector2(6, 7.3));
|
|
vv.insert(X(7), Vector2(8, 9.4));
|
|
string expected =
|
|
"<div>\n"
|
|
"<table class='VectorValues'>\n"
|
|
" <thead>\n"
|
|
" <tr><th>Variable</th><th>value</th></tr>\n"
|
|
" </thead>\n"
|
|
" <tbody>\n"
|
|
" <tr><th>x1</th><td> 2 3.1</td></tr>\n"
|
|
" <tr><th>x2</th><td> 4 5.2</td></tr>\n"
|
|
" <tr><th>x5</th><td> 6 7.3</td></tr>\n"
|
|
" <tr><th>x7</th><td> 8 9.4</td></tr>\n"
|
|
" </tbody>\n"
|
|
"</table>\n"
|
|
"</div>";
|
|
string actual = vv.html();
|
|
EXPECT(actual == expected);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
|
/* ************************************************************************* */
|