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 #include <gtsam/sam/BearingRangeFactor.h>
23 #include <gtsam/slam/BetweenFactor.h>
24 #include <gtsam/slam/GeneralSFMFactor.h>
25 #include <gtsam/nonlinear/PriorFactor.h>
26 #include <gtsam/slam/ProjectionFactor.h>
27 #include <gtsam/sam/RangeFactor.h>
28 #include <gtsam/slam/StereoFactor.h>
29 #include <gtsam/nonlinear/NonlinearEquality.h>
30 #include <gtsam/inference/Symbol.h>
31 #include <gtsam/linear/GaussianISAM.h>
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/base/serializationTestHelpers.h>
48 
49 using namespace std;
50 using namespace gtsam;
51 using namespace gtsam::serializationTestHelpers;
52 
53 // Creating as many permutations of factors as possible
54 typedef PriorFactor<Point2> PriorFactorPoint2;
55 typedef PriorFactor<StereoPoint2> PriorFactorStereoPoint2;
56 typedef PriorFactor<Point3> PriorFactorPoint3;
57 typedef PriorFactor<Rot2> PriorFactorRot2;
58 typedef PriorFactor<Rot3> PriorFactorRot3;
59 typedef PriorFactor<Pose2> PriorFactorPose2;
60 typedef PriorFactor<Pose3> PriorFactorPose3;
61 typedef PriorFactor<Cal3_S2> PriorFactorCal3_S2;
62 typedef PriorFactor<Cal3DS2> PriorFactorCal3DS2;
63 typedef PriorFactor<CalibratedCamera> PriorFactorCalibratedCamera;
64 typedef PriorFactor<PinholeCameraCal3_S2> PriorFactorPinholeCameraCal3_S2;
65 typedef PriorFactor<StereoCamera> PriorFactorStereoCamera;
66 
67 typedef BetweenFactor<Point2> BetweenFactorPoint2;
68 typedef BetweenFactor<Point3> BetweenFactorPoint3;
69 typedef BetweenFactor<Rot2> BetweenFactorRot2;
70 typedef BetweenFactor<Rot3> BetweenFactorRot3;
71 typedef BetweenFactor<Pose2> BetweenFactorPose2;
72 typedef BetweenFactor<Pose3> BetweenFactorPose3;
73 
74 typedef NonlinearEquality<Point2> NonlinearEqualityPoint2;
75 typedef NonlinearEquality<StereoPoint2> NonlinearEqualityStereoPoint2;
76 typedef NonlinearEquality<Point3> NonlinearEqualityPoint3;
77 typedef NonlinearEquality<Rot2> NonlinearEqualityRot2;
78 typedef NonlinearEquality<Rot3> NonlinearEqualityRot3;
79 typedef NonlinearEquality<Pose2> NonlinearEqualityPose2;
80 typedef NonlinearEquality<Pose3> NonlinearEqualityPose3;
81 typedef NonlinearEquality<Cal3_S2> NonlinearEqualityCal3_S2;
82 typedef NonlinearEquality<Cal3DS2> NonlinearEqualityCal3DS2;
83 typedef NonlinearEquality<CalibratedCamera> NonlinearEqualityCalibratedCamera;
84 typedef NonlinearEquality<PinholeCameraCal3_S2> NonlinearEqualityPinholeCameraCal3_S2;
85 typedef NonlinearEquality<StereoCamera> NonlinearEqualityStereoCamera;
86 
87 typedef RangeFactor<Pose2, Point2> RangeFactor2D;
88 typedef RangeFactor<Pose3, Point3> RangeFactor3D;
89 typedef RangeFactor<Pose2, Pose2> RangeFactorPose2;
90 typedef RangeFactor<Pose3, Pose3> RangeFactorPose3;
91 typedef RangeFactor<CalibratedCamera, Point3> RangeFactorCalibratedCameraPoint;
92 typedef RangeFactor<PinholeCameraCal3_S2, Point3> RangeFactorPinholeCameraCal3_S2Point;
93 typedef RangeFactor<CalibratedCamera, CalibratedCamera> RangeFactorCalibratedCamera;
94 typedef RangeFactor<PinholeCameraCal3_S2, PinholeCameraCal3_S2> RangeFactorPinholeCameraCal3_S2;
95 
96 typedef BearingRangeFactor<Pose2, Point2> BearingRangeFactor2D;
97 typedef BearingRangeFactor<Pose3, Point3> BearingRangeFactor3D;
98 
99 typedef GenericProjectionFactor<Pose3, Point3, Cal3_S2> GenericProjectionFactorCal3_S2;
100 typedef GenericProjectionFactor<Pose3, Point3, Cal3DS2> GenericProjectionFactorCal3DS2;
101 
102 typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
103 typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
104 
105 typedef gtsam::GeneralSFMFactor2<gtsam::Cal3_S2> GeneralSFMFactor2Cal3_S2;
106 
107 typedef gtsam::GenericStereoFactor<gtsam::Pose3, gtsam::Point3> GenericStereoFactor3D;
108 
109 
110 // Convenience for named keys
111 using symbol_shorthand::X;
112 using symbol_shorthand::L;
113 
114 
115 /* Create GUIDs for Noisemodels */
116 /* ************************************************************************* */
117 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
118 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
119 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian");
120 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
121 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");
122 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Robust, "gtsam_noiseModel_Robust");
123 
124 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Base , "gtsam_noiseModel_mEstimator_Base");
125 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Null , "gtsam_noiseModel_mEstimator_Null");
126 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Fair , "gtsam_noiseModel_mEstimator_Fair");
127 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Huber, "gtsam_noiseModel_mEstimator_Huber");
128 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Tukey, "gtsam_noiseModel_mEstimator_Tukey");
129 
130 BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
131 BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
132 
133 /* Create GUIDs for geometry */
134 /* ************************************************************************* */
135 GTSAM_VALUE_EXPORT(gtsam::Point2);
136 GTSAM_VALUE_EXPORT(gtsam::StereoPoint2);
137 GTSAM_VALUE_EXPORT(gtsam::Point3);
138 GTSAM_VALUE_EXPORT(gtsam::Rot2);
139 GTSAM_VALUE_EXPORT(gtsam::Rot3);
140 GTSAM_VALUE_EXPORT(gtsam::Pose2);
141 GTSAM_VALUE_EXPORT(gtsam::Pose3);
142 GTSAM_VALUE_EXPORT(gtsam::Cal3_S2);
143 GTSAM_VALUE_EXPORT(gtsam::Cal3DS2);
144 GTSAM_VALUE_EXPORT(gtsam::Cal3_S2Stereo);
145 GTSAM_VALUE_EXPORT(gtsam::CalibratedCamera);
146 GTSAM_VALUE_EXPORT(gtsam::PinholeCameraCal3_S2);
147 GTSAM_VALUE_EXPORT(gtsam::StereoCamera);
148 
149 /* Create GUIDs for factors */
150 /* ************************************************************************* */
151 BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor");
152 BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor");
153 
154 BOOST_CLASS_EXPORT_GUID(PriorFactorPoint2, "gtsam::PriorFactorPoint2");
155 BOOST_CLASS_EXPORT_GUID(PriorFactorStereoPoint2, "gtsam::PriorFactorStereoPoint2");
156 BOOST_CLASS_EXPORT_GUID(PriorFactorPoint3, "gtsam::PriorFactorPoint3");
157 BOOST_CLASS_EXPORT_GUID(PriorFactorRot2, "gtsam::PriorFactorRot2");
158 BOOST_CLASS_EXPORT_GUID(PriorFactorRot3, "gtsam::PriorFactorRot3");
159 BOOST_CLASS_EXPORT_GUID(PriorFactorPose2, "gtsam::PriorFactorPose2");
160 BOOST_CLASS_EXPORT_GUID(PriorFactorPose3, "gtsam::PriorFactorPose3");
161 BOOST_CLASS_EXPORT_GUID(PriorFactorCal3_S2, "gtsam::PriorFactorCal3_S2");
162 BOOST_CLASS_EXPORT_GUID(PriorFactorCal3DS2, "gtsam::PriorFactorCal3DS2");
163 BOOST_CLASS_EXPORT_GUID(PriorFactorCalibratedCamera, "gtsam::PriorFactorCalibratedCamera");
164 BOOST_CLASS_EXPORT_GUID(PriorFactorStereoCamera, "gtsam::PriorFactorStereoCamera");
165 
166 BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint2, "gtsam::BetweenFactorPoint2");
167 BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint3, "gtsam::BetweenFactorPoint3");
168 BOOST_CLASS_EXPORT_GUID(BetweenFactorRot2, "gtsam::BetweenFactorRot2");
169 BOOST_CLASS_EXPORT_GUID(BetweenFactorRot3, "gtsam::BetweenFactorRot3");
170 BOOST_CLASS_EXPORT_GUID(BetweenFactorPose2, "gtsam::BetweenFactorPose2");
171 BOOST_CLASS_EXPORT_GUID(BetweenFactorPose3, "gtsam::BetweenFactorPose3");
172 
173 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint2, "gtsam::NonlinearEqualityPoint2");
174 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoPoint2, "gtsam::NonlinearEqualityStereoPoint2");
175 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint3, "gtsam::NonlinearEqualityPoint3");
176 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityRot2, "gtsam::NonlinearEqualityRot2");
177 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityRot3, "gtsam::NonlinearEqualityRot3");
178 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPose2, "gtsam::NonlinearEqualityPose2");
179 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPose3, "gtsam::NonlinearEqualityPose3");
180 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3_S2, "gtsam::NonlinearEqualityCal3_S2");
181 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3DS2, "gtsam::NonlinearEqualityCal3DS2");
182 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCalibratedCamera, "gtsam::NonlinearEqualityCalibratedCamera");
183 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoCamera, "gtsam::NonlinearEqualityStereoCamera");
184 
185 BOOST_CLASS_EXPORT_GUID(RangeFactor2D, "gtsam::RangeFactor2D");
186 BOOST_CLASS_EXPORT_GUID(RangeFactor3D, "gtsam::RangeFactor3D");
187 BOOST_CLASS_EXPORT_GUID(RangeFactorPose2, "gtsam::RangeFactorPose2");
188 BOOST_CLASS_EXPORT_GUID(RangeFactorPose3, "gtsam::RangeFactorPose3");
189 BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCameraPoint, "gtsam::RangeFactorCalibratedCameraPoint");
190 BOOST_CLASS_EXPORT_GUID(RangeFactorPinholeCameraCal3_S2Point, "gtsam::RangeFactorPinholeCameraCal3_S2Point");
191 BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCamera, "gtsam::RangeFactorCalibratedCamera");
192 BOOST_CLASS_EXPORT_GUID(RangeFactorPinholeCameraCal3_S2, "gtsam::RangeFactorPinholeCameraCal3_S2");
193 
194 BOOST_CLASS_EXPORT_GUID(BearingRangeFactor2D, "gtsam::BearingRangeFactor2D");
195 
196 BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3_S2, "gtsam::GenericProjectionFactorCal3_S2");
197 BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3DS2, "gtsam::GenericProjectionFactorCal3DS2");
198 
199 BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3_S2, "gtsam::GeneralSFMFactorCal3_S2");
200 BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2");
201 
202 BOOST_CLASS_EXPORT_GUID(GeneralSFMFactor2Cal3_S2, "gtsam::GeneralSFMFactor2Cal3_S2");
203 
204 BOOST_CLASS_EXPORT_GUID(GenericStereoFactor3D, "gtsam::GenericStereoFactor3D");
205 
206 
207 /* ************************************************************************* */
208 TEST (testSerializationSLAM, smallExample_linear) {
209  using namespace example;
210 
211  Ordering ordering; ordering += X(1),X(2),L(1);
212  EXPECT(equalsObj(ordering));
213  EXPECT(equalsXML(ordering));
214  EXPECT(equalsBinary(ordering));
215 
216  GaussianFactorGraph fg = createGaussianFactorGraph();
217  EXPECT(equalsObj(fg));
218  EXPECT(equalsXML(fg));
219  EXPECT(equalsBinary(fg));
220 
221  GaussianBayesNet cbn = createSmallGaussianBayesNet();
222  EXPECT(equalsObj(cbn));
223  EXPECT(equalsXML(cbn));
224  EXPECT(equalsBinary(cbn));
225 }
226 
227 /* ************************************************************************* */
228 TEST (testSerializationSLAM, gaussianISAM) {
229  using namespace example;
230  GaussianFactorGraph smoother = createSmoother(7);
231  GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal();
232  GaussianISAM isam(bayesTree);
233 
234  EXPECT(equalsObj(isam));
235  EXPECT(equalsXML(isam));
236  EXPECT(equalsBinary(isam));
237 }
238 
239 /* ************************************************************************* */
240 /* Create GUIDs for factors in simulated2D */
241 BOOST_CLASS_EXPORT_GUID(simulated2D::Prior, "gtsam::simulated2D::Prior" )
242 BOOST_CLASS_EXPORT_GUID(simulated2D::Odometry, "gtsam::simulated2D::Odometry" )
243 BOOST_CLASS_EXPORT_GUID(simulated2D::Measurement, "gtsam::simulated2D::Measurement")
244 
245 /* ************************************************************************* */
246 TEST (testSerializationSLAM, smallExample_nonlinear) {
247  using namespace example;
248  NonlinearFactorGraph nfg = createNonlinearFactorGraph();
249  Values c1 = createValues();
250  EXPECT(equalsObj(nfg));
251  EXPECT(equalsXML(nfg));
252  EXPECT(equalsBinary(nfg));
253 
254  EXPECT(equalsObj(c1));
255  EXPECT(equalsXML(c1));
256  EXPECT(equalsBinary(c1));
257 }
258 
259 /* ************************************************************************* */
260 TEST (testSerializationSLAM, factors) {
261 
262  Point2 point2(1.0, 2.0);
263  StereoPoint2 stereoPoint2(1.0, 2.0, 3.0);
264  Point3 point3(1.0, 2.0, 3.0);
265  Rot2 rot2(1.0);
266  Rot3 rot3(Rot3::RzRyRx(1.0, 2.0, 3.0));
267  Pose2 pose2(rot2, point2);
268  Pose3 pose3(rot3, point3);
269  Cal3_S2 cal3_s2(1.0, 2.0, 3.0, 4.0, 5.0);
270  Cal3DS2 cal3ds2(1.0, 2.0, 3.0, 4.0, 5.0,6.0, 7.0, 8.0, 9.0);
271  Cal3_S2Stereo cal3_s2stereo(1.0, 2.0, 3.0, 4.0, 5.0, 1.0);
272  CalibratedCamera calibratedCamera(pose3);
273  PinholeCamera<Cal3_S2> simpleCamera(pose3, cal3_s2);
274  StereoCamera stereoCamera(pose3, boost::make_shared<Cal3_S2Stereo>(cal3_s2stereo));
275 
276 
277  Symbol a01('a',1), a02('a',2), a03('a',3), a04('a',4), a05('a',5),
278  a06('a',6), a07('a',7), a08('a',8), a09('a',9), a10('a',10),
279  a11('a',11), a12('a',12), a13('a',13), a14('a',14), a15('a',15);
280  Symbol b01('b',1), b02('b',2), b03('b',3), b04('b',4), b05('b',5),
281  b06('b',6), b07('b',7), b08('b',8), b09('b',9), b10('b',10),
282  b11('b',11), b12('b',12), b13('b',13), b14('b',14), b15('b',15);
283 
284  Values values;
285  values.insert(a03, point2);
286  values.insert(a04, stereoPoint2);
287  values.insert(a05, point3);
288  values.insert(a06, rot2);
289  values.insert(a07, rot3);
290  values.insert(a08, pose2);
291  values.insert(a09, pose3);
292  values.insert(a10, cal3_s2);
293  values.insert(a11, cal3ds2);
294  values.insert(a12, calibratedCamera);
295  values.insert(a13, simpleCamera);
296  values.insert(a14, stereoCamera);
297 
298 
299  SharedNoiseModel model1 = noiseModel::Isotropic::Sigma(1, 0.3);
300  SharedNoiseModel model2 = noiseModel::Isotropic::Sigma(2, 0.3);
301  SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.3);
302  SharedNoiseModel model4 = noiseModel::Isotropic::Sigma(4, 0.3);
303  SharedNoiseModel model5 = noiseModel::Isotropic::Sigma(5, 0.3);
304  SharedNoiseModel model6 = noiseModel::Isotropic::Sigma(6, 0.3);
305  SharedNoiseModel model9 = noiseModel::Isotropic::Sigma(9, 0.3);
306  SharedNoiseModel model11 = noiseModel::Isotropic::Sigma(11, 0.3);
307 
308  SharedNoiseModel robust1 = noiseModel::Robust::Create(
309  noiseModel::mEstimator::Huber::Create(10.0, noiseModel::mEstimator::Huber::Scalar),
310  noiseModel::Unit::Create(2));
311 
312  EXPECT(equalsDereferenced(robust1));
313  EXPECT(equalsDereferencedXML(robust1));
314  EXPECT(equalsDereferencedBinary(robust1));
315 
316  PriorFactorPoint2 priorFactorPoint2(a03, point2, model2);
317  PriorFactorStereoPoint2 priorFactorStereoPoint2(a04, stereoPoint2, model3);
318  PriorFactorPoint3 priorFactorPoint3(a05, point3, model3);
319  PriorFactorRot2 priorFactorRot2(a06, rot2, model1);
320  PriorFactorRot3 priorFactorRot3(a07, rot3, model3);
321  PriorFactorPose2 priorFactorPose2(a08, pose2, model3);
322  PriorFactorPose3 priorFactorPose3(a09, pose3, model6);
323  PriorFactorCal3_S2 priorFactorCal3_S2(a10, cal3_s2, model5);
324  PriorFactorCal3DS2 priorFactorCal3DS2(a11, cal3ds2, model9);
325  PriorFactorCalibratedCamera priorFactorCalibratedCamera(a12, calibratedCamera, model6);
326  PriorFactorStereoCamera priorFactorStereoCamera(a14, stereoCamera, model11);
327 
328  BetweenFactorPoint2 betweenFactorPoint2(a03, b03, point2, model2);
329  BetweenFactorPoint3 betweenFactorPoint3(a05, b05, point3, model3);
330  BetweenFactorRot2 betweenFactorRot2(a06, b06, rot2, model1);
331  BetweenFactorRot3 betweenFactorRot3(a07, b07, rot3, model3);
332  BetweenFactorPose2 betweenFactorPose2(a08, b08, pose2, model3);
333  BetweenFactorPose3 betweenFactorPose3(a09, b09, pose3, model6);
334 
335  NonlinearEqualityPoint2 nonlinearEqualityPoint2(a03, point2);
336  NonlinearEqualityStereoPoint2 nonlinearEqualityStereoPoint2(a04, stereoPoint2);
337  NonlinearEqualityPoint3 nonlinearEqualityPoint3(a05, point3);
338  NonlinearEqualityRot2 nonlinearEqualityRot2(a06, rot2);
339  NonlinearEqualityRot3 nonlinearEqualityRot3(a07, rot3);
340  NonlinearEqualityPose2 nonlinearEqualityPose2(a08, pose2);
341  NonlinearEqualityPose3 nonlinearEqualityPose3(a09, pose3);
342  NonlinearEqualityCal3_S2 nonlinearEqualityCal3_S2(a10, cal3_s2);
343  NonlinearEqualityCal3DS2 nonlinearEqualityCal3DS2(a11, cal3ds2);
344  NonlinearEqualityCalibratedCamera nonlinearEqualityCalibratedCamera(a12, calibratedCamera);
345  NonlinearEqualityStereoCamera nonlinearEqualityStereoCamera(a14, stereoCamera);
346 
347  RangeFactor2D rangeFactor2D(a08, a03, 2.0, model1);
348  RangeFactor3D rangeFactor3D(a09, a05, 2.0, model1);
349  RangeFactorPose2 rangeFactorPose2(a08, b08, 2.0, model1);
350  RangeFactorPose3 rangeFactorPose3(a09, b09, 2.0, model1);
351  RangeFactorCalibratedCameraPoint rangeFactorCalibratedCameraPoint(a12, a05, 2.0, model1);
352  RangeFactorPinholeCameraCal3_S2Point rangeFactorPinholeCameraCal3_S2Point(a13, a05, 2.0, model1);
353  RangeFactorCalibratedCamera rangeFactorCalibratedCamera(a12, b12, 2.0, model1);
354  RangeFactorPinholeCameraCal3_S2 rangeFactorPinholeCameraCal3_S2(a13, b13, 2.0, model1);
355 
356  BearingRangeFactor2D bearingRangeFactor2D(a08, a03, rot2, 2.0, model2);
357 
358  GenericProjectionFactorCal3_S2 genericProjectionFactorCal3_S2(point2, model2, a09, a05, boost::make_shared<Cal3_S2>(cal3_s2));
359  GenericProjectionFactorCal3DS2 genericProjectionFactorCal3DS2(point2, model2, a09, a05, boost::make_shared<Cal3DS2>(cal3ds2));
360 
361  GeneralSFMFactorCal3_S2 generalSFMFactorCal3_S2(point2, model2, a13, a05);
362 
363  GeneralSFMFactor2Cal3_S2 generalSFMFactor2Cal3_S2(point2, model2, a09, a05, a10);
364 
365  GenericStereoFactor3D genericStereoFactor3D(stereoPoint2, model3, a09, a05, boost::make_shared<Cal3_S2Stereo>(cal3_s2stereo));
366 
367 
368  NonlinearFactorGraph graph;
369  graph += priorFactorPoint2;
370  graph += priorFactorStereoPoint2;
371  graph += priorFactorPoint3;
372  graph += priorFactorRot2;
373  graph += priorFactorRot3;
374  graph += priorFactorPose2;
375  graph += priorFactorPose3;
376  graph += priorFactorCal3_S2;
377  graph += priorFactorCal3DS2;
378  graph += priorFactorCalibratedCamera;
379  graph += priorFactorStereoCamera;
380 
381  graph += betweenFactorPoint2;
382  graph += betweenFactorPoint3;
383  graph += betweenFactorRot2;
384  graph += betweenFactorRot3;
385  graph += betweenFactorPose2;
386  graph += betweenFactorPose3;
387 
388  graph += nonlinearEqualityPoint2;
389  graph += nonlinearEqualityStereoPoint2;
390  graph += nonlinearEqualityPoint3;
391  graph += nonlinearEqualityRot2;
392  graph += nonlinearEqualityRot3;
393  graph += nonlinearEqualityPose2;
394  graph += nonlinearEqualityPose3;
395  graph += nonlinearEqualityCal3_S2;
396  graph += nonlinearEqualityCal3DS2;
397  graph += nonlinearEqualityCalibratedCamera;
398  graph += nonlinearEqualityStereoCamera;
399 
400  graph += rangeFactor2D;
401  graph += rangeFactor3D;
402  graph += rangeFactorPose2;
403  graph += rangeFactorPose3;
404  graph += rangeFactorCalibratedCameraPoint;
405  graph += rangeFactorPinholeCameraCal3_S2Point;
406  graph += rangeFactorCalibratedCamera;
407  graph += rangeFactorPinholeCameraCal3_S2;
408 
409  graph += bearingRangeFactor2D;
410 
411  graph += genericProjectionFactorCal3_S2;
412  graph += genericProjectionFactorCal3DS2;
413 
414  graph += generalSFMFactorCal3_S2;
415 
416  graph += generalSFMFactor2Cal3_S2;
417 
418  graph += genericStereoFactor3D;
419 
420 
421  // text
422  EXPECT(equalsObj<Symbol>(a01));
423  EXPECT(equalsObj<Symbol>(b02));
424  EXPECT(equalsObj<Values>(values));
425  EXPECT(equalsObj<NonlinearFactorGraph>(graph));
426 
427  EXPECT(equalsObj<PriorFactorPoint2>(priorFactorPoint2));
428  EXPECT(equalsObj<PriorFactorStereoPoint2>(priorFactorStereoPoint2));
429  EXPECT(equalsObj<PriorFactorPoint3>(priorFactorPoint3));
430  EXPECT(equalsObj<PriorFactorRot2>(priorFactorRot2));
431  EXPECT(equalsObj<PriorFactorRot3>(priorFactorRot3));
432  EXPECT(equalsObj<PriorFactorPose2>(priorFactorPose2));
433  EXPECT(equalsObj<PriorFactorPose3>(priorFactorPose3));
434  EXPECT(equalsObj<PriorFactorCal3_S2>(priorFactorCal3_S2));
435  EXPECT(equalsObj<PriorFactorCal3DS2>(priorFactorCal3DS2));
436  EXPECT(equalsObj<PriorFactorCalibratedCamera>(priorFactorCalibratedCamera));
437  EXPECT(equalsObj<PriorFactorStereoCamera>(priorFactorStereoCamera));
438 
439  EXPECT(equalsObj<BetweenFactorPoint2>(betweenFactorPoint2));
440  EXPECT(equalsObj<BetweenFactorPoint3>(betweenFactorPoint3));
441  EXPECT(equalsObj<BetweenFactorRot2>(betweenFactorRot2));
442  EXPECT(equalsObj<BetweenFactorRot3>(betweenFactorRot3));
443  EXPECT(equalsObj<BetweenFactorPose2>(betweenFactorPose2));
444  EXPECT(equalsObj<BetweenFactorPose3>(betweenFactorPose3));
445 
446  EXPECT(equalsObj<NonlinearEqualityPoint2>(nonlinearEqualityPoint2));
447  EXPECT(equalsObj<NonlinearEqualityStereoPoint2>(nonlinearEqualityStereoPoint2));
448  EXPECT(equalsObj<NonlinearEqualityPoint3>(nonlinearEqualityPoint3));
449  EXPECT(equalsObj<NonlinearEqualityRot2>(nonlinearEqualityRot2));
450  EXPECT(equalsObj<NonlinearEqualityRot3>(nonlinearEqualityRot3));
451  EXPECT(equalsObj<NonlinearEqualityPose2>(nonlinearEqualityPose2));
452  EXPECT(equalsObj<NonlinearEqualityPose3>(nonlinearEqualityPose3));
453  EXPECT(equalsObj<NonlinearEqualityCal3_S2>(nonlinearEqualityCal3_S2));
454  EXPECT(equalsObj<NonlinearEqualityCal3DS2>(nonlinearEqualityCal3DS2));
455  EXPECT(equalsObj<NonlinearEqualityCalibratedCamera>(nonlinearEqualityCalibratedCamera));
456  EXPECT(equalsObj<NonlinearEqualityStereoCamera>(nonlinearEqualityStereoCamera));
457 
458  EXPECT(equalsObj<RangeFactor2D>(rangeFactor2D));
459  EXPECT(equalsObj<RangeFactor3D>(rangeFactor3D));
460  EXPECT(equalsObj<RangeFactorPose2>(rangeFactorPose2));
461  EXPECT(equalsObj<RangeFactorPose3>(rangeFactorPose3));
462  EXPECT(equalsObj<RangeFactorCalibratedCameraPoint>(rangeFactorCalibratedCameraPoint));
463  EXPECT(equalsObj<RangeFactorPinholeCameraCal3_S2Point>(rangeFactorPinholeCameraCal3_S2Point));
464  EXPECT(equalsObj<RangeFactorCalibratedCamera>(rangeFactorCalibratedCamera));
465  EXPECT(equalsObj<RangeFactorPinholeCameraCal3_S2>(rangeFactorPinholeCameraCal3_S2));
466 
467  EXPECT(equalsObj<BearingRangeFactor2D>(bearingRangeFactor2D));
468 
469  EXPECT(equalsObj<GenericProjectionFactorCal3_S2>(genericProjectionFactorCal3_S2));
470  EXPECT(equalsObj<GenericProjectionFactorCal3DS2>(genericProjectionFactorCal3DS2));
471 
472  EXPECT(equalsObj<GeneralSFMFactorCal3_S2>(generalSFMFactorCal3_S2));
473 
474  EXPECT(equalsObj<GeneralSFMFactor2Cal3_S2>(generalSFMFactor2Cal3_S2));
475 
476  EXPECT(equalsObj<GenericStereoFactor3D>(genericStereoFactor3D));
477 
478 
479  // xml
480  EXPECT(equalsXML<Symbol>(a01));
481  EXPECT(equalsXML<Symbol>(b02));
482  EXPECT(equalsXML<Values>(values));
483  EXPECT(equalsXML<NonlinearFactorGraph>(graph));
484 
485  EXPECT(equalsXML<PriorFactorPoint2>(priorFactorPoint2));
486  EXPECT(equalsXML<PriorFactorStereoPoint2>(priorFactorStereoPoint2));
487  EXPECT(equalsXML<PriorFactorPoint3>(priorFactorPoint3));
488  EXPECT(equalsXML<PriorFactorRot2>(priorFactorRot2));
489  EXPECT(equalsXML<PriorFactorRot3>(priorFactorRot3));
490  EXPECT(equalsXML<PriorFactorPose2>(priorFactorPose2));
491  EXPECT(equalsXML<PriorFactorPose3>(priorFactorPose3));
492  EXPECT(equalsXML<PriorFactorCal3_S2>(priorFactorCal3_S2));
493  EXPECT(equalsXML<PriorFactorCal3DS2>(priorFactorCal3DS2));
494  EXPECT(equalsXML<PriorFactorCalibratedCamera>(priorFactorCalibratedCamera));
495  EXPECT(equalsXML<PriorFactorStereoCamera>(priorFactorStereoCamera));
496 
497  EXPECT(equalsXML<BetweenFactorPoint2>(betweenFactorPoint2));
498  EXPECT(equalsXML<BetweenFactorPoint3>(betweenFactorPoint3));
499  EXPECT(equalsXML<BetweenFactorRot2>(betweenFactorRot2));
500  EXPECT(equalsXML<BetweenFactorRot3>(betweenFactorRot3));
501  EXPECT(equalsXML<BetweenFactorPose2>(betweenFactorPose2));
502  EXPECT(equalsXML<BetweenFactorPose3>(betweenFactorPose3));
503 
504  EXPECT(equalsXML<NonlinearEqualityPoint2>(nonlinearEqualityPoint2));
505  EXPECT(equalsXML<NonlinearEqualityStereoPoint2>(nonlinearEqualityStereoPoint2));
506  EXPECT(equalsXML<NonlinearEqualityPoint3>(nonlinearEqualityPoint3));
507  EXPECT(equalsXML<NonlinearEqualityRot2>(nonlinearEqualityRot2));
508  EXPECT(equalsXML<NonlinearEqualityRot3>(nonlinearEqualityRot3));
509  EXPECT(equalsXML<NonlinearEqualityPose2>(nonlinearEqualityPose2));
510  EXPECT(equalsXML<NonlinearEqualityPose3>(nonlinearEqualityPose3));
511  EXPECT(equalsXML<NonlinearEqualityCal3_S2>(nonlinearEqualityCal3_S2));
512  EXPECT(equalsXML<NonlinearEqualityCal3DS2>(nonlinearEqualityCal3DS2));
513  EXPECT(equalsXML<NonlinearEqualityCalibratedCamera>(nonlinearEqualityCalibratedCamera));
514  EXPECT(equalsXML<NonlinearEqualityStereoCamera>(nonlinearEqualityStereoCamera));
515 
516  EXPECT(equalsXML<RangeFactor2D>(rangeFactor2D));
517  EXPECT(equalsXML<RangeFactor3D>(rangeFactor3D));
518  EXPECT(equalsXML<RangeFactorPose2>(rangeFactorPose2));
519  EXPECT(equalsXML<RangeFactorPose3>(rangeFactorPose3));
520  EXPECT(equalsXML<RangeFactorCalibratedCameraPoint>(rangeFactorCalibratedCameraPoint));
521  EXPECT(equalsXML<RangeFactorPinholeCameraCal3_S2Point>(rangeFactorPinholeCameraCal3_S2Point));
522  EXPECT(equalsXML<RangeFactorCalibratedCamera>(rangeFactorCalibratedCamera));
523  EXPECT(equalsXML<RangeFactorPinholeCameraCal3_S2>(rangeFactorPinholeCameraCal3_S2));
524 
525  EXPECT(equalsXML<BearingRangeFactor2D>(bearingRangeFactor2D));
526 
527  EXPECT(equalsXML<GenericProjectionFactorCal3_S2>(genericProjectionFactorCal3_S2));
528  EXPECT(equalsXML<GenericProjectionFactorCal3DS2>(genericProjectionFactorCal3DS2));
529 
530  EXPECT(equalsXML<GeneralSFMFactorCal3_S2>(generalSFMFactorCal3_S2));
531 
532  EXPECT(equalsXML<GeneralSFMFactor2Cal3_S2>(generalSFMFactor2Cal3_S2));
533 
534  EXPECT(equalsXML<GenericStereoFactor3D>(genericStereoFactor3D));
535 
536 
537  // binary
538  EXPECT(equalsBinary<Symbol>(a01));
539  EXPECT(equalsBinary<Symbol>(b02));
540  EXPECT(equalsBinary<Values>(values));
541  EXPECT(equalsBinary<NonlinearFactorGraph>(graph));
542 
543  EXPECT(equalsBinary<PriorFactorPoint2>(priorFactorPoint2));
544  EXPECT(equalsBinary<PriorFactorStereoPoint2>(priorFactorStereoPoint2));
545  EXPECT(equalsBinary<PriorFactorPoint3>(priorFactorPoint3));
546  EXPECT(equalsBinary<PriorFactorRot2>(priorFactorRot2));
547  EXPECT(equalsBinary<PriorFactorRot3>(priorFactorRot3));
548  EXPECT(equalsBinary<PriorFactorPose2>(priorFactorPose2));
549  EXPECT(equalsBinary<PriorFactorPose3>(priorFactorPose3));
550  EXPECT(equalsBinary<PriorFactorCal3_S2>(priorFactorCal3_S2));
551  EXPECT(equalsBinary<PriorFactorCal3DS2>(priorFactorCal3DS2));
552  EXPECT(equalsBinary<PriorFactorCalibratedCamera>(priorFactorCalibratedCamera));
553  EXPECT(equalsBinary<PriorFactorStereoCamera>(priorFactorStereoCamera));
554 
555  EXPECT(equalsBinary<BetweenFactorPoint2>(betweenFactorPoint2));
556  EXPECT(equalsBinary<BetweenFactorPoint3>(betweenFactorPoint3));
557  EXPECT(equalsBinary<BetweenFactorRot2>(betweenFactorRot2));
558  EXPECT(equalsBinary<BetweenFactorRot3>(betweenFactorRot3));
559  EXPECT(equalsBinary<BetweenFactorPose2>(betweenFactorPose2));
560  EXPECT(equalsBinary<BetweenFactorPose3>(betweenFactorPose3));
561 
562  EXPECT(equalsBinary<NonlinearEqualityPoint2>(nonlinearEqualityPoint2));
563  EXPECT(equalsBinary<NonlinearEqualityStereoPoint2>(nonlinearEqualityStereoPoint2));
564  EXPECT(equalsBinary<NonlinearEqualityPoint3>(nonlinearEqualityPoint3));
565  EXPECT(equalsBinary<NonlinearEqualityRot2>(nonlinearEqualityRot2));
566  EXPECT(equalsBinary<NonlinearEqualityRot3>(nonlinearEqualityRot3));
567  EXPECT(equalsBinary<NonlinearEqualityPose2>(nonlinearEqualityPose2));
568  EXPECT(equalsBinary<NonlinearEqualityPose3>(nonlinearEqualityPose3));
569  EXPECT(equalsBinary<NonlinearEqualityCal3_S2>(nonlinearEqualityCal3_S2));
570  EXPECT(equalsBinary<NonlinearEqualityCal3DS2>(nonlinearEqualityCal3DS2));
571  EXPECT(equalsBinary<NonlinearEqualityCalibratedCamera>(nonlinearEqualityCalibratedCamera));
572  EXPECT(equalsBinary<NonlinearEqualityStereoCamera>(nonlinearEqualityStereoCamera));
573 
574  EXPECT(equalsBinary<RangeFactor2D>(rangeFactor2D));
575  EXPECT(equalsBinary<RangeFactor3D>(rangeFactor3D));
576  EXPECT(equalsBinary<RangeFactorPose2>(rangeFactorPose2));
577  EXPECT(equalsBinary<RangeFactorPose3>(rangeFactorPose3));
578  EXPECT(equalsBinary<RangeFactorCalibratedCameraPoint>(rangeFactorCalibratedCameraPoint));
579  EXPECT(equalsBinary<RangeFactorPinholeCameraCal3_S2Point>(rangeFactorPinholeCameraCal3_S2Point));
580  EXPECT(equalsBinary<RangeFactorCalibratedCamera>(rangeFactorCalibratedCamera));
581  EXPECT(equalsBinary<RangeFactorPinholeCameraCal3_S2>(rangeFactorPinholeCameraCal3_S2));
582 
583  EXPECT(equalsBinary<BearingRangeFactor2D>(bearingRangeFactor2D));
584 
585  EXPECT(equalsBinary<GenericProjectionFactorCal3_S2>(genericProjectionFactorCal3_S2));
586  EXPECT(equalsBinary<GenericProjectionFactorCal3DS2>(genericProjectionFactorCal3DS2));
587 
588  EXPECT(equalsBinary<GeneralSFMFactorCal3_S2>(generalSFMFactorCal3_S2));
589 
590  EXPECT(equalsBinary<GeneralSFMFactor2Cal3_S2>(generalSFMFactor2Cal3_S2));
591 
592  EXPECT(equalsBinary<GenericStereoFactor3D>(genericStereoFactor3D));
593 }
594 
595 
596 /* ************************************************************************* */
597 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
598 /* ************************************************************************* */
RangeFactorPose2
RangeFactor< Pose2, Pose2 > RangeFactorPose2
Definition: testSerializationSLAM.cpp:89
NonlinearEqualityStereoPoint2
NonlinearEquality< StereoPoint2 > NonlinearEqualityStereoPoint2
Definition: testSerializationSLAM.cpp:75
Scalar
SCALAR Scalar
Definition: bench_gemm.cpp:33
PriorFactorRot3
PriorFactor< Rot3 > PriorFactorRot3
Definition: testSerializationSLAM.cpp:58
RangeFactorPinholeCameraCal3_S2Point
RangeFactor< PinholeCameraCal3_S2, Point3 > RangeFactorPinholeCameraCal3_S2Point
Definition: testSerializationSLAM.cpp:92
NonlinearEqualityRot2
NonlinearEquality< Rot2 > NonlinearEqualityRot2
Definition: testSerializationSLAM.cpp:77
gtsam::example::createSmoother
GaussianFactorGraph createSmoother(int T)
Definition: smallExample.h:464
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::Measurement
This is the base class for all measurement types.
Definition: Measurement.h:11
GenericStereoFactor3D
gtsam::GenericStereoFactor< gtsam::Pose3, gtsam::Point3 > GenericStereoFactor3D
Definition: testSerializationSLAM.cpp:107
gtsam::example::createGaussianFactorGraph
GaussianFactorGraph createGaussianFactorGraph()
Definition: smallExample.h:272
NonlinearEqualityPose3
NonlinearEquality< Pose3 > NonlinearEqualityPose3
Definition: testSerializationSLAM.cpp:80
gtsam::serializationTestHelpers::equalsDereferencedXML
bool equalsDereferencedXML(const T &input=T())
Definition: serializationTestHelpers.h:128
RangeFactorCalibratedCameraPoint
RangeFactor< CalibratedCamera, Point3 > RangeFactorCalibratedCameraPoint
Definition: testSerializationSLAM.cpp:91
RangeFactorPinholeCameraCal3_S2
RangeFactor< PinholeCameraCal3_S2, PinholeCameraCal3_S2 > RangeFactorPinholeCameraCal3_S2
Definition: testSerializationSLAM.cpp:94
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:140
ordering
static enum @843 ordering
GenericProjectionFactorCal3_S2
GenericProjectionFactor< Pose3, Point3, Cal3_S2 > GenericProjectionFactorCal3_S2
Definition: testSerializationSLAM.cpp:99
gtsam::EliminateableFactorGraph::eliminateMultifrontal
boost::shared_ptr< BayesTreeType > eliminateMultifrontal(OptionalOrderingType orderingType=boost::none, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const
Definition: EliminateableFactorGraph-inst.h:83
BearingRangeFactor3D
BearingRangeFactor< Pose3, Point3 > BearingRangeFactor3D
Definition: testSerializationSLAM.cpp:97
gtsam::Point2
Vector2 Point2
Definition: Point2.h:27
BearingRangeFactor2D
BearingRangeFactor< Pose2, Point2 > BearingRangeFactor2D
Definition: testSerializationSLAM.cpp:96
leaf::values
leaf::MyValues values
RangeFactor3D
RangeFactor< Pose3, Point3 > RangeFactor3D
Definition: testSerializationSLAM.cpp:88
NonlinearEqualityPinholeCameraCal3_S2
NonlinearEquality< PinholeCameraCal3_S2 > NonlinearEqualityPinholeCameraCal3_S2
Definition: testSerializationSLAM.cpp:84
gtsam::noiseModel::mEstimator::Tukey
Tukey implements the "Tukey" robust error model (Zhang97ivc)
Definition: LossFunctions.h:230
L
MatrixXd L
Definition: LLT_example.cpp:6
std
Definition: Half.h:150
Cal3DS2.h
Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base.
simple_graph::factors
GaussianFactorGraph factors(list_of(factor1)(factor2)(factor3))
gtsam::serializationTestHelpers::equalsDereferencedBinary
bool equalsDereferencedBinary(const T &input=T())
Definition: serializationTestHelpers.h:169
gtsam::noiseModel::mEstimator::Huber
Huber implements the "Huber" robust error model (Zhang97ivc)
Definition: LossFunctions.h:175
NonlinearEqualityStereoCamera
NonlinearEquality< StereoCamera > NonlinearEqualityStereoCamera
Definition: testSerializationSLAM.cpp:85
PriorFactorRot2
PriorFactor< Rot2 > PriorFactorRot2
Definition: testSerializationSLAM.cpp:57
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
GenericProjectionFactorCal3DS2
GenericProjectionFactor< Pose3, Point3, Cal3DS2 > GenericProjectionFactorCal3DS2
Definition: testSerializationSLAM.cpp:100
model1
noiseModel::Isotropic::shared_ptr model1
Definition: testEssentialMatrixFactor.cpp:26
PriorFactorCalibratedCamera
PriorFactor< CalibratedCamera > PriorFactorCalibratedCamera
Definition: testSerializationSLAM.cpp:63
gtsam::noiseModel::mEstimator::Fair
Fair implements the "Fair" robust error model (Zhang97ivc)
Definition: LossFunctions.h:150
BetweenFactorRot2
BetweenFactor< Rot2 > BetweenFactorRot2
Definition: testSerializationSLAM.cpp:69
gtsam::noiseModel::mEstimator::Null
Null class should behave as Gaussian.
Definition: LossFunctions.h:128
main
int main()
Definition: testSerializationSLAM.cpp:597
PinholeCamera.h
Base class for all pinhole cameras.
GTSAM_VALUE_EXPORT
GTSAM_VALUE_EXPORT(gtsam::Point2)
RangeFactorPose3
RangeFactor< Pose3, Pose3 > RangeFactorPose3
Definition: testSerializationSLAM.cpp:90
gtsam::example::createSmallGaussianBayesNet
GaussianBayesNet createSmallGaussianBayesNet()
Definition: smallExample.h:299
BetweenFactorPoint2
BetweenFactor< Point2 > BetweenFactorPoint2
Definition: testSerializationSLAM.cpp:67
gtsam::Cal3_S2Stereo
Definition: Cal3_S2Stereo.h:30
GeneralSFMFactor2Cal3_S2
gtsam::GeneralSFMFactor2< gtsam::Cal3_S2 > GeneralSFMFactor2Cal3_S2
Definition: testSerializationSLAM.cpp:105
NonlinearEqualityCal3_S2
NonlinearEquality< Cal3_S2 > NonlinearEqualityCal3_S2
Definition: testSerializationSLAM.cpp:81
BetweenFactorPoint3
BetweenFactor< Point3 > BetweenFactorPoint3
Definition: testSerializationSLAM.cpp:68
EXPECT
#define EXPECT(condition)
Definition: Test.h:151
pose3
static Pose3 pose3(rt3, pt3)
model2
auto model2
Definition: testFunctorizedFactor.cpp:38
BOOST_CLASS_EXPORT_GUID
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained,"gtsam_noiseModel_Constrained")
PriorFactorCal3DS2
PriorFactor< Cal3DS2 > PriorFactorCal3DS2
Definition: testSerializationSLAM.cpp:62
BetweenFactorPose2
BetweenFactor< Pose2 > BetweenFactorPose2
Definition: testSerializationSLAM.cpp:71
model3
const SharedNoiseModel model3
Definition: testPoseRotationPrior.cpp:22
isam
NonlinearISAM isam(relinearizeInterval)
point3
static const Point3 point3(0.08, 0.08, 0.0)
gtsam::example::createNonlinearFactorGraph
NonlinearFactorGraph createNonlinearFactorGraph(const SharedNoiseModel &noiseModel1=impl::kSigma0_1, const SharedNoiseModel &noiseModel2=impl::kSigma0_2)
Definition: smallExample.h:207
gtsam::Cal3_S2
Definition: Cal3_S2.h:34
point2
static const Point3 point2(-0.08, 0.08, 0.0)
RangeFactor2D
RangeFactor< Pose2, Point2 > RangeFactor2D
Definition: testSerializationSLAM.cpp:87
PriorFactorStereoCamera
PriorFactor< StereoCamera > PriorFactorStereoCamera
Definition: testSerializationSLAM.cpp:65
NonlinearEqualityPose2
NonlinearEquality< Pose2 > NonlinearEqualityPose2
Definition: testSerializationSLAM.cpp:79
ProjectionFactor.h
Basic bearing factor from 2D measurement.
NonlinearEqualityPoint3
NonlinearEquality< Point3 > NonlinearEqualityPoint3
Definition: testSerializationSLAM.cpp:76
PriorFactorStereoPoint2
PriorFactor< StereoPoint2 > PriorFactorStereoPoint2
Definition: testSerializationSLAM.cpp:55
CalibratedCamera.h
Calibrated camera for which only pose is unknown.
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:736
RangeFactorCalibratedCamera
RangeFactor< CalibratedCamera, CalibratedCamera > RangeFactorCalibratedCamera
Definition: testSerializationSLAM.cpp:93
PriorFactorPoint2
PriorFactor< Point2 > PriorFactorPoint2
Definition: testSerializationSLAM.cpp:54
gtsam
traits
Definition: chartTesting.h:28
NonlinearEqualityCalibratedCamera
NonlinearEquality< CalibratedCamera > NonlinearEqualityCalibratedCamera
Definition: testSerializationSLAM.cpp:83
Cal3_S2Stereo.h
The most common 5DOF 3D->2D calibration + Stereo baseline.
PriorFactorPoint3
PriorFactor< Point3 > PriorFactorPoint3
Definition: testSerializationSLAM.cpp:56
PriorFactorPose3
PriorFactor< Pose3 > PriorFactorPose3
Definition: testSerializationSLAM.cpp:60
SimpleCamera.h
A simple camera class with a Cal3_S2 calibration.
StereoCamera.h
A Stereo Camera based on two Simple Cameras.
NonlinearEqualityRot3
NonlinearEquality< Rot3 > NonlinearEqualityRot3
Definition: testSerializationSLAM.cpp:78
PriorFactorCal3_S2
PriorFactor< Cal3_S2 > PriorFactorCal3_S2
Definition: testSerializationSLAM.cpp:61
PriorFactorPinholeCameraCal3_S2
PriorFactor< PinholeCameraCal3_S2 > PriorFactorPinholeCameraCal3_S2
Definition: testSerializationSLAM.cpp:64
Point3.h
3D Point
StereoPoint2.h
A 2D stereo point (uL,uR,v)
gtsam::HessianFactor
A Gaussian factor using the canonical parameters (information form)
Definition: HessianFactor.h:101
smallExample.h
Create small example with two poses and one landmark.
TEST
TEST(testSerializationSLAM, smallExample_linear)
Definition: testSerializationSLAM.cpp:208
BetweenFactorRot3
BetweenFactor< Rot3 > BetweenFactorRot3
Definition: testSerializationSLAM.cpp:70
pose2
static const Pose3 pose2
Definition: testTriangulation.cpp:48
gtsam::Point3
Vector3 Point3
Definition: Point3.h:35
gtsam::example::createValues
Values createValues()
Definition: smallExample.h:213
Pose2.h
2D Pose
PriorFactorPose2
PriorFactor< Pose2 > PriorFactorPose2
Definition: testSerializationSLAM.cpp:59
X
#define X
Definition: icosphere.cpp:20
GeneralSFMFactor.h
a general SFM factor with an unknown calibration
GeneralSFMFactorCal3DS2
gtsam::GeneralSFMFactor< gtsam::PinholeCameraCal3DS2, gtsam::Point3 > GeneralSFMFactorCal3DS2
Definition: testSerializationSLAM.cpp:103
Point2.h
2D Point
Pose3.h
3D Pose
GeneralSFMFactorCal3_S2
gtsam::GeneralSFMFactor< gtsam::PinholeCameraCal3_S2, gtsam::Point3 > GeneralSFMFactorCal3_S2
Definition: testSerializationSLAM.cpp:102
Rot2.h
2D rotation
c1
static const Key c1
Definition: testSmartProjectionFactor.cpp:34
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:734
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
Rot3.h
3D rotation represented as a rotation matrix or quaternion
NonlinearEqualityCal3DS2
NonlinearEquality< Cal3DS2 > NonlinearEqualityCal3DS2
Definition: testSerializationSLAM.cpp:82
NonlinearEqualityPoint2
NonlinearEquality< Point2 > NonlinearEqualityPoint2
Definition: testSerializationSLAM.cpp:74
BetweenFactorPose3
BetweenFactor< Pose3 > BetweenFactorPose3
Definition: testSerializationSLAM.cpp:72

gtsam
Author(s):
autogenerated on Sat May 8 2021 02:49:32