24 using namespace gtsam;
29 static const size_t m = 3,
n = 3;
31 (
Matrix(
m,
n) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0).finished();
36 (
Matrix(
m,
n) << 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 2.0).finished();
43 (
Matrix(
m,
n) << 3.0, 0.0, 0.0, 0.0, 3.0, 0.0, 0.0, 0.0, 3.0).finished();
48 gfg.
add(
X(1), A01, b0, noiseModel::Diagonal::Sigmas(s0,
true));
49 gfg.
add(
X(0),
A10,
X(1),
A11,
b1, noiseModel::Diagonal::Sigmas(s1,
true));
50 gfg.
add(
X(1),
A21,
b2, noiseModel::Diagonal::Sigmas(s2,
true));