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 <gtsam/base/serializationTestHelpers.h>
26 #include <CppUnitLite/TestHarness.h>
27 
28 using namespace std;
29 using namespace gtsam;
30 using namespace gtsam::serializationTestHelpers;
31 
32 /* ************************************************************************* */
33 // Noise model components
34 /* ************************************************************************* */
35 
36 /* ************************************************************************* */
37 // Export Noisemodels
38 // See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html
39 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained")
40 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal")
41 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian")
42 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit")
43 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic")
44 
45 BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel")
46 BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal")
47 
48 /* ************************************************************************* */
49 // example noise models
50 static noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.2, 0.3));
51 static noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * I_3x3);
52 static noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2);
53 static noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas(Vector3(0.0, 0.0, 0.1));
54 static noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3);
55 
56 /* ************************************************************************* */
57 TEST (Serialization, noiseModels) {
58  // tests using pointers to the derived class
59  EXPECT(equalsDereferenced<noiseModel::Diagonal::shared_ptr>(diag3));
60  EXPECT(equalsDereferencedXML<noiseModel::Diagonal::shared_ptr>(diag3));
61  EXPECT(equalsDereferencedBinary<noiseModel::Diagonal::shared_ptr>(diag3));
62 
63  EXPECT(equalsDereferenced<noiseModel::Gaussian::shared_ptr>(gaussian3));
64  EXPECT(equalsDereferencedXML<noiseModel::Gaussian::shared_ptr>(gaussian3));
65  EXPECT(equalsDereferencedBinary<noiseModel::Gaussian::shared_ptr>(gaussian3));
66 
67  EXPECT(equalsDereferenced<noiseModel::Isotropic::shared_ptr>(iso3));
68  EXPECT(equalsDereferencedXML<noiseModel::Isotropic::shared_ptr>(iso3));
69  EXPECT(equalsDereferencedBinary<noiseModel::Isotropic::shared_ptr>(iso3));
70 
71  EXPECT(equalsDereferenced<noiseModel::Unit::shared_ptr>(unit3));
72  EXPECT(equalsDereferencedXML<noiseModel::Unit::shared_ptr>(unit3));
73  EXPECT(equalsDereferencedBinary<noiseModel::Unit::shared_ptr>(unit3));
74 
75  EXPECT(equalsDereferencedBinary<noiseModel::Constrained::shared_ptr>(constrained3));
76  EXPECT(equalsDereferenced<noiseModel::Constrained::shared_ptr>(constrained3));
77  EXPECT(equalsDereferencedXML<noiseModel::Constrained::shared_ptr>(constrained3));
78 }
79 
80 /* ************************************************************************* */
81 TEST (Serialization, SharedNoiseModel_noiseModels) {
82  SharedNoiseModel diag3_sg = diag3;
83  EXPECT(equalsDereferenced<SharedNoiseModel>(diag3_sg));
84  EXPECT(equalsDereferencedXML<SharedNoiseModel>(diag3_sg));
85  EXPECT(equalsDereferencedBinary<SharedNoiseModel>(diag3_sg));
86 
87  EXPECT(equalsDereferenced<SharedNoiseModel>(diag3));
88  EXPECT(equalsDereferencedXML<SharedNoiseModel>(diag3));
89  EXPECT(equalsDereferencedBinary<SharedNoiseModel>(diag3));
90 
91  EXPECT(equalsDereferenced<SharedNoiseModel>(iso3));
92  EXPECT(equalsDereferencedXML<SharedNoiseModel>(iso3));
93  EXPECT(equalsDereferencedBinary<SharedNoiseModel>(iso3));
94 
95  EXPECT(equalsDereferenced<SharedNoiseModel>(gaussian3));
96  EXPECT(equalsDereferencedXML<SharedNoiseModel>(gaussian3));
97  EXPECT(equalsDereferencedBinary<SharedNoiseModel>(gaussian3));
98 
99  EXPECT(equalsDereferenced<SharedNoiseModel>(unit3));
100  EXPECT(equalsDereferencedXML<SharedNoiseModel>(unit3));
101  EXPECT(equalsDereferencedBinary<SharedNoiseModel>(unit3));
102 
103  EXPECT(equalsDereferencedBinary<SharedNoiseModel>(constrained3));
104  EXPECT(equalsDereferenced<SharedNoiseModel>(constrained3));
105  EXPECT(equalsDereferencedXML<SharedNoiseModel>(constrained3));
106 }
107 
108 /* ************************************************************************* */
109 TEST (Serialization, SharedDiagonal_noiseModels) {
110  EXPECT(equalsDereferenced<SharedDiagonal>(diag3));
111  EXPECT(equalsDereferencedXML<SharedDiagonal>(diag3));
112  EXPECT(equalsDereferencedBinary<SharedDiagonal>(diag3));
113 
114  EXPECT(equalsDereferenced<SharedDiagonal>(iso3));
115  EXPECT(equalsDereferencedXML<SharedDiagonal>(iso3));
116  EXPECT(equalsDereferencedBinary<SharedDiagonal>(iso3));
117 
118  EXPECT(equalsDereferenced<SharedDiagonal>(unit3));
119  EXPECT(equalsDereferencedXML<SharedDiagonal>(unit3));
120  EXPECT(equalsDereferencedBinary<SharedDiagonal>(unit3));
121 
122  EXPECT(equalsDereferencedBinary<SharedDiagonal>(constrained3));
123  EXPECT(equalsDereferenced<SharedDiagonal>(constrained3));
124  EXPECT(equalsDereferencedXML<SharedDiagonal>(constrained3));
125 }
126 
127 /* Create GUIDs for factors */
128 /* ************************************************************************* */
129 BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor")
130 BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor")
131 BOOST_CLASS_EXPORT_GUID(gtsam::GaussianConditional , "gtsam::GaussianConditional")
132 
133 /* ************************************************************************* */
134 TEST (Serialization, linear_factors) {
135  VectorValues values;
136  values.insert(0, (Vector(1) << 1.0).finished());
137  values.insert(1, Vector2(2.0,3.0));
138  values.insert(2, Vector2(4.0,5.0));
139  EXPECT(equalsObj<VectorValues>(values));
140  EXPECT(equalsXML<VectorValues>(values));
141  EXPECT(equalsBinary<VectorValues>(values));
142 
143  Key i1 = 4, i2 = 7;
144  Matrix A1 = I_3x3, A2 = -1.0 * I_3x3;
145  Vector b = Vector::Ones(3);
146  SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector3(1.0, 2.0, 3.0));
147  JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model);
148  EXPECT(equalsObj(jacobianfactor));
149  EXPECT(equalsXML(jacobianfactor));
150  EXPECT(equalsBinary(jacobianfactor));
151 
152  HessianFactor hessianfactor(jacobianfactor);
153  EXPECT(equalsObj(hessianfactor));
154  EXPECT(equalsXML(hessianfactor));
155  EXPECT(equalsBinary(hessianfactor));
156 }
157 
158 /* ************************************************************************* */
159 TEST (Serialization, gaussian_conditional) {
160  Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();
161  Matrix A2 = (Matrix(2, 2) << 6., 0.2, 8., 0.4).finished();
162  Matrix R = (Matrix(2, 2) << 0.1, 0.3, 0.0, 0.34).finished();
163  Vector d(2); d << 0.2, 0.5;
164  GaussianConditional cg(0, d, R, 1, A1, 2, A2);
165 
166  EXPECT(equalsObj(cg));
167  EXPECT(equalsXML(cg));
168  EXPECT(equalsBinary(cg));
169 }
170 
171 /* ************************************************************************* */
172 TEST (Serialization, gaussian_factor_graph) {
173  GaussianFactorGraph graph;
174  {
175  Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();
176  Matrix A2 = (Matrix(2, 2) << 6., 0.2, 8., 0.4).finished();
177  Matrix R = (Matrix(2, 2) << 0.1, 0.3, 0.0, 0.34).finished();
178  Vector d(2); d << 0.2, 0.5;
179  GaussianConditional cg(0, d, R, 1, A1, 2, A2);
180  graph.push_back(cg);
181  }
182 
183  {
184  Key i1 = 4, i2 = 7;
185  Matrix A1 = I_3x3, A2 = -1.0 * I_3x3;
186  Vector b = Vector::Ones(3);
187  SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector3(1.0, 2.0, 3.0));
188  JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model);
189  HessianFactor hessianfactor(jacobianfactor);
190  graph.push_back(jacobianfactor);
191  graph.push_back(hessianfactor);
192  }
193  EXPECT(equalsObj(graph));
194  EXPECT(equalsXML(graph));
195  EXPECT(equalsBinary(graph));
196 }
197 
198 /* ****************************************************************************/
199 TEST(Serialization, gaussian_bayes_net) {
200  // Create an arbitrary Bayes Net
201  GaussianBayesNet gbn;
202  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
203  0, Vector2(1.0, 2.0), (Matrix2() << 3.0, 4.0, 0.0, 6.0).finished(), 3,
204  (Matrix2() << 7.0, 8.0, 9.0, 10.0).finished(), 4,
205  (Matrix2() << 11.0, 12.0, 13.0, 14.0).finished()));
206  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
207  1, Vector2(15.0, 16.0), (Matrix2() << 17.0, 18.0, 0.0, 20.0).finished(),
208  2, (Matrix2() << 21.0, 22.0, 23.0, 24.0).finished(), 4,
209  (Matrix2() << 25.0, 26.0, 27.0, 28.0).finished()));
210  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
211  2, Vector2(29.0, 30.0), (Matrix2() << 31.0, 32.0, 0.0, 34.0).finished(),
212  3, (Matrix2() << 35.0, 36.0, 37.0, 38.0).finished()));
213  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
214  3, Vector2(39.0, 40.0), (Matrix2() << 41.0, 42.0, 0.0, 44.0).finished(),
215  4, (Matrix2() << 45.0, 46.0, 47.0, 48.0).finished()));
216  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
217  4, Vector2(49.0, 50.0), (Matrix2() << 51.0, 52.0, 0.0, 54.0).finished()));
218 
219  std::string serialized = serialize(gbn);
220  GaussianBayesNet actual;
221  deserialize(serialized, actual);
222  EXPECT(assert_equal(gbn, actual));
223 }
224 
225 /* ************************************************************************* */
226 TEST (Serialization, gaussian_bayes_tree) {
227  const Key x1=1, x2=2, x3=3, x4=4;
228  const Ordering chainOrdering {x2, x1, x3, x4};
229  const SharedDiagonal chainNoise = noiseModel::Isotropic::Sigma(1, 0.5);
230  const GaussianFactorGraph chain = {
231  std::make_shared<JacobianFactor>(x2, (Matrix(1, 1) << 1.).finished(), x1, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise),
232  std::make_shared<JacobianFactor>(x2, (Matrix(1, 1) << 1.).finished(), x3, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise),
233  std::make_shared<JacobianFactor>(x3, (Matrix(1, 1) << 1.).finished(), x4, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise),
234  std::make_shared<JacobianFactor>(x4, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise)};
235 
236  GaussianBayesTree init = *chain.eliminateMultifrontal(chainOrdering);
237  GaussianBayesTree expected = *chain.eliminateMultifrontal(chainOrdering);
238  GaussianBayesTree actual;
239 
240  std::string serialized = serialize(init);
241  deserialize(serialized, actual);
242  EXPECT(assert_equal(expected, actual));
243 }
244 
245 /* ************************************************************************* */
246 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
247 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
Gaussian
double Gaussian(double mu, double sigma, double z)
Gaussian density function.
Definition: testGaussianMixture.cpp:47
gtsam::HessianFactor
A Gaussian factor using the canonical parameters (information form)
Definition: HessianFactor.h:99
d
static const double d[K][N]
Definition: igam.h:11
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:43
TestHarness.h
x4
static string x4("x4")
BOOST_CLASS_EXPORT_GUID
BOOST_CLASS_EXPORT_GUID(DecisionTreeFactor, "gtsam_DecisionTreeFactor")
b
Scalar * b
Definition: benchVecAdd.cpp:17
gtsam::JacobianFactor
Definition: JacobianFactor.h:91
HessianFactor.h
Contains the HessianFactor class, a general quadratic factor.
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:44
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
gaussian3
static noiseModel::Gaussian::shared_ptr gaussian3
Definition: testSerializationLinear.cpp:51
gtsam::noiseModel::Constrained
Definition: NoiseModel.h:404
x3
Pose3 x3(Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2)
gtsam::EliminateableFactorGraph::eliminateMultifrontal
std::shared_ptr< BayesTreeType > eliminateMultifrontal(OptionalOrderingType orderingType={}, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
Definition: EliminateableFactorGraph-inst.h:89
diag3
static noiseModel::Diagonal::shared_ptr diag3
Definition: testSerializationLinear.cpp:50
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
ceres::Matrix
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
Definition: gtsam/3rdparty/ceres/eigen.h:42
gtsam::VectorValues
Definition: VectorValues.h:74
x1
Pose3 x1
Definition: testPose3.cpp:663
gtsam::GaussianConditional
Definition: GaussianConditional.h:40
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:764
A2
static const double A2[]
Definition: expn.h:7
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
main
int main()
Definition: testSerializationLinear.cpp:246
unit3
static noiseModel::Unit::shared_ptr unit3
Definition: testSerializationLinear.cpp:54
gtsam::HybridValues::insert
void insert(Key j, const Vector &value)
Definition: HybridValues.cpp:85
VectorValues.h
Factor Graph Values.
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
sampling::gbn
static const GaussianBayesNet gbn
Definition: testGaussianBayesNet.cpp:171
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
serializationTestHelpers.h
TestResult
Definition: TestResult.h:26
JacobianFactor.h
gtsam
traits
Definition: SFMdata.h:40
TEST
TEST(Serialization, noiseModels)
Definition: testSerializationLinear.cpp:57
gtsam::GaussianBayesTree
Definition: GaussianBayesTree.h:49
NoiseModel.h
i1
double i1(double x)
Definition: i1.c:150
iso3
static noiseModel::Isotropic::shared_ptr iso3
Definition: testSerializationLinear.cpp:52
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:147
std
Definition: BFloat16.h:88
gtsam::Diagonal
static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix &covariance)
Definition: ScenarioRunner.h:27
A1
static const double A1[]
Definition: expn.h:6
gtsam::GaussianConditional::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianConditional.h:46
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:41
constrained3
static noiseModel::Constrained::shared_ptr constrained3
Definition: testSerializationLinear.cpp:53
GaussianISAM.h
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
init
Definition: TutorialInplaceLU.cpp:2
gtsam::Ordering
Definition: inference/Ordering.h:33
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
x2
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
R
Rot2 R(Rot2::fromAngle(0.1))
gtsam::GaussianBayesNet
Definition: GaussianBayesNet.h:35

gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:17:15