32 using namespace std::placeholders;
34 using namespace gtsam;
40 std::make_shared<GaussianConditional>(
_x_, Vector1::Constant(9), I_1x1,
_y_, I_1x1),
41 std::make_shared<GaussianConditional>(
_y_, Vector1::Constant(5), I_1x1)};
44 std::make_shared<GaussianConditional>(
_x_, Vector1::Constant(9), I_1x1,
_y_, I_1x1,
45 noiseModel::Isotropic::Sigma(1, 2.0)),
46 std::make_shared<GaussianConditional>(
_y_, Vector1::Constant(5), I_1x1,
47 noiseModel::Isotropic::Sigma(1, 3.0))};
116 {
_y_, Vector1::Constant(5)}};
136 _x_, Vector1::Constant(9), I_1x1,
_y_, I_1x1);
143 {
_y_, Vector1::Constant(5)}};
157 {
_y_, Vector1::Constant(5)} };
171 GaussianConditional::sharedMeanAndStddev(
X(0),
A1,
X(1),
b,
sigma),
172 GaussianDensity::sharedMeanAndStddev(
X(1),
mean,
sigma)};
185 std::mt19937_64
rng(4242);
200 constexpr
size_t N = 1000;
202 for (
size_t i = 0;
i <
N;
i++) {
204 sum +=
pow(X_i[
_y_].
x() - 5.0, 2.0);
228 for (
const auto&
keys :
245 {
_y_, Vector1::Constant(5)}},
246 expected{{
_x_, Vector1::Constant(2)}, {
_y_, Vector1::Constant(3)}};
264 expected{{
_x_, Vector1::Constant(4)}, {
_y_, Vector1::Constant(9)}};
281 0,
Vector2(3.0, 4.0), (Matrix2() << 1.0, 3.0, 0.0, 4.0).finished(),
282 1, (Matrix2() << 2.0, 1.0, 2.0, 3.0).finished(), noiseModel::Isotropic::Sigma(2, 2.0));
285 1,
Vector2(5.0, 6.0), (Matrix2() << 1.0, 1.0, 0.0, 3.0).finished(),
286 2, (Matrix2() << 1.0, 0.0, 5.0, 2.0).finished(), noiseModel::Isotropic::Sigma(2, 2.0));
289 3,
Vector2(7.0, 8.0), (Matrix2() << 1.0, 1.0, 0.0, 5.0).finished(), noiseModel::Isotropic::Sigma(2, 2.0));
291 double expectedDeterminant = 60.0 / 64.0;
302 return 0.5 * (
Rd.first *
values -
Rd.second).squaredNorm();
312 0,
Vector2(1.0,2.0), (Matrix2() << 3.0,4.0,0.0,6.0).finished(),
313 3, (Matrix2() << 7.0,8.0,9.0,10.0).finished(),
314 4, (Matrix2() << 11.0,12.0,13.0,14.0).finished());
316 1,
Vector2(15.0,16.0), (Matrix2() << 17.0,18.0,0.0,20.0).finished(),
317 2, (Matrix2() << 21.0,22.0,23.0,24.0).finished(),
318 4, (Matrix2() << 25.0,26.0,27.0,28.0).finished());
320 2,
Vector2(29.0,30.0), (Matrix2() << 31.0,32.0,0.0,34.0).finished(),
321 3, (Matrix2() << 35.0,36.0,37.0,38.0).finished());
323 3,
Vector2(39.0,40.0), (Matrix2() << 41.0,42.0,0.0,44.0).finished(),
324 4, (Matrix2() << 45.0,46.0,47.0,48.0).finished());
326 4,
Vector2(49.0,50.0), (Matrix2() << 51.0,52.0,0.0,54.0).finished());
329 Matrix hessian = numericalHessian<Vector10>(
330 std::bind(&computeError,
gbn, std::placeholders::_1), Vector10::Zero());
333 Vector gradient = numericalGradient<Vector10>(
334 std::bind(&computeError,
gbn, std::placeholders::_1), Vector10::Zero());
339 Vector denseMatrixGradient = -augmentedHessian.col(10).segment(0,10);
343 double step = -gradient.squaredNorm() / (gradient.transpose() * hessian * gradient)(0);
357 EXPECT(newError < origError);
376 " var11[label=\"11\", pos=\"10,20!\"];\n"
377 " var22[label=\"22\", pos=\"50,20!\"];\n"