29 using namespace gtsam;
46 Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
54 0, 0, 0).finished(),
H, 1
e-6));
68 Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
76 -200, 0, 200).finished(),
H, 1
e-6));
90 Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
98 -360, 80, 440).finished(),
H, 1
e-6));
104 std::default_random_engine
rng(42);
105 std::uniform_real_distribution<double> dist(-0.3, 0.3);
115 for (
int i = 0;
i < 1000; ++
i) {
120 Rot3::Ypr(dist(
rng), dist(
rng), dist(
rng)),
122 Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
127 auto expectedH1 = numericalDerivative11<Vector, Pose2>(
129 return factor.evaluateError(
p, {});},
160 Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
181 0.000012, 0.000000, 0.000000,
182 0.000000, 0.001287, -.001262,
183 0.000000, -.001262, 0.001250).finished(), cov, 3
e-6));
191 ).finished(),
sigma, 1
e-4));
209 Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
216 0, 0, 0).finished(), H1, 1
e-6));
218 0, -200, 0, -200, 0, 0,
219 200, -0, 0, 0, -200, 0).finished(), H2, 1
e-6));
221 0, 0, 0, 1, 0, 0, 0, 0, 0,
222 0, 0, 0, 0, 1, 0, 0, 0, 0).finished(), H3, 1
e-6));
238 Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
246 -200, 0, 200).finished(), H1, 1
e-6));
248 200, -400, -200, -200, 0, -200,
249 400, -200, 200, 0, -200, -200).finished(), H2, 1
e-6));
251 -1, 0, -1, 1, 0, -400, -800, 400, 800,
252 0, -1, 0, 0, 1, -400, -800, 800, 400).finished(), H3, 1
e-6));
268 Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
275 -360, 80, 440).finished(), H1, 1
e-6));
277 440, -640, -200, -280, -80, -360,
278 640, -440, 200, -80, -280, -360).finished(), H2, 1
e-6));
280 -1, 0, -1, 1, 0, -400, -800, 400, 800,
281 0, -1, 0, 0, 1, -400, -800, 800, 400).finished(), H3, 1
e-6));
288 std::default_random_engine
rng(42);
289 std::uniform_real_distribution<double> dist(-0.3, 0.3);
299 for (
int i = 0;
i < 1000; ++
i) {
304 Rot3::Ypr(dist(
rng), dist(
rng), dist(
rng)),
306 Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
316 Matrix expectedH1 = numericalDerivative31<Vector, Pose2, Pose3, Cal3DS2>(
318 return factor.evaluateError(
p, o,
c, {}, {}, {});},
320 Matrix expectedH2 = numericalDerivative32<Vector, Pose2, Pose3, Cal3DS2>(
322 return factor.evaluateError(
p, o,
c, {}, {}, {});},
324 Matrix expectedH3 = numericalDerivative33<Vector, Pose2, Pose3, Cal3DS2>(
326 return factor.evaluateError(
p, o,
c, {}, {}, {});},
340 SharedNoiseModel cNoise = noiseModel::Diagonal::Sigmas(Vector6(10, 10, 10, 10, 10, 10));
341 SharedNoiseModel kNoise = noiseModel::Diagonal::Sigmas(Vector9(0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001));
364 Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
377 initialEstimate.
insert(
C(0), c0);
378 initialEstimate.
insert(
K(0), calib);
404 Matrix c0cov2 = HcTb * c0cov * HcTb.transpose();
407 Vector6 c0sigma = c0cov.diagonal().cwiseSqrt();
415 ).finished(), c0sigma, 1
e-3));
418 Vector6 bTcSigma = c0cov2.diagonal().cwiseSqrt();
426 ).finished(), bTcSigma, 1
e-3));