testPlanarProjectionFactor.cpp
Go to the documentation of this file.
1 
8 #include <random>
9 
10 #include <gtsam/base/Testable.h>
12 #include <gtsam/geometry/Cal3DS2.h>
13 #include <gtsam/geometry/Point3.h>
14 #include <gtsam/geometry/Pose2.h>
15 #include <gtsam/geometry/Pose3.h>
16 #include <gtsam/geometry/Rot2.h>
17 #include <gtsam/geometry/Rot3.h>
18 #include <gtsam/inference/Symbol.h>
23 #include <gtsam/nonlinear/Values.h>
25 
27 
28 using namespace std;
29 using namespace gtsam;
34 
35 /* ************************************************************************* */
37  // Example: center projection and Jacobian
38  Point3 landmark(1, 0, 0);
39  Point2 measured(200, 200);
40  Pose3 offset(
41  Rot3(0, 0, 1,//
42  -1, 0, 0, //
43  0, -1, 0),
44  Vector3(0, 0, 0)
45  );
46  Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
47  SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
49  Pose2 pose(0, 0, 0);
50  Matrix H;
51  CHECK(assert_equal(Vector2(0, 0), factor.evaluateError(pose, H), 1e-6));
52  CHECK(assert_equal((Matrix23() << //
53  0, 200, 200, //
54  0, 0, 0).finished(), H, 1e-6));
55 }
56 
57 /* ************************************************************************* */
59  // Example: upper left corner projection and Jacobian
60  Point3 landmark(1, 1, 1);
61  Point2 measured(0, 0);
62  Pose3 offset(
63  Rot3(0, 0, 1,//
64  -1, 0, 0, //
65  0, -1, 0),
66  Vector3(0, 0, 0)
67  );
68  Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
69  SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
71  Pose2 pose(0, 0, 0);
72  Matrix H;
73  CHECK(assert_equal(Vector2(0, 0), factor.evaluateError(pose, H), 1e-6));
74  CHECK(assert_equal((Matrix23() << //
75  -200, 200, 400, //
76  -200, 0, 200).finished(), H, 1e-6));
77 }
78 
79 /* ************************************************************************* */
81  // Example: upper left corner projection and Jacobian with distortion
82  Point3 landmark(1, 1, 1);
83  Point2 measured(0, 0);
84  Pose3 offset(
85  Rot3(0, 0, 1,//
86  -1, 0, 0, //
87  0, -1, 0),
88  Vector3(0, 0, 0)
89  );
90  Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1); // note distortion
91  SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
93  Pose2 pose(0, 0, 0);
94  Matrix H;
95  CHECK(assert_equal(Vector2(0, 0), factor.evaluateError(pose, H), 1e-6));
96  CHECK(assert_equal((Matrix23() << //
97  -360, 280, 640, //
98  -360, 80, 440).finished(), H, 1e-6));
99 }
100 
101 /* ************************************************************************* */
103  // Verify Jacobians with numeric derivative
104  std::default_random_engine rng(42);
105  std::uniform_real_distribution<double> dist(-0.3, 0.3);
106  SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
107  // center of the random camera poses
108  Pose3 centerOffset(
109  Rot3(0, 0, 1,//
110  -1, 0, 0, //
111  0, -1, 0),
112  Vector3(0, 0, 0)
113  );
114 
115  for (int i = 0; i < 1000; ++i) {
116  Point3 landmark(2 + dist(rng), dist(rng), dist(rng));
117  Point2 measured(200 + 100 * dist(rng), 200 + 100 * dist(rng));
118  Pose3 offset = centerOffset.compose(
119  Pose3(
120  Rot3::Ypr(dist(rng), dist(rng), dist(rng)),
121  Point3(dist(rng), dist(rng), dist(rng))));
122  Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
124  Pose2 pose(dist(rng), dist(rng), dist(rng));
125  Matrix H1;
126  factor.evaluateError(pose, H1);
127  auto expectedH1 = numericalDerivative11<Vector, Pose2>(
128  [&factor](const Pose2& p) {
129  return factor.evaluateError(p, {});},
130  pose);
131  CHECK(assert_equal(expectedH1, H1, 5e-6));
132  }
133 }
134 
135 /* ************************************************************************* */
137  // Example localization
138 
139  SharedNoiseModel pxModel = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
140  // pose model is wide, so the solver finds the right answer.
141  SharedNoiseModel xNoise = noiseModel::Diagonal::Sigmas(Vector3(10, 10, 10));
142 
143  // landmarks
144  Point3 l0(1, 0.1, 1);
145  Point3 l1(1, -0.1, 1);
146 
147  // camera pixels
148  Point2 p0(180, 0);
149  Point2 p1(220, 0);
150 
151  // body
152  Pose2 x0(0, 0, 0);
153 
154  // camera z looking at +x with (xy) antiparallel to (yz)
155  Pose3 c0(
156  Rot3(0, 0, 1, //
157  -1, 0, 0, //
158  0, -1, 0), //
159  Vector3(0, 0, 0));
160  Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
161 
163  graph.add(PlanarProjectionFactor1(X(0), l0, p0, c0, calib, pxModel));
164  graph.add(PlanarProjectionFactor1(X(0), l1, p1, c0, calib, pxModel));
165  graph.add(PriorFactor<Pose2>(X(0), x0, xNoise));
166 
167  Values initialEstimate;
168  initialEstimate.insert(X(0), x0);
169 
170  // run the optimizer
171  LevenbergMarquardtOptimizer optimizer(graph, initialEstimate);
172  Values result = optimizer.optimize();
173 
174  // verify that the optimizer found the right pose.
175  CHECK(assert_equal(x0, result.at<Pose2>(X(0)), 2e-3));
176 
177  // covariance
180  CHECK(assert_equal((Matrix33() << //
181  0.000012, 0.000000, 0.000000, //
182  0.000000, 0.001287, -.001262, //
183  0.000000, -.001262, 0.001250).finished(), cov, 3e-6));
184 
185  // pose stddev
186  Vector3 sigma = cov.diagonal().cwiseSqrt();
187  CHECK(assert_equal((Vector3() << //
188  0.0035,
189  0.0359,
190  0.0354
191  ).finished(), sigma, 1e-4));
192 
193 }
194 
195 /* ************************************************************************* */
197  // Example: center projection and Jacobian
198  Point3 landmark(1, 0, 0);
199  Point2 measured(200, 200);
200  SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
202  Pose2 pose(0, 0, 0);
203  Pose3 offset(
204  Rot3(0, 0, 1,//
205  -1, 0, 0, //
206  0, -1, 0),
207  Vector3(0, 0, 0)
208  );
209  Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
210  Matrix H1;
211  Matrix H2;
212  Matrix H3;
213  CHECK(assert_equal(Vector2(0, 0), factor.evaluateError(pose, offset, calib, H1, H2, H3), 1e-6));
214  CHECK(assert_equal((Matrix23() <<//
215  0, 200, 200,//
216  0, 0, 0).finished(), H1, 1e-6));
217  CHECK(assert_equal((Matrix26() <<//
218  0, -200, 0, -200, 0, 0,//
219  200, -0, 0, 0, -200, 0).finished(), H2, 1e-6));
220  CHECK(assert_equal((Matrix29() <<//
221  0, 0, 0, 1, 0, 0, 0, 0, 0,//
222  0, 0, 0, 0, 1, 0, 0, 0, 0).finished(), H3, 1e-6));
223 }
224 
225 /* ************************************************************************* */
227  Point3 landmark(1, 1, 1);
228  Point2 measured(0, 0);
229  SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
231  Pose2 pose(0, 0, 0);
232  Pose3 offset(
233  Rot3(0, 0, 1,//
234  -1, 0, 0, //
235  0, -1, 0),
236  Vector3(0, 0, 0)
237  );
238  Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
239  Matrix H1;
240  Matrix H2;
241  Matrix H3;
242  gtsam::Vector actual = factor.evaluateError(pose, offset, calib, H1, H2, H3);
243  CHECK(assert_equal(Vector2(0, 0), actual));
244  CHECK(assert_equal((Matrix23() <<//
245  -200, 200, 400,//
246  -200, 0, 200).finished(), H1, 1e-6));
247  CHECK(assert_equal((Matrix26() <<//
248  200, -400, -200, -200, 0, -200,//
249  400, -200, 200, 0, -200, -200).finished(), H2, 1e-6));
250  CHECK(assert_equal((Matrix29() <<//
251  -1, 0, -1, 1, 0, -400, -800, 400, 800,//
252  0, -1, 0, 0, 1, -400, -800, 800, 400).finished(), H3, 1e-6));
253 }
254 
255 /* ************************************************************************* */
257  Point3 landmark(1, 1, 1);
258  Point2 measured(0, 0);
259  SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
261  Pose2 pose(0, 0, 0);
262  Pose3 offset(
263  Rot3(0, 0, 1,//
264  -1, 0, 0, //
265  0, -1, 0),
266  Vector3(0, 0, 0)
267  );
268  Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
269  Matrix H1;
270  Matrix H2;
271  Matrix H3;
272  CHECK(assert_equal(Vector2(0, 0), factor.evaluateError(pose, offset, calib, H1, H2, H3), 1e-6));
273  CHECK(assert_equal((Matrix23() <<//
274  -360, 280, 640,//
275  -360, 80, 440).finished(), H1, 1e-6));
276  CHECK(assert_equal((Matrix26() <<//
277  440, -640, -200, -280, -80, -360,//
278  640, -440, 200, -80, -280, -360).finished(), H2, 1e-6));
279  CHECK(assert_equal((Matrix29() <<//
280  -1, 0, -1, 1, 0, -400, -800, 400, 800,//
281  0, -1, 0, 0, 1, -400, -800, 800, 400).finished(), H3, 1e-6));
282 }
283 
284 /* ************************************************************************* */
286  // Verify Jacobians with numeric derivative
287 
288  std::default_random_engine rng(42);
289  std::uniform_real_distribution<double> dist(-0.3, 0.3);
290  SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
291  // center of the random camera poses
292  Pose3 centerOffset(
293  Rot3(0, 0, 1,//
294  -1, 0, 0, //
295  0, -1, 0),
296  Vector3(0, 0, 0)
297  );
298 
299  for (int i = 0; i < 1000; ++i) {
300  Point3 landmark(2 + dist(rng), dist(rng), dist(rng));
301  Point2 measured(200 + 100 * dist(rng), 200 + 100 * dist(rng));
302  Pose3 offset = centerOffset.compose(
303  Pose3(
304  Rot3::Ypr(dist(rng), dist(rng), dist(rng)),
305  Point3(dist(rng), dist(rng), dist(rng))));
306  Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
307 
309 
310  Pose2 pose(dist(rng), dist(rng), dist(rng));
311 
312  // actual H
313  Matrix H1, H2, H3;
314  factor.evaluateError(pose, offset, calib, H1, H2, H3);
315 
316  Matrix expectedH1 = numericalDerivative31<Vector, Pose2, Pose3, Cal3DS2>(
317  [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) {
318  return factor.evaluateError(p, o, c, {}, {}, {});},
319  pose, offset, calib);
320  Matrix expectedH2 = numericalDerivative32<Vector, Pose2, Pose3, Cal3DS2>(
321  [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) {
322  return factor.evaluateError(p, o, c, {}, {}, {});},
323  pose, offset, calib);
324  Matrix expectedH3 = numericalDerivative33<Vector, Pose2, Pose3, Cal3DS2>(
325  [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) {
326  return factor.evaluateError(p, o, c, {}, {}, {});},
327  pose, offset, calib);
328  CHECK(assert_equal(expectedH1, H1, 5e-6));
329  CHECK(assert_equal(expectedH2, H2, 5e-6));
330  CHECK(assert_equal(expectedH3, H3, 5e-6));
331  }
332 }
333 
334 /* ************************************************************************* */
336  // Example localization
337  SharedNoiseModel pxModel = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
338  SharedNoiseModel xNoise = noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 0.01));
339  // offset model is wide, so the solver finds the right answer.
340  SharedNoiseModel cNoise = noiseModel::Diagonal::Sigmas(Vector6(10, 10, 10, 10, 10, 10));
341  SharedNoiseModel kNoise = noiseModel::Diagonal::Sigmas(Vector9(0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001));
342 
343  // landmarks
344  Point3 l0(1, 0, 1);
345  Point3 l1(1, 0, 0);
346  Point3 l2(1, -1, 1);
347  Point3 l3(2, 2, 1);
348 
349  // camera pixels
350  Point2 p0(200, 200);
351  Point2 p1(200, 400);
352  Point2 p2(400, 200);
353  Point2 p3(0, 200);
354 
355  // body
356  Pose2 x0(0, 0, 0);
357 
358  // camera z looking at +x with (xy) antiparallel to (yz)
359  Pose3 c0(
360  Rot3(0, 0, 1, //
361  -1, 0, 0, //
362  0, -1, 0), //
363  Vector3(0, 0, 1)); // note z offset
364  Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
365 
367  graph.add(PlanarProjectionFactor3(X(0), C(0), K(0), l0, p0, pxModel));
368  graph.add(PlanarProjectionFactor3(X(0), C(0), K(0), l1, p1, pxModel));
369  graph.add(PlanarProjectionFactor3(X(0), C(0), K(0), l2, p2, pxModel));
370  graph.add(PlanarProjectionFactor3(X(0), C(0), K(0), l3, p3, pxModel));
371  graph.add(PriorFactor<Pose2>(X(0), x0, xNoise));
372  graph.add(PriorFactor<Pose3>(C(0), c0, cNoise));
373  graph.add(PriorFactor<Cal3DS2>(K(0), calib, kNoise));
374 
375  Values initialEstimate;
376  initialEstimate.insert(X(0), x0);
377  initialEstimate.insert(C(0), c0);
378  initialEstimate.insert(K(0), calib);
379 
380  // run the optimizer
381  LevenbergMarquardtOptimizer optimizer(graph, initialEstimate);
382  Values result = optimizer.optimize();
383 
384  // verify that the optimizer found the right pose.
385  CHECK(assert_equal(x0, result.at<Pose2>(X(0)), 2e-3));
386 
387  // verify the camera is pointing at +x
388  Pose3 cc0 = result.at<Pose3>(C(0));
389  CHECK(assert_equal(c0, cc0, 5e-3));
390 
391  // verify the calibration
392  CHECK(assert_equal(calib, result.at<Cal3DS2>(K(0)), 2e-3));
393 
395  Matrix x0cov = marginals.marginalCovariance(X(0));
396 
397  // narrow prior => ~zero cov
398  CHECK(assert_equal(Matrix33::Zero(), x0cov, 1e-4));
399 
400  Matrix c0cov = marginals.marginalCovariance(C(0));
401 
402  // invert the camera offset to get covariance in body coordinates
403  Matrix66 HcTb = cc0.inverse().AdjointMap().inverse();
404  Matrix c0cov2 = HcTb * c0cov * HcTb.transpose();
405 
406  // camera-frame stddev
407  Vector6 c0sigma = c0cov.diagonal().cwiseSqrt();
408  CHECK(assert_equal((Vector6() << //
409  0.009,
410  0.011,
411  0.004,
412  0.012,
413  0.012,
414  0.011
415  ).finished(), c0sigma, 1e-3));
416 
417  // body frame stddev
418  Vector6 bTcSigma = c0cov2.diagonal().cwiseSqrt();
419  CHECK(assert_equal((Vector6() << //
420  0.004,
421  0.009,
422  0.011,
423  0.012,
424  0.012,
425  0.012
426  ).finished(), bTcSigma, 1e-3));
427 
428  // narrow prior => ~zero cov
429  CHECK(assert_equal(Matrix99::Zero(), marginals.marginalCovariance(K(0)), 3e-3));
430 }
431 
432 /* ************************************************************************* */
433 int main() {
434  TestResult tr;
435  return TestRegistry::runAllTests(tr);
436 }
437 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
H
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange[*:*] noreverse nowriteback set trange[*:*] noreverse nowriteback set urange[*:*] noreverse nowriteback set vrange[*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
Definition: gnuplot_common_settings.hh:74
Pose2.h
2D Pose
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Definition: NonlinearOptimizer.h:98
l3
Point3 l3(2, 2, 0)
rng
static std::mt19937 rng
Definition: timeFactorOverhead.cpp:31
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Testable.h
Concept check for values that can be used in unit tests.
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:43
gtsam::Cal3DS2
Calibration of a camera with radial distortion that also supports Lie-group behaviors for optimizatio...
Definition: Cal3DS2.h:35
l2
gtsam::Key l2
Definition: testLinearContainerFactor.cpp:24
TestHarness.h
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
gtsam::LieGroup::compose
Class compose(const Class &g) const
Definition: Lie.h:48
l0
static Symbol l0('L', 0)
gtsam::PriorFactor
Definition: nonlinear/PriorFactor.h:30
gtsam::Marginals
Definition: Marginals.h:32
measured
Point2 measured(-17, 30)
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
X
#define X
Definition: icosphere.cpp:20
Point3.h
3D Point
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:44
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::Marginals::marginalCovariance
Matrix marginalCovariance(Key variable) const
Definition: Marginals.cpp:133
Rot2.h
2D rotation
PlanarProjectionFactor.h
for planar smoothing
sampling::sigma
static const double sigma
Definition: testGaussianBayesNet.cpp:170
Rot3.h
3D rotation represented as a rotation matrix or quaternion
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")
TEST
TEST(PlanarProjectionFactor1, Error1)
Definition: testPlanarProjectionFactor.cpp:36
simple::p2
static Point3 p2
Definition: testInitializePose3.cpp:51
gtsam::Values::at
const ValueType at(Key j) const
Definition: Values-inl.h:261
gtsam::PlanarProjectionFactor1
One variable: the pose. Landmark, camera offset, camera calibration are constant. This is intended fo...
Definition: PlanarProjectionFactor.h:109
PriorFactor.h
numericalDerivative.h
Some functions to compute numerical derivatives.
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
gtsam::Pose3::inverse
Pose3 inverse() const
inverse transformation with derivatives
Definition: Pose3.cpp:61
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
x0
static Symbol x0('x', 0)
landmark
static Point3 landmark(0, 0, 5)
gtsam::Pose3::AdjointMap
Matrix6 AdjointMap() const
Definition: Pose3.cpp:69
gtsam::LevenbergMarquardtOptimizer
Definition: LevenbergMarquardtOptimizer.h:35
L
MatrixXd L
Definition: LLT_example.cpp:6
Symbol.h
simple::p3
static Point3 p3
Definition: testInitializePose3.cpp:53
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
p1
Vector3f p1
Definition: MatrixBase_all.cpp:2
TestResult
Definition: TestResult.h:26
offset
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate offset
Definition: gnuplot_common_settings.hh:64
gtsam::PlanarProjectionFactor3
Three unknowns: the pose, the camera offset, and the camera calibration. Landmark is constant....
Definition: PlanarProjectionFactor.h:237
C
Matrix< Scalar, Dynamic, Dynamic > C
Definition: bench_gemm.cpp:50
gtsam
traits
Definition: SFMdata.h:40
Marginals.h
A class for computing marginals in a NonlinearFactorGraph.
K
#define K
Definition: igam.h:8
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam::Values
Definition: Values.h:65
CHECK
#define CHECK(condition)
Definition: Test.h:108
Cal3DS2.h
Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base.
p0
Vector3f p0
Definition: MatrixBase_all.cpp:2
l1
gtsam::Key l1
Definition: testLinearContainerFactor.cpp:24
std
Definition: BFloat16.h:88
p
float * p
Definition: Tutorial_Map_using.cpp:9
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::Point3
Vector3 Point3
Definition: Point3.h:38
main
int main()
Definition: testPlanarProjectionFactor.cpp:433
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
marginals
Marginals marginals(graph, result)
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
Values.h
A non-templated config holding any types of Manifold-group elements.
Pose3.h
3D Pose manifold SO(3) x R^3 and group SE(3)
gtsam::Pose2
Definition: Pose2.h:39


gtsam
Author(s):
autogenerated on Wed Jan 1 2025 04:06:54