testMarginals.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 <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 
37 using namespace std;
38 using namespace gtsam;
39 
40 TEST(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(Vector3(0.3, 0.3, 0.1));
54  Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
55  graph.addPrior(x1, priorMean, priorNoise); // add the factor to the graph
56 
57  /* add odometry */
58  // general noisemodel for odometry
59  SharedDiagonal odometryNoise = noiseModel::Diagonal::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>>(x1, x2, odometry, odometryNoise);
63  graph.emplace_shared<BetweenFactor<Pose2>>(x2, x3, odometry, odometryNoise);
64 
65  /* add measurements */
66  // general noisemodel for measurements
67  SharedDiagonal measurementNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.2));
68 
69  // create the measurement values - indices are (pose id, landmark id)
70  Rot2 bearing11 = Rot2::fromDegrees(45),
71  bearing21 = Rot2::fromDegrees(90),
72  bearing32 = Rot2::fromDegrees(90);
73  double range11 = sqrt(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>>(x1, l1, bearing11, range11, measurementNoise);
79  graph.emplace_shared<BearingRangeFactor<Pose2, Point2>>(x2, l1, bearing21, range21, measurementNoise);
80  graph.emplace_shared<BearingRangeFactor<Pose2, Point2>>(x3, l2, bearing32, range32, measurementNoise);
81 
82  // linearization point for marginals
83  Values soln;
84  soln.insert(x1, Pose2(0.0, 0.0, 0.0));
85  soln.insert(x2, Pose2(2.0, 0.0, 0.0));
86  soln.insert(x3, Pose2(4.0, 0.0, 0.0));
87  soln.insert(l1, Point2(2.0, 2.0));
88  soln.insert(l2, Point2(4.0, 2.0));
89  VectorValues soln_lin;
90  soln_lin.insert(x1, Vector3(0.0, 0.0, 0.0));
91  soln_lin.insert(x2, Vector3(2.0, 0.0, 0.0));
92  soln_lin.insert(x3, Vector3(4.0, 0.0, 0.0));
93  soln_lin.insert(l1, Vector2(2.0, 2.0));
94  soln_lin.insert(l2, 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(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(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(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 /* ************************************************************************* */
195 TEST(Marginals, order) {
196  NonlinearFactorGraph fg;
197  fg.addPrior(0, Pose2(), noiseModel::Unit::Create(3));
198  fg.emplace_shared<BetweenFactor<Pose2>>(0, 1, Pose2(1,0,0), noiseModel::Unit::Create(3));
199  fg.emplace_shared<BetweenFactor<Pose2>>(1, 2, Pose2(1,0,0), noiseModel::Unit::Create(3));
200  fg.emplace_shared<BetweenFactor<Pose2>>(2, 3, Pose2(1,0,0), noiseModel::Unit::Create(3));
201 
202  Values vals;
203  vals.insert(0, Pose2());
204  vals.insert(1, Pose2(1,0,0));
205  vals.insert(2, Pose2(2,0,0));
206  vals.insert(3, Pose2(3,0,0));
207 
208  vals.insert(100, Point2(0,1));
209  vals.insert(101, Point2(1,1));
210 
211  fg.emplace_shared<BearingRangeFactor<Pose2,Point2>>(0, 100,
212  vals.at<Pose2>(0).bearing(vals.at<Point2>(100)),
213  vals.at<Pose2>(0).range(vals.at<Point2>(100)), noiseModel::Unit::Create(2));
214  fg.emplace_shared<BearingRangeFactor<Pose2,Point2>>(0, 101,
215  vals.at<Pose2>(0).bearing(vals.at<Point2>(101)),
216  vals.at<Pose2>(0).range(vals.at<Point2>(101)), noiseModel::Unit::Create(2));
217 
218  fg.emplace_shared<BearingRangeFactor<Pose2,Point2>>(1, 100,
219  vals.at<Pose2>(1).bearing(vals.at<Point2>(100)),
220  vals.at<Pose2>(1).range(vals.at<Point2>(100)), noiseModel::Unit::Create(2));
221  fg.emplace_shared<BearingRangeFactor<Pose2,Point2>>(1, 101,
222  vals.at<Pose2>(1).bearing(vals.at<Point2>(101)),
223  vals.at<Pose2>(1).range(vals.at<Point2>(101)), noiseModel::Unit::Create(2));
224 
225  fg.emplace_shared<BearingRangeFactor<Pose2,Point2>>(2, 100,
226  vals.at<Pose2>(2).bearing(vals.at<Point2>(100)),
227  vals.at<Pose2>(2).range(vals.at<Point2>(100)), noiseModel::Unit::Create(2));
228  fg.emplace_shared<BearingRangeFactor<Pose2,Point2>>(2, 101,
229  vals.at<Pose2>(2).bearing(vals.at<Point2>(101)),
230  vals.at<Pose2>(2).range(vals.at<Point2>(101)), noiseModel::Unit::Create(2));
231 
232  fg.emplace_shared<BearingRangeFactor<Pose2,Point2>>(3, 100,
233  vals.at<Pose2>(3).bearing(vals.at<Point2>(100)),
234  vals.at<Pose2>(3).range(vals.at<Point2>(100)), noiseModel::Unit::Create(2));
235  fg.emplace_shared<BearingRangeFactor<Pose2,Point2>>(3, 101,
236  vals.at<Pose2>(3).bearing(vals.at<Point2>(101)),
237  vals.at<Pose2>(3).range(vals.at<Point2>(101)), noiseModel::Unit::Create(2));
238 
239  auto testMarginals = [&] (Marginals marginals, KeySet set) {
240  KeyVector keys(set.begin(), set.end());
241  JointMarginal joint = marginals.jointMarginalCovariance(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(vals);
256  marginals = Marginals(gfg, vals);
257  set = gfg.keys();
258  testMarginals(marginals, set);
259 }
260 
261 /* ************************************************************************* */
262 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
263 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
Pose2.h
2D Pose
gtsam::GaussianFactorGraph::keys
Keys keys() const
Definition: GaussianFactorGraph.cpp:48
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:43
l2
gtsam::Key l2
Definition: testLinearContainerFactor.cpp:24
TestHarness.h
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
gtsam::VectorValues::insert
iterator insert(const std::pair< Key, Vector > &key_value)
Definition: VectorValues.cpp:90
priorMean
Pose2 priorMean(0.0, 0.0, 0.0)
gtsam::Marginals
Definition: Marginals.h:32
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::NonlinearFactorGraph::linearize
std::shared_ptr< GaussianFactorGraph > linearize(const Values &linearizationPoint) const
Linearize a nonlinear factor graph.
Definition: NonlinearFactorGraph.cpp:239
gtsam::FastSet< Key >
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:44
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:91
gtsam::Marginals::marginalCovariance
Matrix marginalCovariance(Key variable) const
Definition: Marginals.cpp:133
rows
int rows
Definition: Tutorial_commainit_02.cpp:1
x3
Pose3 x3(Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2)
Point2.h
2D Point
odometry
Pose2 odometry(2.0, 0.0, 0.0)
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
BetweenFactor.h
x1
Pose3 x1
Definition: testPose3.cpp:692
gtsam::BearingRangeFactor
Definition: sam/BearingRangeFactor.h:34
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::Marginals::jointMarginalCovariance
JointMarginal jointMarginalCovariance(const KeyVector &variables) const
Definition: Marginals.cpp:138
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:764
gtsam::FactorGraph::keys
KeySet keys() const
Definition: FactorGraph-inst.h:85
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Definition: NonlinearFactorGraph.h:199
Symbol.h
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
priorNoise
auto priorNoise
Definition: doc/Code/OdometryExample.cpp:6
TestResult
Definition: TestResult.h:26
set
void set(Container &c, Position position, const Value &value)
Definition: stdlist_overload.cpp:37
gtsam::Rot2
Definition: Rot2.h:35
gtsam
traits
Definition: ABC.h:17
main
int main()
Definition: testMarginals.cpp:262
Marginals.h
A class for computing marginals in a NonlinearFactorGraph.
NoiseModel.h
gtsam::Values
Definition: Values.h:65
l1
gtsam::Key l1
Definition: testLinearContainerFactor.cpp:24
std
Definition: BFloat16.h:88
odometryNoise
auto odometryNoise
Definition: doc/Code/OdometryExample.cpp:11
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:41
gtsam::Pose2::range
double range(const Point2 &point, OptionalJacobian< 1, 3 > H1={}, OptionalJacobian< 1, 2 > H2={}) const
Definition: Pose2.cpp:285
TEST
TEST(Marginals, planarSLAMmarginals)
Definition: testMarginals.cpp:40
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::JointMarginal
Definition: Marginals.h:137
marginals
Marginals marginals(graph, result)
x2
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
LONGS_EQUAL
#define LONGS_EQUAL(expected, actual)
Definition: Test.h:134
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:153
gtsam::Pose2
Definition: Pose2.h:39
gtsam::Symbol
Definition: inference/Symbol.h:37
BearingRangeFactor.h
a single factor contains both the bearing and the range to prevent handle to pair bearing and range f...
gtsam::BetweenFactor
Definition: BetweenFactor.h:40

gtsam
Author(s):
autogenerated on Wed May 28 2025 03:06:53