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

Fix Vector_() to Vec() in gtsam/linear

release/4.3a0
Jing Dong 2013-10-21 05:12:48 +00:00
parent 615c223f81
commit 40a7153272
12 changed files with 197 additions and 197 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/linear/tests/testErrors.cpp
View File

@ -29,12 +29,12 @@ using namespace gtsam;
TEST( Errors, arithmetic )
{
Errors e;
e += Vector_(2,1.0,2.0), Vector_(3,3.0,4.0,5.0);
e += (Vec(2) << 1.0,2.0), (Vec(3) << 3.0,4.0,5.0);
DOUBLES_EQUAL(1+4+9+16+25,dot(e,e),1e-9);
axpy(2.0,e,e);
Errors expected;
expected += Vector_(2,3.0,6.0), Vector_(3,9.0,12.0,15.0);
expected += (Vec(2) << 3.0,6.0), (Vec(3) << 9.0,12.0,15.0);
CHECK(assert_equal(expected,e));
}

42
gtsam/linear/tests/testGaussianBayesNetUnordered.cpp
View File

@ -38,8 +38,8 @@ using namespace gtsam;
static const Key _x_=0, _y_=1, _z_=2;
static GaussianBayesNet smallBayesNet = list_of
(GaussianConditional(_x_, Vector_(1, 9.0), Matrix_(1, 1, 1.0), _y_, Matrix_(1, 1, 1.0)))
(GaussianConditional(_y_, Vector_(1, 5.0), Matrix_(1, 1, 1.0)));
(GaussianConditional(_x_, (Vec(1) << 9.0), Matrix_(1, 1, 1.0), _y_, Matrix_(1, 1, 1.0)))
(GaussianConditional(_y_, (Vec(1) << 5.0), Matrix_(1, 1, 1.0)));
/* ************************************************************************* */
TEST( GaussianBayesNet, matrix )
@ -51,7 +51,7 @@ TEST( GaussianBayesNet, matrix )
1.0, 1.0,
0.0, 1.0
);
Vector d1 = Vector_(2, 9.0, 5.0);
Vector d1 = (Vec(2) << 9.0, 5.0);
EXPECT(assert_equal(R,R1));
EXPECT(assert_equal(d,d1));
@ -63,8 +63,8 @@ TEST( GaussianBayesNet, optimize )
VectorValues actual = smallBayesNet.optimize();
VectorValues expected = map_list_of
(_x_, Vector_(1, 4.0))
(_y_, Vector_(1, 5.0));
(_x_, (Vec(1) << 4.0))
(_y_, (Vec(1) << 5.0));
EXPECT(assert_equal(expected,actual));
}
@ -78,13 +78,13 @@ TEST( GaussianBayesNet, optimize3 )
// NOTE: we are supplying a new RHS here
VectorValues expected = map_list_of
(_x_, Vector_(1, -1.0))
(_y_, Vector_(1, 5.0));
(_x_, (Vec(1) << -1.0))
(_y_, (Vec(1) << 5.0));
// Test different RHS version
VectorValues gx = map_list_of
(_x_, Vector_(1, 4.0))
(_y_, Vector_(1, 5.0));
(_x_, (Vec(1) << 4.0))
(_y_, (Vec(1) << 5.0));
VectorValues actual = smallBayesNet.backSubstitute(gx);
EXPECT(assert_equal(expected, actual));
}
@ -97,11 +97,11 @@ TEST( GaussianBayesNet, backSubstituteTranspose )
// 5 1 1 3
VectorValues
x = map_list_of
(_x_, Vector_(1, 2.0))
(_y_, Vector_(1, 5.0)),
(_x_, (Vec(1) << 2.0))
(_y_, (Vec(1) << 5.0)),
expected = map_list_of
(_x_, Vector_(1, 2.0))
(_y_, Vector_(1, 3.0));
(_x_, (Vec(1) << 2.0))
(_y_, (Vec(1) << 3.0));
VectorValues actual = smallBayesNet.backSubstituteTranspose(x);
EXPECT(assert_equal(expected, actual));
@ -113,15 +113,15 @@ TEST( GaussianBayesNet, DeterminantTest )
{
GaussianBayesNet cbn;
cbn += GaussianConditional(
0, Vector_( 2, 3.0, 4.0 ), Matrix_(2, 2, 1.0, 3.0, 0.0, 4.0 ),
0, (Vec(2) << 3.0, 4.0 ), Matrix_(2, 2, 1.0, 3.0, 0.0, 4.0 ),
1, Matrix_(2, 2, 2.0, 1.0, 2.0, 3.0), noiseModel::Isotropic::Sigma(2, 2.0));
cbn += GaussianConditional(
1, Vector_( 2, 5.0, 6.0 ), Matrix_(2, 2, 1.0, 1.0, 0.0, 3.0 ),
1, (Vec(2) << 5.0, 6.0 ), Matrix_(2, 2, 1.0, 1.0, 0.0, 3.0 ),
2, Matrix_(2, 2, 1.0, 0.0, 5.0, 2.0), noiseModel::Isotropic::Sigma(2, 2.0));
cbn += GaussianConditional(
3, Vector_( 2, 7.0, 8.0 ), Matrix_(2, 2, 1.0, 1.0, 0.0, 5.0 ), noiseModel::Isotropic::Sigma(2, 2.0));
3, (Vec(2) << 7.0, 8.0 ), Matrix_(2, 2, 1.0, 1.0, 0.0, 5.0 ), noiseModel::Isotropic::Sigma(2, 2.0));
double expectedDeterminant = 60.0 / 64.0;
double actualDeterminant = cbn.determinant();
@ -144,21 +144,21 @@ TEST(GaussianBayesNet, ComputeSteepestDescentPoint) {
// Create an arbitrary Bayes Net
GaussianBayesNet gbn;
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
0, Vector_(2, 1.0,2.0), Matrix_(2,2, 3.0,4.0,0.0,6.0),
0, (Vec(2) << 1.0,2.0), Matrix_(2,2, 3.0,4.0,0.0,6.0),
3, Matrix_(2,2, 7.0,8.0,9.0,10.0),
4, Matrix_(2,2, 11.0,12.0,13.0,14.0)));
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
1, Vector_(2, 15.0,16.0), Matrix_(2,2, 17.0,18.0,0.0,20.0),
1, (Vec(2) << 15.0,16.0), Matrix_(2,2, 17.0,18.0,0.0,20.0),
2, Matrix_(2,2, 21.0,22.0,23.0,24.0),
4, Matrix_(2,2, 25.0,26.0,27.0,28.0)));
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
2, Vector_(2, 29.0,30.0), Matrix_(2,2, 31.0,32.0,0.0,34.0),
2, (Vec(2) << 29.0,30.0), Matrix_(2,2, 31.0,32.0,0.0,34.0),
3, Matrix_(2,2, 35.0,36.0,37.0,38.0)));
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
3, Vector_(2, 39.0,40.0), Matrix_(2,2, 41.0,42.0,0.0,44.0),
3, (Vec(2) << 39.0,40.0), Matrix_(2,2, 41.0,42.0,0.0,44.0),
4, Matrix_(2,2, 45.0,46.0,47.0,48.0)));
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
4, Vector_(2, 49.0,50.0), Matrix_(2,2, 51.0,52.0,0.0,54.0)));
4, (Vec(2) << 49.0,50.0), Matrix_(2,2, 51.0,52.0,0.0,54.0)));
// Compute the Hessian numerically
Matrix hessian = numericalHessian(

34
gtsam/linear/tests/testGaussianBayesTreeUnordered.cpp
View File

@ -38,10 +38,10 @@ namespace {
const Key x1=1, x2=2, x3=3, x4=4;
const SharedDiagonal chainNoise = noiseModel::Isotropic::Sigma(1, 0.5);
const GaussianFactorGraph chain = list_of
(JacobianFactor(x2, Matrix_(1,1,1.), x1, Matrix_(1,1,1.), Vector_(1,1.), chainNoise))
(JacobianFactor(x2, Matrix_(1,1,1.), x3, Matrix_(1,1,1.), Vector_(1,1.), chainNoise))
(JacobianFactor(x3, Matrix_(1,1,1.), x4, Matrix_(1,1,1.), Vector_(1,1.), chainNoise))
(JacobianFactor(x4, Matrix_(1,1,1.), Vector_(1,1.), chainNoise));
(JacobianFactor(x2, Matrix_(1,1,1.), x1, Matrix_(1,1,1.), (Vec(1) << 1.), chainNoise))
(JacobianFactor(x2, Matrix_(1,1,1.), x3, Matrix_(1,1,1.), (Vec(1) << 1.), chainNoise))
(JacobianFactor(x3, Matrix_(1,1,1.), x4, Matrix_(1,1,1.), (Vec(1) << 1.), chainNoise))
(JacobianFactor(x4, Matrix_(1,1,1.), (Vec(1) << 1.), chainNoise));
const Ordering chainOrdering = Ordering(list_of(x2)(x1)(x3)(x4));
/* ************************************************************************* */
@ -89,8 +89,8 @@ TEST( GaussianBayesTree, eliminate )
GaussianBayesTree bayesTree_expected;
bayesTree_expected.insertRoot(
MakeClique(GaussianConditional(pair_list_of (x3, Matrix_(2,1, 2., 0.)) (x4, Matrix_(2,1, 2., 2.)), 2, Vector_(2, 2., 2.)), list_of
(MakeClique(GaussianConditional(pair_list_of (x2, Matrix_(2,1, -2.*sqrt(2.), 0.)) (x1, Matrix_(2,1, -sqrt(2.), -sqrt(2.))) (x3, Matrix_(2,1, -sqrt(2.), sqrt(2.))), 2, Vector_(2, -2.*sqrt(2.), 0.))))));
MakeClique(GaussianConditional(pair_list_of (x3, Matrix_(2,1, 2., 0.)) (x4, Matrix_(2,1, 2., 2.)), 2, (Vec(2) << 2., 2.)), list_of
(MakeClique(GaussianConditional(pair_list_of (x2, Matrix_(2,1, -2.*sqrt(2.), 0.)) (x1, Matrix_(2,1, -sqrt(2.), -sqrt(2.))) (x3, Matrix_(2,1, -sqrt(2.), sqrt(2.))), 2, (Vec(2) << -2.*sqrt(2.), 0.))))));
EXPECT(assert_equal(bayesTree_expected, bt));
}
@ -99,10 +99,10 @@ TEST( GaussianBayesTree, eliminate )
TEST( GaussianBayesTree, optimizeMultiFrontal )
{
VectorValues expected = pair_list_of
(x1, Vector_(1, 0.))
(x2, Vector_(1, 1.))
(x3, Vector_(1, 0.))
(x4, Vector_(1, 1.));
(x1, (Vec(1) << 0.))
(x2, (Vec(1) << 1.))
(x3, (Vec(1) << 0.))
(x4, (Vec(1) << 1.));
VectorValues actual = chain.eliminateMultifrontal(chainOrdering)->optimize();
EXPECT(assert_equal(expected,actual));
@ -212,7 +212,7 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) {
47.0,48.0,
51.0,52.0,
0.0,54.0)),
3, Vector_(6, 29.0,30.0,39.0,40.0,49.0,50.0)), list_of
3, (Vec(6) << 29.0,30.0,39.0,40.0,49.0,50.0)), list_of
(MakeClique(GaussianConditional(
pair_list_of
(0, Matrix_(4,2,
@ -240,7 +240,7 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) {
13.0,14.0,
25.0,26.0,
27.0,28.0)),
2, Vector_(4, 1.0,2.0,15.0,16.0))))));
2, (Vec(4) << 1.0,2.0,15.0,16.0))))));
// Compute the Hessian numerically
Matrix hessian = numericalHessian(
@ -264,11 +264,11 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) {
// Known steepest descent point from Bayes' net version
VectorValues expectedFromBN = pair_list_of
(0, Vector_(2, 0.000129034, 0.000688183))
(1, Vector_(2, 0.0109679, 0.0253767))
(2, Vector_(2, 0.0680441, 0.114496))
(3, Vector_(2, 0.16125, 0.241294))
(4, Vector_(2, 0.300134, 0.423233));
(0, (Vec(2) << 0.000129034, 0.000688183))
(1, (Vec(2) << 0.0109679, 0.0253767))
(2, (Vec(2) << 0.0680441, 0.114496))
(3, (Vec(2) << 0.16125, 0.241294))
(4, (Vec(2) << 0.300134, 0.423233));
// Compute the steepest descent point with the dogleg function
VectorValues actual = bt.optimizeGradientSearch();

26
gtsam/linear/tests/testGaussianConditionalUnordered.cpp
View File

@ -56,8 +56,8 @@ TEST(GaussianConditional, constructor)
-11.3820, -7.2581,
-3.0153, -3.5635);
Vector d = Vector_(2, 1.0, 2.0);
SharedDiagonal s = noiseModel::Diagonal::Sigmas(Vector_(2, 3.0, 4.0));
Vector d = (Vec(2) << 1.0, 2.0);
SharedDiagonal s = noiseModel::Diagonal::Sigmas((Vec(2) << 3.0, 4.0));
vector<pair<Key, Matrix> > terms = pair_list_of
(1, R)
@ -114,9 +114,9 @@ TEST( GaussianConditional, equals )
R(0,0) = 0.1 ; R(1,0) = 0.3;
R(0,1) = 0.0 ; R(1,1) = 0.34;
SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(2, 1.0, 0.34));
SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vec(2) << 1.0, 0.34));
Vector d = Vector_(2, 0.2, 0.5);
Vector d = (Vec(2) << 0.2, 0.5);
GaussianConditional
expected(1, d, R, 2, A1, 10, A2, model),
@ -177,7 +177,7 @@ TEST( GaussianConditional, solve_simple )
GaussianConditional cg(list_of(1)(2), 1, blockMatrix);
// partial solution
Vector sx1 = Vector_(2, 9.0, 10.0);
Vector sx1 = (Vec(2) << 9.0, 10.0);
// elimination order: 1, 2
VectorValues actual = map_list_of
@ -185,7 +185,7 @@ TEST( GaussianConditional, solve_simple )
VectorValues expected = map_list_of
(2, sx1)
(1, Vector_(4, -3.1,-3.4,-11.9,-13.2));
(1, (Vec(4) << -3.1,-3.4,-11.9,-13.2));
// verify indices/size
EXPECT_LONGS_EQUAL(2, (long)cg.size());
@ -213,15 +213,15 @@ TEST( GaussianConditional, solve_multifrontal )
EXPECT(assert_equal(Vector(blockMatrix.full().rightCols(1)), cg.get_d()));
// partial solution
Vector sl1 = Vector_(2, 9.0, 10.0);
Vector sl1 = (Vec(2) << 9.0, 10.0);
// elimination order; _x_, _x1_, _l1_
VectorValues actual = map_list_of
(10, sl1); // parent
VectorValues expected = map_list_of
(1, Vector_(2, -3.1,-3.4))
(2, Vector_(2, -11.9,-13.2))
(1, (Vector)(Vec(2) << -3.1,-3.4))
(2, (Vector)(Vec(2) << -11.9,-13.2))
(10, sl1);
// verify indices/size
@ -258,11 +258,11 @@ TEST( GaussianConditional, solveTranspose ) {
VectorValues
x = map_list_of
(1, Vector_(1,2.))
(2, Vector_(1,5.)),
(1, (Vec(1) << 2.))
(2, (Vec(1) << 5.)),
y = map_list_of
(1, Vector_(1,2.))
(2, Vector_(1,3.));
(1, (Vec(1) << 2.))
(2, (Vec(1) << 3.));
// test functional version
VectorValues actual = cbn.backSubstituteTranspose(x);

2
gtsam/linear/tests/testGaussianDensity.cpp
View File

@ -29,7 +29,7 @@ TEST(GaussianDensity, constructor)
-12.1244, -5.1962,
0., 4.6904);
Vector d = Vector_(2, 1.0, 2.0), s = Vector_(2, 3.0, 4.0);
Vector d = (Vec(2) << 1.0, 2.0), s = (Vec(2) << 3.0, 4.0);
GaussianConditional conditional(1, d, R, noiseModel::Diagonal::Sigmas(s));
GaussianDensity copied(conditional);

36
gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp
View File

@ -43,11 +43,11 @@ static GaussianFactorGraph createSimpleGaussianFactorGraph() {
// linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_]
fg += JacobianFactor(2, 10*eye(2), -1.0*ones(2), unit2);
// odometry between x1 and x2: x2-x1=[0.2;-0.1]
fg += JacobianFactor(2, -10*eye(2), 0, 10*eye(2), Vector_(2, 2.0, -1.0), unit2);
fg += JacobianFactor(2, -10*eye(2), 0, 10*eye(2), (Vec(2) << 2.0, -1.0), unit2);
// measurement between x1 and l1: l1-x1=[0.0;0.2]
fg += JacobianFactor(2, -5*eye(2), 1, 5*eye(2), Vector_(2, 0.0, 1.0), unit2);
fg += JacobianFactor(2, -5*eye(2), 1, 5*eye(2), (Vec(2) << 0.0, 1.0), unit2);
// measurement between x2 and l1: l1-x2=[-0.2;0.3]
fg += JacobianFactor(0, -5*eye(2), 1, 5*eye(2), Vector_(2, -1.0, 1.5), unit2);
fg += JacobianFactor(0, -5*eye(2), 1, 5*eye(2), (Vec(2) << -1.0, 1.5), unit2);
return fg;
}
@ -59,9 +59,9 @@ TEST(GaussianFactorGraph, initialization) {
fg +=
JacobianFactor(0, 10*eye(2), -1.0*ones(2), unit2),
JacobianFactor(0, -10*eye(2),1, 10*eye(2), Vector_(2, 2.0, -1.0), unit2),
JacobianFactor(0, -5*eye(2), 2, 5*eye(2), Vector_(2, 0.0, 1.0), unit2),
JacobianFactor(1, -5*eye(2), 2, 5*eye(2), Vector_(2, -1.0, 1.5), unit2);
JacobianFactor(0, -10*eye(2),1, 10*eye(2), (Vec(2) << 2.0, -1.0), unit2),
JacobianFactor(0, -5*eye(2), 2, 5*eye(2), (Vec(2) << 0.0, 1.0), unit2),
JacobianFactor(1, -5*eye(2), 2, 5*eye(2), (Vec(2) << -1.0, 1.5), unit2);
EXPECT_LONGS_EQUAL(4, (long)fg.size());
@ -106,7 +106,7 @@ TEST(GaussianFactorGraph, sparseJacobian) {
GaussianFactorGraph gfg;
SharedDiagonal model = noiseModel::Isotropic::Sigma(2, 0.5);
gfg.add(0, Matrix_(2,3, 1., 2., 3., 5., 6., 7.), Vector_(2, 4., 8.), model);
gfg.add(0, Matrix_(2,3, 1., 2., 3., 5., 6., 7.), (Vec(2) << 4., 8.), model);
gfg.add(0, Matrix_(2,3, 9.,10., 0., 0., 0., 0.), 1, Matrix_(2,2, 11., 12., 14., 15.), Vector_(2, 13.,16.), model);
Matrix actual = gfg.sparseJacobian_();
@ -125,7 +125,7 @@ TEST(GaussianFactorGraph, matrices) {
GaussianFactorGraph gfg;
SharedDiagonal model = noiseModel::Unit::Create(2);
gfg.add(0, Matrix_(2,3, 1., 2., 3., 5., 6., 7.), Vector_(2, 4., 8.), model);
gfg.add(0, Matrix_(2,3, 1., 2., 3., 5., 6., 7.), (Vec(2) << 4., 8.), model);
gfg.add(0, Matrix_(2,3, 9.,10., 0., 0., 0., 0.), 1, Matrix_(2,2, 11., 12., 14., 15.), Vector_(2, 13.,16.), model);
Matrix jacobian(4,6);
@ -164,9 +164,9 @@ TEST( GaussianFactorGraph, gradient )
// 2*f(x) = 100*(x1+c[X(1)])^2 + 100*(x2-x1-[0.2;-0.1])^2 + 25*(l1-x1-[0.0;0.2])^2 + 25*(l1-x2-[-0.2;0.3])^2
// worked out: df/dx1 = 100*[0.1;0.1] + 100*[0.2;-0.1]) + 25*[0.0;0.2] = [10+20;10-10+5] = [30;5]
VectorValues expected = map_list_of
(1, Vector_(2, 5.0,-12.5))
(2, Vector_(2, 30.0, 5.0))
(0, Vector_(2,-25.0, 17.5));
(1, (Vec(2) << 5.0, -12.5))
(2, (Vec(2) << 30.0, 5.0))
(0, (Vec(2) << -25.0, 17.5));
// Check the gradient at delta=0
VectorValues zero = VectorValues::Zero(expected);
@ -185,15 +185,15 @@ TEST( GaussianFactorGraph, transposeMultiplication )
GaussianFactorGraph A = createSimpleGaussianFactorGraph();
Errors e; e +=
Vector_(2, 0.0, 0.0),
Vector_(2,15.0, 0.0),
Vector_(2, 0.0,-5.0),
Vector_(2,-7.5,-5.0);
(Vec(2) << 0.0, 0.0),
(Vec(2) << 15.0, 0.0),
(Vec(2) << 0.0,-5.0),
(Vec(2) << -7.5,-5.0);
VectorValues expected;
expected.insert(1, Vector_(2, -37.5,-50.0));
expected.insert(2, Vector_(2,-150.0, 25.0));
expected.insert(0, Vector_(2, 187.5, 25.0));
expected.insert(1, (Vec(2) << -37.5,-50.0));
expected.insert(2, (Vec(2) << -150.0, 25.0));
expected.insert(0, (Vec(2) << 187.5, 25.0));
VectorValues actual = A.transposeMultiply(e);
EXPECT(assert_equal(expected, actual));

68
gtsam/linear/tests/testHessianFactorUnordered.cpp
View File

@ -69,14 +69,14 @@ TEST(HessianFactor, ConversionConstructor)
0., 1., 0.00, 0., // f4
0., 0., -1., 0., // f2
0., 0., 0.00, -1.), // f2
Vector_(4, -0.2, 0.3, 0.2, -0.1),
noiseModel::Diagonal::Sigmas(Vector_(4, 0.2, 0.2, 0.1, 0.1)));
(Vec(4) << -0.2, 0.3, 0.2, -0.1),
noiseModel::Diagonal::Sigmas((Vec(4) << 0.2, 0.2, 0.1, 0.1)));
HessianFactor actual(jacobian);
VectorValues values = pair_list_of
(0, Vector_(2, 1.0, 2.0))
(1, Vector_(4, 3.0, 4.0, 5.0, 6.0));
(0, (Vec(2) << 1.0, 2.0))
(1, (Vec(4) << 3.0, 4.0, 5.0, 6.0));
EXPECT_LONGS_EQUAL(2, (long)actual.size());
EXPECT(assert_equal(expected, actual, 1e-9));
@ -87,11 +87,11 @@ TEST(HessianFactor, ConversionConstructor)
TEST(HessianFactor, Constructor1)
{
Matrix G = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0);
Vector g = Vector_(2, -8.0, -9.0);
Vector g = (Vec(2) << -8.0, -9.0);
double f = 10.0;
VectorValues dx = pair_list_of
(0, Vector_(2, 1.5, 2.5));
(0, (Vec(2) << 1.5, 2.5));
HessianFactor factor(0, G, g, f);
@ -112,7 +112,7 @@ TEST(HessianFactor, Constructor1)
/* ************************************************************************* */
TEST(HessianFactor, Constructor1b)
{
Vector mu = Vector_(2,1.0,2.0);
Vector mu = (Vec(2) << 1.0,2.0);
Matrix Sigma = eye(2,2);
HessianFactor factor(0, mu, Sigma);
@ -134,12 +134,12 @@ TEST(HessianFactor, Constructor2)
Matrix G11 = Matrix_(1,1, 1.0);
Matrix G12 = Matrix_(1,2, 2.0, 4.0);
Matrix G22 = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0);
Vector g1 = Vector_(1, -7.0);
Vector g2 = Vector_(2, -8.0, -9.0);
Vector g1 = (Vec(1) << -7.0);
Vector g2 = (Vec(2) << -8.0, -9.0);
double f = 10.0;
Vector dx0 = Vector_(1, 0.5);
Vector dx1 = Vector_(2, 1.5, 2.5);
Vector dx0 = (Vec(1) << 0.5);
Vector dx1 = (Vec(2) << 1.5, 2.5);
VectorValues dx = pair_list_of
(0, dx0)
@ -165,7 +165,7 @@ TEST(HessianFactor, Constructor2)
VectorValues dxLarge = pair_list_of
(0, dx0)
(1, dx1)
(2, Vector_(2, 0.1, 0.2));
(2, (Vec(2) << 0.1, 0.2));
EXPECT_DOUBLES_EQUAL(expected, factor.error(dxLarge), 1e-10);
}
@ -181,15 +181,15 @@ TEST(HessianFactor, Constructor3)
Matrix G33 = Matrix_(3,3, 1.0, 2.0, 3.0, 0.0, 5.0, 6.0, 0.0, 0.0, 9.0);
Vector g1 = Vector_(1, -7.0);
Vector g2 = Vector_(2, -8.0, -9.0);
Vector g3 = Vector_(3, 1.0, 2.0, 3.0);
Vector g1 = (Vec(1) << -7.0);
Vector g2 = (Vec(2) << -8.0, -9.0);
Vector g3 = (Vec(3) << 1.0, 2.0, 3.0);
double f = 10.0;
Vector dx0 = Vector_(1, 0.5);
Vector dx1 = Vector_(2, 1.5, 2.5);
Vector dx2 = Vector_(3, 1.5, 2.5, 3.5);
Vector dx0 = (Vec(1) << 0.5);
Vector dx1 = (Vec(2) << 1.5, 2.5);
Vector dx2 = (Vec(3) << 1.5, 2.5, 3.5);
VectorValues dx = pair_list_of
(0, dx0)
@ -228,15 +228,15 @@ TEST(HessianFactor, ConstructorNWay)
Matrix G33 = Matrix_(3,3, 1.0, 2.0, 3.0, 0.0, 5.0, 6.0, 0.0, 0.0, 9.0);
Vector g1 = Vector_(1, -7.0);
Vector g2 = Vector_(2, -8.0, -9.0);
Vector g3 = Vector_(3, 1.0, 2.0, 3.0);
Vector g1 = (Vec(1) << -7.0);
Vector g2 = (Vec(2) << -8.0, -9.0);
Vector g3 = (Vec(3) << 1.0, 2.0, 3.0);
double f = 10.0;
Vector dx0 = Vector_(1, 0.5);
Vector dx1 = Vector_(2, 1.5, 2.5);
Vector dx2 = Vector_(3, 1.5, 2.5, 3.5);
Vector dx0 = (Vec(1) << 0.5);
Vector dx1 = (Vec(2) << 1.5, 2.5);
Vector dx2 = (Vec(3) << 1.5, 2.5, 3.5);
VectorValues dx = pair_list_of
(0, dx0)
@ -276,8 +276,8 @@ TEST(HessianFactor, CombineAndEliminate)
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0);
Vector b0 = Vector_(3, 1.5, 1.5, 1.5);
Vector s0 = Vector_(3, 1.6, 1.6, 1.6);
Vector b0 = (Vec(3) << 1.5, 1.5, 1.5);
Vector s0 = (Vec(3) << 1.6, 1.6, 1.6);
Matrix A10 = Matrix_(3,3,
2.0, 0.0, 0.0,
@ -287,15 +287,15 @@ TEST(HessianFactor, CombineAndEliminate)
-2.0, 0.0, 0.0,
0.0, -2.0, 0.0,
0.0, 0.0, -2.0);
Vector b1 = Vector_(3, 2.5, 2.5, 2.5);
Vector s1 = Vector_(3, 2.6, 2.6, 2.6);
Vector b1 = (Vec(3) << 2.5, 2.5, 2.5);
Vector s1 = (Vec(3) << 2.6, 2.6, 2.6);
Matrix A21 = Matrix_(3,3,
3.0, 0.0, 0.0,
0.0, 3.0, 0.0,
0.0, 0.0, 3.0);
Vector b2 = Vector_(3, 3.5, 3.5, 3.5);
Vector s2 = Vector_(3, 3.6, 3.6, 3.6);
Vector b2 = (Vec(3) << 3.5, 3.5, 3.5);
Vector s2 = (Vec(3) << 3.6, 3.6, 3.6);
GaussianFactorGraph gfg;
gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
@ -331,7 +331,7 @@ TEST(HessianFactor, eliminate2 )
// sigmas
double sigma1 = 0.2;
double sigma2 = 0.1;
Vector sigmas = Vector_(4, sigma1, sigma1, sigma2, sigma2);
Vector sigmas = (Vec(4) << sigma1, sigma1, sigma2, sigma2);
// the combined linear factor
Matrix Ax2 = Matrix_(4,2,
@ -379,7 +379,7 @@ TEST(HessianFactor, eliminate2 )
-0.20, 0.00,-0.80, 0.00,
+0.00,-0.20,+0.00,-0.80
)/oldSigma;
Vector d = Vector_(2,0.2,-0.14)/oldSigma;
Vector d = (Vec(2) << 0.2,-0.14)/oldSigma;
GaussianConditional expectedCG(0, d, R11, 1, S12);
EXPECT(assert_equal(expectedCG, *actual_Chol.first, 1e-4));
@ -390,7 +390,7 @@ TEST(HessianFactor, eliminate2 )
1.00, 0.00, -1.00, 0.00,
0.00, 1.00, +0.00, -1.00
)/sigma;
Vector b1 = Vector_(2,0.0,0.894427);
Vector b1 = (Vec(2) << 0.0,0.894427);
JacobianFactor expectedLF(1, Bl1x1, b1, noiseModel::Isotropic::Sigma(2,1.0));
EXPECT(assert_equal(HessianFactor(expectedLF), *actual_Chol.second, 1.5e-3));
}
@ -408,7 +408,7 @@ TEST(HessianFactor, combine) {
Matrix A2 = Matrix_(2, 2,
-8.94427191, 0.0,
0.0, -8.94427191);
Vector b = Vector_(2, 2.23606798,-1.56524758);
Vector b = (Vec(2) << 2.23606798,-1.56524758);
SharedDiagonal model = noiseModel::Diagonal::Sigmas(ones(2));
GaussianFactor::shared_ptr f(new JacobianFactor(0, A0, 1, A1, 2, A2, b, model));
GaussianFactorGraph factors = list_of(f);

40
gtsam/linear/tests/testJacobianFactorUnordered.cpp
View File

@ -41,8 +41,8 @@ namespace {
(make_pair(15, 3*Matrix3::Identity()));
// RHS and sigmas
const Vector b = Vector_(3, 1., 2., 3.);
const SharedDiagonal noise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.5, 0.5, 0.5));
const Vector b = (Vec(3) << 1., 2., 3.);
const SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.5, 0.5, 0.5));
}
}
@ -239,22 +239,22 @@ TEST(JacobianFactor, operators )
SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1);
Matrix I = eye(2);
Vector b = Vector_(2,0.2,-0.1);
Vector b = (Vec(2) << 0.2,-0.1);
JacobianFactor lf(1, -I, 2, I, b, sigma0_1);
VectorValues c;
c.insert(1, Vector_(2,10.,20.));
c.insert(2, Vector_(2,30.,60.));
c.insert(1, (Vec(2) << 10.,20.));
c.insert(2, (Vec(2) << 30.,60.));
// test A*x
Vector expectedE = Vector_(2,200.,400.);
Vector expectedE = (Vec(2) << 200.,400.);
Vector actualE = lf * c;
EXPECT(assert_equal(expectedE, actualE));
// test A^e
VectorValues expectedX;
expectedX.insert(1, Vector_(2,-2000.,-4000.));
expectedX.insert(2, Vector_(2, 2000., 4000.));
expectedX.insert(1, (Vec(2) << -2000.,-4000.));
expectedX.insert(2, (Vec(2) << 2000., 4000.));
VectorValues actualX = VectorValues::Zero(expectedX);
lf.transposeMultiplyAdd(1.0, actualE, actualX);
EXPECT(assert_equal(expectedX, actualX));
@ -283,8 +283,8 @@ TEST(JacobianFactor, eliminate)
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0);
Vector b0 = Vector_(3, 1.5, 1.5, 1.5);
Vector s0 = Vector_(3, 1.6, 1.6, 1.6);
Vector b0 = (Vec(3) << 1.5, 1.5, 1.5);
Vector s0 = (Vec(3) << 1.6, 1.6, 1.6);
Matrix A10 = Matrix_(3,3,
2.0, 0.0, 0.0,
@ -294,15 +294,15 @@ TEST(JacobianFactor, eliminate)
-2.0, 0.0, 0.0,
0.0, -2.0, 0.0,
0.0, 0.0, -2.0);
Vector b1 = Vector_(3, 2.5, 2.5, 2.5);
Vector s1 = Vector_(3, 2.6, 2.6, 2.6);
Vector b1 = (Vec(3) << 2.5, 2.5, 2.5);
Vector s1 = (Vec(3) << 2.6, 2.6, 2.6);
Matrix A21 = Matrix_(3,3,
3.0, 0.0, 0.0,
0.0, 3.0, 0.0,
0.0, 0.0, 3.0);
Vector b2 = Vector_(3, 3.5, 3.5, 3.5);
Vector s2 = Vector_(3, 3.6, 3.6, 3.6);
Vector b2 = (Vec(3) << 3.5, 3.5, 3.5);
Vector s2 = (Vec(3) << 3.6, 3.6, 3.6);
GaussianFactorGraph gfg;
gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
@ -334,7 +334,7 @@ TEST(JacobianFactor, eliminate2 )
// sigmas
double sigma1 = 0.2;
double sigma2 = 0.1;
Vector sigmas = Vector_(4, sigma1, sigma1, sigma2, sigma2);
Vector sigmas = (Vec(4) << sigma1, sigma1, sigma2, sigma2);
// the combined linear factor
Matrix Ax2 = Matrix_(4,2,
@ -379,7 +379,7 @@ TEST(JacobianFactor, eliminate2 )
-0.20, 0.00,-0.80, 0.00,
+0.00,-0.20,+0.00,-0.80
)/oldSigma;
Vector d = Vector_(2,0.2,-0.14)/oldSigma;
Vector d = (Vec(2) << 0.2,-0.14)/oldSigma;
GaussianConditional expectedCG(2, d, R11, 11, S12);
EXPECT(assert_equal(expectedCG, *actual.first, 1e-4));
@ -391,7 +391,7 @@ TEST(JacobianFactor, eliminate2 )
1.00, 0.00, -1.00, 0.00,
0.00, 1.00, +0.00, -1.00
)/sigma;
Vector b1 = Vector_(2, 0.0, 0.894427);
Vector b1 = (Vec(2) << 0.0, 0.894427);
JacobianFactor expectedLF(11, Bl1x1, b1);
EXPECT(assert_equal(expectedLF, *actual.second,1e-3));
}
@ -475,7 +475,7 @@ TEST ( JacobianFactor, constraint_eliminate1 )
EXPECT(actual.second->size() == 0);
// verify conditional Gaussian
Vector sigmas = Vector_(2, 0.0, 0.0);
Vector sigmas = (Vec(2) << 0.0, 0.0);
GaussianConditional expCG(1, v, eye(2), noiseModel::Diagonal::Sigmas(sigmas));
EXPECT(assert_equal(expCG, *actual.first));
}
@ -518,8 +518,8 @@ TEST ( JacobianFactor, constraint_eliminate2 )
Matrix S = Matrix_(2,2,
1.0, 2.0,
0.0, 0.0);
Vector d = Vector_(2, 3.0, 0.6666);
Vector sigmas = Vector_(2, 0.0, 0.0);
Vector d = (Vec(2) << 3.0, 0.6666);
Vector sigmas = (Vec(2) << 0.0, 0.0);
GaussianConditional expectedCG(1, d, R, 2, S, noiseModel::Diagonal::Sigmas(sigmas));
EXPECT(assert_equal(expectedCG, *actual.first, 1e-4));
}

14
gtsam/linear/tests/testKalmanFilter.cpp
View File

@ -30,7 +30,7 @@ using namespace gtsam;
/** Small 2D point class implemented as a Vector */
struct State: Vector {
State(double x, double y) :
Vector(Vector_(2, x, y)) {
Vector((Vec(2) << x, y)) {
}
};
@ -67,7 +67,7 @@ TEST( KalmanFilter, linear1 ) {
// Create the controls and measurement properties for our example
Matrix F = eye(2, 2);
Matrix B = eye(2, 2);
Vector u = Vector_(2, 1.0, 0.0);
Vector u = (Vec(2) << 1.0, 0.0);
SharedDiagonal modelQ = noiseModel::Isotropic::Sigma(2, 0.1);
Matrix Q = 0.01*eye(2, 2);
Matrix H = eye(2, 2);
@ -137,7 +137,7 @@ TEST( KalmanFilter, predict ) {
// Create dynamics model
Matrix F = Matrix_(2, 2, 1.0, 0.1, 0.2, 1.1);
Matrix B = Matrix_(2, 3, 1.0, 0.1, 0.2, 1.1, 1.2, 0.8);
Vector u = Vector_(3, 1.0, 0.0, 2.0);
Vector u = (Vec(3) << 1.0, 0.0, 2.0);
Matrix R = Matrix_(2, 2, 1.0, 0.5, 0.0, 3.0);
Matrix M = trans(R)*R;
Matrix Q = inverse(M);
@ -219,7 +219,7 @@ TEST( KalmanFilter, QRvsCholesky ) {
EXPECT(assert_equal(pa->information(), pb->information(), 1e-7));
// and in addition attain the correct covariance
Vector expectedMean = Vector_(9, 0.9814, 1.0200, 1.0190, 1., 1., 1., 1., 1., 1.);
Vector expectedMean = (Vec(9) << 0.9814, 1.0200, 1.0190, 1., 1., 1., 1., 1., 1.);
EXPECT(assert_equal(expectedMean, pa->mean(), 1e-7));
EXPECT(assert_equal(expectedMean, pb->mean(), 1e-7));
Matrix expected = 1e-6*Matrix_(9, 9,
@ -240,8 +240,8 @@ TEST( KalmanFilter, QRvsCholesky ) {
0.0, 9795.9, 83.6, 0.0, 0.0, 0.0, 1000.0, 0.0, 0.0,
-9795.9, 0.0, -5.2, 0.0, 0.0, 0.0, 0.0, 1000.0, 0.0,
-83.6, 5.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000.);
Vector z = Vector_(3, 0.2599 , 1.3327 , 0.2007);
Vector sigmas = Vector_(3, 0.3323 , 0.2470 , 0.1904);
Vector z = (Vec(3) << 0.2599 , 1.3327 , 0.2007);
Vector sigmas = (Vec(3) << 0.3323 , 0.2470 , 0.1904);
SharedDiagonal modelR = noiseModel::Diagonal::Sigmas(sigmas);
// do update
@ -253,7 +253,7 @@ TEST( KalmanFilter, QRvsCholesky ) {
EXPECT(assert_equal(pa2->information(), pb2->information(), 1e-7));
// and in addition attain the correct mean and covariance
Vector expectedMean2 = Vector_(9, 0.9207, 0.9030, 1.0178, 1.0002, 0.9992, 0.9998, 0.9981, 1.0035, 0.9882);
Vector expectedMean2 = (Vec(9) << 0.9207, 0.9030, 1.0178, 1.0002, 0.9992, 0.9998, 0.9981, 1.0035, 0.9882);
EXPECT(assert_equal(expectedMean2, pa2->mean(), 1e-4));// not happy with tolerance here !
EXPECT(assert_equal(expectedMean2, pb2->mean(), 1e-4));// is something still amiss?
Matrix expected2 = 1e-6*Matrix_(9, 9,

48
gtsam/linear/tests/testNoiseModel.cpp
View File

@ -45,17 +45,17 @@ static Matrix Sigma = Matrix_(3, 3,
/* ************************************************************************* */
TEST(NoiseModel, constructors)
{
Vector whitened = Vector_(3,5.0,10.0,15.0);
Vector unwhitened = Vector_(3,10.0,20.0,30.0);
Vector whitened = (Vec(3) << 5.0,10.0,15.0);
Vector unwhitened = (Vec(3) << 10.0,20.0,30.0);
// Construct noise models
vector<Gaussian::shared_ptr> m;
m.push_back(Gaussian::SqrtInformation(R));
m.push_back(Gaussian::Covariance(Sigma));
//m.push_back(Gaussian::Information(Q));
m.push_back(Diagonal::Sigmas(Vector_(3, sigma, sigma, sigma)));
m.push_back(Diagonal::Variances(Vector_(3, var, var, var)));
m.push_back(Diagonal::Precisions(Vector_(3, prc, prc, prc)));
m.push_back(Diagonal::Sigmas((Vec(3) << sigma, sigma, sigma)));
m.push_back(Diagonal::Variances((Vec(3) << var, var, var)));
m.push_back(Diagonal::Precisions((Vec(3) << prc, prc, prc)));
m.push_back(Isotropic::Sigma(3, sigma));
m.push_back(Isotropic::Variance(3, var));
m.push_back(Isotropic::Precision(3, prc));
@ -104,7 +104,7 @@ TEST(NoiseModel, constructors)
/* ************************************************************************* */
TEST(NoiseModel, Unit)
{
Vector v = Vector_(3,5.0,10.0,15.0);
Vector v = (Vec(3) << 5.0,10.0,15.0);
Gaussian::shared_ptr u(Unit::Create(3));
EXPECT(assert_equal(v,u->whiten(v)));
}
@ -114,8 +114,8 @@ TEST(NoiseModel, equals)
{
Gaussian::shared_ptr g1 = Gaussian::SqrtInformation(R),
g2 = Gaussian::SqrtInformation(eye(3,3));
Diagonal::shared_ptr d1 = Diagonal::Sigmas(Vector_(3, sigma, sigma, sigma)),
d2 = Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3));
Diagonal::shared_ptr d1 = Diagonal::Sigmas((Vec(3) << sigma, sigma, sigma)),
d2 = Diagonal::Sigmas((Vec(3) << 0.1, 0.2, 0.3));
Isotropic::shared_ptr i1 = Isotropic::Sigma(3, sigma),
i2 = Isotropic::Sigma(3, 0.7);
@ -133,7 +133,7 @@ TEST(NoiseModel, equals)
///* ************************************************************************* */
//TEST(NoiseModel, ConstrainedSmart )
//{
// Gaussian::shared_ptr nonconstrained = Constrained::MixedSigmas(Vector_(3, sigma, 0.0, sigma), true);
// Gaussian::shared_ptr nonconstrained = Constrained::MixedSigmas((Vec(3) << sigma, 0.0, sigma), true);
// Diagonal::shared_ptr n1 = boost::dynamic_pointer_cast<Diagonal>(nonconstrained);
// Constrained::shared_ptr n2 = boost::dynamic_pointer_cast<Constrained>(nonconstrained);
// EXPECT(n1);
@ -152,8 +152,8 @@ TEST(NoiseModel, ConstrainedConstructors )
Constrained::shared_ptr actual;
size_t d = 3;
double m = 100.0;
Vector sigmas = Vector_(3, sigma, 0.0, 0.0);
Vector mu = Vector_(3, 200.0, 300.0, 400.0);
Vector sigmas = (Vec(3) << sigma, 0.0, 0.0);
Vector mu = (Vec(3) << 200.0, 300.0, 400.0);
actual = Constrained::All(d);
EXPECT(assert_equal(gtsam::repeat(d, 1000.0), actual->mu()));
@ -173,12 +173,12 @@ TEST(NoiseModel, ConstrainedConstructors )
/* ************************************************************************* */
TEST(NoiseModel, ConstrainedMixed )
{
Vector feasible = Vector_(3, 1.0, 0.0, 1.0),
infeasible = Vector_(3, 1.0, 1.0, 1.0);
Diagonal::shared_ptr d = Constrained::MixedSigmas(Vector_(3, sigma, 0.0, sigma));
Vector feasible = (Vec(3) << 1.0, 0.0, 1.0),
infeasible = (Vec(3) << 1.0, 1.0, 1.0);
Diagonal::shared_ptr d = Constrained::MixedSigmas((Vec(3) << sigma, 0.0, sigma));
// NOTE: we catch constrained variables elsewhere, so whitening does nothing
EXPECT(assert_equal(Vector_(3, 0.5, 1.0, 0.5),d->whiten(infeasible)));
EXPECT(assert_equal(Vector_(3, 0.5, 0.0, 0.5),d->whiten(feasible)));
EXPECT(assert_equal((Vec(3) << 0.5, 1.0, 0.5),d->whiten(infeasible)));
EXPECT(assert_equal((Vec(3) << 0.5, 0.0, 0.5),d->whiten(feasible)));
DOUBLES_EQUAL(1000.0 + 0.25 + 0.25,d->distance(infeasible),1e-9);
DOUBLES_EQUAL(0.5,d->distance(feasible),1e-9);
@ -187,13 +187,13 @@ TEST(NoiseModel, ConstrainedMixed )
/* ************************************************************************* */
TEST(NoiseModel, ConstrainedAll )
{
Vector feasible = Vector_(3, 0.0, 0.0, 0.0),
infeasible = Vector_(3, 1.0, 1.0, 1.0);
Vector feasible = (Vec(3) << 0.0, 0.0, 0.0),
infeasible = (Vec(3) << 1.0, 1.0, 1.0);
Constrained::shared_ptr i = Constrained::All(3);
// NOTE: we catch constrained variables elsewhere, so whitening does nothing
EXPECT(assert_equal(Vector_(3, 1.0, 1.0, 1.0),i->whiten(infeasible)));
EXPECT(assert_equal(Vector_(3, 0.0, 0.0, 0.0),i->whiten(feasible)));
EXPECT(assert_equal((Vec(3) << 1.0, 1.0, 1.0),i->whiten(infeasible)));
EXPECT(assert_equal((Vec(3) << 0.0, 0.0, 0.0),i->whiten(feasible)));
DOUBLES_EQUAL(1000.0 * 3.0,i->distance(infeasible),1e-9);
DOUBLES_EQUAL(0.0,i->distance(feasible),1e-9);
@ -207,7 +207,7 @@ namespace exampleQR {
0., -1., 0., 1., 0., 0., 0.3,
1., 0., 0., 0., -1., 0., 0.2,
0., 1., 0., 0., 0., -1., -0.1);
Vector sigmas = Vector_(4, 0.2, 0.2, 0.1, 0.1);
Vector sigmas = (Vec(4) << 0.2, 0.2, 0.1, 0.1);
// the matrix AB yields the following factorized version:
Matrix Rd = Matrix_(4, 6+1,
@ -225,7 +225,7 @@ TEST( NoiseModel, QR )
Matrix Ab2 = exampleQR::Ab; // otherwise overwritten !
// Expected result
Vector expectedSigmas = Vector_(4, 0.0894427, 0.0894427, 0.223607, 0.223607);
Vector expectedSigmas = (Vec(4) << 0.0894427, 0.0894427, 0.223607, 0.223607);
SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas);
// Call Gaussian version
@ -281,7 +281,7 @@ TEST(NoiseModel, ScalarOrVector )
/* ************************************************************************* */
TEST(NoiseModel, WhitenInPlace)
{
Vector sigmas = Vector_(3, 0.1, 0.1, 0.1);
Vector sigmas = (Vec(3) << 0.1, 0.1, 0.1);
SharedDiagonal model = Diagonal::Sigmas(sigmas);
Matrix A = eye(3);
model->WhitenInPlace(A);
@ -305,7 +305,7 @@ TEST(NoiseModel, robustNoise)
{
const double k = 10.0, error1 = 1.0, error2 = 100.0;
Matrix A = Matrix_(2, 2, 1.0, 10.0, 100.0, 1000.0);
Vector b = Vector_(2, error1, error2);
Vector b = (Vec(2) << error1, error2);
const Robust::shared_ptr robust = Robust::Create(
mEstimator::Huber::Create(k, mEstimator::Huber::Scalar),
Unit::Create(2));

20
gtsam/linear/tests/testSerializationLinear.cpp
View File

@ -49,10 +49,10 @@ BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
/* ************************************************************************* */
// example noise models
static noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3));
static noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas((Vec(3) << 0.1, 0.2, 0.3));
static noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * eye(3,3));
static noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2);
static noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas(Vector_(3, 0.0, 0.0, 0.1));
static noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas((Vec(3) << 0.0, 0.0, 0.1));
static noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3);
/* ************************************************************************* */
@ -134,9 +134,9 @@ BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor");
/* ************************************************************************* */
TEST (Serialization, linear_factors) {
VectorValues values;
values.insert(0, Vector_(1, 1.0));
values.insert(1, Vector_(2, 2.0,3.0));
values.insert(2, Vector_(2, 4.0,5.0));
values.insert(0, (Vec(1) << 1.0));
values.insert(1, (Vec(2) << 2.0,3.0));
values.insert(2, (Vec(2) << 4.0,5.0));
EXPECT(equalsObj<VectorValues>(values));
EXPECT(equalsXML<VectorValues>(values));
EXPECT(equalsBinary<VectorValues>(values));
@ -144,7 +144,7 @@ TEST (Serialization, linear_factors) {
Index i1 = 4, i2 = 7;
Matrix A1 = eye(3), A2 = -1.0 * eye(3);
Vector b = ones(3);
SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 1.0, 2.0, 3.0));
SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vec(3) << 1.0, 2.0, 3.0));
JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model);
EXPECT(equalsObj(jacobianfactor));
EXPECT(equalsXML(jacobianfactor));
@ -175,10 +175,10 @@ TEST (Serialization, gaussian_bayes_tree) {
const Ordering chainOrdering = Ordering(list_of(x2)(x1)(x3)(x4));
const SharedDiagonal chainNoise = noiseModel::Isotropic::Sigma(1, 0.5);
const GaussianFactorGraph chain = list_of
(JacobianFactor(x2, Matrix_(1,1,1.), x1, Matrix_(1,1,1.), Vector_(1,1.), chainNoise))
(JacobianFactor(x2, Matrix_(1,1,1.), x3, Matrix_(1,1,1.), Vector_(1,1.), chainNoise))
(JacobianFactor(x3, Matrix_(1,1,1.), x4, Matrix_(1,1,1.), Vector_(1,1.), chainNoise))
(JacobianFactor(x4, Matrix_(1,1,1.), Vector_(1,1.), chainNoise));
(JacobianFactor(x2, Matrix_(1,1,1.), x1, Matrix_(1,1,1.), (Vec(1) << 1.), chainNoise))
(JacobianFactor(x2, Matrix_(1,1,1.), x3, Matrix_(1,1,1.), (Vec(1) << 1.), chainNoise))
(JacobianFactor(x3, Matrix_(1,1,1.), x4, Matrix_(1,1,1.), (Vec(1) << 1.), chainNoise))
(JacobianFactor(x4, Matrix_(1,1,1.), (Vec(1) << 1.), chainNoise));
GaussianBayesTree init = *chain.eliminateMultifrontal(chainOrdering);
GaussianBayesTree expected = *chain.eliminateMultifrontal(chainOrdering);

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

@ -34,10 +34,10 @@ TEST(VectorValues, basics)
// insert
VectorValues 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));
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));
// Check dimensions
LONGS_EQUAL(4, actual.size());
@ -56,12 +56,12 @@ TEST(VectorValues, basics)
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((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]));
FastVector<Key> keys = list_of(0)(1)(2)(5);
EXPECT(assert_equal(Vector_(7, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), actual.vector(keys)));
EXPECT(assert_equal((Vec(7) << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), 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, 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));
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));
VectorValues first;
first.insert(0, Vector_(1, 1.0));
first.insert(1, Vector_(2, 2.0, 3.0));
first.insert(0, (Vec(1) << 1.0));
first.insert(1, (Vec(2) << 2.0, 3.0));
VectorValues second;
second.insert(5, Vector_(2, 6.0, 7.0));
second.insert(2, Vector_(2, 4.0, 5.0));
second.insert(5, (Vec(2) << 6.0, 7.0));
second.insert(2, (Vec(2) << 4.0, 5.0));
VectorValues actual(first, second);
@ -94,14 +94,14 @@ TEST(VectorValues, combine)
TEST(VectorValues, subvector)
{
VectorValues init;
init.insert(10, Vector_(1, 1.0));
init.insert(11, Vector_(2, 2.0, 3.0));
init.insert(12, Vector_(2, 4.0, 5.0));
init.insert(13, Vector_(2, 6.0, 7.0));
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));
std::vector<Key> keys;
keys += 10, 12, 13;
Vector expSubVector = Vector_(5, 1.0, 4.0, 5.0, 6.0, 7.0);
Vector expSubVector = (Vec(5) << 1.0, 4.0, 5.0, 6.0, 7.0);
EXPECT(assert_equal(expSubVector, init.vector(keys)));
}
@ -109,16 +109,16 @@ TEST(VectorValues, subvector)
TEST(VectorValues, LinearAlgebra)
{
VectorValues test1;
test1.insert(0, Vector_(1, 1.0));
test1.insert(1, Vector_(2, 2.0, 3.0));
test1.insert(5, Vector_(2, 6.0, 7.0));
test1.insert(2, Vector_(2, 4.0, 5.0));
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));
VectorValues test2;
test2.insert(0, Vector_(1, 6.0));
test2.insert(1, Vector_(2, 1.0, 6.0));
test2.insert(5, Vector_(2, 4.0, 3.0));
test2.insert(2, Vector_(2, 1.0, 8.0));
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));
// Dot product
double dotExpected = test1.vector().dot(test2.vector());