29 using namespace std::placeholders;
31 using namespace gtsam;
33 using Pairs = std::vector<std::pair<Key, Matrix>>;
37 const SharedDiagonal chainNoise = noiseModel::Isotropic::Sigma(1, 0.5);
39 std::make_shared<JacobianFactor>(
40 x2, I_1x1,
x1, I_1x1, (
Vector(1) << 1.).finished(), chainNoise),
41 std::make_shared<JacobianFactor>(
42 x2, I_1x1,
x3, I_1x1, (
Vector(1) << 1.).finished(), chainNoise),
43 std::make_shared<JacobianFactor>(
44 x3, I_1x1,
x4, I_1x1, (
Vector(1) << 1.).finished(), chainNoise),
45 std::make_shared<JacobianFactor>(
46 x4, I_1x1, (
Vector(1) << 1.).finished(), chainNoise)};
53 return std::make_shared<GaussianBayesTreeClique>(
54 std::make_shared<GaussianConditional>(conditional));
57 typedef std::vector<GaussianBayesTreeClique::shared_ptr> Children;
62 auto clique = LeafClique(conditional);
63 clique->children.assign(children.begin(), children.end());
64 for(Children::const_iterator child = children.begin(); child != children.end(); ++child)
65 (*child)->parent_ = clique;
99 std::map<Key, Matrix>{
100 {
x3, (Matrix21() << 2., 0.).finished()},
101 {
x4, (Matrix21() << 2., 2.).finished()},
106 {
x2, (Matrix21() << -2. *
sqrt(2.), 0.).finished()},
107 {
x1, (Matrix21() << -
sqrt(2.), -
sqrt(2.)).finished()},
108 {
x3, (Matrix21() << -
sqrt(2.),
sqrt(2.)).finished()},
110 2, (
Vector(2) << -2. *
sqrt(2.), 0.).finished());
113 bayesTree_expected.
insertRoot(MakeClique(gc1, Children{LeafClique(gc2)}));
124 {
x4, (
Vector(1) << 1.).finished()}};
126 VectorValues actual = chain.eliminateMultifrontal(chainOrdering)->optimize();
134 Pairs{{11, (
Matrix(3, 1) << 0.0971, 0, 0).finished()},
135 {12, (
Matrix(3, 2) << 0.3171, 0.4387, 0.9502, 0.3816, 0, 0.7655)
137 2,
Vector3(0.2638, 0.1455, 0.1361));
140 {9, (
Matrix(3, 1) << 0.7952, 0, 0).finished()},
141 {10, (
Matrix(3, 2) << 0.4456, 0.7547, 0.6463, 0.2760, 0, 0.6797)
143 {11, (
Matrix(3, 1) << 0.6551, 0.1626, 0.1190).finished()},
144 {12, (
Matrix(3, 2) << 0.4984, 0.5853, 0.9597, 0.2238, 0.3404, 0.7513)
146 2,
Vector3(0.4314, 0.9106, 0.1818));
148 Pairs{{7, (
Matrix(3, 1) << 0.2551, 0, 0).finished()},
149 {8, (
Matrix(3, 2) << 0.8909, 0.1386, 0.9593, 0.1493, 0, 0.2575)
151 {11, (
Matrix(3, 1) << 0.8407, 0.2543, 0.8143).finished()}},
152 2,
Vector3(0.3998, 0.2599, 0.8001));
154 Pairs{{5, (
Matrix(3, 1) << 0.2435, 0, 0).finished()},
155 {6, (
Matrix(3, 2) << 0.4733, 0.1966, 0.3517, 0.2511, 0.8308, 0.0)
162 {7, (
Matrix(3, 1) << 0.5853, 0.5497, 0.9172).finished()},
163 {8, (
Matrix(3, 2) << 0.2858, 0.3804, 0.7572, 0.5678, 0.7537, 0.0759)
165 2,
Vector3(0.8173, 0.8687, 0.0844));
167 Pairs{{3, (
Matrix(3, 1) << 0.0540, 0, 0).finished()},
168 {4, (
Matrix(3, 2) << 0.9340, 0.4694, 0.1299, 0.0119, 0, 0.3371)
170 {6, (
Matrix(3, 2) << 0.1622, 0.5285, 0.7943, 0.1656, 0.3112, 0.6020)
172 2,
Vector3(0.9619, 0.0046, 0.7749));
174 Pairs{{1, (
Matrix(3, 1) << 0.2630, 0, 0).finished()},
175 {2, (
Matrix(3, 2) << 0.7482, 0.2290, 0.4505, 0.9133, 0, 0.1524)
177 {5, (
Matrix(3, 1) << 0.8258, 0.5383, 0.9961).finished()}},
178 2,
Vector3(0.0782, 0.4427, 0.1067));
183 gc1, Children{LeafClique(gc2),
184 MakeClique(gc3, Children{MakeClique(
185 gc4, Children{LeafClique(gc5),
186 LeafClique(gc6)})})}));
189 Matrix expectedCov = (
Matrix(1,1) << 236.5166).finished();
197 Matrix actualCov = (actualA.transpose() * actualA).
inverse();
204 expectedCov = (
Matrix(2,2) <<
206 2886.2, 8471.2).finished();
208 actualJacobianQR = *bt.marginalFactor(6,
EliminateQR);
213 actualA = actualJacobianQR.
getA(actualJacobianQR.
begin());
214 actualCov = (actualA.transpose() * actualA).
inverse();
222 return 0.5 * (
Rd.first *
values -
Rd.second).squaredNorm();
229 Pairs{{2, (
Matrix(6, 2) << 31.0, 32.0, 0.0, 34.0, 0.0, 0.0, 0.0, 0.0, 0.0,
232 {3, (
Matrix(6, 2) << 35.0, 36.0, 37.0, 38.0, 41.0, 42.0, 0.0, 44.0,
235 {4, (
Matrix(6, 2) << 0.0, 0.0, 0.0, 0.0, 45.0, 46.0, 47.0, 48.0,
236 51.0, 52.0, 0.0, 54.0)
238 3, (
Vector(6) << 29.0, 30.0, 39.0, 40.0, 49.0, 50.0).finished()),
239 gc2(
Pairs{{0, (
Matrix(4, 2) << 3.0, 4.0, 0.0, 6.0, 0.0, 0.0, 0.0, 0.0)
241 {1, (
Matrix(4, 2) << 0.0, 0.0, 0.0, 0.0, 17.0, 18.0, 0.0, 20.0)
243 {2, (
Matrix(4, 2) << 0.0, 0.0, 0.0, 0.0, 21.0, 22.0, 23.0, 24.0)
245 {3, (
Matrix(4, 2) << 7.0, 8.0, 9.0, 10.0, 0.0, 0.0, 0.0, 0.0)
247 {4, (
Matrix(4, 2) << 11.0, 12.0, 13.0, 14.0, 25.0, 26.0, 27.0,
250 2, (
Vector(4) << 1.0, 2.0, 15.0, 16.0).finished());
254 bt.
insertRoot(MakeClique(gc1, Children{LeafClique(gc2)}));
257 Matrix hessian = numericalHessian<Vector10>(
258 std::bind(&computeError, bt, std::placeholders::_1), Vector10::Zero());
261 Vector gradient = numericalGradient<Vector10>(
262 std::bind(&computeError, bt, std::placeholders::_1), Vector10::Zero());
267 Vector denseMatrixGradient = -augmentedHessian.col(10).segment(0,10);
271 double step = -gradient.squaredNorm() / (gradient.transpose() * hessian * gradient)(0);
276 {1,
Vector2(0.0109679, 0.0253767)},
277 {2,
Vector2(0.0680441, 0.114496)},
278 {3,
Vector2(0.16125, 0.241294)},
279 {4,
Vector2(0.300134, 0.423233)}};
292 EXPECT(newError < origError);
314 double expectedDeterminant =
sqrt(
H.determinant());
315 double actualDeterminant = bt->determinant();