227 lines
6.7 KiB
C++
227 lines
6.7 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 <CppUnitLite/TestHarness.h>
|
|
|
|
#include <boost/assign/std/vector.hpp>
|
|
#include <boost/assign/list_of.hpp>
|
|
#include <boost/range/adaptor/map.hpp>
|
|
|
|
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));
|
|
actual.insert(1, (Vector(2) << 2, 3));
|
|
actual.insert(5, (Vector(2) << 6, 7));
|
|
actual.insert(2, (Vector(2) << 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), actual[0]));
|
|
EXPECT(assert_equal((Vector(2) << 2, 3), actual[1]));
|
|
EXPECT(assert_equal((Vector(2) << 4, 5), actual[2]));
|
|
EXPECT(assert_equal((Vector(2) << 6, 7), actual[5]));
|
|
FastVector<Key> keys = list_of(0)(1)(2)(5);
|
|
EXPECT(assert_equal((Vector(7) << 1, 2, 3, 4, 5, 6, 7), actual.vector(keys)));
|
|
|
|
// Check exceptions
|
|
CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
|
|
CHECK_EXCEPTION(actual.dim(3), out_of_range);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST(VectorValues, combine)
|
|
{
|
|
VectorValues expected;
|
|
expected.insert(0, (Vector(1) << 1));
|
|
expected.insert(1, (Vector(2) << 2, 3));
|
|
expected.insert(5, (Vector(2) << 6, 7));
|
|
expected.insert(2, (Vector(2) << 4, 5));
|
|
|
|
VectorValues first;
|
|
first.insert(0, (Vector(1) << 1));
|
|
first.insert(1, (Vector(2) << 2, 3));
|
|
|
|
VectorValues second;
|
|
second.insert(5, (Vector(2) << 6, 7));
|
|
second.insert(2, (Vector(2) << 4, 5));
|
|
|
|
VectorValues actual(first, second);
|
|
|
|
EXPECT(assert_equal(expected, actual));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST(VectorValues, subvector)
|
|
{
|
|
VectorValues init;
|
|
init.insert(10, (Vector(1) << 1));
|
|
init.insert(11, (Vector(2) << 2, 3));
|
|
init.insert(12, (Vector(2) << 4, 5));
|
|
init.insert(13, (Vector(2) << 6, 7));
|
|
|
|
std::vector<Key> keys;
|
|
keys += 10, 12, 13;
|
|
Vector expSubVector = (Vector(5) << 1, 4, 5, 6, 7);
|
|
EXPECT(assert_equal(expSubVector, init.vector(keys)));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST(VectorValues, LinearAlgebra)
|
|
{
|
|
VectorValues test1;
|
|
test1.insert(0, (Vector(1) << 1));
|
|
test1.insert(1, (Vector(2) << 2, 3));
|
|
test1.insert(5, (Vector(2) << 6, 7));
|
|
test1.insert(2, (Vector(2) << 4, 5));
|
|
|
|
VectorValues test2;
|
|
test2.insert(0, (Vector(1) << 6));
|
|
test2.insert(1, (Vector(2) << 1, 6));
|
|
test2.insert(5, (Vector(2) << 4, 3));
|
|
test2.insert(2, (Vector(2) << 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));
|
|
expected.insert(1, (Vector(2) << 2, 3));
|
|
expected.insert(2, (Vector(2) << 4, 5));
|
|
expected.insert(5, (Vector(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, note vector() is not guaranteed to give right result
|
|
FastVector<Key> keys = list_of(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));
|
|
vv.insert(1, (Vector(2) << 2, 3));
|
|
vv.insert(2, (Vector(2) << 4, 5));
|
|
vv.insert(5, (Vector(2) << 6, 7));
|
|
vv.insert(7, (Vector(2) << 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
|
|
FastVector<Key> keys = list_of(0)(5);
|
|
EXPECT(assert_equal(expected, vv.vector(keys)));
|
|
|
|
// Test version with dims argument
|
|
EXPECT(assert_equal(expected, vv.vector(dims)));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
|
/* ************************************************************************* */
|