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 ) TEST( Errors, arithmetic )
{ {
Errors e; 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); DOUBLES_EQUAL(1+4+9+16+25,dot(e,e),1e-9);
axpy(2.0,e,e); axpy(2.0,e,e);
Errors expected; 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)); 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 const Key _x_=0, _y_=1, _z_=2;
static GaussianBayesNet smallBayesNet = list_of static GaussianBayesNet smallBayesNet = list_of
(GaussianConditional(_x_, Vector_(1, 9.0), Matrix_(1, 1, 1.0), _y_, Matrix_(1, 1, 1.0))) (GaussianConditional(_x_, (Vec(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(_y_, (Vec(1) << 5.0), Matrix_(1, 1, 1.0)));
/* ************************************************************************* */ /* ************************************************************************* */
TEST( GaussianBayesNet, matrix ) TEST( GaussianBayesNet, matrix )
@ -51,7 +51,7 @@ TEST( GaussianBayesNet, matrix )
1.0, 1.0, 1.0, 1.0,
0.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(R,R1));
EXPECT(assert_equal(d,d1)); EXPECT(assert_equal(d,d1));
@ -63,8 +63,8 @@ TEST( GaussianBayesNet, optimize )
VectorValues actual = smallBayesNet.optimize(); VectorValues actual = smallBayesNet.optimize();
VectorValues expected = map_list_of VectorValues expected = map_list_of
(_x_, Vector_(1, 4.0)) (_x_, (Vec(1) << 4.0))
(_y_, Vector_(1, 5.0)); (_y_, (Vec(1) << 5.0));
EXPECT(assert_equal(expected,actual)); EXPECT(assert_equal(expected,actual));
} }
@ -78,13 +78,13 @@ TEST( GaussianBayesNet, optimize3 )
// NOTE: we are supplying a new RHS here // NOTE: we are supplying a new RHS here
VectorValues expected = map_list_of VectorValues expected = map_list_of
(_x_, Vector_(1, -1.0)) (_x_, (Vec(1) << -1.0))
(_y_, Vector_(1, 5.0)); (_y_, (Vec(1) << 5.0));
// Test different RHS version // Test different RHS version
VectorValues gx = map_list_of VectorValues gx = map_list_of
(_x_, Vector_(1, 4.0)) (_x_, (Vec(1) << 4.0))
(_y_, Vector_(1, 5.0)); (_y_, (Vec(1) << 5.0));
VectorValues actual = smallBayesNet.backSubstitute(gx); VectorValues actual = smallBayesNet.backSubstitute(gx);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
} }
@ -97,11 +97,11 @@ TEST( GaussianBayesNet, backSubstituteTranspose )
// 5 1 1 3 // 5 1 1 3
VectorValues VectorValues
x = map_list_of x = map_list_of
(_x_, Vector_(1, 2.0)) (_x_, (Vec(1) << 2.0))
(_y_, Vector_(1, 5.0)), (_y_, (Vec(1) << 5.0)),
expected = map_list_of expected = map_list_of
(_x_, Vector_(1, 2.0)) (_x_, (Vec(1) << 2.0))
(_y_, Vector_(1, 3.0)); (_y_, (Vec(1) << 3.0));
VectorValues actual = smallBayesNet.backSubstituteTranspose(x); VectorValues actual = smallBayesNet.backSubstituteTranspose(x);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
@ -113,15 +113,15 @@ TEST( GaussianBayesNet, DeterminantTest )
{ {
GaussianBayesNet cbn; GaussianBayesNet cbn;
cbn += GaussianConditional( 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)); 1, Matrix_(2, 2, 2.0, 1.0, 2.0, 3.0), noiseModel::Isotropic::Sigma(2, 2.0));
cbn += GaussianConditional( 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)); 2, Matrix_(2, 2, 1.0, 0.0, 5.0, 2.0), noiseModel::Isotropic::Sigma(2, 2.0));
cbn += GaussianConditional( 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 expectedDeterminant = 60.0 / 64.0;
double actualDeterminant = cbn.determinant(); double actualDeterminant = cbn.determinant();
@ -144,21 +144,21 @@ TEST(GaussianBayesNet, ComputeSteepestDescentPoint) {
// Create an arbitrary Bayes Net // Create an arbitrary Bayes Net
GaussianBayesNet gbn; GaussianBayesNet gbn;
gbn += GaussianConditional::shared_ptr(new GaussianConditional( 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), 3, Matrix_(2,2, 7.0,8.0,9.0,10.0),
4, Matrix_(2,2, 11.0,12.0,13.0,14.0))); 4, Matrix_(2,2, 11.0,12.0,13.0,14.0)));
gbn += GaussianConditional::shared_ptr(new GaussianConditional( 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), 2, Matrix_(2,2, 21.0,22.0,23.0,24.0),
4, Matrix_(2,2, 25.0,26.0,27.0,28.0))); 4, Matrix_(2,2, 25.0,26.0,27.0,28.0)));
gbn += GaussianConditional::shared_ptr(new GaussianConditional( 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))); 3, Matrix_(2,2, 35.0,36.0,37.0,38.0)));
gbn += GaussianConditional::shared_ptr(new GaussianConditional( 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))); 4, Matrix_(2,2, 45.0,46.0,47.0,48.0)));
gbn += GaussianConditional::shared_ptr(new GaussianConditional( 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 // Compute the Hessian numerically
Matrix hessian = numericalHessian( 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 Key x1=1, x2=2, x3=3, x4=4;
const SharedDiagonal chainNoise = noiseModel::Isotropic::Sigma(1, 0.5); const SharedDiagonal chainNoise = noiseModel::Isotropic::Sigma(1, 0.5);
const GaussianFactorGraph chain = list_of 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.), x1, Matrix_(1,1,1.), (Vec(1) << 1.), chainNoise))
(JacobianFactor(x2, Matrix_(1,1,1.), x3, Matrix_(1,1,1.), Vector_(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.), Vector_(1,1.), chainNoise)) (JacobianFactor(x3, Matrix_(1,1,1.), x4, Matrix_(1,1,1.), (Vec(1) << 1.), chainNoise))
(JacobianFactor(x4, Matrix_(1,1,1.), Vector_(1,1.), chainNoise)); (JacobianFactor(x4, Matrix_(1,1,1.), (Vec(1) << 1.), chainNoise));
const Ordering chainOrdering = Ordering(list_of(x2)(x1)(x3)(x4)); const Ordering chainOrdering = Ordering(list_of(x2)(x1)(x3)(x4));
/* ************************************************************************* */ /* ************************************************************************* */
@ -89,8 +89,8 @@ TEST( GaussianBayesTree, eliminate )
GaussianBayesTree bayesTree_expected; GaussianBayesTree bayesTree_expected;
bayesTree_expected.insertRoot( 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 (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, Vector_(2, -2.*sqrt(2.), 0.)))))); (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)); EXPECT(assert_equal(bayesTree_expected, bt));
} }
@ -99,10 +99,10 @@ TEST( GaussianBayesTree, eliminate )
TEST( GaussianBayesTree, optimizeMultiFrontal ) TEST( GaussianBayesTree, optimizeMultiFrontal )
{ {
VectorValues expected = pair_list_of VectorValues expected = pair_list_of
(x1, Vector_(1, 0.)) (x1, (Vec(1) << 0.))
(x2, Vector_(1, 1.)) (x2, (Vec(1) << 1.))
(x3, Vector_(1, 0.)) (x3, (Vec(1) << 0.))
(x4, Vector_(1, 1.)); (x4, (Vec(1) << 1.));
VectorValues actual = chain.eliminateMultifrontal(chainOrdering)->optimize(); VectorValues actual = chain.eliminateMultifrontal(chainOrdering)->optimize();
EXPECT(assert_equal(expected,actual)); EXPECT(assert_equal(expected,actual));
@ -212,7 +212,7 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) {
47.0,48.0, 47.0,48.0,
51.0,52.0, 51.0,52.0,
0.0,54.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( (MakeClique(GaussianConditional(
pair_list_of pair_list_of
(0, Matrix_(4,2, (0, Matrix_(4,2,
@ -240,7 +240,7 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) {
13.0,14.0, 13.0,14.0,
25.0,26.0, 25.0,26.0,
27.0,28.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 // Compute the Hessian numerically
Matrix hessian = numericalHessian( Matrix hessian = numericalHessian(
@ -264,11 +264,11 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) {
// Known steepest descent point from Bayes' net version // Known steepest descent point from Bayes' net version
VectorValues expectedFromBN = pair_list_of VectorValues expectedFromBN = pair_list_of
(0, Vector_(2, 0.000129034, 0.000688183)) (0, (Vec(2) << 0.000129034, 0.000688183))
(1, Vector_(2, 0.0109679, 0.0253767)) (1, (Vec(2) << 0.0109679, 0.0253767))
(2, Vector_(2, 0.0680441, 0.114496)) (2, (Vec(2) << 0.0680441, 0.114496))
(3, Vector_(2, 0.16125, 0.241294)) (3, (Vec(2) << 0.16125, 0.241294))
(4, Vector_(2, 0.300134, 0.423233)); (4, (Vec(2) << 0.300134, 0.423233));
// Compute the steepest descent point with the dogleg function // Compute the steepest descent point with the dogleg function
VectorValues actual = bt.optimizeGradientSearch(); VectorValues actual = bt.optimizeGradientSearch();

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

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

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

@ -29,7 +29,7 @@ TEST(GaussianDensity, constructor)
-12.1244, -5.1962, -12.1244, -5.1962,
0., 4.6904); 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)); GaussianConditional conditional(1, d, R, noiseModel::Diagonal::Sigmas(s));
GaussianDensity copied(conditional); 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_] // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_]
fg += JacobianFactor(2, 10*eye(2), -1.0*ones(2), unit2); fg += JacobianFactor(2, 10*eye(2), -1.0*ones(2), unit2);
// odometry between x1 and x2: x2-x1=[0.2;-0.1] // 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] // 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] // 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; return fg;
} }
@ -59,9 +59,9 @@ TEST(GaussianFactorGraph, initialization) {
fg += fg +=
JacobianFactor(0, 10*eye(2), -1.0*ones(2), unit2), 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, -10*eye(2),1, 10*eye(2), (Vec(2) << 2.0, -1.0), unit2),
JacobianFactor(0, -5*eye(2), 2, 5*eye(2), Vector_(2, 0.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), Vector_(2, -1.0, 1.5), unit2); JacobianFactor(1, -5*eye(2), 2, 5*eye(2), (Vec(2) << -1.0, 1.5), unit2);
EXPECT_LONGS_EQUAL(4, (long)fg.size()); EXPECT_LONGS_EQUAL(4, (long)fg.size());
@ -106,7 +106,7 @@ TEST(GaussianFactorGraph, sparseJacobian) {
GaussianFactorGraph gfg; GaussianFactorGraph gfg;
SharedDiagonal model = noiseModel::Isotropic::Sigma(2, 0.5); 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); 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_(); Matrix actual = gfg.sparseJacobian_();
@ -125,7 +125,7 @@ TEST(GaussianFactorGraph, matrices) {
GaussianFactorGraph gfg; GaussianFactorGraph gfg;
SharedDiagonal model = noiseModel::Unit::Create(2); 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); 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); 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 // 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] // 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 VectorValues expected = map_list_of
(1, Vector_(2, 5.0,-12.5)) (1, (Vec(2) << 5.0, -12.5))
(2, Vector_(2, 30.0, 5.0)) (2, (Vec(2) << 30.0, 5.0))
(0, Vector_(2,-25.0, 17.5)); (0, (Vec(2) << -25.0, 17.5));
// Check the gradient at delta=0 // Check the gradient at delta=0
VectorValues zero = VectorValues::Zero(expected); VectorValues zero = VectorValues::Zero(expected);
@ -185,15 +185,15 @@ TEST( GaussianFactorGraph, transposeMultiplication )
GaussianFactorGraph A = createSimpleGaussianFactorGraph(); GaussianFactorGraph A = createSimpleGaussianFactorGraph();
Errors e; e += Errors e; e +=
Vector_(2, 0.0, 0.0), (Vec(2) << 0.0, 0.0),
Vector_(2,15.0, 0.0), (Vec(2) << 15.0, 0.0),
Vector_(2, 0.0,-5.0), (Vec(2) << 0.0,-5.0),
Vector_(2,-7.5,-5.0); (Vec(2) << -7.5,-5.0);
VectorValues expected; VectorValues expected;
expected.insert(1, Vector_(2, -37.5,-50.0)); expected.insert(1, (Vec(2) << -37.5,-50.0));
expected.insert(2, Vector_(2,-150.0, 25.0)); expected.insert(2, (Vec(2) << -150.0, 25.0));
expected.insert(0, Vector_(2, 187.5, 25.0)); expected.insert(0, (Vec(2) << 187.5, 25.0));
VectorValues actual = A.transposeMultiply(e); VectorValues actual = A.transposeMultiply(e);
EXPECT(assert_equal(expected, actual)); 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., 1., 0.00, 0., // f4
0., 0., -1., 0., // f2 0., 0., -1., 0., // f2
0., 0., 0.00, -1.), // f2 0., 0., 0.00, -1.), // f2
Vector_(4, -0.2, 0.3, 0.2, -0.1), (Vec(4) << -0.2, 0.3, 0.2, -0.1),
noiseModel::Diagonal::Sigmas(Vector_(4, 0.2, 0.2, 0.1, 0.1))); noiseModel::Diagonal::Sigmas((Vec(4) << 0.2, 0.2, 0.1, 0.1)));
HessianFactor actual(jacobian); HessianFactor actual(jacobian);
VectorValues values = pair_list_of VectorValues values = pair_list_of
(0, Vector_(2, 1.0, 2.0)) (0, (Vec(2) << 1.0, 2.0))
(1, Vector_(4, 3.0, 4.0, 5.0, 6.0)); (1, (Vec(4) << 3.0, 4.0, 5.0, 6.0));
EXPECT_LONGS_EQUAL(2, (long)actual.size()); EXPECT_LONGS_EQUAL(2, (long)actual.size());
EXPECT(assert_equal(expected, actual, 1e-9)); EXPECT(assert_equal(expected, actual, 1e-9));
@ -87,11 +87,11 @@ TEST(HessianFactor, ConversionConstructor)
TEST(HessianFactor, Constructor1) TEST(HessianFactor, Constructor1)
{ {
Matrix G = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0); 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; double f = 10.0;
VectorValues dx = pair_list_of 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); HessianFactor factor(0, G, g, f);
@ -112,7 +112,7 @@ TEST(HessianFactor, Constructor1)
/* ************************************************************************* */ /* ************************************************************************* */
TEST(HessianFactor, Constructor1b) TEST(HessianFactor, Constructor1b)
{ {
Vector mu = Vector_(2,1.0,2.0); Vector mu = (Vec(2) << 1.0,2.0);
Matrix Sigma = eye(2,2); Matrix Sigma = eye(2,2);
HessianFactor factor(0, mu, Sigma); HessianFactor factor(0, mu, Sigma);
@ -134,12 +134,12 @@ TEST(HessianFactor, Constructor2)
Matrix G11 = Matrix_(1,1, 1.0); Matrix G11 = Matrix_(1,1, 1.0);
Matrix G12 = Matrix_(1,2, 2.0, 4.0); Matrix G12 = Matrix_(1,2, 2.0, 4.0);
Matrix G22 = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0); Matrix G22 = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0);
Vector g1 = Vector_(1, -7.0); Vector g1 = (Vec(1) << -7.0);
Vector g2 = Vector_(2, -8.0, -9.0); Vector g2 = (Vec(2) << -8.0, -9.0);
double f = 10.0; double f = 10.0;
Vector dx0 = Vector_(1, 0.5); Vector dx0 = (Vec(1) << 0.5);
Vector dx1 = Vector_(2, 1.5, 2.5); Vector dx1 = (Vec(2) << 1.5, 2.5);
VectorValues dx = pair_list_of VectorValues dx = pair_list_of
(0, dx0) (0, dx0)
@ -165,7 +165,7 @@ TEST(HessianFactor, Constructor2)
VectorValues dxLarge = pair_list_of VectorValues dxLarge = pair_list_of
(0, dx0) (0, dx0)
(1, dx1) (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); 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); 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 g1 = (Vec(1) << -7.0);
Vector g2 = Vector_(2, -8.0, -9.0); Vector g2 = (Vec(2) << -8.0, -9.0);
Vector g3 = Vector_(3, 1.0, 2.0, 3.0); Vector g3 = (Vec(3) << 1.0, 2.0, 3.0);
double f = 10.0; double f = 10.0;
Vector dx0 = Vector_(1, 0.5); Vector dx0 = (Vec(1) << 0.5);
Vector dx1 = Vector_(2, 1.5, 2.5); Vector dx1 = (Vec(2) << 1.5, 2.5);
Vector dx2 = Vector_(3, 1.5, 2.5, 3.5); Vector dx2 = (Vec(3) << 1.5, 2.5, 3.5);
VectorValues dx = pair_list_of VectorValues dx = pair_list_of
(0, dx0) (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); 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 g1 = (Vec(1) << -7.0);
Vector g2 = Vector_(2, -8.0, -9.0); Vector g2 = (Vec(2) << -8.0, -9.0);
Vector g3 = Vector_(3, 1.0, 2.0, 3.0); Vector g3 = (Vec(3) << 1.0, 2.0, 3.0);
double f = 10.0; double f = 10.0;
Vector dx0 = Vector_(1, 0.5); Vector dx0 = (Vec(1) << 0.5);
Vector dx1 = Vector_(2, 1.5, 2.5); Vector dx1 = (Vec(2) << 1.5, 2.5);
Vector dx2 = Vector_(3, 1.5, 2.5, 3.5); Vector dx2 = (Vec(3) << 1.5, 2.5, 3.5);
VectorValues dx = pair_list_of VectorValues dx = pair_list_of
(0, dx0) (0, dx0)
@ -276,8 +276,8 @@ TEST(HessianFactor, CombineAndEliminate)
1.0, 0.0, 0.0, 1.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 1.0); 0.0, 0.0, 1.0);
Vector b0 = Vector_(3, 1.5, 1.5, 1.5); Vector b0 = (Vec(3) << 1.5, 1.5, 1.5);
Vector s0 = Vector_(3, 1.6, 1.6, 1.6); Vector s0 = (Vec(3) << 1.6, 1.6, 1.6);
Matrix A10 = Matrix_(3,3, Matrix A10 = Matrix_(3,3,
2.0, 0.0, 0.0, 2.0, 0.0, 0.0,
@ -287,15 +287,15 @@ TEST(HessianFactor, CombineAndEliminate)
-2.0, 0.0, 0.0, -2.0, 0.0, 0.0,
0.0, -2.0, 0.0, 0.0, -2.0, 0.0,
0.0, 0.0, -2.0); 0.0, 0.0, -2.0);
Vector b1 = Vector_(3, 2.5, 2.5, 2.5); Vector b1 = (Vec(3) << 2.5, 2.5, 2.5);
Vector s1 = Vector_(3, 2.6, 2.6, 2.6); Vector s1 = (Vec(3) << 2.6, 2.6, 2.6);
Matrix A21 = Matrix_(3,3, Matrix A21 = Matrix_(3,3,
3.0, 0.0, 0.0, 3.0, 0.0, 0.0,
0.0, 3.0, 0.0, 0.0, 3.0, 0.0,
0.0, 0.0, 3.0); 0.0, 0.0, 3.0);
Vector b2 = Vector_(3, 3.5, 3.5, 3.5); Vector b2 = (Vec(3) << 3.5, 3.5, 3.5);
Vector s2 = Vector_(3, 3.6, 3.6, 3.6); Vector s2 = (Vec(3) << 3.6, 3.6, 3.6);
GaussianFactorGraph gfg; GaussianFactorGraph gfg;
gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true)); gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
@ -331,7 +331,7 @@ TEST(HessianFactor, eliminate2 )
// sigmas // sigmas
double sigma1 = 0.2; double sigma1 = 0.2;
double sigma2 = 0.1; double sigma2 = 0.1;
Vector sigmas = Vector_(4, sigma1, sigma1, sigma2, sigma2); Vector sigmas = (Vec(4) << sigma1, sigma1, sigma2, sigma2);
// the combined linear factor // the combined linear factor
Matrix Ax2 = Matrix_(4,2, Matrix Ax2 = Matrix_(4,2,
@ -379,7 +379,7 @@ TEST(HessianFactor, eliminate2 )
-0.20, 0.00,-0.80, 0.00, -0.20, 0.00,-0.80, 0.00,
+0.00,-0.20,+0.00,-0.80 +0.00,-0.20,+0.00,-0.80
)/oldSigma; )/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); GaussianConditional expectedCG(0, d, R11, 1, S12);
EXPECT(assert_equal(expectedCG, *actual_Chol.first, 1e-4)); EXPECT(assert_equal(expectedCG, *actual_Chol.first, 1e-4));
@ -390,7 +390,7 @@ TEST(HessianFactor, eliminate2 )
1.00, 0.00, -1.00, 0.00, 1.00, 0.00, -1.00, 0.00,
0.00, 1.00, +0.00, -1.00 0.00, 1.00, +0.00, -1.00
)/sigma; )/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)); JacobianFactor expectedLF(1, Bl1x1, b1, noiseModel::Isotropic::Sigma(2,1.0));
EXPECT(assert_equal(HessianFactor(expectedLF), *actual_Chol.second, 1.5e-3)); EXPECT(assert_equal(HessianFactor(expectedLF), *actual_Chol.second, 1.5e-3));
} }
@ -408,7 +408,7 @@ TEST(HessianFactor, combine) {
Matrix A2 = Matrix_(2, 2, Matrix A2 = Matrix_(2, 2,
-8.94427191, 0.0, -8.94427191, 0.0,
0.0, -8.94427191); 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)); SharedDiagonal model = noiseModel::Diagonal::Sigmas(ones(2));
GaussianFactor::shared_ptr f(new JacobianFactor(0, A0, 1, A1, 2, A2, b, model)); GaussianFactor::shared_ptr f(new JacobianFactor(0, A0, 1, A1, 2, A2, b, model));
GaussianFactorGraph factors = list_of(f); GaussianFactorGraph factors = list_of(f);

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

@ -41,8 +41,8 @@ namespace {
(make_pair(15, 3*Matrix3::Identity())); (make_pair(15, 3*Matrix3::Identity()));
// RHS and sigmas // RHS and sigmas
const Vector b = Vector_(3, 1., 2., 3.); const Vector b = (Vec(3) << 1., 2., 3.);
const SharedDiagonal noise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.5, 0.5, 0.5)); 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); SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1);
Matrix I = eye(2); 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); JacobianFactor lf(1, -I, 2, I, b, sigma0_1);
VectorValues c; VectorValues c;
c.insert(1, Vector_(2,10.,20.)); c.insert(1, (Vec(2) << 10.,20.));
c.insert(2, Vector_(2,30.,60.)); c.insert(2, (Vec(2) << 30.,60.));
// test A*x // test A*x
Vector expectedE = Vector_(2,200.,400.); Vector expectedE = (Vec(2) << 200.,400.);
Vector actualE = lf * c; Vector actualE = lf * c;
EXPECT(assert_equal(expectedE, actualE)); EXPECT(assert_equal(expectedE, actualE));
// test A^e // test A^e
VectorValues expectedX; VectorValues expectedX;
expectedX.insert(1, Vector_(2,-2000.,-4000.)); expectedX.insert(1, (Vec(2) << -2000.,-4000.));
expectedX.insert(2, Vector_(2, 2000., 4000.)); expectedX.insert(2, (Vec(2) << 2000., 4000.));
VectorValues actualX = VectorValues::Zero(expectedX); VectorValues actualX = VectorValues::Zero(expectedX);
lf.transposeMultiplyAdd(1.0, actualE, actualX); lf.transposeMultiplyAdd(1.0, actualE, actualX);
EXPECT(assert_equal(expectedX, actualX)); EXPECT(assert_equal(expectedX, actualX));
@ -283,8 +283,8 @@ TEST(JacobianFactor, eliminate)
1.0, 0.0, 0.0, 1.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 1.0); 0.0, 0.0, 1.0);
Vector b0 = Vector_(3, 1.5, 1.5, 1.5); Vector b0 = (Vec(3) << 1.5, 1.5, 1.5);
Vector s0 = Vector_(3, 1.6, 1.6, 1.6); Vector s0 = (Vec(3) << 1.6, 1.6, 1.6);
Matrix A10 = Matrix_(3,3, Matrix A10 = Matrix_(3,3,
2.0, 0.0, 0.0, 2.0, 0.0, 0.0,
@ -294,15 +294,15 @@ TEST(JacobianFactor, eliminate)
-2.0, 0.0, 0.0, -2.0, 0.0, 0.0,
0.0, -2.0, 0.0, 0.0, -2.0, 0.0,
0.0, 0.0, -2.0); 0.0, 0.0, -2.0);
Vector b1 = Vector_(3, 2.5, 2.5, 2.5); Vector b1 = (Vec(3) << 2.5, 2.5, 2.5);
Vector s1 = Vector_(3, 2.6, 2.6, 2.6); Vector s1 = (Vec(3) << 2.6, 2.6, 2.6);
Matrix A21 = Matrix_(3,3, Matrix A21 = Matrix_(3,3,
3.0, 0.0, 0.0, 3.0, 0.0, 0.0,
0.0, 3.0, 0.0, 0.0, 3.0, 0.0,
0.0, 0.0, 3.0); 0.0, 0.0, 3.0);
Vector b2 = Vector_(3, 3.5, 3.5, 3.5); Vector b2 = (Vec(3) << 3.5, 3.5, 3.5);
Vector s2 = Vector_(3, 3.6, 3.6, 3.6); Vector s2 = (Vec(3) << 3.6, 3.6, 3.6);
GaussianFactorGraph gfg; GaussianFactorGraph gfg;
gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true)); gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
@ -334,7 +334,7 @@ TEST(JacobianFactor, eliminate2 )
// sigmas // sigmas
double sigma1 = 0.2; double sigma1 = 0.2;
double sigma2 = 0.1; double sigma2 = 0.1;
Vector sigmas = Vector_(4, sigma1, sigma1, sigma2, sigma2); Vector sigmas = (Vec(4) << sigma1, sigma1, sigma2, sigma2);
// the combined linear factor // the combined linear factor
Matrix Ax2 = Matrix_(4,2, Matrix Ax2 = Matrix_(4,2,
@ -379,7 +379,7 @@ TEST(JacobianFactor, eliminate2 )
-0.20, 0.00,-0.80, 0.00, -0.20, 0.00,-0.80, 0.00,
+0.00,-0.20,+0.00,-0.80 +0.00,-0.20,+0.00,-0.80
)/oldSigma; )/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); GaussianConditional expectedCG(2, d, R11, 11, S12);
EXPECT(assert_equal(expectedCG, *actual.first, 1e-4)); EXPECT(assert_equal(expectedCG, *actual.first, 1e-4));
@ -391,7 +391,7 @@ TEST(JacobianFactor, eliminate2 )
1.00, 0.00, -1.00, 0.00, 1.00, 0.00, -1.00, 0.00,
0.00, 1.00, +0.00, -1.00 0.00, 1.00, +0.00, -1.00
)/sigma; )/sigma;
Vector b1 = Vector_(2, 0.0, 0.894427); Vector b1 = (Vec(2) << 0.0, 0.894427);
JacobianFactor expectedLF(11, Bl1x1, b1); JacobianFactor expectedLF(11, Bl1x1, b1);
EXPECT(assert_equal(expectedLF, *actual.second,1e-3)); EXPECT(assert_equal(expectedLF, *actual.second,1e-3));
} }
@ -475,7 +475,7 @@ TEST ( JacobianFactor, constraint_eliminate1 )
EXPECT(actual.second->size() == 0); EXPECT(actual.second->size() == 0);
// verify conditional Gaussian // 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)); GaussianConditional expCG(1, v, eye(2), noiseModel::Diagonal::Sigmas(sigmas));
EXPECT(assert_equal(expCG, *actual.first)); EXPECT(assert_equal(expCG, *actual.first));
} }
@ -518,8 +518,8 @@ TEST ( JacobianFactor, constraint_eliminate2 )
Matrix S = Matrix_(2,2, Matrix S = Matrix_(2,2,
1.0, 2.0, 1.0, 2.0,
0.0, 0.0); 0.0, 0.0);
Vector d = Vector_(2, 3.0, 0.6666); Vector d = (Vec(2) << 3.0, 0.6666);
Vector sigmas = Vector_(2, 0.0, 0.0); Vector sigmas = (Vec(2) << 0.0, 0.0);
GaussianConditional expectedCG(1, d, R, 2, S, noiseModel::Diagonal::Sigmas(sigmas)); GaussianConditional expectedCG(1, d, R, 2, S, noiseModel::Diagonal::Sigmas(sigmas));
EXPECT(assert_equal(expectedCG, *actual.first, 1e-4)); 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 */ /** Small 2D point class implemented as a Vector */
struct State: Vector { struct State: Vector {
State(double x, double y) : 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 // Create the controls and measurement properties for our example
Matrix F = eye(2, 2); Matrix F = eye(2, 2);
Matrix B = 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); SharedDiagonal modelQ = noiseModel::Isotropic::Sigma(2, 0.1);
Matrix Q = 0.01*eye(2, 2); Matrix Q = 0.01*eye(2, 2);
Matrix H = eye(2, 2); Matrix H = eye(2, 2);
@ -137,7 +137,7 @@ TEST( KalmanFilter, predict ) {
// Create dynamics model // Create dynamics model
Matrix F = Matrix_(2, 2, 1.0, 0.1, 0.2, 1.1); 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); 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 R = Matrix_(2, 2, 1.0, 0.5, 0.0, 3.0);
Matrix M = trans(R)*R; Matrix M = trans(R)*R;
Matrix Q = inverse(M); Matrix Q = inverse(M);
@ -219,7 +219,7 @@ TEST( KalmanFilter, QRvsCholesky ) {
EXPECT(assert_equal(pa->information(), pb->information(), 1e-7)); EXPECT(assert_equal(pa->information(), pb->information(), 1e-7));
// and in addition attain the correct covariance // 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, pa->mean(), 1e-7));
EXPECT(assert_equal(expectedMean, pb->mean(), 1e-7)); EXPECT(assert_equal(expectedMean, pb->mean(), 1e-7));
Matrix expected = 1e-6*Matrix_(9, 9, 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, 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, -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.); -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 z = (Vec(3) << 0.2599 , 1.3327 , 0.2007);
Vector sigmas = Vector_(3, 0.3323 , 0.2470 , 0.1904); Vector sigmas = (Vec(3) << 0.3323 , 0.2470 , 0.1904);
SharedDiagonal modelR = noiseModel::Diagonal::Sigmas(sigmas); SharedDiagonal modelR = noiseModel::Diagonal::Sigmas(sigmas);
// do update // do update
@ -253,7 +253,7 @@ TEST( KalmanFilter, QRvsCholesky ) {
EXPECT(assert_equal(pa2->information(), pb2->information(), 1e-7)); EXPECT(assert_equal(pa2->information(), pb2->information(), 1e-7));
// and in addition attain the correct mean and covariance // 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, pa2->mean(), 1e-4));// not happy with tolerance here !
EXPECT(assert_equal(expectedMean2, pb2->mean(), 1e-4));// is something still amiss? EXPECT(assert_equal(expectedMean2, pb2->mean(), 1e-4));// is something still amiss?
Matrix expected2 = 1e-6*Matrix_(9, 9, 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) TEST(NoiseModel, constructors)
{ {
Vector whitened = Vector_(3,5.0,10.0,15.0); Vector whitened = (Vec(3) << 5.0,10.0,15.0);
Vector unwhitened = Vector_(3,10.0,20.0,30.0); Vector unwhitened = (Vec(3) << 10.0,20.0,30.0);
// Construct noise models // Construct noise models
vector<Gaussian::shared_ptr> m; vector<Gaussian::shared_ptr> m;
m.push_back(Gaussian::SqrtInformation(R)); m.push_back(Gaussian::SqrtInformation(R));
m.push_back(Gaussian::Covariance(Sigma)); m.push_back(Gaussian::Covariance(Sigma));
//m.push_back(Gaussian::Information(Q)); //m.push_back(Gaussian::Information(Q));
m.push_back(Diagonal::Sigmas(Vector_(3, sigma, sigma, sigma))); m.push_back(Diagonal::Sigmas((Vec(3) << sigma, sigma, sigma)));
m.push_back(Diagonal::Variances(Vector_(3, var, var, var))); m.push_back(Diagonal::Variances((Vec(3) << var, var, var)));
m.push_back(Diagonal::Precisions(Vector_(3, prc, prc, prc))); m.push_back(Diagonal::Precisions((Vec(3) << prc, prc, prc)));
m.push_back(Isotropic::Sigma(3, sigma)); m.push_back(Isotropic::Sigma(3, sigma));
m.push_back(Isotropic::Variance(3, var)); m.push_back(Isotropic::Variance(3, var));
m.push_back(Isotropic::Precision(3, prc)); m.push_back(Isotropic::Precision(3, prc));
@ -104,7 +104,7 @@ TEST(NoiseModel, constructors)
/* ************************************************************************* */ /* ************************************************************************* */
TEST(NoiseModel, Unit) 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)); Gaussian::shared_ptr u(Unit::Create(3));
EXPECT(assert_equal(v,u->whiten(v))); EXPECT(assert_equal(v,u->whiten(v)));
} }
@ -114,8 +114,8 @@ TEST(NoiseModel, equals)
{ {
Gaussian::shared_ptr g1 = Gaussian::SqrtInformation(R), Gaussian::shared_ptr g1 = Gaussian::SqrtInformation(R),
g2 = Gaussian::SqrtInformation(eye(3,3)); g2 = Gaussian::SqrtInformation(eye(3,3));
Diagonal::shared_ptr d1 = Diagonal::Sigmas(Vector_(3, sigma, sigma, sigma)), Diagonal::shared_ptr d1 = Diagonal::Sigmas((Vec(3) << sigma, sigma, sigma)),
d2 = Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3)); d2 = Diagonal::Sigmas((Vec(3) << 0.1, 0.2, 0.3));
Isotropic::shared_ptr i1 = Isotropic::Sigma(3, sigma), Isotropic::shared_ptr i1 = Isotropic::Sigma(3, sigma),
i2 = Isotropic::Sigma(3, 0.7); i2 = Isotropic::Sigma(3, 0.7);
@ -133,7 +133,7 @@ TEST(NoiseModel, equals)
///* ************************************************************************* */ ///* ************************************************************************* */
//TEST(NoiseModel, ConstrainedSmart ) //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); // Diagonal::shared_ptr n1 = boost::dynamic_pointer_cast<Diagonal>(nonconstrained);
// Constrained::shared_ptr n2 = boost::dynamic_pointer_cast<Constrained>(nonconstrained); // Constrained::shared_ptr n2 = boost::dynamic_pointer_cast<Constrained>(nonconstrained);
// EXPECT(n1); // EXPECT(n1);
@ -152,8 +152,8 @@ TEST(NoiseModel, ConstrainedConstructors )
Constrained::shared_ptr actual; Constrained::shared_ptr actual;
size_t d = 3; size_t d = 3;
double m = 100.0; double m = 100.0;
Vector sigmas = Vector_(3, sigma, 0.0, 0.0); Vector sigmas = (Vec(3) << sigma, 0.0, 0.0);
Vector mu = Vector_(3, 200.0, 300.0, 400.0); Vector mu = (Vec(3) << 200.0, 300.0, 400.0);
actual = Constrained::All(d); actual = Constrained::All(d);
EXPECT(assert_equal(gtsam::repeat(d, 1000.0), actual->mu())); EXPECT(assert_equal(gtsam::repeat(d, 1000.0), actual->mu()));
@ -173,12 +173,12 @@ TEST(NoiseModel, ConstrainedConstructors )
/* ************************************************************************* */ /* ************************************************************************* */
TEST(NoiseModel, ConstrainedMixed ) TEST(NoiseModel, ConstrainedMixed )
{ {
Vector feasible = Vector_(3, 1.0, 0.0, 1.0), Vector feasible = (Vec(3) << 1.0, 0.0, 1.0),
infeasible = Vector_(3, 1.0, 1.0, 1.0); infeasible = (Vec(3) << 1.0, 1.0, 1.0);
Diagonal::shared_ptr d = Constrained::MixedSigmas(Vector_(3, sigma, 0.0, sigma)); Diagonal::shared_ptr d = Constrained::MixedSigmas((Vec(3) << sigma, 0.0, sigma));
// NOTE: we catch constrained variables elsewhere, so whitening does nothing // 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((Vec(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, 0.0, 0.5),d->whiten(feasible)));
DOUBLES_EQUAL(1000.0 + 0.25 + 0.25,d->distance(infeasible),1e-9); DOUBLES_EQUAL(1000.0 + 0.25 + 0.25,d->distance(infeasible),1e-9);
DOUBLES_EQUAL(0.5,d->distance(feasible),1e-9); DOUBLES_EQUAL(0.5,d->distance(feasible),1e-9);
@ -187,13 +187,13 @@ TEST(NoiseModel, ConstrainedMixed )
/* ************************************************************************* */ /* ************************************************************************* */
TEST(NoiseModel, ConstrainedAll ) TEST(NoiseModel, ConstrainedAll )
{ {
Vector feasible = Vector_(3, 0.0, 0.0, 0.0), Vector feasible = (Vec(3) << 0.0, 0.0, 0.0),
infeasible = Vector_(3, 1.0, 1.0, 1.0); infeasible = (Vec(3) << 1.0, 1.0, 1.0);
Constrained::shared_ptr i = Constrained::All(3); Constrained::shared_ptr i = Constrained::All(3);
// NOTE: we catch constrained variables elsewhere, so whitening does nothing // 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((Vec(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) << 0.0, 0.0, 0.0),i->whiten(feasible)));
DOUBLES_EQUAL(1000.0 * 3.0,i->distance(infeasible),1e-9); DOUBLES_EQUAL(1000.0 * 3.0,i->distance(infeasible),1e-9);
DOUBLES_EQUAL(0.0,i->distance(feasible),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, 0., -1., 0., 1., 0., 0., 0.3,
1., 0., 0., 0., -1., 0., 0.2, 1., 0., 0., 0., -1., 0., 0.2,
0., 1., 0., 0., 0., -1., -0.1); 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: // the matrix AB yields the following factorized version:
Matrix Rd = Matrix_(4, 6+1, Matrix Rd = Matrix_(4, 6+1,
@ -225,7 +225,7 @@ TEST( NoiseModel, QR )
Matrix Ab2 = exampleQR::Ab; // otherwise overwritten ! Matrix Ab2 = exampleQR::Ab; // otherwise overwritten !
// Expected result // 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); SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas);
// Call Gaussian version // Call Gaussian version
@ -281,7 +281,7 @@ TEST(NoiseModel, ScalarOrVector )
/* ************************************************************************* */ /* ************************************************************************* */
TEST(NoiseModel, WhitenInPlace) 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); SharedDiagonal model = Diagonal::Sigmas(sigmas);
Matrix A = eye(3); Matrix A = eye(3);
model->WhitenInPlace(A); model->WhitenInPlace(A);
@ -305,7 +305,7 @@ TEST(NoiseModel, robustNoise)
{ {
const double k = 10.0, error1 = 1.0, error2 = 100.0; const double k = 10.0, error1 = 1.0, error2 = 100.0;
Matrix A = Matrix_(2, 2, 1.0, 10.0, 100.0, 1000.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( const Robust::shared_ptr robust = Robust::Create(
mEstimator::Huber::Create(k, mEstimator::Huber::Scalar), mEstimator::Huber::Create(k, mEstimator::Huber::Scalar),
Unit::Create(2)); 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 // 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::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::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); 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) { TEST (Serialization, linear_factors) {
VectorValues values; VectorValues values;
values.insert(0, Vector_(1, 1.0)); values.insert(0, (Vec(1) << 1.0));
values.insert(1, Vector_(2, 2.0,3.0)); values.insert(1, (Vec(2) << 2.0,3.0));
values.insert(2, Vector_(2, 4.0,5.0)); values.insert(2, (Vec(2) << 4.0,5.0));
EXPECT(equalsObj<VectorValues>(values)); EXPECT(equalsObj<VectorValues>(values));
EXPECT(equalsXML<VectorValues>(values)); EXPECT(equalsXML<VectorValues>(values));
EXPECT(equalsBinary<VectorValues>(values)); EXPECT(equalsBinary<VectorValues>(values));
@ -144,7 +144,7 @@ TEST (Serialization, linear_factors) {
Index i1 = 4, i2 = 7; Index i1 = 4, i2 = 7;
Matrix A1 = eye(3), A2 = -1.0 * eye(3); Matrix A1 = eye(3), A2 = -1.0 * eye(3);
Vector b = ones(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); JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model);
EXPECT(equalsObj(jacobianfactor)); EXPECT(equalsObj(jacobianfactor));
EXPECT(equalsXML(jacobianfactor)); EXPECT(equalsXML(jacobianfactor));
@ -175,10 +175,10 @@ TEST (Serialization, gaussian_bayes_tree) {
const Ordering chainOrdering = Ordering(list_of(x2)(x1)(x3)(x4)); const Ordering chainOrdering = Ordering(list_of(x2)(x1)(x3)(x4));
const SharedDiagonal chainNoise = noiseModel::Isotropic::Sigma(1, 0.5); const SharedDiagonal chainNoise = noiseModel::Isotropic::Sigma(1, 0.5);
const GaussianFactorGraph chain = list_of 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.), x1, Matrix_(1,1,1.), (Vec(1) << 1.), chainNoise))
(JacobianFactor(x2, Matrix_(1,1,1.), x3, Matrix_(1,1,1.), Vector_(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.), Vector_(1,1.), chainNoise)) (JacobianFactor(x3, Matrix_(1,1,1.), x4, Matrix_(1,1,1.), (Vec(1) << 1.), chainNoise))
(JacobianFactor(x4, Matrix_(1,1,1.), Vector_(1,1.), chainNoise)); (JacobianFactor(x4, Matrix_(1,1,1.), (Vec(1) << 1.), chainNoise));
GaussianBayesTree init = *chain.eliminateMultifrontal(chainOrdering); GaussianBayesTree init = *chain.eliminateMultifrontal(chainOrdering);
GaussianBayesTree expected = *chain.eliminateMultifrontal(chainOrdering); GaussianBayesTree expected = *chain.eliminateMultifrontal(chainOrdering);

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

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