testSerializationLinear.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/linear/VectorValues.h>
20 #include <gtsam/linear/JacobianFactor.h>
21 #include <gtsam/linear/HessianFactor.h>
22 #include <gtsam/linear/GaussianISAM.h>
23 #include <gtsam/linear/NoiseModel.h>
24 
25 #include <boost/assign/list_of.hpp>
26 using namespace boost::assign;
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 // Noise model components
37 /* ************************************************************************* */
38 
39 /* ************************************************************************* */
40 // Export Noisemodels
41 // See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html
42 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
43 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
44 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian");
45 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
46 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");
47 
48 BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
49 BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
50 
51 /* ************************************************************************* */
52 // example noise models
53 static noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.2, 0.3));
54 static noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * I_3x3);
55 static noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2);
56 static noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas(Vector3(0.0, 0.0, 0.1));
57 static noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3);
58 
59 /* ************************************************************************* */
60 TEST (Serialization, noiseModels) {
61  // tests using pointers to the derived class
62  EXPECT(equalsDereferenced<noiseModel::Diagonal::shared_ptr>(diag3));
63  EXPECT(equalsDereferencedXML<noiseModel::Diagonal::shared_ptr>(diag3));
64  EXPECT(equalsDereferencedBinary<noiseModel::Diagonal::shared_ptr>(diag3));
65 
66  EXPECT(equalsDereferenced<noiseModel::Gaussian::shared_ptr>(gaussian3));
67  EXPECT(equalsDereferencedXML<noiseModel::Gaussian::shared_ptr>(gaussian3));
68  EXPECT(equalsDereferencedBinary<noiseModel::Gaussian::shared_ptr>(gaussian3));
69 
70  EXPECT(equalsDereferenced<noiseModel::Isotropic::shared_ptr>(iso3));
71  EXPECT(equalsDereferencedXML<noiseModel::Isotropic::shared_ptr>(iso3));
72  EXPECT(equalsDereferencedBinary<noiseModel::Isotropic::shared_ptr>(iso3));
73 
74  EXPECT(equalsDereferenced<noiseModel::Unit::shared_ptr>(unit3));
75  EXPECT(equalsDereferencedXML<noiseModel::Unit::shared_ptr>(unit3));
76  EXPECT(equalsDereferencedBinary<noiseModel::Unit::shared_ptr>(unit3));
77 
78  EXPECT(equalsDereferencedBinary<noiseModel::Constrained::shared_ptr>(constrained3));
79  EXPECT(equalsDereferenced<noiseModel::Constrained::shared_ptr>(constrained3));
80  EXPECT(equalsDereferencedXML<noiseModel::Constrained::shared_ptr>(constrained3));
81 }
82 
83 /* ************************************************************************* */
84 TEST (Serialization, SharedNoiseModel_noiseModels) {
85  SharedNoiseModel diag3_sg = diag3;
86  EXPECT(equalsDereferenced<SharedNoiseModel>(diag3_sg));
87  EXPECT(equalsDereferencedXML<SharedNoiseModel>(diag3_sg));
88  EXPECT(equalsDereferencedBinary<SharedNoiseModel>(diag3_sg));
89 
90  EXPECT(equalsDereferenced<SharedNoiseModel>(diag3));
91  EXPECT(equalsDereferencedXML<SharedNoiseModel>(diag3));
92  EXPECT(equalsDereferencedBinary<SharedNoiseModel>(diag3));
93 
94  EXPECT(equalsDereferenced<SharedNoiseModel>(iso3));
95  EXPECT(equalsDereferencedXML<SharedNoiseModel>(iso3));
96  EXPECT(equalsDereferencedBinary<SharedNoiseModel>(iso3));
97 
98  EXPECT(equalsDereferenced<SharedNoiseModel>(gaussian3));
99  EXPECT(equalsDereferencedXML<SharedNoiseModel>(gaussian3));
100  EXPECT(equalsDereferencedBinary<SharedNoiseModel>(gaussian3));
101 
102  EXPECT(equalsDereferenced<SharedNoiseModel>(unit3));
103  EXPECT(equalsDereferencedXML<SharedNoiseModel>(unit3));
104  EXPECT(equalsDereferencedBinary<SharedNoiseModel>(unit3));
105 
106  EXPECT(equalsDereferencedBinary<SharedNoiseModel>(constrained3));
107  EXPECT(equalsDereferenced<SharedNoiseModel>(constrained3));
108  EXPECT(equalsDereferencedXML<SharedNoiseModel>(constrained3));
109 }
110 
111 /* ************************************************************************* */
112 TEST (Serialization, SharedDiagonal_noiseModels) {
113  EXPECT(equalsDereferenced<SharedDiagonal>(diag3));
114  EXPECT(equalsDereferencedXML<SharedDiagonal>(diag3));
115  EXPECT(equalsDereferencedBinary<SharedDiagonal>(diag3));
116 
117  EXPECT(equalsDereferenced<SharedDiagonal>(iso3));
118  EXPECT(equalsDereferencedXML<SharedDiagonal>(iso3));
119  EXPECT(equalsDereferencedBinary<SharedDiagonal>(iso3));
120 
121  EXPECT(equalsDereferenced<SharedDiagonal>(unit3));
122  EXPECT(equalsDereferencedXML<SharedDiagonal>(unit3));
123  EXPECT(equalsDereferencedBinary<SharedDiagonal>(unit3));
124 
125  EXPECT(equalsDereferencedBinary<SharedDiagonal>(constrained3));
126  EXPECT(equalsDereferenced<SharedDiagonal>(constrained3));
127  EXPECT(equalsDereferencedXML<SharedDiagonal>(constrained3));
128 }
129 
130 /* Create GUIDs for factors */
131 /* ************************************************************************* */
132 BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor");
133 BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor");
134 BOOST_CLASS_EXPORT_GUID(gtsam::GaussianConditional , "gtsam::GaussianConditional");
135 
136 /* ************************************************************************* */
137 TEST (Serialization, linear_factors) {
138  VectorValues values;
139  values.insert(0, (Vector(1) << 1.0).finished());
140  values.insert(1, Vector2(2.0,3.0));
141  values.insert(2, Vector2(4.0,5.0));
142  EXPECT(equalsObj<VectorValues>(values));
143  EXPECT(equalsXML<VectorValues>(values));
144  EXPECT(equalsBinary<VectorValues>(values));
145 
146  Key i1 = 4, i2 = 7;
147  Matrix A1 = I_3x3, A2 = -1.0 * I_3x3;
148  Vector b = Vector::Ones(3);
149  SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector3(1.0, 2.0, 3.0));
150  JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model);
151  EXPECT(equalsObj(jacobianfactor));
152  EXPECT(equalsXML(jacobianfactor));
153  EXPECT(equalsBinary(jacobianfactor));
154 
155  HessianFactor hessianfactor(jacobianfactor);
156  EXPECT(equalsObj(hessianfactor));
157  EXPECT(equalsXML(hessianfactor));
158  EXPECT(equalsBinary(hessianfactor));
159 }
160 
161 /* ************************************************************************* */
162 TEST (Serialization, gaussian_conditional) {
163  Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();
164  Matrix A2 = (Matrix(2, 2) << 6., 0.2, 8., 0.4).finished();
165  Matrix R = (Matrix(2, 2) << 0.1, 0.3, 0.0, 0.34).finished();
166  Vector d(2); d << 0.2, 0.5;
167  GaussianConditional cg(0, d, R, 1, A1, 2, A2);
168 
169  EXPECT(equalsObj(cg));
170  EXPECT(equalsXML(cg));
171  EXPECT(equalsBinary(cg));
172 }
173 
174 /* ************************************************************************* */
175 TEST (Serialization, gaussian_factor_graph) {
176  GaussianFactorGraph graph;
177  {
178  Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();
179  Matrix A2 = (Matrix(2, 2) << 6., 0.2, 8., 0.4).finished();
180  Matrix R = (Matrix(2, 2) << 0.1, 0.3, 0.0, 0.34).finished();
181  Vector d(2); d << 0.2, 0.5;
182  GaussianConditional cg(0, d, R, 1, A1, 2, A2);
183  graph.push_back(cg);
184  }
185 
186  {
187  Key i1 = 4, i2 = 7;
188  Matrix A1 = I_3x3, A2 = -1.0 * I_3x3;
189  Vector b = Vector::Ones(3);
190  SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector3(1.0, 2.0, 3.0));
191  JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model);
192  HessianFactor hessianfactor(jacobianfactor);
193  graph.push_back(jacobianfactor);
194  graph.push_back(hessianfactor);
195  }
196  EXPECT(equalsObj(graph));
197  EXPECT(equalsXML(graph));
198  EXPECT(equalsBinary(graph));
199 }
200 
201 /* ************************************************************************* */
202 TEST (Serialization, gaussian_bayes_tree) {
203  const Key x1=1, x2=2, x3=3, x4=4;
204  const Ordering chainOrdering = Ordering(list_of(x2)(x1)(x3)(x4));
205  const SharedDiagonal chainNoise = noiseModel::Isotropic::Sigma(1, 0.5);
206  const GaussianFactorGraph chain = list_of
207  (JacobianFactor(x2, (Matrix(1, 1) << 1.).finished(), x1, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise))
208  (JacobianFactor(x2, (Matrix(1, 1) << 1.).finished(), x3, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise))
209  (JacobianFactor(x3, (Matrix(1, 1) << 1.).finished(), x4, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise))
210  (JacobianFactor(x4, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise));
211 
212  GaussianBayesTree init = *chain.eliminateMultifrontal(chainOrdering);
213  GaussianBayesTree expected = *chain.eliminateMultifrontal(chainOrdering);
214  GaussianBayesTree actual;
215 
216  std::string serialized = serialize(init);
217  deserialize(serialized, actual);
218  EXPECT(assert_equal(expected, actual));
219 }
220 
221 /* ************************************************************************* */
222 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
223 /* ************************************************************************* */
test_docs.d
d
Definition: test_docs.py:28
gtsam::noiseModel::Unit::shared_ptr
boost::shared_ptr< Unit > shared_ptr
Definition: NoiseModel.h:601
unit3
static noiseModel::Unit::shared_ptr unit3
Definition: testSerializationLinear.cpp:57
diag3
static noiseModel::Diagonal::shared_ptr diag3
Definition: testSerializationLinear.cpp:53
boost::serialization::serialize
void serialize(Archive &ar, Eigen::Matrix< Scalar_, Rows_, Cols_, Ops_, MaxRows_, MaxCols_ > &m, const unsigned int version)
Definition: base/Matrix.h:591
b
Scalar * b
Definition: benchVecAdd.cpp:17
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:974
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
iso3
static noiseModel::Isotropic::shared_ptr iso3
Definition: testSerializationLinear.cpp:55
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
R
Rot2 R(Rot2::fromAngle(0.1))
leaf::values
leaf::MyValues values
std
Definition: Half.h:150
x2
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:166
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
VectorValues.h
Factor Graph Values.
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
EXPECT
#define EXPECT(condition)
Definition: Test.h:151
HessianFactor.h
Contains the HessianFactor class, a general quadratic factor.
gtsam::noiseModel::Diagonal::shared_ptr
boost::shared_ptr< Diagonal > shared_ptr
Definition: NoiseModel.h:301
gtsam::noiseModel::Constrained::shared_ptr
boost::shared_ptr< Constrained > shared_ptr
Definition: NoiseModel.h:407
x3
Pose3 x3(Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2)
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:736
gtsam
traits
Definition: chartTesting.h:28
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:42
gtsam::deserialize
void deserialize(const std::string &serialized, T &output)
deserializes from a string
Definition: base/serialization.h:106
BOOST_CLASS_EXPORT_GUID
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained,"gtsam_noiseModel_Constrained")
x1
Pose3 x1
Definition: testPose3.cpp:588
gtsam::HessianFactor
A Gaussian factor using the canonical parameters (information form)
Definition: HessianFactor.h:101
gaussian3
static noiseModel::Gaussian::shared_ptr gaussian3
Definition: testSerializationLinear.cpp:54
gtsam::noiseModel::Isotropic::shared_ptr
boost::shared_ptr< Isotropic > shared_ptr
Definition: NoiseModel.h:541
gtsam::noiseModel::Gaussian::shared_ptr
boost::shared_ptr< Gaussian > shared_ptr
Definition: NoiseModel.h:189
ceres::Matrix
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
Definition: gtsam/3rdparty/ceres/eigen.h:42
x4
static string x4("x4")
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
gtsam::VectorValues::insert
iterator insert(const std::pair< Key, Vector > &key_value)
Definition: VectorValues.cpp:90
TEST
TEST(Serialization, noiseModels)
Definition: testSerializationLinear.cpp:60
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
constrained3
static noiseModel::Constrained::shared_ptr constrained3
Definition: testSerializationLinear.cpp:56
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:734

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