35 using namespace gtsam;
46 0,
Vector2(1.0,2.0), (
Matrix(2, 2) << 3.0,4.0,0.0,6.0).finished(),
47 3, (
Matrix(2, 2) << 7.0,8.0,9.0,10.0).finished(),
48 4, (
Matrix(2, 2) << 11.0,12.0,13.0,14.0).finished());
50 1,
Vector2(15.0,16.0), (
Matrix(2, 2) << 17.0,18.0,0.0,20.0).finished(),
51 2, (
Matrix(2, 2) << 21.0,22.0,23.0,24.0).finished(),
52 4, (
Matrix(2, 2) << 25.0,26.0,27.0,28.0).finished());
54 2,
Vector2(29.0,30.0), (
Matrix(2, 2) << 31.0,32.0,0.0,34.0).finished(),
55 3, (
Matrix(2, 2) << 35.0,36.0,37.0,38.0).finished());
57 3,
Vector2(39.0,40.0), (
Matrix(2, 2) << 41.0,42.0,0.0,44.0).finished(),
58 4, (
Matrix(2, 2) << 45.0,46.0,47.0,48.0).finished());
60 4,
Vector2(49.0,50.0), (
Matrix(2, 2) << 51.0,52.0,0.0,54.0).finished());
73 VectorValues xb = DoglegOptimizerImpl::ComputeBlend(Delta, xu, xn);
82 0,
Vector2(1.0,2.0), (
Matrix(2, 2) << 3.0,4.0,0.0,6.0).finished(),
83 3, (
Matrix(2, 2) << 7.0,8.0,9.0,10.0).finished(),
84 4, (
Matrix(2, 2) << 11.0,12.0,13.0,14.0).finished());
86 1,
Vector2(15.0,16.0), (
Matrix(2, 2) << 17.0,18.0,0.0,20.0).finished(),
87 2, (
Matrix(2, 2) << 21.0,22.0,23.0,24.0).finished(),
88 4, (
Matrix(2, 2) << 25.0,26.0,27.0,28.0).finished());
90 2,
Vector2(29.0,30.0), (
Matrix(2, 2) << 31.0,32.0,0.0,34.0).finished(),
91 3, (
Matrix(2, 2) << 35.0,36.0,37.0,38.0).finished());
93 3,
Vector2(39.0,40.0), (
Matrix(2, 2) << 41.0,42.0,0.0,44.0).finished(),
94 4, (
Matrix(2, 2) << 45.0,46.0,47.0,48.0).finished());
96 4,
Vector2(49.0,50.0), (
Matrix(2, 2) << 51.0,52.0,0.0,54.0).finished());
127 for(
size_t it=0; it<10; ++it) {
131 double nonlinearError = fg.
error(config);
132 double linearError = linearized->error(config.
zeroVectors());
135 auto gbn = linearized->eliminateSequential();
139 Delta, DoglegOptimizerImpl::SEARCH_EACH_ITERATION, dx_u, dx_n, *
gbn, fg,
140 config, fg.
error(config));
156 auto model = noiseModel::Diagonal::Sigmas(
Vector3(0.2, 0.2, 0.1));
166 params.setVerbosityDL(
"VERBOSITY");