testNonlinearFactor.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 
20 /*STL/C++*/
21 #include <iostream>
22 
23 #include <CppUnitLite/TestHarness.h>
24 
25 // TODO: DANGEROUS, create shared pointers
26 #define GTSAM_MAGIC_GAUSSIAN 2
27 
28 #include <gtsam/base/Testable.h>
29 #include <gtsam/base/Matrix.h>
30 #include <tests/smallExample.h>
31 #include <tests/simulated2D.h>
32 #include <gtsam/linear/GaussianFactor.h>
33 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
34 #include <gtsam/inference/Symbol.h>
35 
36 using namespace std;
37 using namespace gtsam;
38 using namespace example;
39 
40 // Convenience for named keys
41 using symbol_shorthand::X;
42 using symbol_shorthand::L;
43 
44 typedef boost::shared_ptr<NonlinearFactor > shared_nlf;
45 
46 /* ************************************************************************* */
47 TEST( NonlinearFactor, equals )
48 {
49  SharedNoiseModel sigma(noiseModel::Isotropic::Sigma(2,1.0));
50 
51  // create two nonlinear2 factors
52  Point2 z3(0.,-1.);
53  simulated2D::Measurement f0(z3, sigma, X(1),L(1));
54 
55  // measurement between x2 and l1
56  Point2 z4(-1.5, -1.);
57  simulated2D::Measurement f1(z4, sigma, X(2),L(1));
58 
59  CHECK(assert_equal(f0,f0));
60  CHECK(f0.equals(f0));
61  CHECK(!f0.equals(f1));
62  CHECK(!f1.equals(f0));
63 }
64 
65 /* ************************************************************************* */
66 TEST( NonlinearFactor, equals2 )
67 {
68  // create a non linear factor graph
69  NonlinearFactorGraph fg = createNonlinearFactorGraph();
70 
71  // get two factors
72  NonlinearFactorGraph::sharedFactor f0 = fg[0], f1 = fg[1];
73 
74  CHECK(f0->equals(*f0));
75  CHECK(!f0->equals(*f1));
76  CHECK(!f1->equals(*f0));
77 }
78 
79 /* ************************************************************************* */
80 TEST( NonlinearFactor, NonlinearFactor )
81 {
82  // create a non linear factor graph
83  NonlinearFactorGraph fg = createNonlinearFactorGraph();
84 
85  // create a values structure for the non linear factor graph
86  Values cfg = createNoisyValues();
87 
88  // get the factor "f1" from the factor graph
89  NonlinearFactorGraph::sharedFactor factor = fg[0];
90 
91  // calculate the error_vector from the factor "f1"
92  // error_vector = [0.1 0.1]
93  Vector actual_e = boost::dynamic_pointer_cast<NoiseModelFactor>(factor)->unwhitenedError(cfg);
94  CHECK(assert_equal(0.1*Vector::Ones(2),actual_e));
95 
96  // error = 0.5 * [1 1] * [1;1] = 1
97  double expected = 1.0;
98 
99  // calculate the error from the factor "f1"
100  double actual = factor->error(cfg);
101  DOUBLES_EQUAL(expected,actual,0.00000001);
102 }
103 
104 /* ************************************************************************* */
105 TEST( NonlinearFactor, linearize_f1 )
106 {
107  Values c = createNoisyValues();
108 
109  // Grab a non-linear factor
110  NonlinearFactorGraph nfg = createNonlinearFactorGraph();
111  NonlinearFactorGraph::sharedFactor nlf = nfg[0];
112 
113  // We linearize at noisy config from SmallExample
114  GaussianFactor::shared_ptr actual = nlf->linearize(c);
115 
116  GaussianFactorGraph lfg = createGaussianFactorGraph();
117  GaussianFactor::shared_ptr expected = lfg[0];
118 
119  CHECK(assert_equal(*expected,*actual));
120 
121  // The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector
122  // Hence i.e., b = approximates z-h(x0) = error_vector(x0)
123  //CHECK(assert_equal(nlf->error_vector(c),actual->get_b()));
124 }
125 
126 /* ************************************************************************* */
127 TEST( NonlinearFactor, linearize_f2 )
128 {
129  Values c = createNoisyValues();
130 
131  // Grab a non-linear factor
132  NonlinearFactorGraph nfg = createNonlinearFactorGraph();
133  NonlinearFactorGraph::sharedFactor nlf = nfg[1];
134 
135  // We linearize at noisy config from SmallExample
136  GaussianFactor::shared_ptr actual = nlf->linearize(c);
137 
138  GaussianFactorGraph lfg = createGaussianFactorGraph();
139  GaussianFactor::shared_ptr expected = lfg[1];
140 
141  CHECK(assert_equal(*expected,*actual));
142 }
143 
144 /* ************************************************************************* */
145 TEST( NonlinearFactor, linearize_f3 )
146 {
147  // Grab a non-linear factor
148  NonlinearFactorGraph nfg = createNonlinearFactorGraph();
149  NonlinearFactorGraph::sharedFactor nlf = nfg[2];
150 
151  // We linearize at noisy config from SmallExample
152  Values c = createNoisyValues();
153  GaussianFactor::shared_ptr actual = nlf->linearize(c);
154 
155  GaussianFactorGraph lfg = createGaussianFactorGraph();
156  GaussianFactor::shared_ptr expected = lfg[2];
157 
158  CHECK(assert_equal(*expected,*actual));
159 }
160 
161 /* ************************************************************************* */
162 TEST( NonlinearFactor, linearize_f4 )
163 {
164  // Grab a non-linear factor
165  NonlinearFactorGraph nfg = createNonlinearFactorGraph();
166  NonlinearFactorGraph::sharedFactor nlf = nfg[3];
167 
168  // We linearize at noisy config from SmallExample
169  Values c = createNoisyValues();
170  GaussianFactor::shared_ptr actual = nlf->linearize(c);
171 
172  GaussianFactorGraph lfg = createGaussianFactorGraph();
173  GaussianFactor::shared_ptr expected = lfg[3];
174 
175  CHECK(assert_equal(*expected,*actual));
176 }
177 
178 /* ************************************************************************* */
179 TEST( NonlinearFactor, size )
180 {
181  // create a non linear factor graph
182  NonlinearFactorGraph fg = createNonlinearFactorGraph();
183 
184  // create a values structure for the non linear factor graph
185  Values cfg = createNoisyValues();
186 
187  // get some factors from the graph
188  NonlinearFactorGraph::sharedFactor factor1 = fg[0], factor2 = fg[1],
189  factor3 = fg[2];
190 
191  CHECK(factor1->size() == 1);
192  CHECK(factor2->size() == 2);
193  CHECK(factor3->size() == 2);
194 }
195 
196 /* ************************************************************************* */
197 TEST( NonlinearFactor, linearize_constraint1 )
198 {
199  SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(Vector2(0.2,0));
200 
201  Point2 mu(1., -1.);
202  NonlinearFactorGraph::sharedFactor f0(new simulated2D::Prior(mu, constraint, X(1)));
203 
204  Values config;
205  config.insert(X(1), Point2(1.0, 2.0));
206  GaussianFactor::shared_ptr actual = f0->linearize(config);
207 
208  // create expected
209  Vector2 b(0., -3.);
210  JacobianFactor expected(X(1), (Matrix(2, 2) << 5.0, 0.0, 0.0, 1.0).finished(), b,
211  noiseModel::Constrained::MixedSigmas(Vector2(1,0)));
212  CHECK(assert_equal((const GaussianFactor&)expected, *actual));
213 }
214 
215 /* ************************************************************************* */
216 TEST( NonlinearFactor, linearize_constraint2 )
217 {
218  SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(Vector2(0.2,0));
219 
220  Point2 z3(1.,-1.);
221  simulated2D::Measurement f0(z3, constraint, X(1),L(1));
222 
223  Values config;
224  config.insert(X(1), Point2(1.0, 2.0));
225  config.insert(L(1), Point2(5.0, 4.0));
226  GaussianFactor::shared_ptr actual = f0.linearize(config);
227 
228  // create expected
229  Matrix2 A; A << 5.0, 0.0, 0.0, 1.0;
230  Vector2 b(-15., -3.);
231  JacobianFactor expected(X(1), -1*A, L(1), A, b,
232  noiseModel::Constrained::MixedSigmas(Vector2(1,0)));
233  CHECK(assert_equal((const GaussianFactor&)expected, *actual));
234 }
235 
236 /* ************************************************************************* */
237 TEST( NonlinearFactor, cloneWithNewNoiseModel )
238 {
239  // create original factor
240  double sigma1 = 0.1;
241  NonlinearFactorGraph nfg = example::nonlinearFactorGraphWithGivenSigma(sigma1);
242 
243  // create expected
244  double sigma2 = 10;
245  NonlinearFactorGraph expected = example::nonlinearFactorGraphWithGivenSigma(sigma2);
246 
247  // create actual
248  NonlinearFactorGraph actual;
249  SharedNoiseModel noise2 = noiseModel::Isotropic::Sigma(2,sigma2);
250  actual.push_back( boost::dynamic_pointer_cast<NoiseModelFactor>(nfg[0])->cloneWithNewNoiseModel(noise2) );
251 
252  // check it's all good
253  CHECK(assert_equal(expected, actual));
254 }
255 
256 /* ************************************************************************* */
257 class TestFactor4 : public NoiseModelFactor4<double, double, double, double> {
258 public:
259  typedef NoiseModelFactor4<double, double, double, double> Base;
260  TestFactor4() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4)) {}
261 
262  Vector
263  evaluateError(const double& x1, const double& x2, const double& x3, const double& x4,
264  boost::optional<Matrix&> H1 = boost::none,
265  boost::optional<Matrix&> H2 = boost::none,
266  boost::optional<Matrix&> H3 = boost::none,
267  boost::optional<Matrix&> H4 = boost::none) const override {
268  if(H1) {
269  *H1 = (Matrix(1, 1) << 1.0).finished();
270  *H2 = (Matrix(1, 1) << 2.0).finished();
271  *H3 = (Matrix(1, 1) << 3.0).finished();
272  *H4 = (Matrix(1, 1) << 4.0).finished();
273  }
274  return (Vector(1) << x1 + x2 + x3 + x4).finished();
275  }
276 
277  gtsam::NonlinearFactor::shared_ptr clone() const override {
278  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
279  gtsam::NonlinearFactor::shared_ptr(new TestFactor4(*this))); }
280 };
281 
282 /* ************************************ */
283 TEST(NonlinearFactor, NoiseModelFactor4) {
284  TestFactor4 tf;
285  Values tv;
286  tv.insert(X(1), double((1.0)));
287  tv.insert(X(2), double((2.0)));
288  tv.insert(X(3), double((3.0)));
289  tv.insert(X(4), double((4.0)));
290  EXPECT(assert_equal((Vector(1) << 10.0).finished(), tf.unwhitenedError(tv)));
291  DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9);
292  JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv)));
293  LONGS_EQUAL((long)X(1), (long)jf.keys()[0]);
294  LONGS_EQUAL((long)X(2), (long)jf.keys()[1]);
295  LONGS_EQUAL((long)X(3), (long)jf.keys()[2]);
296  LONGS_EQUAL((long)X(4), (long)jf.keys()[3]);
297  EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 0.5).finished(), jf.getA(jf.begin())));
298  EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.0).finished(), jf.getA(jf.begin()+1)));
299  EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.5).finished(), jf.getA(jf.begin()+2)));
300  EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.0).finished(), jf.getA(jf.begin()+3)));
301  EXPECT(assert_equal((Vector)(Vector(1) << -5.0).finished(), jf.getb()));
302 }
303 
304 /* ************************************************************************* */
305 class TestFactor5 : public NoiseModelFactor5<double, double, double, double, double> {
306 public:
307  typedef NoiseModelFactor5<double, double, double, double, double> Base;
308  TestFactor5() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4), X(5)) {}
309 
310  Vector
311  evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5,
312  boost::optional<Matrix&> H1 = boost::none,
313  boost::optional<Matrix&> H2 = boost::none,
314  boost::optional<Matrix&> H3 = boost::none,
315  boost::optional<Matrix&> H4 = boost::none,
316  boost::optional<Matrix&> H5 = boost::none) const override {
317  if(H1) {
318  *H1 = (Matrix(1, 1) << 1.0).finished();
319  *H2 = (Matrix(1, 1) << 2.0).finished();
320  *H3 = (Matrix(1, 1) << 3.0).finished();
321  *H4 = (Matrix(1, 1) << 4.0).finished();
322  *H5 = (Matrix(1, 1) << 5.0).finished();
323  }
324  return (Vector(1) << x1 + x2 + x3 + x4 + x5).finished();
325  }
326 };
327 
328 /* ************************************ */
329 TEST(NonlinearFactor, NoiseModelFactor5) {
330  TestFactor5 tf;
331  Values tv;
332  tv.insert(X(1), double((1.0)));
333  tv.insert(X(2), double((2.0)));
334  tv.insert(X(3), double((3.0)));
335  tv.insert(X(4), double((4.0)));
336  tv.insert(X(5), double((5.0)));
337  EXPECT(assert_equal((Vector(1) << 15.0).finished(), tf.unwhitenedError(tv)));
338  DOUBLES_EQUAL(56.25/2.0, tf.error(tv), 1e-9);
339  JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv)));
340  LONGS_EQUAL((long)X(1), (long)jf.keys()[0]);
341  LONGS_EQUAL((long)X(2), (long)jf.keys()[1]);
342  LONGS_EQUAL((long)X(3), (long)jf.keys()[2]);
343  LONGS_EQUAL((long)X(4), (long)jf.keys()[3]);
344  LONGS_EQUAL((long)X(5), (long)jf.keys()[4]);
345  EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 0.5).finished(), jf.getA(jf.begin())));
346  EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.0).finished(), jf.getA(jf.begin()+1)));
347  EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.5).finished(), jf.getA(jf.begin()+2)));
348  EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.0).finished(), jf.getA(jf.begin()+3)));
349  EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.5).finished(), jf.getA(jf.begin()+4)));
350  EXPECT(assert_equal((Vector)(Vector(1) << -7.5).finished(), jf.getb()));
351 }
352 
353 /* ************************************************************************* */
354 class TestFactor6 : public NoiseModelFactor6<double, double, double, double, double, double> {
355 public:
356  typedef NoiseModelFactor6<double, double, double, double, double, double> Base;
357  TestFactor6() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4), X(5), X(6)) {}
358 
359  Vector
360  evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6,
361  boost::optional<Matrix&> H1 = boost::none,
362  boost::optional<Matrix&> H2 = boost::none,
363  boost::optional<Matrix&> H3 = boost::none,
364  boost::optional<Matrix&> H4 = boost::none,
365  boost::optional<Matrix&> H5 = boost::none,
366  boost::optional<Matrix&> H6 = boost::none) const override {
367  if(H1) {
368  *H1 = (Matrix(1, 1) << 1.0).finished();
369  *H2 = (Matrix(1, 1) << 2.0).finished();
370  *H3 = (Matrix(1, 1) << 3.0).finished();
371  *H4 = (Matrix(1, 1) << 4.0).finished();
372  *H5 = (Matrix(1, 1) << 5.0).finished();
373  *H6 = (Matrix(1, 1) << 6.0).finished();
374  }
375  return (Vector(1) << x1 + x2 + x3 + x4 + x5 + x6).finished();
376  }
377 
378 };
379 
380 /* ************************************ */
381 TEST(NonlinearFactor, NoiseModelFactor6) {
382  TestFactor6 tf;
383  Values tv;
384  tv.insert(X(1), double((1.0)));
385  tv.insert(X(2), double((2.0)));
386  tv.insert(X(3), double((3.0)));
387  tv.insert(X(4), double((4.0)));
388  tv.insert(X(5), double((5.0)));
389  tv.insert(X(6), double((6.0)));
390  EXPECT(assert_equal((Vector(1) << 21.0).finished(), tf.unwhitenedError(tv)));
391  DOUBLES_EQUAL(110.25/2.0, tf.error(tv), 1e-9);
392  JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv)));
393  LONGS_EQUAL((long)X(1), (long)jf.keys()[0]);
394  LONGS_EQUAL((long)X(2), (long)jf.keys()[1]);
395  LONGS_EQUAL((long)X(3), (long)jf.keys()[2]);
396  LONGS_EQUAL((long)X(4), (long)jf.keys()[3]);
397  LONGS_EQUAL((long)X(5), (long)jf.keys()[4]);
398  LONGS_EQUAL((long)X(6), (long)jf.keys()[5]);
399  EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 0.5).finished(), jf.getA(jf.begin())));
400  EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.0).finished(), jf.getA(jf.begin()+1)));
401  EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.5).finished(), jf.getA(jf.begin()+2)));
402  EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.0).finished(), jf.getA(jf.begin()+3)));
403  EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.5).finished(), jf.getA(jf.begin()+4)));
404  EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 3.0).finished(), jf.getA(jf.begin()+5)));
405  EXPECT(assert_equal((Vector)(Vector(1) << -10.5).finished(), jf.getb()));
406 
407 }
408 
409 /* ************************************************************************* */
410 TEST( NonlinearFactor, clone_rekey )
411 {
412  shared_nlf init(new TestFactor4());
413  EXPECT_LONGS_EQUAL((long)X(1), (long)init->keys()[0]);
414  EXPECT_LONGS_EQUAL((long)X(2), (long)init->keys()[1]);
415  EXPECT_LONGS_EQUAL((long)X(3), (long)init->keys()[2]);
416  EXPECT_LONGS_EQUAL((long)X(4), (long)init->keys()[3]);
417 
418  // Standard clone
419  shared_nlf actClone = init->clone();
420  EXPECT(actClone.get() != init.get()); // Ensure different pointers
421  EXPECT(assert_equal(*init, *actClone));
422 
423  // Re-key factor - clones with different keys
424  KeyVector new_keys {X(5),X(6),X(7),X(8)};
425  shared_nlf actRekey = init->rekey(new_keys);
426  EXPECT(actRekey.get() != init.get()); // Ensure different pointers
427 
428  // Ensure init is unchanged
429  EXPECT_LONGS_EQUAL((long)X(1), (long)init->keys()[0]);
430  EXPECT_LONGS_EQUAL((long)X(2), (long)init->keys()[1]);
431  EXPECT_LONGS_EQUAL((long)X(3), (long)init->keys()[2]);
432  EXPECT_LONGS_EQUAL((long)X(4), (long)init->keys()[3]);
433 
434  // Check new keys
435  EXPECT_LONGS_EQUAL((long)X(5), (long)actRekey->keys()[0]);
436  EXPECT_LONGS_EQUAL((long)X(6), (long)actRekey->keys()[1]);
437  EXPECT_LONGS_EQUAL((long)X(7), (long)actRekey->keys()[2]);
438  EXPECT_LONGS_EQUAL((long)X(8), (long)actRekey->keys()[3]);
439 }
440 
441 /* ************************************************************************* */
442 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
443 /* ************************************************************************* */
CHECK
#define CHECK(condition)
Definition: Test.h:109
gtsam::NoiseModelFactor::linearize
boost::shared_ptr< GaussianFactor > linearize(const Values &x) const override
Definition: NonlinearFactor.cpp:141
E1::A
gtsam::NoiseModelFactor::error
double error(const Values &c) const override
Definition: NonlinearFactor.cpp:127
gtsam::NoiseModelFactor6::unwhitenedError
Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const override
Definition: NonlinearFactor.h:728
gtsam::NoiseModelFactor5::unwhitenedError
Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const override
Definition: NonlinearFactor.h:640
gtsam::example::createNoisyValues
Values createNoisyValues()
Definition: smallExample.h:245
b
Scalar * b
Definition: benchVecAdd.cpp:17
Testable.h
Concept check for values that can be used in unit tests.
simple_graph::sigma1
double sigma1
Definition: testJacobianFactor.cpp:191
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::Measurement
This is the base class for all measurement types.
Definition: Measurement.h:11
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam::example::createGaussianFactorGraph
GaussianFactorGraph createGaussianFactorGraph()
Definition: smallExample.h:272
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:140
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:974
TEST
TEST(NonlinearFactor, equals)
Definition: testNonlinearFactor.cpp:47
TestFactor5::Base
NoiseModelFactor5< double, double, double, double, double > Base
Definition: testNonlinearFactor.cpp:307
TestFactor4::evaluateError
Vector evaluateError(const double &x1, const double &x2, const double &x3, const double &x4, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none, boost::optional< Matrix & > H3=boost::none, boost::optional< Matrix & > H4=boost::none) const override
Definition: testNonlinearFactor.cpp:263
DOUBLES_EQUAL
#define DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:142
gtsam::Point2
Vector2 Point2
Definition: Point2.h:27
mu
double mu
Definition: testBoundingConstraint.cpp:37
sigma
static const double sigma
Definition: testSmartProjectionPoseFactor.cpp:36
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
GaussianFactor.h
A factor with a quadratic error function - a Gaussian.
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
gtsam::example::nonlinearFactorGraphWithGivenSigma
NonlinearFactorGraph nonlinearFactorGraphWithGivenSigma(const double sigma)
Definition: smallExample.h:353
L
MatrixXd L
Definition: LLT_example.cpp:6
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
x5
static string x5("x5")
simple_graph::factor3
JacobianFactor factor3(keyY, A32, keyZ, A33, b3, noiseModel::Isotropic::Sigma(2, sigma3))
TestFactor6::Base
NoiseModelFactor6< double, double, double, double, double, double > Base
Definition: testNonlinearFactor.cpp:356
shared_nlf
boost::shared_ptr< NonlinearFactor > shared_nlf
Definition: testNonlinearFactor.cpp:44
size
Scalar Scalar int size
Definition: benchVecAdd.cpp:17
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
z3
static const Unit3 z3
Definition: testRotateFactor.cpp:46
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
TestFactor4::Base
NoiseModelFactor4< double, double, double, double > Base
Definition: testNonlinearFactor.cpp:259
EXPECT
#define EXPECT(condition)
Definition: Test.h:151
gtsam::NonlinearFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:53
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::example::createNonlinearFactorGraph
NonlinearFactorGraph createNonlinearFactorGraph(const SharedNoiseModel &noiseModel1=impl::kSigma0_1, const SharedNoiseModel &noiseModel2=impl::kSigma0_2)
Definition: smallExample.h:207
gtsam::NoiseModelFactor4::unwhitenedError
Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const override
Definition: NonlinearFactor.h:556
gtsam::GaussianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
x3
Pose3 x3(Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2)
main
int main()
Definition: testNonlinearFactor.cpp:442
LONGS_EQUAL
#define LONGS_EQUAL(expected, actual)
Definition: Test.h:135
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:736
gtsam
traits
Definition: chartTesting.h:28
TestFactor5::evaluateError
Vector evaluateError(const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none, boost::optional< Matrix & > H3=boost::none, boost::optional< Matrix & > H4=boost::none, boost::optional< Matrix & > H5=boost::none) const override
Definition: testNonlinearFactor.cpp:311
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
simulated2D.h
measurement functions and derivatives for simulated 2D robot
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:42
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:155
unary::f1
Point2 f1(const Point3 &p, OptionalJacobian< 2, 3 > H)
Definition: testExpression.cpp:82
x1
Pose3 x1
Definition: testPose3.cpp:588
init
detail::initimpl::constructor< Args... > init()
Binds an existing constructor taking arguments Args...
Definition: pybind11.h:1460
smallExample.h
Create small example with two poses and one landmark.
simple_graph::factor2
JacobianFactor factor2(keyX, A21, keyY, A22, b2, noiseModel::Isotropic::Sigma(2, sigma2))
gtsam::Diagonal
static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix &covariance)
Definition: ScenarioRunner.h:26
TestFactor4::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: testNonlinearFactor.cpp:277
ceres::Matrix
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
Definition: gtsam/3rdparty/ceres/eigen.h:42
x4
static string x4("x4")
gtsam::NonlinearFactorGraph::linearize
boost::shared_ptr< GaussianFactorGraph > linearize(const Values &linearizationPoint) const
Linearize a nonlinear factor graph.
Definition: NonlinearFactorGraph.cpp:340
X
#define X
Definition: icosphere.cpp:20
simple_graph::sigma2
double sigma2
Definition: testJacobianFactor.cpp:196
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
TestFactor6::evaluateError
Vector evaluateError(const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none, boost::optional< Matrix & > H3=boost::none, boost::optional< Matrix & > H4=boost::none, boost::optional< Matrix & > H5=boost::none, boost::optional< Matrix & > H6=boost::none) const override
Definition: testNonlinearFactor.cpp:360
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:734
simple_graph::factor1
JacobianFactor factor1(keyX, A11, b1, noiseModel::Isotropic::Sigma(2, sigma1))

gtsam
Author(s):
autogenerated on Sat May 8 2021 02:48:21