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

merge Mandy + Fan's sparseJacobian unit test additions

release/4.3a0
Gerry Chen 2021-01-18 20:20:05 -05:00
parent 56e9b3ac9f
commit a477ec6811
1 changed files with 50 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -36,9 +36,18 @@ using namespace boost::assign;
using namespace std;
using namespace gtsam;
// static SharedDiagonal
// sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1), sigma_02 = noiseModel::Isotropic::Sigma(2,0.2),
// constraintModel = noiseModel::Constrained::All(2);
typedef boost::tuple<size_t, size_t, double> BoostTriplet;
bool triplet_equal(BoostTriplet a, BoostTriplet b) {
if (a.get<0>() == b.get<0>() && a.get<1>() == b.get<1>() &&
a.get<2>() == b.get<2>()) return true;
cout << "not equal:" << endl;
cout << "\texpected: "
"(" << a.get<0>() << ", " << a.get<1>() << ") = " << a.get<2>() << endl;
cout << "\tactual: "
"(" << b.get<0>() << ", " << b.get<1>() << ") = " << b.get<2>() << endl;
return false;
}
/* ************************************************************************* */
TEST(GaussianFactorGraph, initialization) {
@ -73,37 +82,48 @@ TEST(GaussianFactorGraph, sparseJacobian) {
// 5 6 7 0 0 8
// 9 10 0 11 12 13
// 0 0 0 14 15 16
// Expected - NOTE that we transpose this!
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 expected = expectedT.transpose();
GaussianFactorGraph gfg;
SharedDiagonal model = noiseModel::Isotropic::Sigma(2, 0.5);
gfg.add(0, (Matrix(2, 3) << 1., 2., 3., 5., 6., 7.).finished(), Vector2(4., 8.), model);
gfg.add(0, (Matrix(2, 3) << 9., 10., 0., 0., 0., 0.).finished(), 1,
(Matrix(2, 2) << 11., 12., 14., 15.).finished(), Vector2(13., 16.), model);
const Key x123 = 0, x45 = 1;
gfg.add(x123, (Matrix(2, 3) << 1, 2, 3, 5, 6, 7).finished(),
Vector2(4, 8), model);
gfg.add(x123, (Matrix(2, 3) << 9, 10, 0, 0, 0, 0).finished(),
x45, (Matrix(2, 2) << 11, 12, 14, 15.).finished(),
Vector2(13, 16), model);
Matrix actual = gfg.sparseJacobian_();
// Check version for MATLAB - NOTE that we transpose this!
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();
EXPECT(assert_equal(expected, actual));
// matrix form (matlab)
Matrix expectedMatlab = expectedT.transpose();
EXPECT(assert_equal(expectedMatlab, gfg.sparseJacobian_()));
// BoostTriplets
auto boostActual = gfg.sparseJacobian();
// check the triplets size...
EXPECT_LONGS_EQUAL(16, boostActual.size());
// check content
for (int i = 0; i < 16; i++) {
EXPECT(triplet_equal(
BoostTriplet(expectedT(i, 0) - 1, expectedT(i, 1) - 1, expectedT(i, 2)),
boostActual.at(i)));
}
}
/* ************************************************************************* */