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 // Create GUIDs for Noisemodels
37 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal")
38 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Base , "gtsam_noiseModel_mEstimator_Base")
39 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Null , "gtsam_noiseModel_mEstimator_Null")
40 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Fair , "gtsam_noiseModel_mEstimator_Fair")
41 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Huber, "gtsam_noiseModel_mEstimator_Huber")
42 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Tukey, "gtsam_noiseModel_mEstimator_Tukey")
43 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained")
44 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit")
45 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic,"gtsam_noiseModel_Isotropic")
46 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Robust, "gtsam_noiseModel_Robust")
47 BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel")
48 BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal")
49 
50 /* ************************************************************************* */
51 // Create GUIDs for factors
52 BOOST_CLASS_EXPORT_GUID(gtsam::PriorFactor<gtsam::Pose3>, "gtsam::PriorFactor<gtsam::Pose3>")
53 BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor")
54 BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor")
55 BOOST_CLASS_EXPORT_GUID(gtsam::GaussianConditional , "gtsam::GaussianConditional")
56 
57 
58 /* ************************************************************************* */
59 // Export all classes derived from Value
60 GTSAM_VALUE_EXPORT(gtsam::Cal3_S2)
61 GTSAM_VALUE_EXPORT(gtsam::Cal3Bundler)
62 GTSAM_VALUE_EXPORT(gtsam::Point3)
63 GTSAM_VALUE_EXPORT(gtsam::Pose3)
64 GTSAM_VALUE_EXPORT(gtsam::Rot3)
65 GTSAM_VALUE_EXPORT(gtsam::PinholeCamera<Cal3_S2>)
66 GTSAM_VALUE_EXPORT(gtsam::PinholeCamera<Cal3DS2>)
67 GTSAM_VALUE_EXPORT(gtsam::PinholeCamera<Cal3Bundler>)
68 
69 namespace detail {
70 template<class T> struct pack {
71  typedef T type;
72 };
73 }
74 
75 /* ************************************************************************* */
76 typedef PinholeCamera<Cal3_S2> PinholeCal3S2;
77 typedef PinholeCamera<Cal3DS2> PinholeCal3DS2;
78 typedef PinholeCamera<Cal3Bundler> PinholeCal3Bundler;
79 
80 /* ************************************************************************* */
81 static Point3 pt3(1.0, 2.0, 3.0);
82 static Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0);
83 static Pose3 pose3(rt3, pt3);
84 
85 static Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5);
86 static Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
87 static Cal3Bundler cal3(1.0, 2.0, 3.0);
88 
89 TEST (Serialization, TemplatedValues) {
90  EXPECT(equalsObj(pt3));
91  GenericValue<Point3> chv1(pt3);
92  EXPECT(equalsObj(chv1));
93  PinholeCal3S2 pc(pose3,cal1);
94  EXPECT(equalsObj(pc));
95 
96  Values values;
97  values.insert(1,pt3);
98 
99  EXPECT(equalsObj(values));
100  values.insert(Symbol('a',0), PinholeCal3S2(pose3, cal1));
101  values.insert(Symbol('s',5), PinholeCal3DS2(pose3, cal2));
102  values.insert(Symbol('d',47), PinholeCal3Bundler(pose3, cal3));
103  values.insert(Symbol('a',5), PinholeCal3S2(pose3, cal1));
104  EXPECT(equalsObj(values));
105  EXPECT(equalsXML(values));
106  EXPECT(equalsBinary(values));
107 }
108 
119 TEST(Serialization, NoiseModelFactor1_backwards_compatibility) {
120  PriorFactor<Pose3> factor(
121  12345, Pose3(Rot3::RzRyRx(Vector3(1., 2., 3.)), Point3(4., 5., 6.)),
122  noiseModel::Unit::Create(6));
123 
124  // roundtrip (sanity check)
125  PriorFactor<Pose3> factor_deserialized_str_2 = PriorFactor<Pose3>();
126  roundtrip(factor, factor_deserialized_str_2);
127  EXPECT(assert_equal(factor, factor_deserialized_str_2));
128 
129 #if !defined(GTSAM_USE_QUATERNIONS) && !defined(GTSAM_USE_TBB)
130  // Deserialize string
131  std::string serialized_str =
132  "22 serialization::archive 15 1 0\n"
133  "0 0 0 0 0 0 0 1 0 12345 0 1 6 21 gtsam_noiseModel_Unit 1 0\n"
134  "1 1 0\n"
135  "2 1 0\n"
136  "3 0 0 0 0 6 0 1 0 0 0 6 1.00000000000000000e+00 1.00000000000000000e+00 "
137  "1.00000000000000000e+00 1.00000000000000000e+00 1.00000000000000000e+00 "
138  "1.00000000000000000e+00 6 1.00000000000000000e+00 "
139  "1.00000000000000000e+00 1.00000000000000000e+00 1.00000000000000000e+00 "
140  "1.00000000000000000e+00 1.00000000000000000e+00 1.00000000000000000e+00 "
141  "1.00000000000000000e+00 0 0 0 0 4.11982245665682978e-01 "
142  "-8.33737651774156818e-01 -3.67630462924899259e-01 "
143  "-5.87266449276209815e-02 -4.26917621276207360e-01 "
144  "9.02381585483330806e-01 -9.09297426825681709e-01 "
145  "-3.50175488374014632e-01 -2.24845095366152908e-01 0 0 "
146  "4.00000000000000000e+00 5.00000000000000000e+00 "
147  "6.00000000000000000e+00\n";
148 
149  PriorFactor<Pose3> factor_deserialized_str = PriorFactor<Pose3>();
150  deserializeFromString(serialized_str, factor_deserialized_str);
151  EXPECT(assert_equal(factor, factor_deserialized_str));
152 
153  // Deserialize XML
154  PriorFactor<Pose3> factor_deserialized_xml = PriorFactor<Pose3>();
155  #if !defined(__QNX__)
156  deserializeFromXMLFile(GTSAM_SOURCE_TREE_DATASET_DIR
157  "/../../gtsam/nonlinear/tests/priorFactor.xml",
158  factor_deserialized_xml);
159  #else
160  /*QNX is cross-compiled for, and does not support make, cmake, or ctest natively. Additionally, space is a large constraint on embedded systems.
161  * Therefore, running tests must be done by manually copying test executables and required data files over, preferably avoiding
162  * copying the whole source tree. Thus, we set the default file lookup to the same folder. Please copy data files to the same folder as this test.
163  * for more info, check PR#1968 https://github.com/borglab/gtsam/pull/1968
164  */
165  bool c = deserializeFromXMLFile(
166  "priorFactor.xml",
167  factor_deserialized_xml);
168  #endif
169  EXPECT(assert_equal(factor, factor_deserialized_xml));
170 #endif
171 }
172 
173 TEST(Serialization, ISAM2) {
174  gtsam::ISAM2Params parameters;
175  gtsam::ISAM2 solver(parameters);
176  gtsam::NonlinearFactorGraph graph;
177  gtsam::Values initialValues;
178  initialValues.clear();
179 
180  gtsam::Vector6 temp6;
181  for (int i = 0; i < 6; ++i) {
182  temp6[i] = 0.0001;
183  }
184  gtsam::noiseModel::Diagonal::shared_ptr noiseModel = gtsam::noiseModel::Diagonal::Sigmas(temp6);
185 
186  gtsam::Pose3 pose0(gtsam::Rot3(), gtsam::Point3(0, 0, 0));
187  gtsam::Symbol symbol0('x', 0);
188  graph.add(gtsam::PriorFactor<gtsam::Pose3>(symbol0, pose0, noiseModel));
189  initialValues.insert(symbol0, pose0);
190 
191  solver.update(graph, initialValues,
192  gtsam::FastVector<gtsam::FactorIndex>());
193 
194  std::string binaryPath = "saved_solver.dat";
195  try {
196  std::ofstream outputStream(binaryPath, std::ios::out | std::ios::binary);
197  boost::archive::binary_oarchive outputArchive(outputStream);
198  outputArchive << solver;
199  } catch(...) {
200  EXPECT(false);
201  }
202 
203  gtsam::ISAM2 solverFromDisk;
204  try {
205  std::ifstream ifs(binaryPath, std::ios::in | std::ios::binary);
206  boost::archive::binary_iarchive inputArchive(ifs);
207  inputArchive >> solverFromDisk;
208  } catch(...) {
209  EXPECT(false);
210  }
211 
212  gtsam::Pose3 p1, p2;
213  try {
214  p1 = solver.calculateEstimate<gtsam::Pose3>(symbol0);
215  } catch(std::exception &e) {
216  EXPECT(false);
217  }
218 
219  try {
220  p2 = solverFromDisk.calculateEstimate<gtsam::Pose3>(symbol0);
221  } catch(std::exception &e) {
222  EXPECT(false);
223  }
224  EXPECT(assert_equal(p1, p2));
225 }
226 
227 /* ************************************************************************* */
228 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
229 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::ISAM2Params
Definition: ISAM2Params.h:136
pose3
Definition: testFrobeniusFactor.cpp:229
Pose2.h
2D Pose
gtsam::ISAM2
Definition: ISAM2.h:45
gtsam::HessianFactor
A Gaussian factor using the canonical parameters (information form)
Definition: HessianFactor.h:99
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
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
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:76
gtsam::noiseModel::Diagonal
Definition: NoiseModel.h:301
gtsam::JacobianFactor
Definition: JacobianFactor.h:91
rt3
static Rot3 rt3
Definition: testSerializationNonlinear.cpp:82
simple::pose0
static Pose3 pose0
Definition: testInitializePose3.cpp:56
gtsam::PriorFactor
Definition: nonlinear/PriorFactor.h:30
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:247
detail
Definition: testSerializationNonlinear.cpp:69
gtsam::noiseModel::Diagonal::Sigmas
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
Definition: NoiseModel.cpp:283
pt3
static Point3 pt3(1.0, 2.0, 3.0)
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:44
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
pruning_fixture::factor
DecisionTreeFactor factor(D &C &B &A, "0.0 0.0 0.0 0.60658897 0.61241912 0.61241969 0.61247685 0.61247742 0.0 " "0.0 0.0 0.99995287 1.0 1.0 1.0 1.0")
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:764
PinholeCal3Bundler
PinholeCamera< Cal3Bundler > PinholeCal3Bundler
Definition: testSerializationNonlinear.cpp:78
PinholeCamera.h
Base class for all pinhole cameras.
gtsam::Cal3Bundler
Calibration used by Bundler.
Definition: Cal3Bundler.h:32
Symbol.h
gtsam::HybridValues::insert
void insert(Key j, const Vector &value)
Definition: HybridValues.cpp:85
main
int main()
Definition: testSerializationNonlinear.cpp:228
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
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:786
ISAM2.h
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
gtsam
traits
Definition: SFMdata.h:40
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:321
gtsam::Values::clear
void clear()
Definition: Values.h:347
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:156
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:41
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:70
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:89
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:77
gtsam::Symbol
Definition: inference/Symbol.h:37
detail::pack::type
T type
Definition: testSerializationNonlinear.cpp:71
cal3
static Cal3Bundler cal3(1.0, 2.0, 3.0)

gtsam
Author(s):
autogenerated on Wed Mar 19 2025 03:07:47