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 testSerializationSLAM.cpp
14 * @brief test serialization
15 * @author Richard Roberts
16 * @date Feb 7, 2012
17 */
18
19#include <CppUnitLite/TestHarness.h>
20
21#include <tests/smallExample.h>
22
23#include <gtsam/sam/RangeFactor.h>
24#include <gtsam/sam/BearingRangeFactor.h>
25
26#include <gtsam/slam/BetweenFactor.h>
27#include <gtsam/slam/GeneralSFMFactor.h>
28#include <gtsam/slam/ProjectionFactor.h>
29#include <gtsam/slam/dataset.h>
30#include <gtsam/slam/StereoFactor.h>
31
32#include <gtsam/geometry/Point2.h>
33#include <gtsam/geometry/StereoPoint2.h>
34#include <gtsam/geometry/Point3.h>
35#include <gtsam/geometry/Rot2.h>
36#include <gtsam/geometry/Rot3.h>
37#include <gtsam/geometry/Pose2.h>
38#include <gtsam/geometry/Pose3.h>
39#include <gtsam/geometry/Cal3_S2.h>
40#include <gtsam/geometry/Cal3DS2.h>
41#include <gtsam/geometry/Cal3_S2Stereo.h>
42#include <gtsam/geometry/CalibratedCamera.h>
43#include <gtsam/geometry/PinholeCamera.h>
44#include <gtsam/geometry/StereoCamera.h>
45#include <gtsam/geometry/SimpleCamera.h>
46
47#include <gtsam/nonlinear/NonlinearEquality.h>
48#include <gtsam/nonlinear/PriorFactor.h>
49#include <gtsam/linear/SubgraphPreconditioner.h>
50#include <gtsam/linear/GaussianISAM.h>
51#include <gtsam/inference/Symbol.h>
52#include <gtsam/base/serializationTestHelpers.h>
53#include <gtsam/base/std_optional_serialization.h>
54
55#include <boost/archive/xml_iarchive.hpp>
56#include <boost/serialization/export.hpp>
57
58using namespace std;
59using namespace gtsam;
60using namespace gtsam::serializationTestHelpers;
61
62// Creating as many permutations of factors as possible
63typedef PriorFactor<Point2> PriorFactorPoint2;
64typedef PriorFactor<StereoPoint2> PriorFactorStereoPoint2;
65typedef PriorFactor<Point3> PriorFactorPoint3;
66typedef PriorFactor<Rot2> PriorFactorRot2;
67typedef PriorFactor<Rot3> PriorFactorRot3;
68typedef PriorFactor<Pose2> PriorFactorPose2;
69typedef PriorFactor<Pose3> PriorFactorPose3;
70typedef PriorFactor<Cal3_S2> PriorFactorCal3_S2;
71typedef PriorFactor<Cal3DS2> PriorFactorCal3DS2;
72typedef PriorFactor<CalibratedCamera> PriorFactorCalibratedCamera;
73typedef PriorFactor<PinholeCameraCal3_S2> PriorFactorPinholeCameraCal3_S2;
74typedef PriorFactor<StereoCamera> PriorFactorStereoCamera;
75
76typedef BetweenFactor<Point2> BetweenFactorPoint2;
77typedef BetweenFactor<Point3> BetweenFactorPoint3;
78typedef BetweenFactor<Rot2> BetweenFactorRot2;
79typedef BetweenFactor<Rot3> BetweenFactorRot3;
80typedef BetweenFactor<Pose2> BetweenFactorPose2;
81typedef BetweenFactor<Pose3> BetweenFactorPose3;
82
83typedef NonlinearEquality<Point2> NonlinearEqualityPoint2;
84typedef NonlinearEquality<StereoPoint2> NonlinearEqualityStereoPoint2;
85typedef NonlinearEquality<Point3> NonlinearEqualityPoint3;
86typedef NonlinearEquality<Rot2> NonlinearEqualityRot2;
87typedef NonlinearEquality<Rot3> NonlinearEqualityRot3;
88typedef NonlinearEquality<Pose2> NonlinearEqualityPose2;
89typedef NonlinearEquality<Pose3> NonlinearEqualityPose3;
90typedef NonlinearEquality<Cal3_S2> NonlinearEqualityCal3_S2;
91typedef NonlinearEquality<Cal3DS2> NonlinearEqualityCal3DS2;
92typedef NonlinearEquality<CalibratedCamera> NonlinearEqualityCalibratedCamera;
93typedef NonlinearEquality<PinholeCameraCal3_S2> NonlinearEqualityPinholeCameraCal3_S2;
94typedef NonlinearEquality<StereoCamera> NonlinearEqualityStereoCamera;
95
96typedef RangeFactor<Pose2, Point2> RangeFactor2D;
97typedef RangeFactor<Pose3, Point3> RangeFactor3D;
98typedef RangeFactor<Pose2, Pose2> RangeFactorPose2;
99typedef RangeFactor<Pose3, Pose3> RangeFactorPose3;
100typedef RangeFactor<CalibratedCamera, Point3> RangeFactorCalibratedCameraPoint;
101typedef RangeFactor<PinholeCameraCal3_S2, Point3> RangeFactorPinholeCameraCal3_S2Point;
102typedef RangeFactor<CalibratedCamera, CalibratedCamera> RangeFactorCalibratedCamera;
103typedef RangeFactor<PinholeCameraCal3_S2, PinholeCameraCal3_S2> RangeFactorPinholeCameraCal3_S2;
104
105typedef BearingRangeFactor<Pose2, Point2> BearingRangeFactor2D;
106typedef BearingRangeFactor<Pose3, Point3> BearingRangeFactor3D;
107
108typedef GenericProjectionFactor<Pose3, Point3, Cal3_S2> GenericProjectionFactorCal3_S2;
109typedef GenericProjectionFactor<Pose3, Point3, Cal3DS2> GenericProjectionFactorCal3DS2;
110
111typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
112typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
113
114typedef gtsam::GeneralSFMFactor2<gtsam::Cal3_S2> GeneralSFMFactor2Cal3_S2;
115
116typedef gtsam::GenericStereoFactor<gtsam::Pose3, gtsam::Point3> GenericStereoFactor3D;
117
118
119// Convenience for named keys
120using symbol_shorthand::X;
121using symbol_shorthand::L;
122
123
124/* Create GUIDs for Noisemodels */
125/* ************************************************************************* */
126BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained")
127BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal")
128BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian")
129BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit")
130BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic")
131BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Robust, "gtsam_noiseModel_Robust")
132
133BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Base , "gtsam_noiseModel_mEstimator_Base")
134BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Null , "gtsam_noiseModel_mEstimator_Null")
135BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Fair , "gtsam_noiseModel_mEstimator_Fair")
136BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Huber, "gtsam_noiseModel_mEstimator_Huber")
137BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Tukey, "gtsam_noiseModel_mEstimator_Tukey")
138
139BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel")
140BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal")
141
142/* Create GUIDs for geometry */
143/* ************************************************************************* */
144GTSAM_VALUE_EXPORT(gtsam::Point2)
145GTSAM_VALUE_EXPORT(gtsam::StereoPoint2)
146GTSAM_VALUE_EXPORT(gtsam::Point3)
147GTSAM_VALUE_EXPORT(gtsam::Rot2)
148GTSAM_VALUE_EXPORT(gtsam::Rot3)
149GTSAM_VALUE_EXPORT(gtsam::Pose2)
150GTSAM_VALUE_EXPORT(gtsam::Pose3)
151GTSAM_VALUE_EXPORT(gtsam::Cal3_S2)
152GTSAM_VALUE_EXPORT(gtsam::Cal3DS2)
153GTSAM_VALUE_EXPORT(gtsam::Cal3_S2Stereo)
154GTSAM_VALUE_EXPORT(gtsam::CalibratedCamera)
155GTSAM_VALUE_EXPORT(gtsam::PinholeCameraCal3_S2)
156GTSAM_VALUE_EXPORT(gtsam::StereoCamera)
157
158/* Create GUIDs for factors */
159/* ************************************************************************* */
160BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor")
161BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor")
162
163BOOST_CLASS_EXPORT_GUID(PriorFactorPoint2, "gtsam::PriorFactorPoint2")
164BOOST_CLASS_EXPORT_GUID(PriorFactorStereoPoint2, "gtsam::PriorFactorStereoPoint2")
165BOOST_CLASS_EXPORT_GUID(PriorFactorPoint3, "gtsam::PriorFactorPoint3")
166BOOST_CLASS_EXPORT_GUID(PriorFactorRot2, "gtsam::PriorFactorRot2")
167BOOST_CLASS_EXPORT_GUID(PriorFactorRot3, "gtsam::PriorFactorRot3")
168BOOST_CLASS_EXPORT_GUID(PriorFactorPose2, "gtsam::PriorFactorPose2")
169BOOST_CLASS_EXPORT_GUID(PriorFactorPose3, "gtsam::PriorFactorPose3")
170BOOST_CLASS_EXPORT_GUID(PriorFactorCal3_S2, "gtsam::PriorFactorCal3_S2")
171BOOST_CLASS_EXPORT_GUID(PriorFactorCal3DS2, "gtsam::PriorFactorCal3DS2")
172BOOST_CLASS_EXPORT_GUID(PriorFactorCalibratedCamera, "gtsam::PriorFactorCalibratedCamera")
173BOOST_CLASS_EXPORT_GUID(PriorFactorStereoCamera, "gtsam::PriorFactorStereoCamera")
174
175BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint2, "gtsam::BetweenFactorPoint2")
176BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint3, "gtsam::BetweenFactorPoint3")
177BOOST_CLASS_EXPORT_GUID(BetweenFactorRot2, "gtsam::BetweenFactorRot2")
178BOOST_CLASS_EXPORT_GUID(BetweenFactorRot3, "gtsam::BetweenFactorRot3")
179BOOST_CLASS_EXPORT_GUID(BetweenFactorPose2, "gtsam::BetweenFactorPose2")
180BOOST_CLASS_EXPORT_GUID(BetweenFactorPose3, "gtsam::BetweenFactorPose3")
181
182BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint2, "gtsam::NonlinearEqualityPoint2")
183BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoPoint2, "gtsam::NonlinearEqualityStereoPoint2")
184BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint3, "gtsam::NonlinearEqualityPoint3")
185BOOST_CLASS_EXPORT_GUID(NonlinearEqualityRot2, "gtsam::NonlinearEqualityRot2")
186BOOST_CLASS_EXPORT_GUID(NonlinearEqualityRot3, "gtsam::NonlinearEqualityRot3")
187BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPose2, "gtsam::NonlinearEqualityPose2")
188BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPose3, "gtsam::NonlinearEqualityPose3")
189BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3_S2, "gtsam::NonlinearEqualityCal3_S2")
190BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3DS2, "gtsam::NonlinearEqualityCal3DS2")
191BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCalibratedCamera, "gtsam::NonlinearEqualityCalibratedCamera")
192BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoCamera, "gtsam::NonlinearEqualityStereoCamera")
193
194BOOST_CLASS_EXPORT_GUID(RangeFactor2D, "gtsam::RangeFactor2D")
195BOOST_CLASS_EXPORT_GUID(RangeFactor3D, "gtsam::RangeFactor3D")
196BOOST_CLASS_EXPORT_GUID(RangeFactorPose2, "gtsam::RangeFactorPose2")
197BOOST_CLASS_EXPORT_GUID(RangeFactorPose3, "gtsam::RangeFactorPose3")
198BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCameraPoint, "gtsam::RangeFactorCalibratedCameraPoint")
199BOOST_CLASS_EXPORT_GUID(RangeFactorPinholeCameraCal3_S2Point, "gtsam::RangeFactorPinholeCameraCal3_S2Point")
200BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCamera, "gtsam::RangeFactorCalibratedCamera")
201BOOST_CLASS_EXPORT_GUID(RangeFactorPinholeCameraCal3_S2, "gtsam::RangeFactorPinholeCameraCal3_S2")
202
203BOOST_CLASS_EXPORT_GUID(BearingRangeFactor2D, "gtsam::BearingRangeFactor2D")
204
205BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3_S2, "gtsam::GenericProjectionFactorCal3_S2")
206BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3DS2, "gtsam::GenericProjectionFactorCal3DS2")
207
208BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3_S2, "gtsam::GeneralSFMFactorCal3_S2")
209BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2")
210
211BOOST_CLASS_EXPORT_GUID(GeneralSFMFactor2Cal3_S2, "gtsam::GeneralSFMFactor2Cal3_S2")
212
213BOOST_CLASS_EXPORT_GUID(GenericStereoFactor3D, "gtsam::GenericStereoFactor3D")
214
215
216/* ************************************************************************* */
217TEST (testSerializationSLAM, smallExample_linear) {
218 using namespace example;
219
220 const Ordering ordering{X(j: 1), X(j: 2), L(j: 1)};
221 EXPECT(equalsObj(ordering));
222 EXPECT(equalsXML(ordering));
223 EXPECT(equalsBinary(ordering));
224
225 GaussianFactorGraph fg = createGaussianFactorGraph();
226 EXPECT(equalsObj(fg));
227 EXPECT(equalsXML(fg));
228 EXPECT(equalsBinary(fg));
229
230 GaussianBayesNet cbn = createSmallGaussianBayesNet();
231 EXPECT(equalsObj(cbn));
232 EXPECT(equalsXML(cbn));
233 EXPECT(equalsBinary(cbn));
234}
235
236/* ************************************************************************* */
237TEST (testSerializationSLAM, gaussianISAM) {
238 using namespace example;
239 GaussianFactorGraph smoother = createSmoother(T: 7);
240 GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal();
241 GaussianISAM isam(bayesTree);
242
243 EXPECT(equalsObj(isam));
244 EXPECT(equalsXML(isam));
245 EXPECT(equalsBinary(isam));
246}
247
248/* ************************************************************************* */
249/* Create GUIDs for factors in simulated2D */
250BOOST_CLASS_EXPORT_GUID(simulated2D::Prior, "gtsam::simulated2D::Prior" )
251BOOST_CLASS_EXPORT_GUID(simulated2D::Odometry, "gtsam::simulated2D::Odometry" )
252BOOST_CLASS_EXPORT_GUID(simulated2D::Measurement, "gtsam::simulated2D::Measurement")
253
254/* ************************************************************************* */
255TEST (testSerializationSLAM, smallExample_nonlinear) {
256 using namespace example;
257 NonlinearFactorGraph nfg = createNonlinearFactorGraph();
258 Values c1 = createValues();
259 EXPECT(equalsObj(nfg));
260 EXPECT(equalsXML(nfg));
261 EXPECT(equalsBinary(nfg));
262
263 EXPECT(equalsObj(c1));
264 EXPECT(equalsXML(c1));
265 EXPECT(equalsBinary(c1));
266}
267
268/* ************************************************************************* */
269TEST (testSerializationSLAM, factors) {
270
271 Point2 point2(1.0, 2.0);
272 StereoPoint2 stereoPoint2(1.0, 2.0, 3.0);
273 Point3 point3(1.0, 2.0, 3.0);
274 Rot2 rot2(1.0);
275 Rot3 rot3(Rot3::RzRyRx(x: 1.0, y: 2.0, z: 3.0));
276 Pose2 pose2(rot2, point2);
277 Pose3 pose3(rot3, point3);
278 Cal3_S2 cal3_s2(1.0, 2.0, 3.0, 4.0, 5.0);
279 Cal3DS2 cal3ds2(1.0, 2.0, 3.0, 4.0, 5.0,6.0, 7.0, 8.0, 9.0);
280 Cal3_S2Stereo cal3_s2stereo(1.0, 2.0, 3.0, 4.0, 5.0, 1.0);
281 CalibratedCamera calibratedCamera(pose3);
282 PinholeCamera<Cal3_S2> simpleCamera(pose3, cal3_s2);
283 StereoCamera stereoCamera(pose3, std::make_shared<Cal3_S2Stereo>(args&: cal3_s2stereo));
284
285
286 Symbol a01('a',1), a02('a',2), a03('a',3), a04('a',4), a05('a',5),
287 a06('a',6), a07('a',7), a08('a',8), a09('a',9), a10('a',10),
288 a11('a',11), a12('a',12), a13('a',13), a14('a',14), a15('a',15);
289 Symbol b01('b',1), b02('b',2), b03('b',3), b04('b',4), b05('b',5),
290 b06('b',6), b07('b',7), b08('b',8), b09('b',9), b10('b',10),
291 b11('b',11), b12('b',12), b13('b',13), b14('b',14), b15('b',15);
292
293 Values values;
294 values.insert(j: a03, val: point2);
295 values.insert(j: a04, val: stereoPoint2);
296 values.insert(j: a05, val: point3);
297 values.insert(j: a06, val: rot2);
298 values.insert(j: a07, val: rot3);
299 values.insert(j: a08, val: pose2);
300 values.insert(j: a09, val: pose3);
301 values.insert(j: a10, val: cal3_s2);
302 values.insert(j: a11, val: cal3ds2);
303 values.insert(j: a12, val: calibratedCamera);
304 values.insert(j: a13, val: simpleCamera);
305 values.insert(j: a14, val: stereoCamera);
306
307
308 SharedNoiseModel model1 = noiseModel::Isotropic::Sigma(dim: 1, sigma: 0.3);
309 SharedNoiseModel model2 = noiseModel::Isotropic::Sigma(dim: 2, sigma: 0.3);
310 SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(dim: 3, sigma: 0.3);
311 SharedNoiseModel model4 = noiseModel::Isotropic::Sigma(dim: 4, sigma: 0.3);
312 SharedNoiseModel model5 = noiseModel::Isotropic::Sigma(dim: 5, sigma: 0.3);
313 SharedNoiseModel model6 = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.3);
314 SharedNoiseModel model9 = noiseModel::Isotropic::Sigma(dim: 9, sigma: 0.3);
315 SharedNoiseModel model11 = noiseModel::Isotropic::Sigma(dim: 11, sigma: 0.3);
316
317 SharedNoiseModel robust1 = noiseModel::Robust::Create(
318 robust: noiseModel::mEstimator::Huber::Create(k: 10.0, reweight: noiseModel::mEstimator::Huber::Scalar),
319 noise: noiseModel::Unit::Create(dim: 2));
320
321 EXPECT(equalsDereferenced(robust1));
322 EXPECT(equalsDereferencedXML(robust1));
323 EXPECT(equalsDereferencedBinary(robust1));
324
325 PriorFactorPoint2 priorFactorPoint2(a03, point2, model2);
326 PriorFactorStereoPoint2 priorFactorStereoPoint2(a04, stereoPoint2, model3);
327 PriorFactorPoint3 priorFactorPoint3(a05, point3, model3);
328 PriorFactorRot2 priorFactorRot2(a06, rot2, model1);
329 PriorFactorRot3 priorFactorRot3(a07, rot3, model3);
330 PriorFactorPose2 priorFactorPose2(a08, pose2, model3);
331 PriorFactorPose3 priorFactorPose3(a09, pose3, model6);
332 PriorFactorCal3_S2 priorFactorCal3_S2(a10, cal3_s2, model5);
333 PriorFactorCal3DS2 priorFactorCal3DS2(a11, cal3ds2, model9);
334 PriorFactorCalibratedCamera priorFactorCalibratedCamera(a12, calibratedCamera, model6);
335 PriorFactorStereoCamera priorFactorStereoCamera(a14, stereoCamera, model11);
336
337 BetweenFactorPoint2 betweenFactorPoint2(a03, b03, point2, model2);
338 BetweenFactorPoint3 betweenFactorPoint3(a05, b05, point3, model3);
339 BetweenFactorRot2 betweenFactorRot2(a06, b06, rot2, model1);
340 BetweenFactorRot3 betweenFactorRot3(a07, b07, rot3, model3);
341 BetweenFactorPose2 betweenFactorPose2(a08, b08, pose2, model3);
342 BetweenFactorPose3 betweenFactorPose3(a09, b09, pose3, model6);
343
344 NonlinearEqualityPoint2 nonlinearEqualityPoint2(a03, point2);
345 NonlinearEqualityStereoPoint2 nonlinearEqualityStereoPoint2(a04, stereoPoint2);
346 NonlinearEqualityPoint3 nonlinearEqualityPoint3(a05, point3);
347 NonlinearEqualityRot2 nonlinearEqualityRot2(a06, rot2);
348 NonlinearEqualityRot3 nonlinearEqualityRot3(a07, rot3);
349 NonlinearEqualityPose2 nonlinearEqualityPose2(a08, pose2);
350 NonlinearEqualityPose3 nonlinearEqualityPose3(a09, pose3);
351 NonlinearEqualityCal3_S2 nonlinearEqualityCal3_S2(a10, cal3_s2);
352 NonlinearEqualityCal3DS2 nonlinearEqualityCal3DS2(a11, cal3ds2);
353 NonlinearEqualityCalibratedCamera nonlinearEqualityCalibratedCamera(a12, calibratedCamera);
354 NonlinearEqualityStereoCamera nonlinearEqualityStereoCamera(a14, stereoCamera);
355
356 RangeFactor2D rangeFactor2D(a08, a03, 2.0, model1);
357 RangeFactor3D rangeFactor3D(a09, a05, 2.0, model1);
358 RangeFactorPose2 rangeFactorPose2(a08, b08, 2.0, model1);
359 RangeFactorPose3 rangeFactorPose3(a09, b09, 2.0, model1);
360 RangeFactorCalibratedCameraPoint rangeFactorCalibratedCameraPoint(a12, a05, 2.0, model1);
361 RangeFactorPinholeCameraCal3_S2Point rangeFactorPinholeCameraCal3_S2Point(a13, a05, 2.0, model1);
362 RangeFactorCalibratedCamera rangeFactorCalibratedCamera(a12, b12, 2.0, model1);
363 RangeFactorPinholeCameraCal3_S2 rangeFactorPinholeCameraCal3_S2(a13, b13, 2.0, model1);
364
365 BearingRangeFactor2D bearingRangeFactor2D(a08, a03, rot2, 2.0, model2);
366
367 GenericProjectionFactorCal3_S2 genericProjectionFactorCal3_S2(point2, model2, a09, a05, std::make_shared<Cal3_S2>(args&: cal3_s2));
368 GenericProjectionFactorCal3DS2 genericProjectionFactorCal3DS2(point2, model2, a09, a05, std::make_shared<Cal3DS2>(args&: cal3ds2));
369
370 GeneralSFMFactorCal3_S2 generalSFMFactorCal3_S2(point2, model2, a13, a05);
371
372 GeneralSFMFactor2Cal3_S2 generalSFMFactor2Cal3_S2(point2, model2, a09, a05, a10);
373
374 GenericStereoFactor3D genericStereoFactor3D(stereoPoint2, model3, a09, a05, std::make_shared<Cal3_S2Stereo>(args&: cal3_s2stereo));
375
376
377 NonlinearFactorGraph graph;
378 graph += priorFactorPoint2;
379 graph += priorFactorStereoPoint2;
380 graph += priorFactorPoint3;
381 graph += priorFactorRot2;
382 graph += priorFactorRot3;
383 graph += priorFactorPose2;
384 graph += priorFactorPose3;
385 graph += priorFactorCal3_S2;
386 graph += priorFactorCal3DS2;
387 graph += priorFactorCalibratedCamera;
388 graph += priorFactorStereoCamera;
389
390 graph += betweenFactorPoint2;
391 graph += betweenFactorPoint3;
392 graph += betweenFactorRot2;
393 graph += betweenFactorRot3;
394 graph += betweenFactorPose2;
395 graph += betweenFactorPose3;
396
397 graph += nonlinearEqualityPoint2;
398 graph += nonlinearEqualityStereoPoint2;
399 graph += nonlinearEqualityPoint3;
400 graph += nonlinearEqualityRot2;
401 graph += nonlinearEqualityRot3;
402 graph += nonlinearEqualityPose2;
403 graph += nonlinearEqualityPose3;
404 graph += nonlinearEqualityCal3_S2;
405 graph += nonlinearEqualityCal3DS2;
406 graph += nonlinearEqualityCalibratedCamera;
407 graph += nonlinearEqualityStereoCamera;
408
409 graph += rangeFactor2D;
410 graph += rangeFactor3D;
411 graph += rangeFactorPose2;
412 graph += rangeFactorPose3;
413 graph += rangeFactorCalibratedCameraPoint;
414 graph += rangeFactorPinholeCameraCal3_S2Point;
415 graph += rangeFactorCalibratedCamera;
416 graph += rangeFactorPinholeCameraCal3_S2;
417
418 graph += bearingRangeFactor2D;
419
420 graph += genericProjectionFactorCal3_S2;
421 graph += genericProjectionFactorCal3DS2;
422
423 graph += generalSFMFactorCal3_S2;
424
425 graph += generalSFMFactor2Cal3_S2;
426
427 graph += genericStereoFactor3D;
428
429
430 // text
431 EXPECT(equalsObj<Symbol>(a01));
432 EXPECT(equalsObj<Symbol>(b02));
433 EXPECT(equalsObj<Values>(values));
434 EXPECT(equalsObj<NonlinearFactorGraph>(graph));
435
436 EXPECT(equalsObj<PriorFactorPoint2>(priorFactorPoint2));
437 EXPECT(equalsObj<PriorFactorStereoPoint2>(priorFactorStereoPoint2));
438 EXPECT(equalsObj<PriorFactorPoint3>(priorFactorPoint3));
439 EXPECT(equalsObj<PriorFactorRot2>(priorFactorRot2));
440 EXPECT(equalsObj<PriorFactorRot3>(priorFactorRot3));
441 EXPECT(equalsObj<PriorFactorPose2>(priorFactorPose2));
442 EXPECT(equalsObj<PriorFactorPose3>(priorFactorPose3));
443 EXPECT(equalsObj<PriorFactorCal3_S2>(priorFactorCal3_S2));
444 EXPECT(equalsObj<PriorFactorCal3DS2>(priorFactorCal3DS2));
445 EXPECT(equalsObj<PriorFactorCalibratedCamera>(priorFactorCalibratedCamera));
446 EXPECT(equalsObj<PriorFactorStereoCamera>(priorFactorStereoCamera));
447
448 EXPECT(equalsObj<BetweenFactorPoint2>(betweenFactorPoint2));
449 EXPECT(equalsObj<BetweenFactorPoint3>(betweenFactorPoint3));
450 EXPECT(equalsObj<BetweenFactorRot2>(betweenFactorRot2));
451 EXPECT(equalsObj<BetweenFactorRot3>(betweenFactorRot3));
452 EXPECT(equalsObj<BetweenFactorPose2>(betweenFactorPose2));
453 EXPECT(equalsObj<BetweenFactorPose3>(betweenFactorPose3));
454
455 EXPECT(equalsObj<NonlinearEqualityPoint2>(nonlinearEqualityPoint2));
456 EXPECT(equalsObj<NonlinearEqualityStereoPoint2>(nonlinearEqualityStereoPoint2));
457 EXPECT(equalsObj<NonlinearEqualityPoint3>(nonlinearEqualityPoint3));
458 EXPECT(equalsObj<NonlinearEqualityRot2>(nonlinearEqualityRot2));
459 EXPECT(equalsObj<NonlinearEqualityRot3>(nonlinearEqualityRot3));
460 EXPECT(equalsObj<NonlinearEqualityPose2>(nonlinearEqualityPose2));
461 EXPECT(equalsObj<NonlinearEqualityPose3>(nonlinearEqualityPose3));
462 EXPECT(equalsObj<NonlinearEqualityCal3_S2>(nonlinearEqualityCal3_S2));
463 EXPECT(equalsObj<NonlinearEqualityCal3DS2>(nonlinearEqualityCal3DS2));
464 EXPECT(equalsObj<NonlinearEqualityCalibratedCamera>(nonlinearEqualityCalibratedCamera));
465 EXPECT(equalsObj<NonlinearEqualityStereoCamera>(nonlinearEqualityStereoCamera));
466
467 EXPECT(equalsObj<RangeFactor2D>(rangeFactor2D));
468 EXPECT(equalsObj<RangeFactor3D>(rangeFactor3D));
469 EXPECT(equalsObj<RangeFactorPose2>(rangeFactorPose2));
470 EXPECT(equalsObj<RangeFactorPose3>(rangeFactorPose3));
471 EXPECT(equalsObj<RangeFactorCalibratedCameraPoint>(rangeFactorCalibratedCameraPoint));
472 EXPECT(equalsObj<RangeFactorPinholeCameraCal3_S2Point>(rangeFactorPinholeCameraCal3_S2Point));
473 EXPECT(equalsObj<RangeFactorCalibratedCamera>(rangeFactorCalibratedCamera));
474 EXPECT(equalsObj<RangeFactorPinholeCameraCal3_S2>(rangeFactorPinholeCameraCal3_S2));
475
476 EXPECT(equalsObj<BearingRangeFactor2D>(bearingRangeFactor2D));
477
478 EXPECT(equalsObj<GenericProjectionFactorCal3_S2>(genericProjectionFactorCal3_S2));
479 EXPECT(equalsObj<GenericProjectionFactorCal3DS2>(genericProjectionFactorCal3DS2));
480
481 EXPECT(equalsObj<GeneralSFMFactorCal3_S2>(generalSFMFactorCal3_S2));
482
483 EXPECT(equalsObj<GeneralSFMFactor2Cal3_S2>(generalSFMFactor2Cal3_S2));
484
485 EXPECT(equalsObj<GenericStereoFactor3D>(genericStereoFactor3D));
486
487
488 // xml
489 EXPECT(equalsXML<Symbol>(a01));
490 EXPECT(equalsXML<Symbol>(b02));
491 EXPECT(equalsXML<Values>(values));
492 EXPECT(equalsXML<NonlinearFactorGraph>(graph));
493
494 EXPECT(equalsXML<PriorFactorPoint2>(priorFactorPoint2));
495 EXPECT(equalsXML<PriorFactorStereoPoint2>(priorFactorStereoPoint2));
496 EXPECT(equalsXML<PriorFactorPoint3>(priorFactorPoint3));
497 EXPECT(equalsXML<PriorFactorRot2>(priorFactorRot2));
498 EXPECT(equalsXML<PriorFactorRot3>(priorFactorRot3));
499 EXPECT(equalsXML<PriorFactorPose2>(priorFactorPose2));
500 EXPECT(equalsXML<PriorFactorPose3>(priorFactorPose3));
501 EXPECT(equalsXML<PriorFactorCal3_S2>(priorFactorCal3_S2));
502 EXPECT(equalsXML<PriorFactorCal3DS2>(priorFactorCal3DS2));
503 EXPECT(equalsXML<PriorFactorCalibratedCamera>(priorFactorCalibratedCamera));
504 EXPECT(equalsXML<PriorFactorStereoCamera>(priorFactorStereoCamera));
505
506 EXPECT(equalsXML<BetweenFactorPoint2>(betweenFactorPoint2));
507 EXPECT(equalsXML<BetweenFactorPoint3>(betweenFactorPoint3));
508 EXPECT(equalsXML<BetweenFactorRot2>(betweenFactorRot2));
509 EXPECT(equalsXML<BetweenFactorRot3>(betweenFactorRot3));
510 EXPECT(equalsXML<BetweenFactorPose2>(betweenFactorPose2));
511 EXPECT(equalsXML<BetweenFactorPose3>(betweenFactorPose3));
512
513 EXPECT(equalsXML<NonlinearEqualityPoint2>(nonlinearEqualityPoint2));
514 EXPECT(equalsXML<NonlinearEqualityStereoPoint2>(nonlinearEqualityStereoPoint2));
515 EXPECT(equalsXML<NonlinearEqualityPoint3>(nonlinearEqualityPoint3));
516 EXPECT(equalsXML<NonlinearEqualityRot2>(nonlinearEqualityRot2));
517 EXPECT(equalsXML<NonlinearEqualityRot3>(nonlinearEqualityRot3));
518 EXPECT(equalsXML<NonlinearEqualityPose2>(nonlinearEqualityPose2));
519 EXPECT(equalsXML<NonlinearEqualityPose3>(nonlinearEqualityPose3));
520 EXPECT(equalsXML<NonlinearEqualityCal3_S2>(nonlinearEqualityCal3_S2));
521 EXPECT(equalsXML<NonlinearEqualityCal3DS2>(nonlinearEqualityCal3DS2));
522 EXPECT(equalsXML<NonlinearEqualityCalibratedCamera>(nonlinearEqualityCalibratedCamera));
523 EXPECT(equalsXML<NonlinearEqualityStereoCamera>(nonlinearEqualityStereoCamera));
524
525 EXPECT(equalsXML<RangeFactor2D>(rangeFactor2D));
526 EXPECT(equalsXML<RangeFactor3D>(rangeFactor3D));
527 EXPECT(equalsXML<RangeFactorPose2>(rangeFactorPose2));
528 EXPECT(equalsXML<RangeFactorPose3>(rangeFactorPose3));
529 EXPECT(equalsXML<RangeFactorCalibratedCameraPoint>(rangeFactorCalibratedCameraPoint));
530 EXPECT(equalsXML<RangeFactorPinholeCameraCal3_S2Point>(rangeFactorPinholeCameraCal3_S2Point));
531 EXPECT(equalsXML<RangeFactorCalibratedCamera>(rangeFactorCalibratedCamera));
532 EXPECT(equalsXML<RangeFactorPinholeCameraCal3_S2>(rangeFactorPinholeCameraCal3_S2));
533
534 EXPECT(equalsXML<BearingRangeFactor2D>(bearingRangeFactor2D));
535
536 EXPECT(equalsXML<GenericProjectionFactorCal3_S2>(genericProjectionFactorCal3_S2));
537 EXPECT(equalsXML<GenericProjectionFactorCal3DS2>(genericProjectionFactorCal3DS2));
538
539 EXPECT(equalsXML<GeneralSFMFactorCal3_S2>(generalSFMFactorCal3_S2));
540
541 EXPECT(equalsXML<GeneralSFMFactor2Cal3_S2>(generalSFMFactor2Cal3_S2));
542
543 EXPECT(equalsXML<GenericStereoFactor3D>(genericStereoFactor3D));
544
545
546 // binary
547 EXPECT(equalsBinary<Symbol>(a01));
548 EXPECT(equalsBinary<Symbol>(b02));
549 EXPECT(equalsBinary<Values>(values));
550 EXPECT(equalsBinary<NonlinearFactorGraph>(graph));
551
552 EXPECT(equalsBinary<PriorFactorPoint2>(priorFactorPoint2));
553 EXPECT(equalsBinary<PriorFactorStereoPoint2>(priorFactorStereoPoint2));
554 EXPECT(equalsBinary<PriorFactorPoint3>(priorFactorPoint3));
555 EXPECT(equalsBinary<PriorFactorRot2>(priorFactorRot2));
556 EXPECT(equalsBinary<PriorFactorRot3>(priorFactorRot3));
557 EXPECT(equalsBinary<PriorFactorPose2>(priorFactorPose2));
558 EXPECT(equalsBinary<PriorFactorPose3>(priorFactorPose3));
559 EXPECT(equalsBinary<PriorFactorCal3_S2>(priorFactorCal3_S2));
560 EXPECT(equalsBinary<PriorFactorCal3DS2>(priorFactorCal3DS2));
561 EXPECT(equalsBinary<PriorFactorCalibratedCamera>(priorFactorCalibratedCamera));
562 EXPECT(equalsBinary<PriorFactorStereoCamera>(priorFactorStereoCamera));
563
564 EXPECT(equalsBinary<BetweenFactorPoint2>(betweenFactorPoint2));
565 EXPECT(equalsBinary<BetweenFactorPoint3>(betweenFactorPoint3));
566 EXPECT(equalsBinary<BetweenFactorRot2>(betweenFactorRot2));
567 EXPECT(equalsBinary<BetweenFactorRot3>(betweenFactorRot3));
568 EXPECT(equalsBinary<BetweenFactorPose2>(betweenFactorPose2));
569 EXPECT(equalsBinary<BetweenFactorPose3>(betweenFactorPose3));
570
571 EXPECT(equalsBinary<NonlinearEqualityPoint2>(nonlinearEqualityPoint2));
572 EXPECT(equalsBinary<NonlinearEqualityStereoPoint2>(nonlinearEqualityStereoPoint2));
573 EXPECT(equalsBinary<NonlinearEqualityPoint3>(nonlinearEqualityPoint3));
574 EXPECT(equalsBinary<NonlinearEqualityRot2>(nonlinearEqualityRot2));
575 EXPECT(equalsBinary<NonlinearEqualityRot3>(nonlinearEqualityRot3));
576 EXPECT(equalsBinary<NonlinearEqualityPose2>(nonlinearEqualityPose2));
577 EXPECT(equalsBinary<NonlinearEqualityPose3>(nonlinearEqualityPose3));
578 EXPECT(equalsBinary<NonlinearEqualityCal3_S2>(nonlinearEqualityCal3_S2));
579 EXPECT(equalsBinary<NonlinearEqualityCal3DS2>(nonlinearEqualityCal3DS2));
580 EXPECT(equalsBinary<NonlinearEqualityCalibratedCamera>(nonlinearEqualityCalibratedCamera));
581 EXPECT(equalsBinary<NonlinearEqualityStereoCamera>(nonlinearEqualityStereoCamera));
582
583 EXPECT(equalsBinary<RangeFactor2D>(rangeFactor2D));
584 EXPECT(equalsBinary<RangeFactor3D>(rangeFactor3D));
585 EXPECT(equalsBinary<RangeFactorPose2>(rangeFactorPose2));
586 EXPECT(equalsBinary<RangeFactorPose3>(rangeFactorPose3));
587 EXPECT(equalsBinary<RangeFactorCalibratedCameraPoint>(rangeFactorCalibratedCameraPoint));
588 EXPECT(equalsBinary<RangeFactorPinholeCameraCal3_S2Point>(rangeFactorPinholeCameraCal3_S2Point));
589 EXPECT(equalsBinary<RangeFactorCalibratedCamera>(rangeFactorCalibratedCamera));
590 EXPECT(equalsBinary<RangeFactorPinholeCameraCal3_S2>(rangeFactorPinholeCameraCal3_S2));
591
592 EXPECT(equalsBinary<BearingRangeFactor2D>(bearingRangeFactor2D));
593
594 EXPECT(equalsBinary<GenericProjectionFactorCal3_S2>(genericProjectionFactorCal3_S2));
595 EXPECT(equalsBinary<GenericProjectionFactorCal3DS2>(genericProjectionFactorCal3DS2));
596
597 EXPECT(equalsBinary<GeneralSFMFactorCal3_S2>(generalSFMFactorCal3_S2));
598
599 EXPECT(equalsBinary<GeneralSFMFactor2Cal3_S2>(generalSFMFactor2Cal3_S2));
600
601 EXPECT(equalsBinary<GenericStereoFactor3D>(genericStereoFactor3D));
602}
603
604/* ************************************************************************* */
605// Read from XML file
606namespace {
607static GaussianFactorGraph read(const string& name) {
608 auto inputFile = findExampleDataFile(name);
609 ifstream is(inputFile);
610 if (!is.is_open()) throw runtime_error("Cannot find file " + inputFile);
611 boost::archive::xml_iarchive in_archive(is);
612 GaussianFactorGraph Ab;
613 in_archive >> boost::serialization::make_nvp(n: "graph", v&: Ab);
614 return Ab;
615}
616} // namespace
617
618/* ************************************************************************* */
619// Read from XML file
620TEST(SubgraphSolver, Solves) {
621 using gtsam::example::planarGraph;
622
623 // Create preconditioner
624 SubgraphPreconditioner system;
625
626 // We test on three different graphs
627 const auto Ab1 = planarGraph(N: 3).first;
628 const auto Ab2 = read(name: "toy3D");
629 const auto Ab3 = read(name: "randomGrid3D");
630
631 // For all graphs, test solve and solveTranspose
632 for (const auto& Ab : {Ab1, Ab2, Ab3}) {
633 // Call build, a non-const method needed to make solve work :-(
634 KeyInfo keyInfo(Ab);
635 std::map<Key, Vector> lambda;
636 system.build(gfg: Ab, info: keyInfo, lambda);
637
638 // Create a perturbed (non-zero) RHS
639 const auto xbar = system.Rc1().optimize(); // merely for use in zero below
640 auto values_y = VectorValues::Zero(other: xbar);
641 auto it = values_y.begin();
642 it->second.setConstant(100);
643 ++it;
644 it->second.setConstant(-100);
645
646 // Solve the VectorValues way
647 auto values_x = system.Rc1().backSubstitute(gx: values_y);
648
649 // Solve the matrix way, this really just checks BN::backSubstitute
650 // This only works with Rc1 ordering, not with keyInfo !
651 // TODO(frank): why does this not work with an arbitrary ordering?
652 const auto ord = system.Rc1().ordering();
653 const Matrix R1 = system.Rc1().matrix(ordering: ord).first;
654 auto ord_y = values_y.vector(keys: ord);
655 auto vector_x = R1.inverse() * ord_y;
656 EXPECT(assert_equal(vector_x, values_x.vector(ord)));
657
658 // Test that 'solve' does implement x = R^{-1} y
659 // We do this by asserting it gives same answer as backSubstitute
660 // Only works with keyInfo ordering:
661 const auto ordering = keyInfo.ordering();
662 auto vector_y = values_y.vector(keys: ordering);
663 const size_t N = R1.cols();
664 Vector solve_x = Vector::Zero(size: N);
665 system.solve(y: vector_y, x&: solve_x);
666 EXPECT(assert_equal(values_x.vector(ordering), solve_x));
667
668 // Test that transposeSolve does implement x = R^{-T} y
669 // We do this by asserting it gives same answer as backSubstituteTranspose
670 auto values_x2 = system.Rc1().backSubstituteTranspose(gx: values_y);
671 Vector solveT_x = Vector::Zero(size: N);
672 system.transposeSolve(y: vector_y, x&: solveT_x);
673 EXPECT(assert_equal(values_x2.vector(ordering), solveT_x));
674 }
675}
676
677/* ************************************************************************* */
678int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr); }
679/* ************************************************************************* */
680