testSerializationSlam.cpp
Go to the documentation of this file.
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 
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 
58 using namespace std;
59 using namespace gtsam;
60 using namespace gtsam::serializationTestHelpers;
61 
62 // Creating as many permutations of factors as possible
63 typedef PriorFactor<Point2> PriorFactorPoint2;
64 typedef PriorFactor<StereoPoint2> PriorFactorStereoPoint2;
65 typedef PriorFactor<Point3> PriorFactorPoint3;
66 typedef PriorFactor<Rot2> PriorFactorRot2;
67 typedef PriorFactor<Rot3> PriorFactorRot3;
68 typedef PriorFactor<Pose2> PriorFactorPose2;
69 typedef PriorFactor<Pose3> PriorFactorPose3;
70 typedef PriorFactor<Cal3_S2> PriorFactorCal3_S2;
71 typedef PriorFactor<Cal3DS2> PriorFactorCal3DS2;
72 typedef PriorFactor<CalibratedCamera> PriorFactorCalibratedCamera;
73 typedef PriorFactor<PinholeCameraCal3_S2> PriorFactorPinholeCameraCal3_S2;
74 typedef PriorFactor<StereoCamera> PriorFactorStereoCamera;
75 
76 typedef BetweenFactor<Point2> BetweenFactorPoint2;
77 typedef BetweenFactor<Point3> BetweenFactorPoint3;
78 typedef BetweenFactor<Rot2> BetweenFactorRot2;
79 typedef BetweenFactor<Rot3> BetweenFactorRot3;
80 typedef BetweenFactor<Pose2> BetweenFactorPose2;
81 typedef BetweenFactor<Pose3> BetweenFactorPose3;
82 
83 typedef NonlinearEquality<Point2> NonlinearEqualityPoint2;
84 typedef NonlinearEquality<StereoPoint2> NonlinearEqualityStereoPoint2;
85 typedef NonlinearEquality<Point3> NonlinearEqualityPoint3;
86 typedef NonlinearEquality<Rot2> NonlinearEqualityRot2;
87 typedef NonlinearEquality<Rot3> NonlinearEqualityRot3;
88 typedef NonlinearEquality<Pose2> NonlinearEqualityPose2;
89 typedef NonlinearEquality<Pose3> NonlinearEqualityPose3;
90 typedef NonlinearEquality<Cal3_S2> NonlinearEqualityCal3_S2;
91 typedef NonlinearEquality<Cal3DS2> NonlinearEqualityCal3DS2;
92 typedef NonlinearEquality<CalibratedCamera> NonlinearEqualityCalibratedCamera;
93 typedef NonlinearEquality<PinholeCameraCal3_S2> NonlinearEqualityPinholeCameraCal3_S2;
94 typedef NonlinearEquality<StereoCamera> NonlinearEqualityStereoCamera;
95 
96 typedef RangeFactor<Pose2, Point2> RangeFactor2D;
97 typedef RangeFactor<Pose3, Point3> RangeFactor3D;
98 typedef RangeFactor<Pose2, Pose2> RangeFactorPose2;
99 typedef RangeFactor<Pose3, Pose3> RangeFactorPose3;
100 typedef RangeFactor<CalibratedCamera, Point3> RangeFactorCalibratedCameraPoint;
101 typedef RangeFactor<PinholeCameraCal3_S2, Point3> RangeFactorPinholeCameraCal3_S2Point;
102 typedef RangeFactor<CalibratedCamera, CalibratedCamera> RangeFactorCalibratedCamera;
103 typedef RangeFactor<PinholeCameraCal3_S2, PinholeCameraCal3_S2> RangeFactorPinholeCameraCal3_S2;
104 
105 typedef BearingRangeFactor<Pose2, Point2> BearingRangeFactor2D;
106 typedef BearingRangeFactor<Pose3, Point3> BearingRangeFactor3D;
107 
108 typedef GenericProjectionFactor<Pose3, Point3, Cal3_S2> GenericProjectionFactorCal3_S2;
109 typedef GenericProjectionFactor<Pose3, Point3, Cal3DS2> GenericProjectionFactorCal3DS2;
110 
111 typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
112 typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
113 
114 typedef gtsam::GeneralSFMFactor2<gtsam::Cal3_S2> GeneralSFMFactor2Cal3_S2;
115 
116 typedef gtsam::GenericStereoFactor<gtsam::Pose3, gtsam::Point3> GenericStereoFactor3D;
117 
118 
119 // Convenience for named keys
120 using symbol_shorthand::X;
121 using symbol_shorthand::L;
122 
123 
124 /* Create GUIDs for Noisemodels */
125 /* ************************************************************************* */
126 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained")
127 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal")
128 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian")
129 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit")
130 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic")
131 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Robust, "gtsam_noiseModel_Robust")
132 
133 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Base , "gtsam_noiseModel_mEstimator_Base")
134 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Null , "gtsam_noiseModel_mEstimator_Null")
135 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Fair , "gtsam_noiseModel_mEstimator_Fair")
136 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Huber, "gtsam_noiseModel_mEstimator_Huber")
137 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Tukey, "gtsam_noiseModel_mEstimator_Tukey")
138 
139 BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel")
140 BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal")
141 
142 /* Create GUIDs for geometry */
143 /* ************************************************************************* */
144 GTSAM_VALUE_EXPORT(gtsam::Point2)
145 GTSAM_VALUE_EXPORT(gtsam::StereoPoint2)
146 GTSAM_VALUE_EXPORT(gtsam::Point3)
147 GTSAM_VALUE_EXPORT(gtsam::Rot2)
148 GTSAM_VALUE_EXPORT(gtsam::Rot3)
149 GTSAM_VALUE_EXPORT(gtsam::Pose2)
150 GTSAM_VALUE_EXPORT(gtsam::Pose3)
151 GTSAM_VALUE_EXPORT(gtsam::Cal3_S2)
152 GTSAM_VALUE_EXPORT(gtsam::Cal3DS2)
153 GTSAM_VALUE_EXPORT(gtsam::Cal3_S2Stereo)
154 GTSAM_VALUE_EXPORT(gtsam::CalibratedCamera)
155 GTSAM_VALUE_EXPORT(gtsam::PinholeCameraCal3_S2)
156 GTSAM_VALUE_EXPORT(gtsam::StereoCamera)
157 
158 /* Create GUIDs for factors */
159 /* ************************************************************************* */
160 BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor")
161 BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor")
162 
163 BOOST_CLASS_EXPORT_GUID(PriorFactorPoint2, "gtsam::PriorFactorPoint2")
164 BOOST_CLASS_EXPORT_GUID(PriorFactorStereoPoint2, "gtsam::PriorFactorStereoPoint2")
165 BOOST_CLASS_EXPORT_GUID(PriorFactorPoint3, "gtsam::PriorFactorPoint3")
166 BOOST_CLASS_EXPORT_GUID(PriorFactorRot2, "gtsam::PriorFactorRot2")
167 BOOST_CLASS_EXPORT_GUID(PriorFactorRot3, "gtsam::PriorFactorRot3")
168 BOOST_CLASS_EXPORT_GUID(PriorFactorPose2, "gtsam::PriorFactorPose2")
169 BOOST_CLASS_EXPORT_GUID(PriorFactorPose3, "gtsam::PriorFactorPose3")
170 BOOST_CLASS_EXPORT_GUID(PriorFactorCal3_S2, "gtsam::PriorFactorCal3_S2")
171 BOOST_CLASS_EXPORT_GUID(PriorFactorCal3DS2, "gtsam::PriorFactorCal3DS2")
172 BOOST_CLASS_EXPORT_GUID(PriorFactorCalibratedCamera, "gtsam::PriorFactorCalibratedCamera")
173 BOOST_CLASS_EXPORT_GUID(PriorFactorStereoCamera, "gtsam::PriorFactorStereoCamera")
174 
175 BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint2, "gtsam::BetweenFactorPoint2")
176 BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint3, "gtsam::BetweenFactorPoint3")
177 BOOST_CLASS_EXPORT_GUID(BetweenFactorRot2, "gtsam::BetweenFactorRot2")
178 BOOST_CLASS_EXPORT_GUID(BetweenFactorRot3, "gtsam::BetweenFactorRot3")
179 BOOST_CLASS_EXPORT_GUID(BetweenFactorPose2, "gtsam::BetweenFactorPose2")
180 BOOST_CLASS_EXPORT_GUID(BetweenFactorPose3, "gtsam::BetweenFactorPose3")
181 
182 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint2, "gtsam::NonlinearEqualityPoint2")
183 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoPoint2, "gtsam::NonlinearEqualityStereoPoint2")
184 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint3, "gtsam::NonlinearEqualityPoint3")
185 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityRot2, "gtsam::NonlinearEqualityRot2")
186 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityRot3, "gtsam::NonlinearEqualityRot3")
187 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPose2, "gtsam::NonlinearEqualityPose2")
188 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPose3, "gtsam::NonlinearEqualityPose3")
189 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3_S2, "gtsam::NonlinearEqualityCal3_S2")
190 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3DS2, "gtsam::NonlinearEqualityCal3DS2")
191 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCalibratedCamera, "gtsam::NonlinearEqualityCalibratedCamera")
192 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoCamera, "gtsam::NonlinearEqualityStereoCamera")
193 
194 BOOST_CLASS_EXPORT_GUID(RangeFactor2D, "gtsam::RangeFactor2D")
195 BOOST_CLASS_EXPORT_GUID(RangeFactor3D, "gtsam::RangeFactor3D")
196 BOOST_CLASS_EXPORT_GUID(RangeFactorPose2, "gtsam::RangeFactorPose2")
197 BOOST_CLASS_EXPORT_GUID(RangeFactorPose3, "gtsam::RangeFactorPose3")
198 BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCameraPoint, "gtsam::RangeFactorCalibratedCameraPoint")
199 BOOST_CLASS_EXPORT_GUID(RangeFactorPinholeCameraCal3_S2Point, "gtsam::RangeFactorPinholeCameraCal3_S2Point")
200 BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCamera, "gtsam::RangeFactorCalibratedCamera")
201 BOOST_CLASS_EXPORT_GUID(RangeFactorPinholeCameraCal3_S2, "gtsam::RangeFactorPinholeCameraCal3_S2")
202 
203 BOOST_CLASS_EXPORT_GUID(BearingRangeFactor2D, "gtsam::BearingRangeFactor2D")
204 
205 BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3_S2, "gtsam::GenericProjectionFactorCal3_S2")
206 BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3DS2, "gtsam::GenericProjectionFactorCal3DS2")
207 
208 BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3_S2, "gtsam::GeneralSFMFactorCal3_S2")
209 BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2")
210 
211 BOOST_CLASS_EXPORT_GUID(GeneralSFMFactor2Cal3_S2, "gtsam::GeneralSFMFactor2Cal3_S2")
212 
213 BOOST_CLASS_EXPORT_GUID(GenericStereoFactor3D, "gtsam::GenericStereoFactor3D")
214 
215 
216 /* ************************************************************************* */
217 TEST (testSerializationSLAM, smallExample_linear) {
218  using namespace example;
219 
220  const Ordering ordering{X(1), X(2), L(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 /* ************************************************************************* */
237 TEST (testSerializationSLAM, gaussianISAM) {
238  using namespace example;
239  GaussianFactorGraph smoother = createSmoother(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 */
250 BOOST_CLASS_EXPORT_GUID(simulated2D::Prior, "gtsam::simulated2D::Prior" )
251 BOOST_CLASS_EXPORT_GUID(simulated2D::Odometry, "gtsam::simulated2D::Odometry" )
252 BOOST_CLASS_EXPORT_GUID(simulated2D::Measurement, "gtsam::simulated2D::Measurement")
253 
254 /* ************************************************************************* */
255 TEST (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 /* ************************************************************************* */
269 TEST (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(1.0, 2.0, 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>(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(a03, point2);
295  values.insert(a04, stereoPoint2);
296  values.insert(a05, point3);
297  values.insert(a06, rot2);
298  values.insert(a07, rot3);
299  values.insert(a08, pose2);
300  values.insert(a09, pose3);
301  values.insert(a10, cal3_s2);
302  values.insert(a11, cal3ds2);
303  values.insert(a12, calibratedCamera);
304  values.insert(a13, simpleCamera);
305  values.insert(a14, stereoCamera);
306 
307 
308  SharedNoiseModel model1 = noiseModel::Isotropic::Sigma(1, 0.3);
309  SharedNoiseModel model2 = noiseModel::Isotropic::Sigma(2, 0.3);
310  SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.3);
311  SharedNoiseModel model4 = noiseModel::Isotropic::Sigma(4, 0.3);
312  SharedNoiseModel model5 = noiseModel::Isotropic::Sigma(5, 0.3);
313  SharedNoiseModel model6 = noiseModel::Isotropic::Sigma(6, 0.3);
314  SharedNoiseModel model9 = noiseModel::Isotropic::Sigma(9, 0.3);
315  SharedNoiseModel model11 = noiseModel::Isotropic::Sigma(11, 0.3);
316 
317  SharedNoiseModel robust1 = noiseModel::Robust::Create(
318  noiseModel::mEstimator::Huber::Create(10.0, noiseModel::mEstimator::Huber::Scalar),
319  noiseModel::Unit::Create(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>(cal3_s2));
368  GenericProjectionFactorCal3DS2 genericProjectionFactorCal3DS2(point2, model2, a09, a05, std::make_shared<Cal3DS2>(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>(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
606 namespace {
607 static 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("graph", Ab);
614  return Ab;
615 }
616 } // namespace
617 
618 /* ************************************************************************* */
619 // Read from XML file
620 TEST(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(3).first;
628  const auto Ab2 = read("toy3D");
629  const auto Ab3 = read("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(Ab, 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(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(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(ord).first;
654  auto ord_y = values_y.vector(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(ordering);
663  const size_t N = R1.cols();
664  Vector solve_x = Vector::Zero(N);
665  system.solve(vector_y, 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(values_y);
671  Vector solveT_x = Vector::Zero(N);
672  system.transposeSolve(vector_y, solveT_x);
673  EXPECT(assert_equal(values_x2.vector(ordering), solveT_x));
674  }
675 }
676 
677 /* ************************************************************************* */
678 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
679 /* ************************************************************************* */
NonlinearEqualityStereoCamera
NonlinearEquality< StereoCamera > NonlinearEqualityStereoCamera
Definition: testSerializationSlam.cpp:94
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
PriorFactorRot2
PriorFactor< Rot2 > PriorFactorRot2
Definition: testSerializationSlam.cpp:66
Gaussian
double Gaussian(double mu, double sigma, double z)
Gaussian density function.
Definition: testGaussianMixture.cpp:47
gtsam::KeyInfo::ordering
const Ordering & ordering() const
Return the ordering.
Definition: IterativeSolver.h:159
BetweenFactorPose2
BetweenFactor< Pose2 > BetweenFactorPose2
Definition: testSerializationSlam.cpp:80
gtsam::SO::inverse
SO inverse() const
inverse of a rotation = transpose
Definition: SOn.h:195
NonlinearEqualityPinholeCameraCal3_S2
NonlinearEquality< PinholeCameraCal3_S2 > NonlinearEqualityPinholeCameraCal3_S2
Definition: testSerializationSlam.cpp:93
Pose2.h
2D Pose
NonlinearEqualityCal3_S2
NonlinearEquality< Cal3_S2 > NonlinearEqualityCal3_S2
Definition: testSerializationSlam.cpp:90
name
Annotation for function names.
Definition: attr.h:51
ProjectionFactor.h
Reprojection of a LANDMARK to a 2D point.
model2
auto model2
Definition: testFunctorizedFactor.cpp:40
NonlinearEqualityPoint3
NonlinearEquality< Point3 > NonlinearEqualityPoint3
Definition: testSerializationSlam.cpp:85
gtsam::RangeFactor
Definition: sam/RangeFactor.h:35
main
int main()
Definition: testSerializationSlam.cpp:678
PriorFactorCal3_S2
PriorFactor< Cal3_S2 > PriorFactorCal3_S2
Definition: testSerializationSlam.cpp:70
gtsam::HessianFactor
A Gaussian factor using the canonical parameters (information form)
Definition: HessianFactor.h:100
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
simulated2D
Definition: simulated2D.h:28
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
gtsam::Cal3DS2
Calibration of a camera with radial distortion that also supports Lie-group behaviors for optimizatio...
Definition: Cal3DS2.h:35
TestHarness.h
Eigen::TensorSycl::internal::read
static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ::Eigen::internal::enable_if< PacketLoad, PacketType >::type read(const TensorMapper &tensorMapper, const StorageIndex &NCIndex, const StorageIndex &CIndex, const StorageIndex &ld)
read, a template function used for loading the data from global memory. This function is used to guar...
Definition: TensorContractionSycl.h:177
BOOST_CLASS_EXPORT_GUID
BOOST_CLASS_EXPORT_GUID(DecisionTreeFactor, "gtsam_DecisionTreeFactor")
gtsam::GeneralSFMFactor2
Definition: GeneralSFMFactor.h:208
simple_graph::factors
const GaussianFactorGraph factors
Definition: testJacobianFactor.cpp:213
RangeFactor.h
Serializable factor induced by a range measurement.
gtsam::GaussianBayesNet::ordering
Ordering ordering() const
Definition: GaussianBayesNet.cpp:187
pose3
static Pose3 pose3(rt3, pt3)
gtsam::JacobianFactor
Definition: JacobianFactor.h:91
gtsam::SubgraphPreconditioner
Definition: SubgraphPreconditioner.h:54
point3
static const Point3 point3(0.08, 0.08, 0.0)
gtsam::PriorFactor
Definition: nonlinear/PriorFactor.h:30
BetweenFactorRot3
BetweenFactor< Rot3 > BetweenFactorRot3
Definition: testSerializationSlam.cpp:79
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
GeneralSFMFactor.h
a general SFM factor with an unknown calibration
simple::pose2
static Pose3 pose2
Definition: testInitializePose3.cpp:58
X
#define X
Definition: icosphere.cpp:20
Point3.h
3D Point
gtsam::GenericStereoFactor
Definition: StereoFactor.h:32
BearingRangeFactor2D
BearingRangeFactor< Pose2, Point2 > BearingRangeFactor2D
Definition: testSerializationSlam.cpp:105
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
GTSAM_VALUE_EXPORT
#define GTSAM_VALUE_EXPORT(Type)
use this macro instead of BOOST_CLASS_EXPORT for GenericValues
Definition: GenericValue.h:195
gtsam::NonlinearEquality
Definition: NonlinearEquality.h:43
CalibratedCamera.h
Calibrated camera for which only pose is unknown.
RangeFactor3D
RangeFactor< Pose3, Point3 > RangeFactor3D
Definition: testSerializationSlam.cpp:97
gtsam::noiseModel::Constrained
Definition: NoiseModel.h:404
Rot2.h
2D rotation
point2
static const Point3 point2(-0.08, 0.08, 0.0)
GenericStereoFactor3D
gtsam::GenericStereoFactor< gtsam::Pose3, gtsam::Point3 > GenericStereoFactor3D
Definition: testSerializationSlam.cpp:116
RangeFactor2D
RangeFactor< Pose2, Point2 > RangeFactor2D
Definition: testSerializationSlam.cpp:96
gtsam::EliminateableFactorGraph::eliminateMultifrontal
std::shared_ptr< BayesTreeType > eliminateMultifrontal(OptionalOrderingType orderingType={}, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
Definition: EliminateableFactorGraph-inst.h:89
gtsam::KeyInfo
Definition: IterativeSolver.h:127
Point2.h
2D Point
gtsam::Measurement
This is the base class for all measurement types.
Definition: Measurement.h:11
Rot3.h
3D rotation represented as a rotation matrix or quaternion
so3::R1
SO3 R1
Definition: testShonanFactor.cpp:41
c1
static double c1
Definition: airy.c:54
RangeFactorPose3
RangeFactor< Pose3, Pose3 > RangeFactorPose3
Definition: testSerializationSlam.cpp:99
BetweenFactorRot2
BetweenFactor< Rot2 > BetweenFactorRot2
Definition: testSerializationSlam.cpp:78
example
Definition: testOrdering.cpp:28
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
PriorFactorPose2
PriorFactor< Pose2 > PriorFactorPose2
Definition: testSerializationSlam.cpp:68
gtsam::example::createNonlinearFactorGraph
NonlinearFactorGraph createNonlinearFactorGraph(const SharedNoiseModel &noiseModel1=impl::kSigma0_1, const SharedNoiseModel &noiseModel2=impl::kSigma0_2)
Definition: smallExample.h:206
PriorFactor.h
PriorFactorPoint2
PriorFactor< Point2 > PriorFactorPoint2
Definition: testSerializationSlam.cpp:63
RangeFactorCalibratedCamera
RangeFactor< CalibratedCamera, CalibratedCamera > RangeFactorCalibratedCamera
Definition: testSerializationSlam.cpp:102
inputFile
string inputFile
Definition: SolverComparer.cpp:89
dataset.h
utility functions for loading datasets
PriorFactorStereoPoint2
PriorFactor< StereoPoint2 > PriorFactorStereoPoint2
Definition: testSerializationSlam.cpp:64
BetweenFactor.h
gtsam::example::createGaussianFactorGraph
GaussianFactorGraph createGaussianFactorGraph()
Definition: smallExample.h:270
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::example::createSmallGaussianBayesNet
GaussianBayesNet createSmallGaussianBayesNet()
Definition: smallExample.h:297
gtsam::BearingRangeFactor
Definition: sam/BearingRangeFactor.h:34
gtsam::Pose3
Definition: Pose3.h:37
gtsam::SubgraphPreconditioner::transposeSolve
void transposeSolve(const Vector &y, Vector &x) const override
implement x = R^{-T} y
Definition: SubgraphPreconditioner.cpp:223
StereoFactor.h
A non-linear factor for stereo measurements.
gtsam::PinholeCamera
Definition: PinholeCamera.h:33
isam
NonlinearISAM isam(relinearizeInterval)
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
RangeFactorCalibratedCameraPoint
RangeFactor< CalibratedCamera, Point3 > RangeFactorCalibratedCameraPoint
Definition: testSerializationSlam.cpp:100
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:764
gtsam::GenericProjectionFactor
Definition: ProjectionFactor.h:40
gtsam::SubgraphPreconditioner::Rc1
const GaussianBayesNet & Rc1() const
Definition: SubgraphPreconditioner.h:90
gtsam::GaussianBayesNet::optimize
VectorValues optimize() const
Definition: GaussianBayesNet.cpp:44
BetweenFactorPoint3
BetweenFactor< Point3 > BetweenFactorPoint3
Definition: testSerializationSlam.cpp:77
PinholeCamera.h
Base class for all pinhole cameras.
NonlinearEqualityPose3
NonlinearEquality< Pose3 > NonlinearEqualityPose3
Definition: testSerializationSlam.cpp:89
NonlinearEqualityPoint2
NonlinearEquality< Point2 > NonlinearEqualityPoint2
Definition: testSerializationSlam.cpp:83
L
MatrixXd L
Definition: LLT_example.cpp:6
Symbol.h
gtsam::HybridValues::insert
void insert(Key j, const Vector &value)
Definition: HybridValues.cpp:85
gtsam::GaussianBayesNet::matrix
std::pair< Matrix, Vector > matrix(const Ordering &ordering) const
Definition: GaussianBayesNet.cpp:205
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
Cal3_S2Stereo.h
The most common 5DOF 3D->2D calibration + Stereo baseline.
NonlinearEquality.h
gtsam::GaussianBayesNet::backSubstituteTranspose
VectorValues backSubstituteTranspose(const VectorValues &gx) const
Definition: GaussianBayesNet.cpp:145
StereoPoint2.h
A 2D stereo point (uL,uR,v)
NonlinearEqualityPose2
NonlinearEquality< Pose2 > NonlinearEqualityPose2
Definition: testSerializationSlam.cpp:88
RangeFactorPinholeCameraCal3_S2
RangeFactor< PinholeCameraCal3_S2, PinholeCameraCal3_S2 > RangeFactorPinholeCameraCal3_S2
Definition: testSerializationSlam.cpp:103
TEST
TEST(testSerializationSLAM, smallExample_linear)
Definition: testSerializationSlam.cpp:217
gtsam::CalibratedCamera
Definition: CalibratedCamera.h:251
GeneralSFMFactorCal3_S2
gtsam::GeneralSFMFactor< gtsam::PinholeCameraCal3_S2, gtsam::Point3 > GeneralSFMFactorCal3_S2
Definition: testSerializationSlam.cpp:111
BetweenFactorPose3
BetweenFactor< Pose3 > BetweenFactorPose3
Definition: testSerializationSlam.cpp:81
lambda
static double lambda[]
Definition: jv.c:524
ordering
static enum @1096 ordering
serializationTestHelpers.h
NonlinearEqualityRot3
NonlinearEquality< Rot3 > NonlinearEqualityRot3
Definition: testSerializationSlam.cpp:87
TestResult
Definition: TestResult.h:26
std_optional_serialization.h
NonlinearEqualityRot2
NonlinearEquality< Rot2 > NonlinearEqualityRot2
Definition: testSerializationSlam.cpp:86
RangeFactorPose2
RangeFactor< Pose2, Pose2 > RangeFactorPose2
Definition: testSerializationSlam.cpp:98
gtsam::example::planarGraph
std::pair< GaussianFactorGraph, VectorValues > planarGraph(size_t N)
Definition: smallExample.h:626
gtsam::Rot2
Definition: Rot2.h:35
gtsam::SubgraphPreconditioner::solve
void solve(const Vector &y, Vector &x) const override
implement x = R^{-1} y
Definition: SubgraphPreconditioner.cpp:201
GeneralSFMFactor2Cal3_S2
gtsam::GeneralSFMFactor2< gtsam::Cal3_S2 > GeneralSFMFactor2Cal3_S2
Definition: testSerializationSlam.cpp:114
simulated2D::GenericPrior
Definition: simulated2D.h:129
gtsam
traits
Definition: SFMdata.h:40
gtsam::GaussianBayesTree
Definition: GaussianBayesTree.h:49
gtsam::SO::cols
size_t cols() const
Definition: SOn.h:160
gtsam::Cal3_S2Stereo
The most common 5DOF 3D->2D calibration, stereo version.
Definition: Cal3_S2Stereo.h:30
PriorFactorPoint3
PriorFactor< Point3 > PriorFactorPoint3
Definition: testSerializationSlam.cpp:65
gtsam::GaussianISAM
Definition: GaussianISAM.h:27
GeneralSFMFactorCal3DS2
gtsam::GeneralSFMFactor< gtsam::PinholeCameraCal3DS2, gtsam::Point3 > GeneralSFMFactorCal3DS2
Definition: testSerializationSlam.cpp:112
gtsam::Values
Definition: Values.h:65
Cal3DS2.h
Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base.
gtsam::StereoPoint2
Definition: StereoPoint2.h:34
BearingRangeFactor3D
BearingRangeFactor< Pose3, Point3 > BearingRangeFactor3D
Definition: testSerializationSlam.cpp:106
std
Definition: BFloat16.h:88
PriorFactorPinholeCameraCal3_S2
PriorFactor< PinholeCameraCal3_S2 > PriorFactorPinholeCameraCal3_S2
Definition: testSerializationSlam.cpp:73
NonlinearEqualityCal3DS2
NonlinearEquality< Cal3DS2 > NonlinearEqualityCal3DS2
Definition: testSerializationSlam.cpp:91
SubgraphPreconditioner.h
gtsam::GaussianBayesNet::backSubstitute
VectorValues backSubstitute(const VectorValues &gx) const
Definition: GaussianBayesNet.cpp:129
PriorFactorPose3
PriorFactor< Pose3 > PriorFactorPose3
Definition: testSerializationSlam.cpp:69
gtsam::Diagonal
static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix &covariance)
Definition: ScenarioRunner.h:27
gtsam::GeneralSFMFactor
Definition: GeneralSFMFactor.h:59
exampleQR::Ab
Matrix Ab
Definition: testNoiseModel.cpp:207
RangeFactorPinholeCameraCal3_S2Point
RangeFactor< PinholeCameraCal3_S2, Point3 > RangeFactorPinholeCameraCal3_S2Point
Definition: testSerializationSlam.cpp:101
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::StereoCamera
Definition: StereoCamera.h:47
gtsam::SubgraphPreconditioner::build
void build(const GaussianFactorGraph &gfg, const KeyInfo &info, const std::map< Key, Vector > &lambda) override
build/factorize the preconditioner
Definition: SubgraphPreconditioner.cpp:253
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::example::createValues
Values createValues()
Definition: smallExample.h:212
PriorFactorStereoCamera
PriorFactor< StereoCamera > PriorFactorStereoCamera
Definition: testSerializationSlam.cpp:74
gtsam::findExampleDataFile
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
Definition: dataset.cpp:70
N
#define N
Definition: igam.h:9
GenericProjectionFactorCal3DS2
GenericProjectionFactor< Pose3, Point3, Cal3DS2 > GenericProjectionFactorCal3DS2
Definition: testSerializationSlam.cpp:109
SimpleCamera.h
A simple camera class with a Cal3_S2 calibration.
PriorFactorRot3
PriorFactor< Rot3 > PriorFactorRot3
Definition: testSerializationSlam.cpp:67
model3
const SharedNoiseModel model3
Definition: testPoseRotationPrior.cpp:22
GaussianISAM.h
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
GenericProjectionFactorCal3_S2
GenericProjectionFactor< Pose3, Point3, Cal3_S2 > GenericProjectionFactorCal3_S2
Definition: testSerializationSlam.cpp:108
gtsam::SubgraphSolver
Definition: SubgraphSolver.h:76
BetweenFactorPoint2
BetweenFactor< Point2 > BetweenFactorPoint2
Definition: testSerializationSlam.cpp:76
model1
noiseModel::Isotropic::shared_ptr model1
Definition: testEssentialMatrixFactor.cpp:26
smallExample.h
Create small example with two poses and one landmark.
gtsam::Ordering
Definition: inference/Ordering.h:33
PriorFactorCal3DS2
PriorFactor< Cal3DS2 > PriorFactorCal3DS2
Definition: testSerializationSlam.cpp:71
NonlinearEqualityCalibratedCamera
NonlinearEquality< CalibratedCamera > NonlinearEqualityCalibratedCamera
Definition: testSerializationSlam.cpp:92
gtsam::GaussianBayesNet
Definition: GaussianBayesNet.h:35
PriorFactorCalibratedCamera
PriorFactor< CalibratedCamera > PriorFactorCalibratedCamera
Definition: testSerializationSlam.cpp:72
NonlinearEqualityStereoPoint2
NonlinearEquality< StereoPoint2 > NonlinearEqualityStereoPoint2
Definition: testSerializationSlam.cpp:84
Pose3.h
3D Pose
gtsam::example::createSmoother
GaussianFactorGraph createSmoother(int T)
Definition: smallExample.h:465
StereoCamera.h
A Stereo Camera based on two Simple Cameras.
gtsam::Pose2
Definition: Pose2.h:39
Scalar
SCALAR Scalar
Definition: bench_gemm.cpp:46
gtsam::Symbol
Definition: inference/Symbol.h:37
simulated2D::Odometry
GenericOdometry< Point2 > Odometry
Definition: simulated2D.h:277
BearingRangeFactor.h
a single factor contains both the bearing and the range to prevent handle to pair bearing and range f...
gtsam::BetweenFactor
Definition: BetweenFactor.h:40

gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:08:34