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
12/**
13 * @file testMarginals.cpp
14 * @brief
15 * @author Richard Roberts
16 * @date May 14, 2012
17 */
18
19#include <CppUnitLite/TestHarness.h>
20
21// for all nonlinear keys
22#include <gtsam/inference/Symbol.h>
23
24// for points and poses
25#include <gtsam/geometry/Point2.h>
26#include <gtsam/geometry/Pose2.h>
27
28// for modeling measurement uncertainty - all models included here
29#include <gtsam/linear/NoiseModel.h>
30
31// add in headers for specific factors
32#include <gtsam/slam/BetweenFactor.h>
33#include <gtsam/sam/BearingRangeFactor.h>
34
35#include <gtsam/nonlinear/Marginals.h>
36
37using namespace std;
38using namespace gtsam;
39
40TEST(Marginals, planarSLAMmarginals) {
41
42 // Taken from PlanarSLAMSelfContained_advanced
43
44 // create keys for variables
45 Symbol x1('x',1), x2('x',2), x3('x',3);
46 Symbol l1('l',1), l2('l',2);
47
48 // create graph container and add factors to it
49 NonlinearFactorGraph graph;
50
51 /* add prior */
52 // gaussian for prior
53 SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(sigmas: Vector3(0.3, 0.3, 0.1));
54 Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
55 graph.addPrior(key: x1, prior: priorMean, model: priorNoise); // add the factor to the graph
56
57 /* add odometry */
58 // general noisemodel for odometry
59 SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas(sigmas: Vector3(0.2, 0.2, 0.1));
60 Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
61 // create between factors to represent odometry
62 graph.emplace_shared<BetweenFactor<Pose2>>(args&: x1, args&: x2, args&: odometry, args&: odometryNoise);
63 graph.emplace_shared<BetweenFactor<Pose2>>(args&: x2, args&: x3, args&: odometry, args&: odometryNoise);
64
65 /* add measurements */
66 // general noisemodel for measurements
67 SharedDiagonal measurementNoise = noiseModel::Diagonal::Sigmas(sigmas: Vector2(0.1, 0.2));
68
69 // create the measurement values - indices are (pose id, landmark id)
70 Rot2 bearing11 = Rot2::fromDegrees(theta: 45),
71 bearing21 = Rot2::fromDegrees(theta: 90),
72 bearing32 = Rot2::fromDegrees(theta: 90);
73 double range11 = sqrt(x: 4.0+4.0),
74 range21 = 2.0,
75 range32 = 2.0;
76
77 // create bearing/range factors
78 graph.emplace_shared<BearingRangeFactor<Pose2, Point2>>(args&: x1, args&: l1, args&: bearing11, args&: range11, args&: measurementNoise);
79 graph.emplace_shared<BearingRangeFactor<Pose2, Point2>>(args&: x2, args&: l1, args&: bearing21, args&: range21, args&: measurementNoise);
80 graph.emplace_shared<BearingRangeFactor<Pose2, Point2>>(args&: x3, args&: l2, args&: bearing32, args&: range32, args&: measurementNoise);
81
82 // linearization point for marginals
83 Values soln;
84 soln.insert(j: x1, val: Pose2(0.0, 0.0, 0.0));
85 soln.insert(j: x2, val: Pose2(2.0, 0.0, 0.0));
86 soln.insert(j: x3, val: Pose2(4.0, 0.0, 0.0));
87 soln.insert(j: l1, val: Point2(2.0, 2.0));
88 soln.insert(j: l2, val: Point2(4.0, 2.0));
89 VectorValues soln_lin;
90 soln_lin.insert(j: x1, value: Vector3(0.0, 0.0, 0.0));
91 soln_lin.insert(j: x2, value: Vector3(2.0, 0.0, 0.0));
92 soln_lin.insert(j: x3, value: Vector3(4.0, 0.0, 0.0));
93 soln_lin.insert(j: l1, value: Vector2(2.0, 2.0));
94 soln_lin.insert(j: l2, value: Vector2(4.0, 2.0));
95
96 Matrix expectedx1(3,3);
97 expectedx1 <<
98 0.09, -7.1942452e-18, -1.27897692e-17,
99 -7.1942452e-18, 0.09, 1.27897692e-17,
100 -1.27897692e-17, 1.27897692e-17, 0.01;
101 Matrix expectedx2(3,3);
102 expectedx2 <<
103 0.120967742, -0.00129032258, 0.00451612903,
104 -0.00129032258, 0.158387097, 0.0206451613,
105 0.00451612903, 0.0206451613, 0.0177419355;
106 Matrix expectedx3(3,3);
107 expectedx3 <<
108 0.160967742, 0.00774193548, 0.00451612903,
109 0.00774193548, 0.351935484, 0.0561290323,
110 0.00451612903, 0.0561290323, 0.0277419355;
111 Matrix expectedl1(2,2);
112 expectedl1 <<
113 0.168709677, -0.0477419355,
114 -0.0477419355, 0.163548387;
115 Matrix expectedl2(2,2);
116 expectedl2 <<
117 0.293870968, -0.104516129,
118 -0.104516129, 0.391935484;
119
120 auto testMarginals = [&] (Marginals marginals) {
121 EXPECT(assert_equal(expectedx1, marginals.marginalCovariance(x1), 1e-8));
122 EXPECT(assert_equal(expectedx2, marginals.marginalCovariance(x2), 1e-8));
123 EXPECT(assert_equal(expectedx3, marginals.marginalCovariance(x3), 1e-8));
124 EXPECT(assert_equal(expectedl1, marginals.marginalCovariance(l1), 1e-8));
125 EXPECT(assert_equal(expectedl2, marginals.marginalCovariance(l2), 1e-8));
126 };
127
128 auto testJointMarginals = [&] (Marginals marginals) {
129 // Check joint marginals for 3 variables
130 Matrix expected_l2x1x3(8,8);
131 expected_l2x1x3 <<
132 0.293871159514111, -0.104516127560770, 0.090000180000270, -0.000000000000000, -0.020000000000000, 0.151935669757191, -0.104516127560770, -0.050967744878460,
133 -0.104516127560770, 0.391935664055174, 0.000000000000000, 0.090000180000270, 0.040000000000000, 0.007741936219615, 0.351935664055174, 0.056129031890193,
134 0.090000180000270, 0.000000000000000, 0.090000180000270, -0.000000000000000, 0.000000000000000, 0.090000180000270, 0.000000000000000, 0.000000000000000,
135 -0.000000000000000, 0.090000180000270, -0.000000000000000, 0.090000180000270, 0.000000000000000, -0.000000000000000, 0.090000180000270, 0.000000000000000,
136 -0.020000000000000, 0.040000000000000, 0.000000000000000, 0.000000000000000, 0.010000000000000, 0.000000000000000, 0.040000000000000, 0.010000000000000,
137 0.151935669757191, 0.007741936219615, 0.090000180000270, -0.000000000000000, 0.000000000000000, 0.160967924878730, 0.007741936219615, 0.004516127560770,
138 -0.104516127560770, 0.351935664055174, 0.000000000000000, 0.090000180000270, 0.040000000000000, 0.007741936219615, 0.351935664055174, 0.056129031890193,
139 -0.050967744878460, 0.056129031890193, 0.000000000000000, 0.000000000000000, 0.010000000000000, 0.004516127560770, 0.056129031890193, 0.027741936219615;
140 KeyVector variables {x1, l2, x3};
141 JointMarginal joint_l2x1x3 = marginals.jointMarginalCovariance(variables);
142 EXPECT(assert_equal(Matrix(expected_l2x1x3.block(0,0,2,2)), Matrix(joint_l2x1x3(l2,l2)), 1e-6));
143 EXPECT(assert_equal(Matrix(expected_l2x1x3.block(2,0,3,2)), Matrix(joint_l2x1x3(x1,l2)), 1e-6));
144 EXPECT(assert_equal(Matrix(expected_l2x1x3.block(5,0,3,2)), Matrix(joint_l2x1x3(x3,l2)), 1e-6));
145
146 EXPECT(assert_equal(Matrix(expected_l2x1x3.block(0,2,2,3)), Matrix(joint_l2x1x3(l2,x1)), 1e-6));
147 EXPECT(assert_equal(Matrix(expected_l2x1x3.block(2,2,3,3)), Matrix(joint_l2x1x3(x1,x1)), 1e-6));
148 EXPECT(assert_equal(Matrix(expected_l2x1x3.block(5,2,3,3)), Matrix(joint_l2x1x3(x3,x1)), 1e-6));
149
150 EXPECT(assert_equal(Matrix(expected_l2x1x3.block(0,5,2,3)), Matrix(joint_l2x1x3(l2,x3)), 1e-6));
151 EXPECT(assert_equal(Matrix(expected_l2x1x3.block(2,5,3,3)), Matrix(joint_l2x1x3(x1,x3)), 1e-6));
152 EXPECT(assert_equal(Matrix(expected_l2x1x3.block(5,5,3,3)), Matrix(joint_l2x1x3(x3,x3)), 1e-6));
153
154 // Check joint marginals for 2 variables (different code path than >2 variable case above)
155 Matrix expected_l2x1(5,5);
156 expected_l2x1 <<
157 0.293871159514111, -0.104516127560770, 0.090000180000270, -0.000000000000000, -0.020000000000000,
158 -0.104516127560770, 0.391935664055174, 0.000000000000000, 0.090000180000270, 0.040000000000000,
159 0.090000180000270, 0.000000000000000, 0.090000180000270, -0.000000000000000, 0.000000000000000,
160 -0.000000000000000, 0.090000180000270, -0.000000000000000, 0.090000180000270, 0.000000000000000,
161 -0.020000000000000, 0.040000000000000, 0.000000000000000, 0.000000000000000, 0.010000000000000;
162 variables.resize(new_size: 2);
163 variables[0] = l2;
164 variables[1] = x1;
165 JointMarginal joint_l2x1 = marginals.jointMarginalCovariance(variables);
166 EXPECT(assert_equal(Matrix(expected_l2x1.block(0,0,2,2)), Matrix(joint_l2x1(l2,l2)), 1e-6));
167 EXPECT(assert_equal(Matrix(expected_l2x1.block(2,0,3,2)), Matrix(joint_l2x1(x1,l2)), 1e-6));
168 EXPECT(assert_equal(Matrix(expected_l2x1.block(0,2,2,3)), Matrix(joint_l2x1(l2,x1)), 1e-6));
169 EXPECT(assert_equal(Matrix(expected_l2x1.block(2,2,3,3)), Matrix(joint_l2x1(x1,x1)), 1e-6));
170
171 // Check joint marginal for 1 variable (different code path than >1 variable cases above)
172 variables.resize(new_size: 1);
173 variables[0] = x1;
174 JointMarginal joint_x1 = marginals.jointMarginalCovariance(variables);
175 EXPECT(assert_equal(expectedx1, Matrix(joint_l2x1(x1,x1)), 1e-6));
176 };
177
178 Marginals marginals;
179
180 marginals = Marginals(graph, soln, Marginals::CHOLESKY);
181 testMarginals(marginals);
182 marginals = Marginals(graph, soln, Marginals::QR);
183 testMarginals(marginals);
184 testJointMarginals(marginals);
185
186 GaussianFactorGraph gfg = *graph.linearize(linearizationPoint: soln);
187 marginals = Marginals(gfg, soln_lin, Marginals::CHOLESKY);
188 testMarginals(marginals);
189 marginals = Marginals(gfg, soln_lin, Marginals::QR);
190 testMarginals(marginals);
191 testJointMarginals(marginals);
192}
193
194/* ************************************************************************* */
195TEST(Marginals, order) {
196 NonlinearFactorGraph fg;
197 fg.addPrior(key: 0, prior: Pose2(), model: noiseModel::Unit::Create(dim: 3));
198 fg.emplace_shared<BetweenFactor<Pose2>>(args: 0, args: 1, args: Pose2(1,0,0), args: noiseModel::Unit::Create(dim: 3));
199 fg.emplace_shared<BetweenFactor<Pose2>>(args: 1, args: 2, args: Pose2(1,0,0), args: noiseModel::Unit::Create(dim: 3));
200 fg.emplace_shared<BetweenFactor<Pose2>>(args: 2, args: 3, args: Pose2(1,0,0), args: noiseModel::Unit::Create(dim: 3));
201
202 Values vals;
203 vals.insert(j: 0, val: Pose2());
204 vals.insert(j: 1, val: Pose2(1,0,0));
205 vals.insert(j: 2, val: Pose2(2,0,0));
206 vals.insert(j: 3, val: Pose2(3,0,0));
207
208 vals.insert(j: 100, val: Point2(0,1));
209 vals.insert(j: 101, val: Point2(1,1));
210
211 fg.emplace_shared<BearingRangeFactor<Pose2,Point2>>(args: 0, args: 100,
212 args: vals.at<Pose2>(j: 0).bearing(point: vals.at<Point2>(j: 100)),
213 args: vals.at<Pose2>(j: 0).range(point: vals.at<Point2>(j: 100)), args: noiseModel::Unit::Create(dim: 2));
214 fg.emplace_shared<BearingRangeFactor<Pose2,Point2>>(args: 0, args: 101,
215 args: vals.at<Pose2>(j: 0).bearing(point: vals.at<Point2>(j: 101)),
216 args: vals.at<Pose2>(j: 0).range(point: vals.at<Point2>(j: 101)), args: noiseModel::Unit::Create(dim: 2));
217
218 fg.emplace_shared<BearingRangeFactor<Pose2,Point2>>(args: 1, args: 100,
219 args: vals.at<Pose2>(j: 1).bearing(point: vals.at<Point2>(j: 100)),
220 args: vals.at<Pose2>(j: 1).range(point: vals.at<Point2>(j: 100)), args: noiseModel::Unit::Create(dim: 2));
221 fg.emplace_shared<BearingRangeFactor<Pose2,Point2>>(args: 1, args: 101,
222 args: vals.at<Pose2>(j: 1).bearing(point: vals.at<Point2>(j: 101)),
223 args: vals.at<Pose2>(j: 1).range(point: vals.at<Point2>(j: 101)), args: noiseModel::Unit::Create(dim: 2));
224
225 fg.emplace_shared<BearingRangeFactor<Pose2,Point2>>(args: 2, args: 100,
226 args: vals.at<Pose2>(j: 2).bearing(point: vals.at<Point2>(j: 100)),
227 args: vals.at<Pose2>(j: 2).range(point: vals.at<Point2>(j: 100)), args: noiseModel::Unit::Create(dim: 2));
228 fg.emplace_shared<BearingRangeFactor<Pose2,Point2>>(args: 2, args: 101,
229 args: vals.at<Pose2>(j: 2).bearing(point: vals.at<Point2>(j: 101)),
230 args: vals.at<Pose2>(j: 2).range(point: vals.at<Point2>(j: 101)), args: noiseModel::Unit::Create(dim: 2));
231
232 fg.emplace_shared<BearingRangeFactor<Pose2,Point2>>(args: 3, args: 100,
233 args: vals.at<Pose2>(j: 3).bearing(point: vals.at<Point2>(j: 100)),
234 args: vals.at<Pose2>(j: 3).range(point: vals.at<Point2>(j: 100)), args: noiseModel::Unit::Create(dim: 2));
235 fg.emplace_shared<BearingRangeFactor<Pose2,Point2>>(args: 3, args: 101,
236 args: vals.at<Pose2>(j: 3).bearing(point: vals.at<Point2>(j: 101)),
237 args: vals.at<Pose2>(j: 3).range(point: vals.at<Point2>(j: 101)), args: noiseModel::Unit::Create(dim: 2));
238
239 auto testMarginals = [&] (Marginals marginals, KeySet set) {
240 KeyVector keys(set.begin(), set.end());
241 JointMarginal joint = marginals.jointMarginalCovariance(variables: keys);
242
243 LONGS_EQUAL(3, (long)joint(0,0).rows());
244 LONGS_EQUAL(3, (long)joint(1,1).rows());
245 LONGS_EQUAL(3, (long)joint(2,2).rows());
246 LONGS_EQUAL(3, (long)joint(3,3).rows());
247 LONGS_EQUAL(2, (long)joint(100,100).rows());
248 LONGS_EQUAL(2, (long)joint(101,101).rows());
249 };
250
251 Marginals marginals(fg, vals);
252 KeySet set = fg.keys();
253 testMarginals(marginals, set);
254
255 GaussianFactorGraph gfg = *fg.linearize(linearizationPoint: vals);
256 marginals = Marginals(gfg, vals);
257 set = gfg.keys();
258 testMarginals(marginals, set);
259}
260
261/* ************************************************************************* */
262int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr);}
263/* ************************************************************************* */
264