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

Fix Matrix_(...) to Mat() <<... in tests

release/4.3a0
Jing Dong 2013-11-13 06:02:35 +00:00
parent 0b3aac7048
commit 05467e4774
6 changed files with 72 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
tests/smallExample.h
View File

@ -297,8 +297,8 @@ GaussianFactorGraph createGaussianFactorGraph() {
*/
GaussianBayesNet createSmallGaussianBayesNet() {
using namespace impl;
Matrix R11 = Matrix_(1, 1, 1.0), S12 = Matrix_(1, 1, 1.0);
Matrix R22 = Matrix_(1, 1, 1.0);
Matrix R11 = (Mat(1, 1) << 1.0), S12 = (Mat(1, 1) << 1.0);
Matrix R22 = (Mat(1, 1) << 1.0);
Vector d1(1), d2(1);
d1(0) = 9;
d2(0) = 5;
@ -323,7 +323,7 @@ Point2 h(const Point2& v) {
}
Matrix H(const Point2& v) {
return Matrix_(2, 2,
return (Mat(2, 2) <<
-sin(v.x()), 0.0,
0.0, cos(v.y()));
}

4
tests/testBoundingConstraint.cpp
View File

@ -129,8 +129,8 @@ TEST( testBoundingConstraint, unary_linearization_active) {
config2.insert(key, pt2);
GaussianFactor::shared_ptr actual1 = constraint1.linearize(config2);
GaussianFactor::shared_ptr actual2 = constraint2.linearize(config2);
JacobianFactor expected1(key, Matrix_(1, 2, 1.0, 0.0), repeat(1, 3.0), hard_model1);
JacobianFactor expected2(key, Matrix_(1, 2, 0.0, 1.0), repeat(1, 5.0), hard_model1);
JacobianFactor expected1(key, (Mat(1, 2) << 1.0, 0.0), repeat(1, 3.0), hard_model1);
JacobianFactor expected2(key, (Mat(1, 2) << 0.0, 1.0), repeat(1, 5.0), hard_model1);
EXPECT(assert_equal((const GaussianFactor&)expected1, *actual1, tol));
EXPECT(assert_equal((const GaussianFactor&)expected2, *actual2, tol));
}

44
tests/testDoglegOptimizer.cpp
View File

@ -48,21 +48,21 @@ TEST(DoglegOptimizer, ComputeBlend) {
// Create an arbitrary Bayes Net
GaussianBayesNet gbn;
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
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)));
0, (Vec(2) << 1.0,2.0), (Mat(2, 2) << 3.0,4.0,0.0,6.0),
3, (Mat(2, 2) << 7.0,8.0,9.0,10.0),
4, (Mat(2, 2) << 11.0,12.0,13.0,14.0)));
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
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)));
1, (Vec(2) << 15.0,16.0), (Mat(2, 2) << 17.0,18.0,0.0,20.0),
2, (Mat(2, 2) << 21.0,22.0,23.0,24.0),
4, (Mat(2, 2) << 25.0,26.0,27.0,28.0)));
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
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)));
2, (Vec(2) << 29.0,30.0), (Mat(2, 2) << 31.0,32.0,0.0,34.0),
3, (Mat(2, 2) << 35.0,36.0,37.0,38.0)));
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
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)));
3, (Vec(2) << 39.0,40.0), (Mat(2, 2) << 41.0,42.0,0.0,44.0),
4, (Mat(2, 2) << 45.0,46.0,47.0,48.0)));
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
4, (Vec(2) << 49.0,50.0), Matrix_(2,2, 51.0,52.0,0.0,54.0)));
4, (Vec(2) << 49.0,50.0), (Mat(2, 2) << 51.0,52.0,0.0,54.0)));
// Compute steepest descent point
VectorValues xu = gbn.optimizeGradientSearch();
@ -84,21 +84,21 @@ TEST(DoglegOptimizer, ComputeDoglegPoint) {
// Create an arbitrary Bayes Net
GaussianBayesNet gbn;
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
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)));
0, (Vec(2) << 1.0,2.0), (Mat(2, 2) << 3.0,4.0,0.0,6.0),
3, (Mat(2, 2) << 7.0,8.0,9.0,10.0),
4, (Mat(2, 2) << 11.0,12.0,13.0,14.0)));
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
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)));
1, (Vec(2) << 15.0,16.0), (Mat(2, 2) << 17.0,18.0,0.0,20.0),
2, (Mat(2, 2) << 21.0,22.0,23.0,24.0),
4, (Mat(2, 2) << 25.0,26.0,27.0,28.0)));
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
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)));
2, (Vec(2) << 29.0,30.0), (Mat(2, 2) << 31.0,32.0,0.0,34.0),
3, (Mat(2, 2) << 35.0,36.0,37.0,38.0)));
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
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)));
3, (Vec(2) << 39.0,40.0), (Mat(2, 2) << 41.0,42.0,0.0,44.0),
4, (Mat(2, 2) << 45.0,46.0,47.0,48.0)));
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
4, (Vec(2) << 49.0,50.0), Matrix_(2,2, 51.0,52.0,0.0,54.0)));
4, (Vec(2) << 49.0,50.0), (Mat(2, 2) << 51.0,52.0,0.0,54.0)));
// Compute dogleg point for different deltas

18
tests/testGaussianBayesTree.cpp
View File

@ -77,7 +77,7 @@ TEST( GaussianBayesTree, linear_smoother_shortcuts )
// Check the conditional P(C3|Root)
double sigma3 = 0.61808;
Matrix A56 = Matrix_(2,2,-0.382022,0.,0.,-0.382022);
Matrix A56 = (Mat(2,2) << -0.382022,0.,0.,-0.382022);
GaussianBayesNet expected3;
expected3 += GaussianConditional(X(5), zero(2), eye(2)/sigma3, X(6), A56/sigma3);
GaussianBayesTree::sharedClique C3 = bayesTree[X(4)];
@ -86,7 +86,7 @@ TEST( GaussianBayesTree, linear_smoother_shortcuts )
// Check the conditional P(C4|Root)
double sigma4 = 0.661968;
Matrix A46 = Matrix_(2,2,-0.146067,0.,0.,-0.146067);
Matrix A46 = (Mat(2,2) << -0.146067,0.,0.,-0.146067);
GaussianBayesNet expected4;
expected4 += GaussianConditional(X(4), zero(2), eye(2)/sigma4, X(6), A46/sigma4);
GaussianBayesTree::sharedClique C4 = bayesTree[X(3)];
@ -296,13 +296,13 @@ TEST(GaussianBayesTree, shortcut_overlapping_separator)
// f(6,7)
GaussianFactorGraph fg;
noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1);
fg.add(1, Matrix_(1,1, 1.0), 3, Matrix_(1,1, 2.0), 5, Matrix_(1,1, 3.0), (Vec(1) << 4.0), model);
fg.add(1, Matrix_(1,1, 5.0), (Vec(1) << 6.0), model);
fg.add(2, Matrix_(1,1, 7.0), 4, Matrix_(1,1, 8.0), 5, Matrix_(1,1, 9.0), (Vec(1) << 10.0), model);
fg.add(2, Matrix_(1,1, 11.0), (Vec(1) << 12.0), model);
fg.add(5, Matrix_(1,1, 13.0), 6, Matrix_(1,1, 14.0), (Vec(1) << 15.0), model);
fg.add(6, Matrix_(1,1, 17.0), 7, Matrix_(1,1, 18.0), (Vec(1) << 19.0), model);
fg.add(7, Matrix_(1,1, 20.0), (Vec(1) << 21.0), model);
fg.add(1, (Mat(1, 1) << 1.0), 3, (Mat(1, 1) << 2.0), 5, (Mat(1, 1) << 3.0), (Vec(1) << 4.0), model);
fg.add(1, (Mat(1, 1) << 5.0), (Vec(1) << 6.0), model);
fg.add(2, (Mat(1, 1) << 7.0), 4, (Mat(1, 1) << 8.0), 5, (Mat(1, 1) << 9.0), (Vec(1) << 10.0), model);
fg.add(2, (Mat(1, 1) << 11.0), (Vec(1) << 12.0), model);
fg.add(5, (Mat(1, 1) << 13.0), 6, (Mat(1, 1) << 14.0), (Vec(1) << 15.0), model);
fg.add(6, (Mat(1, 1) << 17.0), 7, (Mat(1, 1) << 18.0), (Vec(1) << 19.0), model);
fg.add(7, (Mat(1, 1) << 20.0), (Vec(1) << 21.0), model);
// Eliminate into BayesTree
// c(6,7)

8
tests/testGaussianFactorGraphB.cpp
View File

@ -134,12 +134,12 @@ TEST( GaussianFactorGraph, eliminateOne_x1_fast )
GaussianConditional expected(ordering[X(1)],15*d,R11,ordering[L(1)],S12,ordering[X(2)],S13,sigma);
// Create expected remaining new factor
JacobianFactor expectedFactor(1, Matrix_(4,2,
JacobianFactor expectedFactor(1, (Mat(4,2) <<
4.714045207910318, 0.,
0., 4.714045207910318,
0., 0.,
0., 0.),
2, Matrix_(4,2,
2, (Mat(4,2) <<
-2.357022603955159, 0.,
0., -2.357022603955159,
7.071067811865475, 0.,
@ -405,10 +405,10 @@ TEST( GaussianFactorGraph, elimination )
// Check matrix
Matrix R;Vector d;
boost::tie(R,d) = matrix(bayesNet);
Matrix expected = Matrix_(2,2,
Matrix expected = (Mat(2, 2) <<
0.707107, -0.353553,
0.0, 0.612372);
Matrix expected2 = Matrix_(2,2,
Matrix expected2 = (Mat(2, 2) <<
0.707107, -0.353553,
0.0, -0.612372);
EXPECT(equal_with_abs_tol(expected, R, 1e-6) || equal_with_abs_tol(expected2, R, 1e-6));

64
tests/testNonlinearFactor.cpp
View File

@ -209,7 +209,7 @@ TEST( NonlinearFactor, linearize_constraint1 )
// create expected
Vector b = (Vec(2) << 0., -3.);
JacobianFactor expected(X(1), Matrix_(2,2, 5.0, 0.0, 0.0, 1.0), b,
JacobianFactor expected(X(1), (Mat(2, 2) << 5.0, 0.0, 0.0, 1.0), b,
noiseModel::Constrained::MixedSigmas((Vec(2) << 1.0, 0.0)));
CHECK(assert_equal((const GaussianFactor&)expected, *actual));
}
@ -229,7 +229,7 @@ TEST( NonlinearFactor, linearize_constraint2 )
GaussianFactor::shared_ptr actual = f0.linearize(config);
// create expected
Matrix A = Matrix_(2,2, 5.0, 0.0, 0.0, 1.0);
Matrix A = (Mat(2, 2) << 5.0, 0.0, 0.0, 1.0);
Vector b = (Vec(2) << -15., -3.);
JacobianFactor expected(X(1), -1*A, L(1), A, b,
noiseModel::Constrained::MixedSigmas((Vec(2) << 1.0, 0.0)));
@ -249,10 +249,10 @@ public:
boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none) const {
if(H1) {
*H1 = Matrix_(1,1, 1.0);
*H2 = Matrix_(1,1, 2.0);
*H3 = Matrix_(1,1, 3.0);
*H4 = Matrix_(1,1, 4.0);
*H1 = (Mat(1, 1) << 1.0);
*H2 = (Mat(1, 1) << 2.0);
*H3 = (Mat(1, 1) << 3.0);
*H4 = (Mat(1, 1) << 4.0);
}
return (Vector(1) << x1 + x2 + x3 + x4).finished();
}
@ -277,10 +277,10 @@ TEST(NonlinearFactor, NoiseModelFactor4) {
LONGS_EQUAL((long)X(2), (long)jf.keys()[1]);
LONGS_EQUAL((long)X(3), (long)jf.keys()[2]);
LONGS_EQUAL((long)X(4), (long)jf.keys()[3]);
EXPECT(assert_equal(Matrix_(1,1, 0.5), jf.getA(jf.begin())));
EXPECT(assert_equal(Matrix_(1,1, 1.0), jf.getA(jf.begin()+1)));
EXPECT(assert_equal(Matrix_(1,1, 1.5), jf.getA(jf.begin()+2)));
EXPECT(assert_equal(Matrix_(1,1, 2.0), jf.getA(jf.begin()+3)));
EXPECT(assert_equal((Matrix)(Mat(1, 1) << 0.5), jf.getA(jf.begin())));
EXPECT(assert_equal((Matrix)(Mat(1, 1) << 1.0), jf.getA(jf.begin()+1)));
EXPECT(assert_equal((Matrix)(Mat(1, 1) << 1.5), jf.getA(jf.begin()+2)));
EXPECT(assert_equal((Matrix)(Mat(1, 1) << 2.0), jf.getA(jf.begin()+3)));
EXPECT(assert_equal((Vector)(Vec(1) << -5.0), jf.getb()));
}
@ -298,11 +298,11 @@ public:
boost::optional<Matrix&> H4 = boost::none,
boost::optional<Matrix&> H5 = boost::none) const {
if(H1) {
*H1 = Matrix_(1,1, 1.0);
*H2 = Matrix_(1,1, 2.0);
*H3 = Matrix_(1,1, 3.0);
*H4 = Matrix_(1,1, 4.0);
*H5 = Matrix_(1,1, 5.0);
*H1 = (Mat(1, 1) << 1.0);
*H2 = (Mat(1, 1) << 2.0);
*H3 = (Mat(1, 1) << 3.0);
*H4 = (Mat(1, 1) << 4.0);
*H5 = (Mat(1, 1) << 5.0);
}
return (Vector(1) << x1 + x2 + x3 + x4 + x5).finished();
}
@ -325,11 +325,11 @@ TEST(NonlinearFactor, NoiseModelFactor5) {
LONGS_EQUAL((long)X(3), (long)jf.keys()[2]);
LONGS_EQUAL((long)X(4), (long)jf.keys()[3]);
LONGS_EQUAL((long)X(5), (long)jf.keys()[4]);
EXPECT(assert_equal(Matrix_(1,1, 0.5), jf.getA(jf.begin())));
EXPECT(assert_equal(Matrix_(1,1, 1.0), jf.getA(jf.begin()+1)));
EXPECT(assert_equal(Matrix_(1,1, 1.5), jf.getA(jf.begin()+2)));
EXPECT(assert_equal(Matrix_(1,1, 2.0), jf.getA(jf.begin()+3)));
EXPECT(assert_equal(Matrix_(1,1, 2.5), jf.getA(jf.begin()+4)));
EXPECT(assert_equal((Matrix)(Mat(1, 1) << 0.5), jf.getA(jf.begin())));
EXPECT(assert_equal((Matrix)(Mat(1, 1) << 1.0), jf.getA(jf.begin()+1)));
EXPECT(assert_equal((Matrix)(Mat(1, 1) << 1.5), jf.getA(jf.begin()+2)));
EXPECT(assert_equal((Matrix)(Mat(1, 1) << 2.0), jf.getA(jf.begin()+3)));
EXPECT(assert_equal((Matrix)(Mat(1, 1) << 2.5), jf.getA(jf.begin()+4)));
EXPECT(assert_equal((Vector)(Vec(1) << -7.5), jf.getb()));
}
@ -348,12 +348,12 @@ public:
boost::optional<Matrix&> H5 = boost::none,
boost::optional<Matrix&> H6 = boost::none) const {
if(H1) {
*H1 = Matrix_(1,1, 1.0);
*H2 = Matrix_(1,1, 2.0);
*H3 = Matrix_(1,1, 3.0);
*H4 = Matrix_(1,1, 4.0);
*H5 = Matrix_(1,1, 5.0);
*H6 = Matrix_(1,1, 6.0);
*H1 = (Mat(1, 1) << 1.0);
*H2 = (Mat(1, 1) << 2.0);
*H3 = (Mat(1, 1) << 3.0);
*H4 = (Mat(1, 1) << 4.0);
*H5 = (Mat(1, 1) << 5.0);
*H6 = (Mat(1, 1) << 6.0);
}
return (Vector(1) << x1 + x2 + x3 + x4 + x5 + x6).finished();
}
@ -379,12 +379,12 @@ TEST(NonlinearFactor, NoiseModelFactor6) {
LONGS_EQUAL((long)X(4), (long)jf.keys()[3]);
LONGS_EQUAL((long)X(5), (long)jf.keys()[4]);
LONGS_EQUAL((long)X(6), (long)jf.keys()[5]);
EXPECT(assert_equal(Matrix_(1,1, 0.5), jf.getA(jf.begin())));
EXPECT(assert_equal(Matrix_(1,1, 1.0), jf.getA(jf.begin()+1)));
EXPECT(assert_equal(Matrix_(1,1, 1.5), jf.getA(jf.begin()+2)));
EXPECT(assert_equal(Matrix_(1,1, 2.0), jf.getA(jf.begin()+3)));
EXPECT(assert_equal(Matrix_(1,1, 2.5), jf.getA(jf.begin()+4)));
EXPECT(assert_equal(Matrix_(1,1, 3.0), jf.getA(jf.begin()+5)));
EXPECT(assert_equal((Matrix)(Mat(1, 1) << 0.5), jf.getA(jf.begin())));
EXPECT(assert_equal((Matrix)(Mat(1, 1) << 1.0), jf.getA(jf.begin()+1)));
EXPECT(assert_equal((Matrix)(Mat(1, 1) << 1.5), jf.getA(jf.begin()+2)));
EXPECT(assert_equal((Matrix)(Mat(1, 1) << 2.0), jf.getA(jf.begin()+3)));
EXPECT(assert_equal((Matrix)(Mat(1, 1) << 2.5), jf.getA(jf.begin()+4)));
EXPECT(assert_equal((Matrix)(Mat(1, 1) << 3.0), jf.getA(jf.begin()+5)));
EXPECT(assert_equal((Vector)(Vec(1) << -10.5), jf.getb()));
}