testSerializationNonlinear.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 <gtsam/nonlinear/Values.h>
20 #include <gtsam/nonlinear/ISAM2.h>
21 #include <gtsam/inference/Symbol.h>
22 #include <gtsam/geometry/PinholeCamera.h>
23 #include <gtsam/geometry/Pose2.h>
24 #include <gtsam/geometry/Cal3_S2.h>
25 #include <gtsam/geometry/Cal3DS2.h>
26 #include <gtsam/geometry/Cal3Bundler.h>
27 
28 #include <gtsam/base/serializationTestHelpers.h>
29 #include <CppUnitLite/TestHarness.h>
30 
31 using namespace std;
32 using namespace gtsam;
33 using namespace gtsam::serializationTestHelpers;
34 
35 
36 /* ************************************************************************* */
37 // Create GUIDs for Noisemodels
38 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal")
39 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Base , "gtsam_noiseModel_mEstimator_Base")
40 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Null , "gtsam_noiseModel_mEstimator_Null")
41 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Fair , "gtsam_noiseModel_mEstimator_Fair")
42 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Huber, "gtsam_noiseModel_mEstimator_Huber")
43 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Tukey, "gtsam_noiseModel_mEstimator_Tukey")
44 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained")
45 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit")
46 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic,"gtsam_noiseModel_Isotropic")
47 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Robust, "gtsam_noiseModel_Robust")
48 BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel")
49 BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal")
50 
51 /* ************************************************************************* */
52 // Create GUIDs for factors
53 BOOST_CLASS_EXPORT_GUID(gtsam::PriorFactor<gtsam::Pose3>, "gtsam::PriorFactor<gtsam::Pose3>")
54 BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor")
55 BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor")
56 BOOST_CLASS_EXPORT_GUID(gtsam::GaussianConditional , "gtsam::GaussianConditional")
57 
58 
59 /* ************************************************************************* */
60 // Export all classes derived from Value
61 GTSAM_VALUE_EXPORT(gtsam::Cal3_S2)
62 GTSAM_VALUE_EXPORT(gtsam::Cal3Bundler)
63 GTSAM_VALUE_EXPORT(gtsam::Point3)
64 GTSAM_VALUE_EXPORT(gtsam::Pose3)
65 GTSAM_VALUE_EXPORT(gtsam::Rot3)
66 GTSAM_VALUE_EXPORT(gtsam::PinholeCamera<Cal3_S2>)
67 GTSAM_VALUE_EXPORT(gtsam::PinholeCamera<Cal3DS2>)
68 GTSAM_VALUE_EXPORT(gtsam::PinholeCamera<Cal3Bundler>)
69 
70 namespace detail {
71 template<class T> struct pack {
72  typedef T type;
73 };
74 }
75 
76 /* ************************************************************************* */
77 typedef PinholeCamera<Cal3_S2> PinholeCal3S2;
78 typedef PinholeCamera<Cal3DS2> PinholeCal3DS2;
79 typedef PinholeCamera<Cal3Bundler> PinholeCal3Bundler;
80 
81 /* ************************************************************************* */
82 static Point3 pt3(1.0, 2.0, 3.0);
83 static Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0);
84 static Pose3 pose3(rt3, pt3);
85 
86 static Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5);
87 static Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
88 static Cal3Bundler cal3(1.0, 2.0, 3.0);
89 
90 TEST (Serialization, TemplatedValues) {
91  EXPECT(equalsObj(pt3));
92  GenericValue<Point3> chv1(pt3);
93  EXPECT(equalsObj(chv1));
94  PinholeCal3S2 pc(pose3,cal1);
95  EXPECT(equalsObj(pc));
96 
97  Values values;
98  values.insert(1,pt3);
99 
100  EXPECT(equalsObj(values));
101  values.insert(Symbol('a',0), PinholeCal3S2(pose3, cal1));
102  values.insert(Symbol('s',5), PinholeCal3DS2(pose3, cal2));
103  values.insert(Symbol('d',47), PinholeCal3Bundler(pose3, cal3));
104  values.insert(Symbol('a',5), PinholeCal3S2(pose3, cal1));
105  EXPECT(equalsObj(values));
106  EXPECT(equalsXML(values));
107  EXPECT(equalsBinary(values));
108 }
109 
120 TEST(Serialization, NoiseModelFactor1_backwards_compatibility) {
121  PriorFactor<Pose3> factor(
122  12345, Pose3(Rot3::RzRyRx(Vector3(1., 2., 3.)), Point3(4., 5., 6.)),
123  noiseModel::Unit::Create(6));
124 
125  // roundtrip (sanity check)
126  PriorFactor<Pose3> factor_deserialized_str_2 = PriorFactor<Pose3>();
127  roundtrip(factor, factor_deserialized_str_2);
128  EXPECT(assert_equal(factor, factor_deserialized_str_2));
129 
130 #if !defined(GTSAM_USE_QUATERNIONS) && !defined(GTSAM_USE_TBB)
131  // Deserialize string
132  std::string serialized_str =
133  "22 serialization::archive 15 1 0\n"
134  "0 0 0 0 0 0 0 1 0 12345 0 1 6 21 gtsam_noiseModel_Unit 1 0\n"
135  "1 1 0\n"
136  "2 1 0\n"
137  "3 0 0 0 0 6 0 1 0 0 0 6 1.00000000000000000e+00 1.00000000000000000e+00 "
138  "1.00000000000000000e+00 1.00000000000000000e+00 1.00000000000000000e+00 "
139  "1.00000000000000000e+00 6 1.00000000000000000e+00 "
140  "1.00000000000000000e+00 1.00000000000000000e+00 1.00000000000000000e+00 "
141  "1.00000000000000000e+00 1.00000000000000000e+00 1.00000000000000000e+00 "
142  "1.00000000000000000e+00 0 0 0 0 4.11982245665682978e-01 "
143  "-8.33737651774156818e-01 -3.67630462924899259e-01 "
144  "-5.87266449276209815e-02 -4.26917621276207360e-01 "
145  "9.02381585483330806e-01 -9.09297426825681709e-01 "
146  "-3.50175488374014632e-01 -2.24845095366152908e-01 0 0 "
147  "4.00000000000000000e+00 5.00000000000000000e+00 "
148  "6.00000000000000000e+00\n";
149 
150  PriorFactor<Pose3> factor_deserialized_str = PriorFactor<Pose3>();
151  deserializeFromString(serialized_str, factor_deserialized_str);
152  EXPECT(assert_equal(factor, factor_deserialized_str));
153 
154  // Deserialize XML
155  PriorFactor<Pose3> factor_deserialized_xml = PriorFactor<Pose3>();
156  deserializeFromXMLFile(GTSAM_SOURCE_TREE_DATASET_DIR
157  "/../../gtsam/nonlinear/tests/priorFactor.xml",
158  factor_deserialized_xml);
159  EXPECT(assert_equal(factor, factor_deserialized_xml));
160 #endif
161 }
162 
163 TEST(Serialization, ISAM2) {
164  gtsam::ISAM2Params parameters;
165  gtsam::ISAM2 solver(parameters);
166  gtsam::NonlinearFactorGraph graph;
167  gtsam::Values initialValues;
168  initialValues.clear();
169 
170  gtsam::Vector6 temp6;
171  for (int i = 0; i < 6; ++i) {
172  temp6[i] = 0.0001;
173  }
174  gtsam::noiseModel::Diagonal::shared_ptr noiseModel = gtsam::noiseModel::Diagonal::Sigmas(temp6);
175 
176  gtsam::Pose3 pose0(gtsam::Rot3(), gtsam::Point3(0, 0, 0));
177  gtsam::Symbol symbol0('x', 0);
178  graph.add(gtsam::PriorFactor<gtsam::Pose3>(symbol0, pose0, noiseModel));
179  initialValues.insert(symbol0, pose0);
180 
181  solver.update(graph, initialValues,
182  gtsam::FastVector<gtsam::FactorIndex>());
183 
184  std::string binaryPath = "saved_solver.dat";
185  try {
186  std::ofstream outputStream(binaryPath, std::ios::out | std::ios::binary);
187  boost::archive::binary_oarchive outputArchive(outputStream);
188  outputArchive << solver;
189  } catch(...) {
190  EXPECT(false);
191  }
192 
193  gtsam::ISAM2 solverFromDisk;
194  try {
195  std::ifstream ifs(binaryPath, std::ios::in | std::ios::binary);
196  boost::archive::binary_iarchive inputArchive(ifs);
197  inputArchive >> solverFromDisk;
198  } catch(...) {
199  EXPECT(false);
200  }
201 
202  gtsam::Pose3 p1, p2;
203  try {
204  p1 = solver.calculateEstimate<gtsam::Pose3>(symbol0);
205  } catch(std::exception &e) {
206  EXPECT(false);
207  }
208 
209  try {
210  p2 = solverFromDisk.calculateEstimate<gtsam::Pose3>(symbol0);
211  } catch(std::exception &e) {
212  EXPECT(false);
213  }
214  EXPECT(assert_equal(p1, p2));
215 }
216 
217 /* ************************************************************************* */
218 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
219 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::ISAM2Params
Definition: ISAM2Params.h:136
Pose2.h
2D Pose
gtsam::ISAM2
Definition: ISAM2.h:45
gtsam::HessianFactor
A Gaussian factor using the canonical parameters (information form)
Definition: HessianFactor.h:100
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
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
BOOST_CLASS_EXPORT_GUID
BOOST_CLASS_EXPORT_GUID(DecisionTreeFactor, "gtsam_DecisionTreeFactor")
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
Definition: FastVector.h:34
PinholeCal3S2
PinholeCamera< Cal3_S2 > PinholeCal3S2
Definition: testSerializationNonlinear.cpp:77
gtsam::noiseModel::Diagonal
Definition: NoiseModel.h:288
gtsam::JacobianFactor
Definition: JacobianFactor.h:91
rt3
static Rot3 rt3
Definition: testSerializationNonlinear.cpp:83
simple::pose0
static Pose3 pose0
Definition: testInitializePose3.cpp:56
gtsam::PriorFactor
Definition: nonlinear/PriorFactor.h:30
detail
Definition: testSerializationNonlinear.cpp:70
gtsam::noiseModel::Diagonal::Sigmas
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
Definition: NoiseModel.cpp:270
pt3
static Point3 pt3(1.0, 2.0, 3.0)
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
pose3
static Pose3 pose3(rt3, pt3)
GTSAM_VALUE_EXPORT
#define GTSAM_VALUE_EXPORT(Type)
use this macro instead of BOOST_CLASS_EXPORT for GenericValues
Definition: GenericValue.h:195
solver
BiCGSTAB< SparseMatrix< double > > solver
Definition: BiCGSTAB_simple.cpp:5
gtsam::GenericValue
Definition: GenericValue.h:44
simple::p2
static Point3 p2
Definition: testInitializePose3.cpp:51
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::Pose3
Definition: Pose3.h:37
parameters
static ConjugateGradientParameters parameters
Definition: testIterative.cpp:33
gtsam::PinholeCamera
Definition: PinholeCamera.h:33
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::GaussianConditional
Definition: GaussianConditional.h:40
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:743
PinholeCal3Bundler
PinholeCamera< Cal3Bundler > PinholeCal3Bundler
Definition: testSerializationNonlinear.cpp:79
PinholeCamera.h
Base class for all pinhole cameras.
gtsam::Cal3Bundler
Calibration used by Bundler.
Definition: Cal3Bundler.h:32
Symbol.h
main
int main()
Definition: testSerializationNonlinear.cpp:218
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:741
Eigen::Triplet< double >
cal1
static Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5)
pc
int RealScalar int RealScalar int RealScalar * pc
Definition: level1_cplx_impl.h:119
out
std::ofstream out("Result.txt")
p1
Vector3f p1
Definition: MatrixBase_all.cpp:2
serializationTestHelpers.h
TestResult
Definition: TestResult.h:26
gtsam::ISAM2::calculateEstimate
Values calculateEstimate() const
Definition: ISAM2.cpp:782
ISAM2.h
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
gtsam
traits
Definition: chartTesting.h:28
leaf::values
leaf::MyValues values
gtsam::Values
Definition: Values.h:65
Cal3DS2.h
Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base.
std
Definition: BFloat16.h:88
gtsam::noiseModel::Diagonal::shared_ptr
std::shared_ptr< Diagonal > shared_ptr
Definition: NoiseModel.h:307
gtsam::Values::clear
void clear()
Definition: Values.h:347
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
detail::pack
Definition: testSerializationNonlinear.cpp:71
gtsam::FactorGraph::add
IsDerived< DERIVEDFACTOR > add(std::shared_ptr< DERIVEDFACTOR > factor)
add is a synonym for push_back.
Definition: FactorGraph.h:171
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
TEST
TEST(Serialization, TemplatedValues)
Definition: testSerializationNonlinear.cpp:90
cal2
static Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0)
Cal3Bundler.h
Calibration used by Bundler.
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
Values.h
A non-templated config holding any types of Manifold-group elements.
PinholeCal3DS2
PinholeCamera< Cal3DS2 > PinholeCal3DS2
Definition: testSerializationNonlinear.cpp:78
gtsam::Symbol
Definition: inference/Symbol.h:37
detail::pack::type
T type
Definition: testSerializationNonlinear.cpp:72
cal3
static Cal3Bundler cal3(1.0, 2.0, 3.0)

gtsam
Author(s):
autogenerated on Thu Jun 13 2024 03:10:19