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

roll back some cosmetic changes to minimize the diff

release/4.3a0
Gerry Chen 2021-01-19 11:01:25 -05:00
parent d8491b27fb
commit 25e3b5609e
1 changed files with 26 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -82,6 +82,29 @@ TEST(GaussianFactorGraph, sparseJacobian) {
// 5 6 7 0 0 8 // 5 6 7 0 0 8
// 9 10 0 11 12 13 // 9 10 0 11 12 13
// 0 0 0 14 15 16 // 0 0 0 14 15 16
// Expected
Matrix expected = (Matrix(16, 3) <<
1., 1., 2.,
1., 2., 4.,
1., 3., 6.,
2., 1.,10.,
2., 2.,12.,
2., 3.,14.,
1., 6., 8.,
2., 6.,16.,
3., 1.,18.,
3., 2.,20.,
3., 4.,22.,
3., 5.,24.,
4., 4.,28.,
4., 5.,30.,
3., 6.,26.,
4., 6.,32.).finished();
// expected: in matlab format - NOTE the transpose!)
Matrix expectedMatlab = expected.transpose();
GaussianFactorGraph gfg; GaussianFactorGraph gfg;
SharedDiagonal model = noiseModel::Isotropic::Sigma(2, 0.5); SharedDiagonal model = noiseModel::Isotropic::Sigma(2, 0.5);
const Key x123 = 0, x45 = 1; const Key x123 = 0, x45 = 1;
@ -91,28 +114,9 @@ TEST(GaussianFactorGraph, sparseJacobian) {
x45, (Matrix(2, 2) << 11, 12, 14, 15.).finished(), x45, (Matrix(2, 2) << 11, 12, 14, 15.).finished(),
Vector2(13, 16), model); Vector2(13, 16), model);
// Check version for MATLAB - NOTE that we transpose this! Matrix actual = gfg.sparseJacobian_();
Matrix expectedT = (Matrix(16, 3) <<
1, 1, 2.,
1, 2, 4.,
1, 3, 6.,
2, 1, 10.,
2, 2, 12.,
2, 3, 14.,
1, 6, 8.,
2, 6, 16.,
3, 1, 18.,
3, 2, 20.,
3, 4, 22.,
3, 5, 24.,
4, 4, 28.,
4, 5, 30.,
3, 6, 26.,
4, 6, 32.).finished();
// matrix form (matlab) EXPECT(assert_equal(expected, actual));
Matrix expectedMatlab = expectedT.transpose();
EXPECT(assert_equal(expectedMatlab, gfg.sparseJacobian_()));
// BoostTriplets // BoostTriplets
auto boostActual = gfg.sparseJacobian(); auto boostActual = gfg.sparseJacobian();
@ -121,7 +125,7 @@ TEST(GaussianFactorGraph, sparseJacobian) {
// check content // check content
for (int i = 0; i < 16; i++) { for (int i = 0; i < 16; i++) {
EXPECT(triplet_equal( EXPECT(triplet_equal(
BoostTriplet(expectedT(i, 0) - 1, expectedT(i, 1) - 1, expectedT(i, 2)), BoostTriplet(expected(i, 0) - 1, expected(i, 1) - 1, expected(i, 2)),
boostActual.at(i))); boostActual.at(i)));
} }
} }