1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
12/**
13 * @file testSmartStereoProjectionFactorPP.cpp
14 * @brief Unit tests for SmartStereoProjectionFactorPP Class
15 * @author Luca Carlone
16 * @date March 2021
17 */
18
19#include <gtsam/slam/tests/smartFactorScenarios.h>
20#include <gtsam_unstable/slam/SmartStereoProjectionFactorPP.h>
21#include <gtsam_unstable/slam/ProjectionFactorPPP.h>
22#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
23#include <gtsam/slam/PoseTranslationPrior.h>
24#include <gtsam/slam/ProjectionFactor.h>
25#include <gtsam/slam/StereoFactor.h>
26#include <CppUnitLite/TestHarness.h>
27#include <iostream>
28
29using namespace std;
30using namespace gtsam;
31
32namespace {
33// make a realistic calibration matrix
34static double b = 1;
35
36static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov, w, h, b));
37static Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1200, 0, 640, 480,
38 b));
39
40static SmartStereoProjectionParams params;
41
42// static bool manageDegeneracy = true;
43// Create a noise model for the pixel error
44static SharedNoiseModel model(noiseModel::Isotropic::Sigma(dim: 3, sigma: 0.1));
45
46// Convenience for named keys
47using symbol_shorthand::L;
48using symbol_shorthand::X;
49
50// tests data
51static Symbol x1('X', 1);
52static Symbol x2('X', 2);
53static Symbol x3('X', 3);
54static Symbol body_P_cam1_key('P', 1);
55static Symbol body_P_cam2_key('P', 2);
56static Symbol body_P_cam3_key('P', 3);
57
58static Key poseKey1(x1);
59static Key poseExtrinsicKey1(body_P_cam1_key);
60static Key poseExtrinsicKey2(body_P_cam2_key);
61static StereoPoint2 measurement1(
62 323.0, 300.0, 240.0); // potentially use more reasonable measurement value?
63static StereoPoint2 measurement2(
64 350.0, 200.0, 240.0); // potentially use more reasonable measurement value?
65static Pose3 body_P_sensor1(Rot3::RzRyRx(x: -M_PI_2, y: 0.0, z: -M_PI_2),
66 Point3(0.25, -0.10, 1.0));
67
68static double missing_uR = std::numeric_limits<double>::quiet_NaN();
69
70vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
71 const StereoCamera& cam2,
72 const StereoCamera& cam3,
73 Point3 landmark) {
74 vector<StereoPoint2> measurements_cam;
75
76 StereoPoint2 cam1_uv1 = cam1.project(point: landmark);
77 StereoPoint2 cam2_uv1 = cam2.project(point: landmark);
78 StereoPoint2 cam3_uv1 = cam3.project(point: landmark);
79 measurements_cam.push_back(x: cam1_uv1);
80 measurements_cam.push_back(x: cam2_uv1);
81 measurements_cam.push_back(x: cam3_uv1);
82
83 return measurements_cam;
84}
85
86LevenbergMarquardtParams lm_params;
87} // namespace
88
89/* ************************************************************************* */
90TEST( SmartStereoProjectionFactorPP, params) {
91 SmartStereoProjectionParams p;
92
93 // check default values and "get"
94 EXPECT(p.getLinearizationMode() == HESSIAN);
95 EXPECT(p.getDegeneracyMode() == IGNORE_DEGENERACY);
96 EXPECT_DOUBLES_EQUAL(p.getRetriangulationThreshold(), 1e-5, 1e-9);
97 EXPECT(p.getVerboseCheirality() == false);
98 EXPECT(p.getThrowCheirality() == false);
99
100 // check "set"
101 p.setLinearizationMode(JACOBIAN_SVD);
102 p.setDegeneracyMode(ZERO_ON_DEGENERACY);
103 p.setRankTolerance(100);
104 p.setEnableEPI(true);
105 p.setLandmarkDistanceThreshold(200);
106 p.setDynamicOutlierRejectionThreshold(3);
107 p.setRetriangulationThreshold(1e-2);
108
109 EXPECT(p.getLinearizationMode() == JACOBIAN_SVD);
110 EXPECT(p.getDegeneracyMode() == ZERO_ON_DEGENERACY);
111 EXPECT_DOUBLES_EQUAL(p.getTriangulationParameters().rankTolerance, 100, 1e-5);
112 EXPECT(p.getTriangulationParameters().enableEPI == true);
113 EXPECT_DOUBLES_EQUAL(p.getTriangulationParameters().landmarkDistanceThreshold, 200, 1e-5);
114 EXPECT_DOUBLES_EQUAL(p.getTriangulationParameters().dynamicOutlierRejectionThreshold, 3, 1e-5);
115 EXPECT_DOUBLES_EQUAL(p.getRetriangulationThreshold(), 1e-2, 1e-5);
116}
117
118/* ************************************************************************* */
119TEST( SmartStereoProjectionFactorPP, Constructor) {
120 SmartStereoProjectionFactorPP::shared_ptr factor1(new SmartStereoProjectionFactorPP(model));
121}
122
123/* ************************************************************************* */
124TEST( SmartStereoProjectionFactorPP, Constructor2) {
125 SmartStereoProjectionFactorPP factor1(model, params);
126}
127
128/* ************************************************************************* */
129TEST( SmartStereoProjectionFactorPP, Constructor3) {
130 SmartStereoProjectionFactorPP::shared_ptr factor1(new SmartStereoProjectionFactorPP(model));
131 factor1->add(measured: measurement1, world_P_body_key: poseKey1, body_P_cam_key: poseExtrinsicKey1, K);
132}
133
134/* ************************************************************************* */
135TEST( SmartStereoProjectionFactorPP, Constructor4) {
136 SmartStereoProjectionFactorPP factor1(model, params);
137 factor1.add(measured: measurement1, world_P_body_key: poseKey1, body_P_cam_key: poseExtrinsicKey1, K);
138}
139
140/* ************************************************************************* */
141TEST( SmartStereoProjectionFactorPP, Equals ) {
142 SmartStereoProjectionFactorPP::shared_ptr factor1(new SmartStereoProjectionFactorPP(model));
143 factor1->add(measured: measurement1, world_P_body_key: poseKey1, body_P_cam_key: poseExtrinsicKey1, K);
144
145 SmartStereoProjectionFactorPP::shared_ptr factor2(new SmartStereoProjectionFactorPP(model));
146 factor2->add(measured: measurement1, world_P_body_key: poseKey1, body_P_cam_key: poseExtrinsicKey1, K);
147 // check these are equal
148 EXPECT(assert_equal(*factor1, *factor2));
149
150 SmartStereoProjectionFactorPP::shared_ptr factor3(new SmartStereoProjectionFactorPP(model));
151 factor3->add(measured: measurement2, world_P_body_key: poseKey1, body_P_cam_key: poseExtrinsicKey1, K);
152 // check these are different
153 EXPECT(!factor1->equals(*factor3));
154
155 SmartStereoProjectionFactorPP::shared_ptr factor4(new SmartStereoProjectionFactorPP(model));
156 factor4->add(measured: measurement1, world_P_body_key: poseKey1, body_P_cam_key: poseExtrinsicKey2, K);
157 // check these are different
158 EXPECT(!factor1->equals(*factor4));
159}
160
161/* *************************************************************************/
162TEST_UNSAFE( SmartStereoProjectionFactorPP, noiseless_error_identityExtrinsics ) {
163 // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
164 Pose3 w_Pose_cam1 = Pose3(Rot3::Ypr(y: -M_PI / 2, p: 0., r: -M_PI / 2),
165 Point3(0, 0, 1));
166 StereoCamera w_Camera_cam1(w_Pose_cam1, K2);
167
168 // create second camera 1 meter to the right of first camera
169 Pose3 w_Pose_cam2 = w_Pose_cam1 * Pose3(Rot3(), Point3(1, 0, 0));
170 StereoCamera w_Camera_cam2(w_Pose_cam2, K2);
171
172 // landmark ~5 meters infront of camera
173 Point3 landmark(5, 0.5, 1.2);
174
175 // 1. Project two landmarks into two cameras and triangulate
176 StereoPoint2 cam1_uv = w_Camera_cam1.project(point: landmark);
177 StereoPoint2 cam2_uv = w_Camera_cam2.project(point: landmark);
178
179 Values values;
180 values.insert(j: x1, val: w_Pose_cam1);
181 values.insert(j: x2, val: w_Pose_cam2);
182 values.insert(j: body_P_cam1_key, val: Pose3::Identity());
183 values.insert(j: body_P_cam2_key, val: Pose3::Identity());
184
185 SmartStereoProjectionFactorPP factor1(model);
186 factor1.add(measured: cam1_uv, world_P_body_key: x1, body_P_cam_key: body_P_cam1_key, K: K2);
187 factor1.add(measured: cam2_uv, world_P_body_key: x2, body_P_cam_key: body_P_cam2_key, K: K2);
188
189 double actualError = factor1.error(values);
190 double expectedError = 0.0;
191 EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
192
193 SmartStereoProjectionFactorPP::Cameras cameras = factor1.cameras(values);
194 double actualError2 = factor1.totalReprojectionError(cameras);
195 EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7);
196}
197
198/* *************************************************************************/
199TEST_UNSAFE( SmartStereoProjectionFactorPP, noiseless_error_multipleExtrinsics ) {
200 // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
201 Pose3 w_Pose_cam1 = Pose3(Rot3::Ypr(y: -M_PI / 2, p: 0., r: -M_PI / 2),
202 Point3(0, 0, 1));
203 StereoCamera w_Camera_cam1(w_Pose_cam1, K2);
204
205 // create second camera 1 meter to the right of first camera
206 Pose3 w_Pose_cam2 = w_Pose_cam1 * Pose3(Rot3(), Point3(1, 0, 0));
207 StereoCamera w_Camera_cam2(w_Pose_cam2, K2);
208
209 // landmark ~5 meters infront of camera
210 Point3 landmark(5, 0.5, 1.2);
211
212 // 1. Project two landmarks into two cameras and triangulate
213 StereoPoint2 cam1_uv = w_Camera_cam1.project(point: landmark);
214 StereoPoint2 cam2_uv = w_Camera_cam2.project(point: landmark);
215
216 Pose3 body_Pose_cam1 = Pose3(Rot3::Ypr(y: -M_PI / 2, p: 0., r: 0.0),
217 Point3(0, 1, 0));
218 Pose3 body_Pose_cam2 = Pose3(Rot3::Ypr(y: -M_PI / 4, p: 0., r: 0.0),
219 Point3(1, 1, 0));
220 Pose3 w_Pose_body1 = w_Pose_cam1.compose(g: body_Pose_cam1.inverse());
221 Pose3 w_Pose_body2 = w_Pose_cam2.compose(g: body_Pose_cam2.inverse());
222
223 Values values;
224 values.insert(j: x1, val: w_Pose_body1);
225 values.insert(j: x2, val: w_Pose_body2);
226 values.insert(j: body_P_cam1_key, val: body_Pose_cam1);
227 values.insert(j: body_P_cam2_key, val: body_Pose_cam2);
228
229 SmartStereoProjectionFactorPP factor1(model);
230 factor1.add(measured: cam1_uv, world_P_body_key: x1, body_P_cam_key: body_P_cam1_key, K: K2);
231 factor1.add(measured: cam2_uv, world_P_body_key: x2, body_P_cam_key: body_P_cam2_key, K: K2);
232
233 double actualError = factor1.error(values);
234 double expectedError = 0.0;
235 EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
236
237 SmartStereoProjectionFactorPP::Cameras cameras = factor1.cameras(values);
238 double actualError2 = factor1.totalReprojectionError(cameras);
239 EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7);
240}
241
242/* *************************************************************************/
243TEST( SmartProjectionPoseFactor, noiseless_error_multipleExtrinsics_missingMeasurements ) {
244
245 // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
246 Pose3 w_Pose_cam1 = Pose3(Rot3::Ypr(y: -M_PI / 2, p: 0., r: -M_PI / 2),
247 Point3(0, 0, 1));
248 StereoCamera w_Camera_cam1(w_Pose_cam1, K2);
249
250 // create second camera 1 meter to the right of first camera
251 Pose3 w_Pose_cam2 = w_Pose_cam1 * Pose3(Rot3(), Point3(1, 0, 0));
252 StereoCamera w_Camera_cam2(w_Pose_cam2, K2);
253
254 // landmark ~5 meters in front of camera
255 Point3 landmark(5, 0.5, 1.2);
256
257 // 1. Project two landmarks into two cameras and triangulate
258 StereoPoint2 cam1_uv = w_Camera_cam1.project(point: landmark);
259 StereoPoint2 cam2_uv = w_Camera_cam2.project(point: landmark);
260 StereoPoint2 cam2_uv_right_missing(cam2_uv.uL(),missing_uR,cam2_uv.v());
261
262 // fake extrinsic calibration
263 Pose3 body_Pose_cam1 = Pose3(Rot3::Ypr(y: -M_PI, p: 1., r: 0.1),
264 Point3(0, 1, 0));
265 Pose3 body_Pose_cam2 = Pose3(Rot3::Ypr(y: -M_PI / 4, p: 0.1, r: 1.0),
266 Point3(1, 1, 1));
267 Pose3 w_Pose_body1 = w_Pose_cam1.compose(g: body_Pose_cam1.inverse());
268 Pose3 w_Pose_body2 = w_Pose_cam2.compose(g: body_Pose_cam2.inverse());
269
270 Values values;
271 values.insert(j: x1, val: w_Pose_body1);
272 values.insert(j: x2, val: w_Pose_body2);
273 values.insert(j: body_P_cam1_key, val: body_Pose_cam1);
274 values.insert(j: body_P_cam2_key, val: body_Pose_cam2);
275
276 SmartStereoProjectionFactorPP factor1(model);
277 factor1.add(measured: cam1_uv, world_P_body_key: x1, body_P_cam_key: body_P_cam1_key, K: K2);
278 factor1.add(measured: cam2_uv_right_missing, world_P_body_key: x2, body_P_cam_key: body_P_cam2_key, K: K2);
279
280 double actualError = factor1.error(values);
281 double expectedError = 0.0;
282 EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
283
284 // TEST TRIANGULATION WITH MISSING VALUES: i) right pixel of second camera is missing:
285 SmartStereoProjectionFactorPP::Cameras cameras = factor1.cameras(values);
286 double actualError2 = factor1.totalReprojectionError(cameras);
287 EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7);
288
289 // The following are generically exercising the triangulation
290 CameraSet<StereoCamera> cams{w_Camera_cam1, w_Camera_cam2};
291 TriangulationResult result = factor1.triangulateSafe(cameras: cams);
292 CHECK(result);
293 EXPECT(assert_equal(landmark, *result, 1e-7));
294
295 // TEST TRIANGULATION WITH MISSING VALUES: ii) right pixels of both cameras are missing:
296 SmartStereoProjectionFactorPP factor2(model);
297 StereoPoint2 cam1_uv_right_missing(cam1_uv.uL(),missing_uR,cam1_uv.v());
298 factor2.add(measured: cam1_uv_right_missing, world_P_body_key: x1, body_P_cam_key: body_P_cam1_key, K: K2);
299 factor2.add(measured: cam2_uv_right_missing, world_P_body_key: x2, body_P_cam_key: body_P_cam2_key, K: K2);
300 result = factor2.triangulateSafe(cameras: cams);
301 CHECK(result);
302 EXPECT(assert_equal(landmark, *result, 1e-7));
303}
304
305/* *************************************************************************/
306TEST( SmartStereoProjectionFactorPP, noisy_error_multipleExtrinsics ) {
307 // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
308 Pose3 w_Pose_cam1 = Pose3(Rot3::Ypr(y: -M_PI / 2, p: 0., r: -M_PI / 2),
309 Point3(0, 0, 1));
310 StereoCamera w_Camera_cam1(w_Pose_cam1, K2);
311
312 // create second camera 1 meter to the right of first camera
313 Pose3 w_Pose_cam2 = w_Pose_cam1 * Pose3(Rot3(), Point3(1, 0, 0));
314 StereoCamera w_Camera_cam2(w_Pose_cam2, K2);
315
316 // landmark ~5 meters infront of camera
317 Point3 landmark(5, 0.5, 1.2);
318
319 // 1. Project two landmarks into two cameras and triangulate
320 StereoPoint2 pixelError(0.2, 0.2, 0);
321 StereoPoint2 cam1_uv = w_Camera_cam1.project(point: landmark) + pixelError;
322 StereoPoint2 cam2_uv = w_Camera_cam2.project(point: landmark);
323
324 // fake extrinsic calibration
325 Pose3 body_Pose_cam1 = Pose3(Rot3::Ypr(y: -M_PI, p: 1., r: 0.1),
326 Point3(0, 1, 0));
327 Pose3 body_Pose_cam2 = Pose3(Rot3::Ypr(y: -M_PI / 4, p: 0.1, r: 1.0),
328 Point3(1, 1, 1));
329 Pose3 w_Pose_body1 = w_Pose_cam1.compose(g: body_Pose_cam1.inverse());
330 Pose3 w_Pose_body2 = w_Pose_cam2.compose(g: body_Pose_cam2.inverse());
331
332 Values values;
333 values.insert(j: x1, val: w_Pose_body1);
334 Pose3 noise_pose = Pose3(Rot3::Ypr(y: -M_PI / 10, p: 0., r: -M_PI / 10),
335 Point3(0.5, 0.1, 0.3));
336 values.insert(j: x2, val: w_Pose_body2.compose(g: noise_pose));
337 values.insert(j: body_P_cam1_key, val: body_Pose_cam1);
338 values.insert(j: body_P_cam2_key, val: body_Pose_cam2);
339
340 SmartStereoProjectionFactorPP::shared_ptr factor1(new SmartStereoProjectionFactorPP(model));
341 factor1->add(measured: cam1_uv, world_P_body_key: x1, body_P_cam_key: body_P_cam1_key, K);
342 factor1->add(measured: cam2_uv, world_P_body_key: x2, body_P_cam_key: body_P_cam2_key, K);
343
344 double actualError1 = factor1->error(values);
345
346 SmartStereoProjectionFactorPP::shared_ptr factor2(new SmartStereoProjectionFactorPP(model));
347 vector<StereoPoint2> measurements;
348 measurements.push_back(x: cam1_uv);
349 measurements.push_back(x: cam2_uv);
350
351 vector<std::shared_ptr<Cal3_S2Stereo> > Ks; ///< shared pointer to calibration object (one for each camera)
352 Ks.push_back(x: K);
353 Ks.push_back(x: K);
354
355 KeyVector poseKeys;
356 poseKeys.push_back(x: x1);
357 poseKeys.push_back(x: x2);
358
359 KeyVector extrinsicKeys;
360 extrinsicKeys.push_back(x: body_P_cam1_key);
361 extrinsicKeys.push_back(x: body_P_cam2_key);
362
363 factor2->add(measurements, w_P_body_keys: poseKeys, body_P_cam_keys: extrinsicKeys, Ks);
364
365 double actualError2 = factor2->error(values);
366
367 DOUBLES_EQUAL(actualError1, actualError2, 1e-7);
368 DOUBLES_EQUAL(actualError1, 5381978, 1); // value freeze
369}
370
371/* *************************************************************************/
372TEST( SmartStereoProjectionFactorPP, 3poses_optimization_multipleExtrinsics ) {
373
374 // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
375 Pose3 w_Pose_cam1 = Pose3(Rot3::Ypr(y: -M_PI / 2, p: 0., r: -M_PI / 2), Point3(0, 0, 1));
376 StereoCamera cam1(w_Pose_cam1, K2);
377
378 // create second camera 1 meter to the right of first camera
379 Pose3 w_Pose_cam2 = w_Pose_cam1 * Pose3(Rot3(), Point3(1, 0, 0));
380 StereoCamera cam2(w_Pose_cam2, K2);
381
382 // create third camera 1 meter above the first camera
383 Pose3 w_Pose_cam3 = w_Pose_cam1 * Pose3(Rot3(), Point3(0, -1, 0));
384 StereoCamera cam3(w_Pose_cam3, K2);
385
386 // three landmarks ~5 meters infront of camera
387 Point3 landmark1(5, 0.5, 1.2);
388 Point3 landmark2(5, -0.5, 1.2);
389 Point3 landmark3(3, 0, 3.0);
390
391 // 1. Project three landmarks into three cameras and triangulate
392 vector<StereoPoint2> measurements_l1 = stereo_projectToMultipleCameras(cam1,
393 cam2, cam3, landmark: landmark1);
394 vector<StereoPoint2> measurements_l2 = stereo_projectToMultipleCameras(cam1,
395 cam2, cam3, landmark: landmark2);
396 vector<StereoPoint2> measurements_l3 = stereo_projectToMultipleCameras(cam1,
397 cam2, cam3, landmark: landmark3);
398
399 KeyVector poseKeys;
400 poseKeys.push_back(x: x1);
401 poseKeys.push_back(x: x2);
402 poseKeys.push_back(x: x3);
403
404 KeyVector extrinsicKeys;
405 extrinsicKeys.push_back(x: body_P_cam1_key);
406 extrinsicKeys.push_back(x: body_P_cam2_key);
407 extrinsicKeys.push_back(x: body_P_cam3_key);
408
409 SmartStereoProjectionParams smart_params;
410 smart_params.triangulation.enableEPI = true;
411 SmartStereoProjectionFactorPP::shared_ptr smartFactor1(new SmartStereoProjectionFactorPP(model, smart_params));
412 smartFactor1->add(measurements: measurements_l1, w_P_body_keys: poseKeys, body_P_cam_keys: extrinsicKeys, K: K2);
413
414 SmartStereoProjectionFactorPP::shared_ptr smartFactor2(new SmartStereoProjectionFactorPP(model, smart_params));
415 smartFactor2->add(measurements: measurements_l2, w_P_body_keys: poseKeys, body_P_cam_keys: extrinsicKeys, K: K2);
416
417 SmartStereoProjectionFactorPP::shared_ptr smartFactor3(new SmartStereoProjectionFactorPP(model, smart_params));
418 smartFactor3->add(measurements: measurements_l3, w_P_body_keys: poseKeys, body_P_cam_keys: extrinsicKeys, K: K2);
419
420 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.10);
421
422 // Values
423 Pose3 body_Pose_cam1 = Pose3(Rot3::Ypr(y: -M_PI, p: 1., r: 0.1),Point3(0, 1, 0));
424 Pose3 body_Pose_cam2 = Pose3(Rot3::Ypr(y: -M_PI / 4, p: 0.1, r: 1.0),Point3(1, 1, 1));
425 Pose3 body_Pose_cam3 = Pose3::Identity();
426 Pose3 w_Pose_body1 = w_Pose_cam1.compose(g: body_Pose_cam1.inverse());
427 Pose3 w_Pose_body2 = w_Pose_cam2.compose(g: body_Pose_cam2.inverse());
428 Pose3 w_Pose_body3 = w_Pose_cam3.compose(g: body_Pose_cam3.inverse());
429
430 Values values;
431 Pose3 noise_pose = Pose3(Rot3::Ypr(y: -M_PI / 100, p: 0., r: -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise
432 values.insert(j: x1, val: w_Pose_body1);
433 values.insert(j: x2, val: w_Pose_body2);
434 values.insert(j: x3, val: w_Pose_body3);
435 values.insert(j: body_P_cam1_key, val: body_Pose_cam1);
436 values.insert(j: body_P_cam2_key, val: body_Pose_cam2);
437 // initialize third calibration with some noise, we expect it to move back to original pose3
438 values.insert(j: body_P_cam3_key, val: body_Pose_cam3 * noise_pose);
439
440 // Graph
441 NonlinearFactorGraph graph;
442 graph.push_back(factor: smartFactor1);
443 graph.push_back(factor: smartFactor2);
444 graph.push_back(factor: smartFactor3);
445 graph.addPrior(key: x1, prior: w_Pose_body1, model: noisePrior);
446 graph.addPrior(key: x2, prior: w_Pose_body2, model: noisePrior);
447 graph.addPrior(key: x3, prior: w_Pose_body3, model: noisePrior);
448 // we might need some prior on the calibration too
449 graph.addPrior(key: body_P_cam1_key, prior: body_Pose_cam1, model: noisePrior);
450 graph.addPrior(key: body_P_cam2_key, prior: body_Pose_cam2, model: noisePrior);
451
452 EXPECT(
453 assert_equal(
454 Pose3(
455 Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
456 -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
457 Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3) * values.at<Pose3>(body_P_cam3_key)));
458
459 // cout << std::setprecision(10) << "\n----SmartStereoFactor graph initial error: " << graph.error(values) << endl;
460 EXPECT_DOUBLES_EQUAL(833953.92789459578, graph.error(values), 1e-7); // initial error (note - this also matches error below)
461
462 // get triangulated landmarks from smart factors
463 Point3 landmark1_smart = *smartFactor1->point();
464 Point3 landmark2_smart = *smartFactor2->point();
465 Point3 landmark3_smart = *smartFactor3->point();
466
467 // cost is large before optimization
468 double initialErrorSmart = graph.error(values);
469 EXPECT_DOUBLES_EQUAL(833953.92789459461, initialErrorSmart, 1e-5);
470
471 Values result;
472 gttic_(SmartStereoProjectionFactorPP);
473 LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
474 result = optimizer.optimize();
475 gttoc_(SmartStereoProjectionFactorPP);
476 tictoc_finishedIteration_();
477
478 // cout << std::setprecision(10) << "SmartStereoFactor graph optimized error: " << graph.error(result) << endl;
479 EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-5);
480
481 GaussianFactorGraph::shared_ptr GFG = graph.linearize(linearizationPoint: result);
482 VectorValues delta = GFG->optimize();
483 VectorValues expected = VectorValues::Zero(other: delta);
484 EXPECT(assert_equal(expected, delta, 1e-6));
485
486 // result.print("results of 3 camera, 3 landmark optimization \n");
487 EXPECT(assert_equal(body_Pose_cam3, result.at<Pose3>(body_P_cam3_key)));
488
489 // ***************************************************************
490 // Same problem with regular Stereo factors, expect same error!
491 // ****************************************************************
492
493 // add landmarks to values
494 Values values2;
495 values2.insert(j: x1, val: w_Pose_cam1); // note: this is the *camera* pose now
496 values2.insert(j: x2, val: w_Pose_cam2);
497 values2.insert(j: x3, val: w_Pose_cam3 * noise_pose); // equivalent to perturbing the extrinsic calibration
498 values2.insert(j: L(j: 1), val: landmark1_smart);
499 values2.insert(j: L(j: 2), val: landmark2_smart);
500 values2.insert(j: L(j: 3), val: landmark3_smart);
501
502 // add factors
503 NonlinearFactorGraph graph2;
504
505 graph2.addPrior(key: x1, prior: w_Pose_cam1, model: noisePrior);
506 graph2.addPrior(key: x2, prior: w_Pose_cam2, model: noisePrior);
507
508 typedef GenericStereoFactor<Pose3, Point3> ProjectionFactor;
509
510 bool verboseCheirality = true;
511
512 // NOTE: we cannot repeate this with ProjectionFactor, since they are not suitable for stereo calibration
513 graph2.push_back(factor: ProjectionFactor(measurements_l1[0], model, x1, L(j: 1), K2, false, verboseCheirality));
514 graph2.push_back(factor: ProjectionFactor(measurements_l1[1], model, x2, L(j: 1), K2, false, verboseCheirality));
515 graph2.push_back(factor: ProjectionFactor(measurements_l1[2], model, x3, L(j: 1), K2, false, verboseCheirality));
516
517 graph2.push_back(factor: ProjectionFactor(measurements_l2[0], model, x1, L(j: 2), K2, false, verboseCheirality));
518 graph2.push_back(factor: ProjectionFactor(measurements_l2[1], model, x2, L(j: 2), K2, false, verboseCheirality));
519 graph2.push_back(factor: ProjectionFactor(measurements_l2[2], model, x3, L(j: 2), K2, false, verboseCheirality));
520
521 graph2.push_back(factor: ProjectionFactor(measurements_l3[0], model, x1, L(j: 3), K2, false, verboseCheirality));
522 graph2.push_back(factor: ProjectionFactor(measurements_l3[1], model, x2, L(j: 3), K2, false, verboseCheirality));
523 graph2.push_back(factor: ProjectionFactor(measurements_l3[2], model, x3, L(j: 3), K2, false, verboseCheirality));
524
525 // cout << std::setprecision(10) << "\n----StereoFactor graph initial error: " << graph2.error(values) << endl;
526 EXPECT_DOUBLES_EQUAL(833953.92789459578, graph2.error(values2), 1e-7);
527 EXPECT_DOUBLES_EQUAL(initialErrorSmart, graph2.error(values2), 1e-7); // identical to previous case!
528
529 LevenbergMarquardtOptimizer optimizer2(graph2, values2, lm_params);
530 Values result2 = optimizer2.optimize();
531 EXPECT_DOUBLES_EQUAL(0, graph2.error(result2), 1e-5);
532}
533
534/* *************************************************************************/
535TEST( SmartStereoProjectionFactorPP, 3poses_error_sameExtrinsicKey ) {
536
537 // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
538 Pose3 w_Pose_cam1 = Pose3(Rot3::Ypr(y: -M_PI / 2, p: 0., r: -M_PI / 2), Point3(0, 0, 1));
539 StereoCamera cam1(w_Pose_cam1, K2);
540
541 // create second camera 1 meter to the right of first camera
542 Pose3 w_Pose_cam2 = w_Pose_cam1 * Pose3(Rot3(), Point3(1, 0, 0));
543 StereoCamera cam2(w_Pose_cam2, K2);
544
545 // create third camera 1 meter above the first camera
546 Pose3 w_Pose_cam3 = w_Pose_cam1 * Pose3(Rot3(), Point3(0, -1, 0));
547 StereoCamera cam3(w_Pose_cam3, K2);
548
549 // three landmarks ~5 meters infront of camera
550 Point3 landmark1(5, 0.5, 1.2);
551 Point3 landmark2(5, -0.5, 1.2);
552 Point3 landmark3(3, 0, 3.0);
553
554 // 1. Project three landmarks into three cameras and triangulate
555 vector<StereoPoint2> measurements_l1 = stereo_projectToMultipleCameras(cam1,
556 cam2, cam3, landmark: landmark1);
557 vector<StereoPoint2> measurements_l2 = stereo_projectToMultipleCameras(cam1,
558 cam2, cam3, landmark: landmark2);
559 vector<StereoPoint2> measurements_l3 = stereo_projectToMultipleCameras(cam1,
560 cam2, cam3, landmark: landmark3);
561
562 KeyVector poseKeys;
563 poseKeys.push_back(x: x1);
564 poseKeys.push_back(x: x2);
565 poseKeys.push_back(x: x3);
566
567 Symbol body_P_cam_key('P', 0);
568 KeyVector extrinsicKeys;
569 extrinsicKeys.push_back(x: body_P_cam_key);
570 extrinsicKeys.push_back(x: body_P_cam_key);
571 extrinsicKeys.push_back(x: body_P_cam_key);
572
573 SmartStereoProjectionParams smart_params;
574 smart_params.triangulation.enableEPI = true;
575 SmartStereoProjectionFactorPP::shared_ptr smartFactor1(new SmartStereoProjectionFactorPP(model, smart_params));
576 smartFactor1->add(measurements: measurements_l1, w_P_body_keys: poseKeys, body_P_cam_keys: extrinsicKeys, K: K2);
577
578 SmartStereoProjectionFactorPP::shared_ptr smartFactor2(new SmartStereoProjectionFactorPP(model, smart_params));
579 smartFactor2->add(measurements: measurements_l2, w_P_body_keys: poseKeys, body_P_cam_keys: extrinsicKeys, K: K2);
580
581 SmartStereoProjectionFactorPP::shared_ptr smartFactor3(new SmartStereoProjectionFactorPP(model, smart_params));
582 smartFactor3->add(measurements: measurements_l3, w_P_body_keys: poseKeys, body_P_cam_keys: extrinsicKeys, K: K2);
583
584 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.10);
585
586 // Values
587 Pose3 body_Pose_cam = Pose3(Rot3::Ypr(y: -M_PI, p: 1., r: 0.1),Point3(0, 1, 0));
588 Pose3 w_Pose_body1 = w_Pose_cam1.compose(g: body_Pose_cam.inverse());
589 Pose3 w_Pose_body2 = w_Pose_cam2.compose(g: body_Pose_cam.inverse());
590 Pose3 w_Pose_body3 = w_Pose_cam3.compose(g: body_Pose_cam.inverse());
591
592 Values values; // all noiseless
593 Pose3 noise_pose = Pose3(Rot3::Ypr(y: -M_PI / 100, p: 0., r: -M_PI / 100), Point3(0.01, 0.01, 0.01)); // smaller noise
594 values.insert(j: x1, val: w_Pose_body1);
595 values.insert(j: x2, val: w_Pose_body2);
596 values.insert(j: x3, val: w_Pose_body3);
597 values.insert(j: body_P_cam_key, val: body_Pose_cam);
598
599 // Graph
600 NonlinearFactorGraph graph;
601 graph.push_back(factor: smartFactor1);
602 graph.push_back(factor: smartFactor2);
603 graph.push_back(factor: smartFactor3);
604 graph.addPrior(key: x1, prior: w_Pose_body1, model: noisePrior);
605 graph.addPrior(key: x2, prior: w_Pose_body2, model: noisePrior);
606 graph.addPrior(key: x3, prior: w_Pose_body3, model: noisePrior);
607
608 // cost is large before optimization
609 double initialErrorSmart = graph.error(values);
610 EXPECT_DOUBLES_EQUAL(0.0, initialErrorSmart, 1e-5); // initial guess is noisy, so nonzero error
611}
612
613/* *************************************************************************/
614TEST( SmartStereoProjectionFactorPP, 3poses_noisy_error_sameExtrinsicKey ) {
615
616 // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
617 Pose3 w_Pose_cam1 = Pose3(Rot3::Ypr(y: -M_PI / 2, p: 0., r: -M_PI / 2), Point3(0, 0, 1));
618 StereoCamera cam1(w_Pose_cam1, K2);
619
620 // create second camera 1 meter to the right of first camera
621 Pose3 w_Pose_cam2 = w_Pose_cam1 * Pose3(Rot3(), Point3(1, 0, 0));
622 StereoCamera cam2(w_Pose_cam2, K2);
623
624 // create third camera 1 meter above the first camera
625 Pose3 w_Pose_cam3 = w_Pose_cam1 * Pose3(Rot3(), Point3(0, -1, 0));
626 StereoCamera cam3(w_Pose_cam3, K2);
627
628 // three landmarks ~5 meters infront of camera
629 Point3 landmark1(5, 0.5, 1.2);
630 Point3 landmark2(5, -0.5, 1.2);
631 Point3 landmark3(3, 0, 3.0);
632
633 // 1. Project three landmarks into three cameras and triangulate
634 vector<StereoPoint2> measurements_l1 = stereo_projectToMultipleCameras(cam1,
635 cam2, cam3, landmark: landmark1);
636 vector<StereoPoint2> measurements_l2 = stereo_projectToMultipleCameras(cam1,
637 cam2, cam3, landmark: landmark2);
638 vector<StereoPoint2> measurements_l3 = stereo_projectToMultipleCameras(cam1,
639 cam2, cam3, landmark: landmark3);
640
641 double initialError_expected, initialError_actual;
642 {
643 KeyVector poseKeys;
644 poseKeys.push_back(x: x1);
645 poseKeys.push_back(x: x2);
646 poseKeys.push_back(x: x3);
647
648 KeyVector extrinsicKeys;
649 extrinsicKeys.push_back(x: body_P_cam1_key);
650 extrinsicKeys.push_back(x: body_P_cam2_key);
651 extrinsicKeys.push_back(x: body_P_cam3_key);
652
653 SmartStereoProjectionParams smart_params;
654 smart_params.triangulation.enableEPI = true;
655 SmartStereoProjectionFactorPP::shared_ptr smartFactor1(new SmartStereoProjectionFactorPP(model, smart_params));
656 smartFactor1->add(measurements: measurements_l1, w_P_body_keys: poseKeys, body_P_cam_keys: extrinsicKeys, K: K2);
657
658 SmartStereoProjectionFactorPP::shared_ptr smartFactor2(new SmartStereoProjectionFactorPP(model, smart_params));
659 smartFactor2->add(measurements: measurements_l2, w_P_body_keys: poseKeys, body_P_cam_keys: extrinsicKeys, K: K2);
660
661 SmartStereoProjectionFactorPP::shared_ptr smartFactor3(new SmartStereoProjectionFactorPP(model, smart_params));
662 smartFactor3->add(measurements: measurements_l3, w_P_body_keys: poseKeys, body_P_cam_keys: extrinsicKeys, K: K2);
663
664 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.10);
665
666 // Values
667 Pose3 body_Pose_cam1 = Pose3(Rot3::Ypr(y: -M_PI, p: 1., r: 0.1),Point3(0, 1, 0));
668 Pose3 body_Pose_cam2 = body_Pose_cam1;
669 Pose3 body_Pose_cam3 = body_Pose_cam1;
670 Pose3 w_Pose_body1 = w_Pose_cam1.compose(g: body_Pose_cam1.inverse());
671 Pose3 w_Pose_body2 = w_Pose_cam2.compose(g: body_Pose_cam2.inverse());
672 Pose3 w_Pose_body3 = w_Pose_cam3.compose(g: body_Pose_cam3.inverse());
673
674 Values values;
675 Pose3 noise_pose = Pose3(Rot3::Ypr(y: -M_PI / 100, p: 0., r: -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise
676 values.insert(j: x1, val: w_Pose_body1);
677 values.insert(j: x2, val: w_Pose_body2);
678 values.insert(j: x3, val: w_Pose_body3);
679 values.insert(j: body_P_cam1_key, val: body_Pose_cam1 * noise_pose);
680 values.insert(j: body_P_cam2_key, val: body_Pose_cam2 * noise_pose);
681 // initialize third calibration with some noise, we expect it to move back to original pose3
682 values.insert(j: body_P_cam3_key, val: body_Pose_cam3 * noise_pose);
683
684 // Graph
685 NonlinearFactorGraph graph;
686 graph.push_back(factor: smartFactor1);
687 graph.push_back(factor: smartFactor2);
688 graph.push_back(factor: smartFactor3);
689 graph.addPrior(key: x1, prior: w_Pose_body1, model: noisePrior);
690 graph.addPrior(key: x2, prior: w_Pose_body2, model: noisePrior);
691 graph.addPrior(key: x3, prior: w_Pose_body3, model: noisePrior);
692
693 initialError_expected = graph.error(values);
694 }
695
696 {
697 KeyVector poseKeys;
698 poseKeys.push_back(x: x1);
699 poseKeys.push_back(x: x2);
700 poseKeys.push_back(x: x3);
701
702 KeyVector extrinsicKeys;
703 extrinsicKeys.push_back(x: body_P_cam1_key);
704 extrinsicKeys.push_back(x: body_P_cam1_key);
705 extrinsicKeys.push_back(x: body_P_cam1_key);
706
707 SmartStereoProjectionParams smart_params;
708 smart_params.triangulation.enableEPI = true;
709 SmartStereoProjectionFactorPP::shared_ptr smartFactor1(new SmartStereoProjectionFactorPP(model, smart_params));
710 smartFactor1->add(measurements: measurements_l1, w_P_body_keys: poseKeys, body_P_cam_keys: extrinsicKeys, K: K2);
711
712 SmartStereoProjectionFactorPP::shared_ptr smartFactor2(new SmartStereoProjectionFactorPP(model, smart_params));
713 smartFactor2->add(measurements: measurements_l2, w_P_body_keys: poseKeys, body_P_cam_keys: extrinsicKeys, K: K2);
714
715 SmartStereoProjectionFactorPP::shared_ptr smartFactor3(new SmartStereoProjectionFactorPP(model, smart_params));
716 smartFactor3->add(measurements: measurements_l3, w_P_body_keys: poseKeys, body_P_cam_keys: extrinsicKeys, K: K2);
717
718 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.10);
719
720 // Values
721 Pose3 body_Pose_cam1 = Pose3(Rot3::Ypr(y: -M_PI, p: 1., r: 0.1),Point3(0, 1, 0));
722 Pose3 w_Pose_body1 = w_Pose_cam1.compose(g: body_Pose_cam1.inverse());
723 Pose3 w_Pose_body2 = w_Pose_cam2.compose(g: body_Pose_cam1.inverse());
724 Pose3 w_Pose_body3 = w_Pose_cam3.compose(g: body_Pose_cam1.inverse());
725
726 Values values;
727 Pose3 noise_pose = Pose3(Rot3::Ypr(y: -M_PI / 100, p: 0., r: -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise
728 values.insert(j: x1, val: w_Pose_body1);
729 values.insert(j: x2, val: w_Pose_body2);
730 values.insert(j: x3, val: w_Pose_body3);
731 values.insert(j: body_P_cam1_key, val: body_Pose_cam1 * noise_pose);
732
733 // Graph
734 NonlinearFactorGraph graph;
735 graph.push_back(factor: smartFactor1);
736 graph.push_back(factor: smartFactor2);
737 graph.push_back(factor: smartFactor3);
738 graph.addPrior(key: x1, prior: w_Pose_body1, model: noisePrior);
739 graph.addPrior(key: x2, prior: w_Pose_body2, model: noisePrior);
740 graph.addPrior(key: x3, prior: w_Pose_body3, model: noisePrior);
741
742 initialError_actual = graph.error(values);
743 }
744
745 //std::cout << " initialError_expected " << initialError_expected << std::endl;
746 EXPECT_DOUBLES_EQUAL(initialError_expected, initialError_actual, 1e-7);
747}
748
749/* *************************************************************************/
750TEST( SmartStereoProjectionFactorPP, 3poses_optimization_sameExtrinsicKey ) {
751
752 // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
753 Pose3 w_Pose_cam1 = Pose3(Rot3::Ypr(y: -M_PI / 2, p: 0., r: -M_PI / 2), Point3(0, 0, 1));
754 StereoCamera cam1(w_Pose_cam1, K2);
755
756 // create second camera 1 meter to the right of first camera
757 Pose3 w_Pose_cam2 = w_Pose_cam1 * Pose3(Rot3(), Point3(1, 0, 0));
758 StereoCamera cam2(w_Pose_cam2, K2);
759
760 // create third camera 1 meter above the first camera
761 Pose3 w_Pose_cam3 = w_Pose_cam1 * Pose3(Rot3(), Point3(0, -1, 0));
762 StereoCamera cam3(w_Pose_cam3, K2);
763
764 // three landmarks ~5 meters infront of camera
765 Point3 landmark1(5, 0.5, 1.2);
766 Point3 landmark2(5, -0.5, 1.2);
767 Point3 landmark3(3, 0, 3.0);
768
769 // 1. Project three landmarks into three cameras and triangulate
770 vector<StereoPoint2> measurements_l1 = stereo_projectToMultipleCameras(cam1,
771 cam2, cam3, landmark: landmark1);
772 vector<StereoPoint2> measurements_l2 = stereo_projectToMultipleCameras(cam1,
773 cam2, cam3, landmark: landmark2);
774 vector<StereoPoint2> measurements_l3 = stereo_projectToMultipleCameras(cam1,
775 cam2, cam3, landmark: landmark3);
776
777 KeyVector poseKeys;
778 poseKeys.push_back(x: x1);
779 poseKeys.push_back(x: x2);
780 poseKeys.push_back(x: x3);
781
782 Symbol body_P_cam_key('P', 0);
783 KeyVector extrinsicKeys;
784 extrinsicKeys.push_back(x: body_P_cam_key);
785 extrinsicKeys.push_back(x: body_P_cam_key);
786 extrinsicKeys.push_back(x: body_P_cam_key);
787
788 SmartStereoProjectionParams smart_params;
789 smart_params.triangulation.enableEPI = true;
790 SmartStereoProjectionFactorPP::shared_ptr smartFactor1(new SmartStereoProjectionFactorPP(model, smart_params));
791 smartFactor1->add(measurements: measurements_l1, w_P_body_keys: poseKeys, body_P_cam_keys: extrinsicKeys, K: K2);
792
793 SmartStereoProjectionFactorPP::shared_ptr smartFactor2(new SmartStereoProjectionFactorPP(model, smart_params));
794 smartFactor2->add(measurements: measurements_l2, w_P_body_keys: poseKeys, body_P_cam_keys: extrinsicKeys, K: K2);
795
796 SmartStereoProjectionFactorPP::shared_ptr smartFactor3(new SmartStereoProjectionFactorPP(model, smart_params));
797 smartFactor3->add(measurements: measurements_l3, w_P_body_keys: poseKeys, body_P_cam_keys: extrinsicKeys, K: K2);
798
799 // relevant poses:
800 Pose3 body_Pose_cam = Pose3(Rot3::Ypr(y: -M_PI, p: 1., r: 0.1),Point3(0, 1, 0));
801 Pose3 w_Pose_body1 = w_Pose_cam1.compose(g: body_Pose_cam.inverse());
802 Pose3 w_Pose_body2 = w_Pose_cam2.compose(g: body_Pose_cam.inverse());
803 Pose3 w_Pose_body3 = w_Pose_cam3.compose(g: body_Pose_cam.inverse());
804
805 // Graph
806 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.10);
807 NonlinearFactorGraph graph;
808 graph.push_back(factor: smartFactor1);
809 graph.push_back(factor: smartFactor2);
810 graph.push_back(factor: smartFactor3);
811 graph.addPrior(key: x1, prior: w_Pose_body1, model: noisePrior);
812 graph.addPrior(key: x2, prior: w_Pose_body2, model: noisePrior);
813 graph.addPrior(key: x3, prior: w_Pose_body3, model: noisePrior);
814 // we might need some prior on the calibration too
815 // graph.addPrior(body_P_cam_key, body_Pose_cam, noisePrior); // no need! the measurements will fix this!
816
817 // Values
818 Values values;
819 Pose3 noise_pose = Pose3(Rot3::Ypr(y: -M_PI / 100, p: 0., r: -M_PI / 100), Point3(0.01, 0.01, 0.01)); // smaller noise
820 values.insert(j: x1, val: w_Pose_body1);
821 values.insert(j: x2, val: w_Pose_body2);
822 values.insert(j: x3, val: w_Pose_body3);
823 values.insert(j: body_P_cam_key, val: body_Pose_cam*noise_pose);
824
825 // cost is large before optimization
826 double initialErrorSmart = graph.error(values);
827 EXPECT_DOUBLES_EQUAL(31986.961831653316, initialErrorSmart, 1e-5); // initial guess is noisy, so nonzero error
828
829 /////////////////////////////////////////////////////////////////
830 // What the factor is doing is the following Schur complement math (this matches augmentedHessianPP in code):
831 // size_t numMeasurements = measured_.size();
832 // Matrix F = Matrix::Zero(3*numMeasurements, 6 * nrUniqueKeys);
833 // for(size_t k=0; k<numMeasurements; k++){
834 // Key key_body = w_P_body_keys_.at(k);
835 // Key key_cal = body_P_cam_keys_.at(k);
836 // F.block<3,6>( 3*k , 6*keyToSlotMap[key_body] ) = Fs[k].block<3,6>(0,0);
837 // F.block<3,6>( 3*k , 6*keyToSlotMap[key_cal] ) = Fs[k].block<3,6>(0,6);
838 // }
839 // Matrix augH = Matrix::Zero(6*nrUniqueKeys+1,6*nrUniqueKeys+1);
840 // augH.block(0,0,6*nrUniqueKeys,6*nrUniqueKeys) = F.transpose() * F - F.transpose() * E * P * E.transpose() * F;
841 // Matrix infoVec = F.transpose() * b - F.transpose() * E * P * E.transpose() * b;
842 // augH.block(0,6*nrUniqueKeys,6*nrUniqueKeys,1) = infoVec;
843 // augH.block(6*nrUniqueKeys,0,1,6*nrUniqueKeys) = infoVec.transpose();
844 // augH(6*nrUniqueKeys,6*nrUniqueKeys) = b.squaredNorm();
845 // // The following is close to zero:
846 // std::cout << "norm diff: \n"<< Matrix(augH - Matrix(augmentedHessianUniqueKeys.selfadjointView())).lpNorm<Eigen::Infinity>() << std::endl;
847 // std::cout << "TEST MATRIX:" << std::endl;
848 // augmentedHessianUniqueKeys = SymmetricBlockMatrix(dims, augH);
849 /////////////////////////////////////////////////////////////////
850
851 Values result;
852 gttic_(SmartStereoProjectionFactorPP);
853 LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
854 result = optimizer.optimize();
855 gttoc_(SmartStereoProjectionFactorPP);
856 tictoc_finishedIteration_();
857
858 EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-5);
859
860 // This passes on my machine but gets and indeterminant linear system exception in CI.
861 // This is a very redundant test, so it's not a problem to omit.
862 // GaussianFactorGraph::shared_ptr GFG = graph.linearize(result);
863 // Matrix H = GFG->hessian().first;
864 // double det = H.determinant();
865 // // std::cout << "det " << det << std::endl; // det = 2.27581e+80 (so it's not underconstrained)
866 // VectorValues delta = GFG->optimize();
867 // VectorValues expected = VectorValues::Zero(delta);
868 // EXPECT(assert_equal(expected, delta, 1e-4));
869}
870
871/* *************************************************************************/
872TEST( SmartStereoProjectionFactorPP, 3poses_optimization_2ExtrinsicKeys ) {
873
874 // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
875 Pose3 w_Pose_cam1 = Pose3(Rot3::Ypr(y: -M_PI / 2, p: 0., r: -M_PI / 2), Point3(0, 0, 1));
876 StereoCamera cam1(w_Pose_cam1, K2);
877
878 // create second camera 1 meter to the right of first camera
879 Pose3 w_Pose_cam2 = w_Pose_cam1 * Pose3(Rot3(), Point3(1, 0, 0));
880 StereoCamera cam2(w_Pose_cam2, K2);
881
882 // create third camera 1 meter above the first camera
883 Pose3 w_Pose_cam3 = w_Pose_cam1 * Pose3(Rot3(), Point3(0, -1, 0));
884 StereoCamera cam3(w_Pose_cam3, K2);
885
886 // three landmarks ~5 meters infront of camera
887 Point3 landmark1(5, 0.5, 1.2);
888 Point3 landmark2(5, -0.5, 1.2);
889 Point3 landmark3(3, 0, 3.0);
890
891 // 1. Project three landmarks into three cameras and triangulate
892 vector<StereoPoint2> measurements_l1 = stereo_projectToMultipleCameras(cam1,
893 cam2, cam3, landmark: landmark1);
894 vector<StereoPoint2> measurements_l2 = stereo_projectToMultipleCameras(cam1,
895 cam2, cam3, landmark: landmark2);
896 vector<StereoPoint2> measurements_l3 = stereo_projectToMultipleCameras(cam1,
897 cam2, cam3, landmark: landmark3);
898
899 KeyVector poseKeys;
900 poseKeys.push_back(x: x1);
901 poseKeys.push_back(x: x2);
902 poseKeys.push_back(x: x3);
903
904 KeyVector extrinsicKeys;
905 extrinsicKeys.push_back(x: body_P_cam1_key);
906 extrinsicKeys.push_back(x: body_P_cam1_key);
907 extrinsicKeys.push_back(x: body_P_cam3_key);
908
909 SmartStereoProjectionParams smart_params;
910 smart_params.triangulation.enableEPI = true;
911 SmartStereoProjectionFactorPP::shared_ptr smartFactor1(new SmartStereoProjectionFactorPP(model, smart_params));
912 smartFactor1->add(measurements: measurements_l1, w_P_body_keys: poseKeys, body_P_cam_keys: extrinsicKeys, K: K2);
913
914 SmartStereoProjectionFactorPP::shared_ptr smartFactor2(new SmartStereoProjectionFactorPP(model, smart_params));
915 smartFactor2->add(measurements: measurements_l2, w_P_body_keys: poseKeys, body_P_cam_keys: extrinsicKeys, K: K2);
916
917 SmartStereoProjectionFactorPP::shared_ptr smartFactor3(new SmartStereoProjectionFactorPP(model, smart_params));
918 smartFactor3->add(measurements: measurements_l3, w_P_body_keys: poseKeys, body_P_cam_keys: extrinsicKeys, K: K2);
919
920 // relevant poses:
921 Pose3 body_Pose_cam = Pose3(Rot3::Ypr(y: -M_PI, p: 1., r: 0.1),Point3(0, 1, 0));
922 Pose3 w_Pose_body1 = w_Pose_cam1.compose(g: body_Pose_cam.inverse());
923 Pose3 w_Pose_body2 = w_Pose_cam2.compose(g: body_Pose_cam.inverse());
924 Pose3 w_Pose_body3 = w_Pose_cam3.compose(g: body_Pose_cam.inverse());
925
926 // Graph
927 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.10);
928 NonlinearFactorGraph graph;
929 graph.push_back(factor: smartFactor1);
930 graph.push_back(factor: smartFactor2);
931 graph.push_back(factor: smartFactor3);
932 graph.addPrior(key: x1, prior: w_Pose_body1, model: noisePrior);
933 graph.addPrior(key: x2, prior: w_Pose_body2, model: noisePrior);
934 graph.addPrior(key: x3, prior: w_Pose_body3, model: noisePrior);
935 // graph.addPrior(body_P_cam1_key, body_Pose_cam, noisePrior);
936 // we might need some prior on the calibration too
937 // graph.addPrior(body_P_cam_key, body_Pose_cam, noisePrior); // no need! the measurements will fix this!
938
939 // Values
940 Values values;
941 Pose3 noise_pose = Pose3(Rot3::Ypr(y: -M_PI / 100, p: 0., r: -M_PI / 100), Point3(0.01, 0.01, 0.01)); // smaller noise
942 values.insert(j: x1, val: w_Pose_body1);
943 values.insert(j: x2, val: w_Pose_body2);
944 values.insert(j: x3, val: w_Pose_body3);
945 values.insert(j: body_P_cam1_key, val: body_Pose_cam*noise_pose);
946 values.insert(j: body_P_cam3_key, val: body_Pose_cam*noise_pose);
947
948 // cost is large before optimization
949 double initialErrorSmart = graph.error(values);
950 EXPECT_DOUBLES_EQUAL(31986.961831653316, initialErrorSmart, 1e-5); // initial guess is noisy, so nonzero error
951
952 Values result;
953 gttic_(SmartStereoProjectionFactorPP);
954 LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
955 result = optimizer.optimize();
956 gttoc_(SmartStereoProjectionFactorPP);
957 tictoc_finishedIteration_();
958
959 EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-5);
960
961 // NOTE: the following would fail since the problem is underconstrained (while LM above works just fine!)
962 // GaussianFactorGraph::shared_ptr GFG = graph.linearize(result);
963 // VectorValues delta = GFG->optimize();
964 // VectorValues expected = VectorValues::Zero(delta);
965 // EXPECT(assert_equal(expected, delta, 1e-4));
966}
967
968/* *************************************************************************/
969TEST( SmartStereoProjectionFactorPP, monocular_multipleExtrinsicKeys ){
970 // make a realistic calibration matrix
971 double fov = 60; // degrees
972 size_t w=640,h=480;
973
974 Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h));
975
976 // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
977 Pose3 cameraPose1 = Pose3(Rot3::Ypr(y: -M_PI/2, p: 0., r: -M_PI/2), gtsam::Point3(0,0,1)); // body poses
978 Pose3 cameraPose2 = cameraPose1 * Pose3(Rot3(), Point3(1,0,0));
979 Pose3 cameraPose3 = cameraPose1 * Pose3(Rot3(), Point3(0,-1,0));
980
981 PinholeCamera<Cal3_S2> cam1(cameraPose1, *K); // with camera poses
982 PinholeCamera<Cal3_S2> cam2(cameraPose2, *K);
983 PinholeCamera<Cal3_S2> cam3(cameraPose3, *K);
984
985 // create arbitrary body_Pose_sensor (transforms from sensor to body)
986 Pose3 sensor_to_body = Pose3(Rot3::Ypr(y: -M_PI/2, p: 0., r: -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); //
987
988 // These are the poses we want to estimate, from camera measurements
989 Pose3 bodyPose1 = cameraPose1.compose(g: sensor_to_body.inverse());
990 Pose3 bodyPose2 = cameraPose2.compose(g: sensor_to_body.inverse());
991 Pose3 bodyPose3 = cameraPose3.compose(g: sensor_to_body.inverse());
992
993 // three landmarks ~5 meters infront of camera
994 Point3 landmark1(5, 0.5, 1.2);
995 Point3 landmark2(5, -0.5, 1.2);
996 Point3 landmark3(5, 0, 3.0);
997
998 Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
999
1000 // Project three landmarks into three cameras
1001 projectToMultipleCameras(cam1, cam2, cam3, landmark: landmark1, measurements_cam&: measurements_cam1);
1002 projectToMultipleCameras(cam1, cam2, cam3, landmark: landmark2, measurements_cam&: measurements_cam2);
1003 projectToMultipleCameras(cam1, cam2, cam3, landmark: landmark3, measurements_cam&: measurements_cam3);
1004
1005 // convert measurement to (degenerate) stereoPoint2 (with right pixel being NaN)
1006 vector<StereoPoint2> measurements_cam1_stereo, measurements_cam2_stereo, measurements_cam3_stereo;
1007 for(size_t k=0; k<measurements_cam1.size();k++)
1008 measurements_cam1_stereo.push_back(x: StereoPoint2(measurements_cam1[k].x() , missing_uR , measurements_cam1[k].y()));
1009
1010 for(size_t k=0; k<measurements_cam2.size();k++)
1011 measurements_cam2_stereo.push_back(x: StereoPoint2(measurements_cam2[k].x() , missing_uR , measurements_cam2[k].y()));
1012
1013 for(size_t k=0; k<measurements_cam3.size();k++)
1014 measurements_cam3_stereo.push_back(x: StereoPoint2(measurements_cam3[k].x() , missing_uR , measurements_cam3[k].y()));
1015
1016 KeyVector poseKeys;
1017 poseKeys.push_back(x: x1);
1018 poseKeys.push_back(x: x2);
1019 poseKeys.push_back(x: x3);
1020
1021 Symbol body_P_cam_key('P', 0);
1022 KeyVector extrinsicKeys;
1023 extrinsicKeys.push_back(x: body_P_cam_key);
1024 extrinsicKeys.push_back(x: body_P_cam_key);
1025 extrinsicKeys.push_back(x: body_P_cam_key);
1026
1027 SmartStereoProjectionParams smart_params;
1028 smart_params.setRankTolerance(1.0);
1029 smart_params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY);
1030 smart_params.setEnableEPI(false);
1031
1032 Cal3_S2Stereo::shared_ptr Kmono(new Cal3_S2Stereo(fov,w,h,b));
1033
1034 SmartStereoProjectionFactorPP smartFactor1(model, smart_params);
1035 smartFactor1.add(measurements: measurements_cam1_stereo, w_P_body_keys: poseKeys, body_P_cam_keys: extrinsicKeys, K: Kmono);
1036
1037 SmartStereoProjectionFactorPP smartFactor2(model, smart_params);
1038 smartFactor2.add(measurements: measurements_cam2_stereo, w_P_body_keys: poseKeys, body_P_cam_keys: extrinsicKeys, K: Kmono);
1039
1040 SmartStereoProjectionFactorPP smartFactor3(model, smart_params);
1041 smartFactor3.add(measurements: measurements_cam3_stereo, w_P_body_keys: poseKeys, body_P_cam_keys: extrinsicKeys, K: Kmono);
1042
1043 // Graph
1044 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.10);
1045 NonlinearFactorGraph graph;
1046 graph.push_back(factor: smartFactor1);
1047 graph.push_back(factor: smartFactor2);
1048 graph.push_back(factor: smartFactor3);
1049 graph.addPrior(key: x1, prior: bodyPose1, model: noisePrior);
1050 graph.addPrior(key: x2, prior: bodyPose2, model: noisePrior);
1051 graph.addPrior(key: x3, prior: bodyPose3, model: noisePrior);
1052 // we might need some prior on the calibration too
1053 // graph.addPrior(body_P_cam_key, body_Pose_cam, noisePrior); // no need! the measurements will fix this!
1054
1055 // Check errors at ground truth poses
1056 Values gtValues;
1057 gtValues.insert(j: x1, val: bodyPose1);
1058 gtValues.insert(j: x2, val: bodyPose2);
1059 gtValues.insert(j: x3, val: bodyPose3);
1060 gtValues.insert(j: body_P_cam_key, val: sensor_to_body);
1061 double actualError = graph.error(values: gtValues);
1062 double expectedError = 0.0;
1063 DOUBLES_EQUAL(expectedError, actualError, 1e-7)
1064
1065 // noisy values
1066 Values values;
1067 Pose3 noise_pose = Pose3(Rot3::Ypr(y: -M_PI / 100, p: 0., r: -M_PI / 100), Point3(0.01, 0.01, 0.01)); // smaller noise
1068 values.insert(j: x1, val: bodyPose1);
1069 values.insert(j: x2, val: bodyPose2);
1070 values.insert(j: x3, val: bodyPose3);
1071 values.insert(j: body_P_cam_key, val: sensor_to_body*noise_pose);
1072
1073 // cost is large before optimization
1074 double initialErrorSmart = graph.error(values);
1075 EXPECT_DOUBLES_EQUAL(2379.0012816261642, initialErrorSmart, 1e-5); // freeze value
1076
1077 Values result;
1078 gttic_(SmartStereoProjectionFactorPP);
1079 LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
1080 result = optimizer.optimize();
1081 gttoc_(SmartStereoProjectionFactorPP);
1082 tictoc_finishedIteration_();
1083
1084 EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-5);
1085 EXPECT(assert_equal(sensor_to_body,result.at<Pose3>(body_P_cam_key), 1e-1));
1086}
1087
1088/* *************************************************************************/
1089TEST( SmartStereoProjectionFactorPP, landmarkDistance ) {
1090
1091 // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
1092 Pose3 pose1 = Pose3(Rot3::Ypr(y: -M_PI / 2, p: 0., r: -M_PI / 2), Point3(0, 0, 1));
1093 StereoCamera cam1(pose1, K);
1094 // create second camera 1 meter to the right of first camera
1095 Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0));
1096 StereoCamera cam2(pose2, K);
1097 // create third camera 1 meter above the first camera
1098 Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0));
1099 StereoCamera cam3(pose3, K);
1100
1101 KeyVector views;
1102 views.push_back(x: x1);
1103 views.push_back(x: x2);
1104 views.push_back(x: x3);
1105
1106 Symbol body_P_cam_key('P', 0);
1107 KeyVector extrinsicKeys;
1108 extrinsicKeys.push_back(x: body_P_cam_key);
1109 extrinsicKeys.push_back(x: body_P_cam_key);
1110 extrinsicKeys.push_back(x: body_P_cam_key);
1111
1112 // three landmarks ~5 meters infront of camera
1113 Point3 landmark1(5, 0.5, 1.2);
1114 Point3 landmark2(5, -0.5, 1.2);
1115 Point3 landmark3(3, 0, 3.0);
1116
1117 // 1. Project three landmarks into three cameras and triangulate
1118 vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
1119 cam2, cam3, landmark: landmark1);
1120 vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1,
1121 cam2, cam3, landmark: landmark2);
1122 vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
1123 cam2, cam3, landmark: landmark3);
1124
1125 SmartStereoProjectionParams params;
1126 params.setLinearizationMode(HESSIAN);
1127 params.setLandmarkDistanceThreshold(2);
1128
1129 SmartStereoProjectionFactorPP::shared_ptr smartFactor1(new SmartStereoProjectionFactorPP(model, params));
1130 smartFactor1->add(measurements: measurements_cam1, w_P_body_keys: views, body_P_cam_keys: extrinsicKeys, K);
1131
1132 SmartStereoProjectionFactorPP::shared_ptr smartFactor2(new SmartStereoProjectionFactorPP(model, params));
1133 smartFactor2->add(measurements: measurements_cam2, w_P_body_keys: views, body_P_cam_keys: extrinsicKeys, K);
1134
1135 SmartStereoProjectionFactorPP::shared_ptr smartFactor3(new SmartStereoProjectionFactorPP(model, params));
1136 smartFactor3->add(measurements: measurements_cam3, w_P_body_keys: views, body_P_cam_keys: extrinsicKeys, K);
1137
1138 // create graph
1139 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.10);
1140 NonlinearFactorGraph graph;
1141 graph.push_back(factor: smartFactor1);
1142 graph.push_back(factor: smartFactor2);
1143 graph.push_back(factor: smartFactor3);
1144 graph.addPrior(key: x1, prior: pose1, model: noisePrior);
1145 graph.addPrior(key: x2, prior: pose2, model: noisePrior);
1146 graph.addPrior(key: body_P_cam_key, prior: Pose3::Identity(), model: noisePrior);
1147
1148 // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
1149 Pose3 noise_pose = Pose3(Rot3::Ypr(y: -M_PI / 100, p: 0., r: -M_PI / 100),
1150 Point3(0.1, 0.1, 0.1)); // smaller noise
1151 Values values;
1152 values.insert(j: x1, val: pose1);
1153 values.insert(j: x2, val: pose2);
1154 values.insert(j: x3, val: pose3 * noise_pose);
1155 values.insert(j: body_P_cam_key, val: Pose3::Identity());
1156
1157 // All smart factors are disabled and pose should remain where it is
1158 Values result;
1159 LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
1160 result = optimizer.optimize();
1161 EXPECT(assert_equal(values.at<Pose3>(x3), result.at<Pose3>(x3), 1e-5));
1162 EXPECT_DOUBLES_EQUAL(graph.error(values), graph.error(result), 1e-5);
1163}
1164
1165/* *************************************************************************/
1166TEST( SmartStereoProjectionFactorPP, dynamicOutlierRejection ) {
1167
1168 KeyVector views;
1169 views.push_back(x: x1);
1170 views.push_back(x: x2);
1171 views.push_back(x: x3);
1172
1173 Symbol body_P_cam_key('P', 0);
1174 KeyVector extrinsicKeys;
1175 extrinsicKeys.push_back(x: body_P_cam_key);
1176 extrinsicKeys.push_back(x: body_P_cam_key);
1177 extrinsicKeys.push_back(x: body_P_cam_key);
1178
1179 // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
1180 Pose3 pose1 = Pose3(Rot3::Ypr(y: -M_PI / 2, p: 0., r: -M_PI / 2), Point3(0, 0, 1));
1181 StereoCamera cam1(pose1, K);
1182 // create second camera 1 meter to the right of first camera
1183 Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0));
1184 StereoCamera cam2(pose2, K);
1185 // create third camera 1 meter above the first camera
1186 Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0));
1187 StereoCamera cam3(pose3, K);
1188
1189 // three landmarks ~5 meters infront of camera
1190 Point3 landmark1(5, 0.5, 1.2);
1191 Point3 landmark2(5, -0.5, 1.2);
1192 Point3 landmark3(3, 0, 3.0);
1193 Point3 landmark4(5, -0.5, 1);
1194
1195 // 1. Project four landmarks into three cameras and triangulate
1196 vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
1197 cam2, cam3, landmark: landmark1);
1198 vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1,
1199 cam2, cam3, landmark: landmark2);
1200 vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
1201 cam2, cam3, landmark: landmark3);
1202 vector<StereoPoint2> measurements_cam4 = stereo_projectToMultipleCameras(cam1,
1203 cam2, cam3, landmark: landmark4);
1204
1205 measurements_cam4.at(n: 0) = measurements_cam4.at(n: 0) + StereoPoint2(10, 10, 1); // add outlier
1206
1207 SmartStereoProjectionParams params;
1208 params.setLinearizationMode(HESSIAN);
1209 params.setDynamicOutlierRejectionThreshold(1);
1210
1211 SmartStereoProjectionFactorPP::shared_ptr smartFactor1(new SmartStereoProjectionFactorPP(model, params));
1212 smartFactor1->add(measurements: measurements_cam1, w_P_body_keys: views, body_P_cam_keys: extrinsicKeys, K);
1213
1214 SmartStereoProjectionFactorPP::shared_ptr smartFactor2(new SmartStereoProjectionFactorPP(model, params));
1215 smartFactor2->add(measurements: measurements_cam2, w_P_body_keys: views, body_P_cam_keys: extrinsicKeys, K);
1216
1217 SmartStereoProjectionFactorPP::shared_ptr smartFactor3(new SmartStereoProjectionFactorPP(model, params));
1218 smartFactor3->add(measurements: measurements_cam3, w_P_body_keys: views, body_P_cam_keys: extrinsicKeys, K);
1219
1220 SmartStereoProjectionFactorPP::shared_ptr smartFactor4(new SmartStereoProjectionFactorPP(model, params));
1221 smartFactor4->add(measurements: measurements_cam4, w_P_body_keys: views, body_P_cam_keys: extrinsicKeys, K);
1222
1223 // same as factor 4, but dynamic outlier rejection is off
1224 SmartStereoProjectionFactorPP::shared_ptr smartFactor4b(new SmartStereoProjectionFactorPP(model));
1225 smartFactor4b->add(measurements: measurements_cam4, w_P_body_keys: views, body_P_cam_keys: extrinsicKeys, K);
1226
1227 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.10);
1228
1229 NonlinearFactorGraph graph;
1230 graph.push_back(factor: smartFactor1);
1231 graph.push_back(factor: smartFactor2);
1232 graph.push_back(factor: smartFactor3);
1233 graph.push_back(factor: smartFactor4);
1234 graph.addPrior(key: x1, prior: pose1, model: noisePrior);
1235 graph.addPrior(key: x2, prior: pose2, model: noisePrior);
1236 graph.addPrior(key: x3, prior: pose3, model: noisePrior);
1237
1238 Pose3 noise_pose = Pose3(Rot3::Ypr(y: -M_PI / 100, p: 0., r: -M_PI / 100),
1239 Point3(0.1, 0.1, 0.1)); // smaller noise
1240 Values values;
1241 values.insert(j: x1, val: pose1);
1242 values.insert(j: x2, val: pose2);
1243 values.insert(j: x3, val: pose3);
1244 values.insert(j: body_P_cam_key, val: Pose3::Identity());
1245
1246 EXPECT_DOUBLES_EQUAL(0, smartFactor1->error(values), 1e-9);
1247 EXPECT_DOUBLES_EQUAL(0, smartFactor2->error(values), 1e-9);
1248 EXPECT_DOUBLES_EQUAL(0, smartFactor3->error(values), 1e-9);
1249 // zero error due to dynamic outlier rejection
1250 EXPECT_DOUBLES_EQUAL(0, smartFactor4->error(values), 1e-9);
1251
1252 // dynamic outlier rejection is off
1253 EXPECT_DOUBLES_EQUAL(6147.3947317473921, smartFactor4b->error(values), 1e-9);
1254
1255 // Factors 1-3 should have valid point, factor 4 should not
1256 EXPECT(smartFactor1->point());
1257 EXPECT(smartFactor2->point());
1258 EXPECT(smartFactor3->point());
1259 EXPECT(smartFactor4->point().outlier());
1260 EXPECT(smartFactor4b->point());
1261
1262 // Factor 4 is disabled, pose 3 stays put
1263 Values result;
1264 LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
1265 result = optimizer.optimize();
1266 EXPECT(assert_equal(Pose3::Identity(), result.at<Pose3>(body_P_cam_key)));
1267}
1268
1269/* ************************************************************************* */
1270int main() {
1271 TestResult tr;
1272 return TestRegistry::runAllTests(result&: tr);
1273}
1274/* ************************************************************************* */
1275
1276