testLinearContainerFactor.cpp
Go to the documentation of this file.
1 
8 #include <CppUnitLite/TestHarness.h>
9 #include <gtsam/base/TestableAssertions.h>
10 #include <gtsam/geometry/Point3.h>
11 #include <gtsam/geometry/Pose2.h>
12 #include <gtsam/inference/Symbol.h>
13 #include <gtsam/linear/HessianFactor.h>
14 #include <gtsam/linear/VectorValues.h>
15 #include <gtsam/nonlinear/LinearContainerFactor.h>
16 #include <gtsam/slam/BetweenFactor.h>
17 
18 using namespace std;
19 using namespace gtsam;
20 
21 const gtsam::noiseModel::Diagonal::shared_ptr diag_model2 = noiseModel::Diagonal::Sigmas(Vector2(1.0, 1.0));
22 const double tol = 1e-5;
23 
24 gtsam::Key l1 = 101, l2 = 102, x1 = 1, x2 = 2;
25 
26 Point2 landmark1(5.0, 1.5), landmark2(7.0, 1.5);
27 Pose2 poseA1(0.0, 0.0, 0.0), poseA2(2.0, 0.0, 0.0);
28 
29 /* ************************************************************************* */
30 TEST(TestLinearContainerFactor, generic_jacobian_factor) {
31 
32  Matrix A1 = (Matrix(2, 2) <<
33  2.74222, -0.0067457,
34  0.0, 2.63624).finished();
35  Matrix A2 = (Matrix(2, 2) <<
36  -0.0455167, -0.0443573,
37  -0.0222154, -0.102489).finished();
38  Vector b = Vector2(0.0277052,
39  -0.0533393);
40 
41  JacobianFactor expLinFactor(l1, A1, l2, A2, b, diag_model2);
42 
43  LinearContainerFactor actFactor(expLinFactor);
44  EXPECT_LONGS_EQUAL(2, actFactor.size());
45  EXPECT(actFactor.isJacobian());
46  EXPECT(!actFactor.isHessian());
47 
48  // check keys
49  EXPECT(assert_container_equality({l1, l2}, actFactor.keys()));
50 
51  Values values;
52  values.insert(l1, landmark1);
53  values.insert(l2, landmark2);
54  values.insert(x1, poseA1);
55  values.insert(x2, poseA2);
56 
57  // Check reconstruction
58  GaussianFactor::shared_ptr actLinearizationA = actFactor.linearize(values);
59  EXPECT(assert_equal(*expLinFactor.clone(), *actLinearizationA, tol));
60 }
61 
62 /* ************************************************************************* */
63 TEST(TestLinearContainerFactor, jacobian_factor_withlinpoints) {
64 
65  Matrix A1 = (Matrix(2, 2) <<
66  2.74222, -0.0067457,
67  0.0, 2.63624).finished();
68  Matrix A2 = (Matrix(2, 2) <<
69  -0.0455167, -0.0443573,
70  -0.0222154, -0.102489).finished();
71  Vector b = Vector2(0.0277052,
72  -0.0533393);
73 
74  JacobianFactor expLinFactor(l1, A1, l2, A2, b, diag_model2);
75 
76  Values values;
77  values.insert(l1, landmark1);
78  values.insert(l2, landmark2);
79  values.insert(x1, poseA1);
80  values.insert(x2, poseA2);
81 
82  LinearContainerFactor actFactor(expLinFactor, values);
83  LinearContainerFactor actFactorNolin(expLinFactor);
84 
85  EXPECT(assert_equal(actFactor, actFactor, tol));
86  EXPECT(assert_inequal(actFactor, actFactorNolin, tol));
87  EXPECT(assert_inequal(actFactorNolin, actFactor, tol));
88 
89  // Check contents
90  Values expLinPoint;
91  expLinPoint.insert(l1, landmark1);
92  expLinPoint.insert(l2, landmark2);
93  CHECK(actFactor.linearizationPoint());
94  EXPECT(actFactor.hasLinearizationPoint());
95  EXPECT(assert_equal(expLinPoint, *actFactor.linearizationPoint()));
96 
97  // Check error evaluation
98  Vector delta_l1 = Vector2(1.0, 2.0);
99  Vector delta_l2 = Vector2(3.0, 4.0);
100 
101  VectorValues delta = values.zeroVectors();
102  delta.at(l1) = delta_l1;
103  delta.at(l2) = delta_l2;
104  Values noisyValues = values.retract(delta);
105  double expError = expLinFactor.error(delta);
106  EXPECT_DOUBLES_EQUAL(expError, actFactor.error(noisyValues), tol);
107  EXPECT_DOUBLES_EQUAL(expLinFactor.error(values.zeroVectors()), actFactor.error(values), tol);
108 
109  // Check linearization with corrections for updated linearization point
110  GaussianFactor::shared_ptr actLinearizationB = actFactor.linearize(noisyValues);
111  Vector bprime = b - A1 * delta_l1 - A2 * delta_l2;
112  JacobianFactor expLinFactor2(l1, A1, l2, A2, bprime, diag_model2);
113  EXPECT(assert_equal(*expLinFactor2.clone(), *actLinearizationB, tol));
114 }
115 
116 /* ************************************************************************* */
117 TEST(TestLinearContainerFactor, generic_hessian_factor) {
118  Matrix G11 = (Matrix(1, 1) << 1.0).finished();
119  Matrix G12 = (Matrix(1, 2) << 2.0, 4.0).finished();
120  Matrix G13 = (Matrix(1, 3) << 3.0, 6.0, 9.0).finished();
121 
122  Matrix G22 = (Matrix(2, 2) << 3.0, 5.0,
123  0.0, 6.0).finished();
124  Matrix G23 = (Matrix(2, 3) << 4.0, 6.0, 8.0,
125  1.0, 2.0, 4.0).finished();
126 
127  Matrix G33 = (Matrix(3, 3) << 1.0, 2.0, 3.0,
128  0.0, 5.0, 6.0,
129  0.0, 0.0, 9.0).finished();
130 
131  Vector g1 = (Vector(1) << -7.0).finished();
132  Vector g2 = Vector2(-8.0, -9.0);
133  Vector g3 = Vector3(1.0, 2.0, 3.0);
134 
135  double f = 10.0;
136 
137  HessianFactor initFactor(x1, x2, l1,
138  G11, G12, G13, g1, G22, G23, g2, G33, g3, f);
139 
140  Values values;
141  values.insert(l1, landmark1);
142  values.insert(l2, landmark2);
143  values.insert(x1, poseA1);
144  values.insert(x2, poseA2);
145 
146  LinearContainerFactor actFactor(initFactor);
147  EXPECT(!actFactor.isJacobian());
148  EXPECT(actFactor.isHessian());
149 
150  GaussianFactor::shared_ptr actLinearization1 = actFactor.linearize(values);
151  EXPECT(assert_equal(*initFactor.clone(), *actLinearization1, tol));
152 }
153 
154 /* ************************************************************************* */
155 TEST(TestLinearContainerFactor, hessian_factor_withlinpoints) {
156  // 2 variable example, one pose, one landmark (planar)
157  // Initial ordering: x1, l1
158 
159  Matrix G11 = (Matrix(3, 3) <<
160  1.0, 2.0, 3.0,
161  0.0, 5.0, 6.0,
162  0.0, 0.0, 9.0).finished();
163  Matrix G12 = (Matrix(3, 2) <<
164  1.0, 2.0,
165  3.0, 5.0,
166  4.0, 6.0).finished();
167  Vector g1 = Vector3(1.0, 2.0, 3.0);
168 
169  Matrix G22 = (Matrix(2, 2) <<
170  0.5, 0.2,
171  0.0, 0.6).finished();
172 
173  Vector g2 = Vector2(-8.0, -9.0);
174 
175  double f = 10.0;
176 
177  // Construct full matrices
178  Matrix G(5,5);
179  G << G11, G12, Matrix::Zero(2,3), G22;
180 
181  HessianFactor initFactor(x1, l1, G11, G12, g1, G22, g2, f);
182 
183  Values linearizationPoint, expLinPoints;
184  linearizationPoint.insert(l1, landmark1);
185  linearizationPoint.insert(x1, poseA1);
186  expLinPoints = linearizationPoint;
187  linearizationPoint.insert(x2, poseA2);
188 
189  LinearContainerFactor actFactor(initFactor, linearizationPoint);
190  EXPECT(!actFactor.isJacobian());
191  EXPECT(actFactor.isHessian());
192 
193  EXPECT(actFactor.hasLinearizationPoint());
194  Values actLinPoint = *actFactor.linearizationPoint();
195  EXPECT(assert_equal(expLinPoints, actLinPoint));
196 
197  // Create delta
198  Vector delta_l1 = Vector2(1.0, 2.0);
199  Vector delta_x1 = Vector3(3.0, 4.0, 0.5);
200  Vector delta_x2 = Vector3(6.0, 7.0, 0.3);
201 
202  // Check error calculation
203  VectorValues delta = linearizationPoint.zeroVectors();
204  delta.at(l1) = delta_l1;
205  delta.at(x1) = delta_x1;
206  delta.at(x2) = delta_x2;
207  EXPECT(assert_equal((Vector(5) << 3.0, 4.0, 0.5, 1.0, 2.0).finished(), delta.vector(initFactor.keys())));
208  Values noisyValues = linearizationPoint.retract(delta);
209 
210  double expError = initFactor.error(delta);
211  EXPECT_DOUBLES_EQUAL(expError, actFactor.error(noisyValues), tol);
212  EXPECT_DOUBLES_EQUAL(initFactor.error(linearizationPoint.zeroVectors()), actFactor.error(linearizationPoint), tol);
213 
214  // Compute updated versions
215  Vector dv = (Vector(5) << 3.0, 4.0, 0.5, 1.0, 2.0).finished();
216  Vector g(5); g << g1, g2;
217  Vector g_prime = g - G.selfadjointView<Eigen::Upper>() * dv;
218 
219  // Check linearization with corrections for updated linearization point
220  Vector g1_prime = g_prime.head(3);
221  Vector g2_prime = g_prime.tail(2);
222  double f_prime = f + dv.transpose() * G.selfadjointView<Eigen::Upper>() * dv - 2.0 * dv.transpose() * g;
223  HessianFactor expNewFactor(x1, l1, G11, G12, g1_prime, G22, g2_prime, f_prime);
224  EXPECT(assert_equal(*expNewFactor.clone(), *actFactor.linearize(noisyValues), tol));
225 }
226 
227 /* ************************************************************************* */
228 TEST(TestLinearContainerFactor, Creation) {
229  // Create a set of local keys (No robot label)
230  Key l1 = 11, l3 = 13, l5 = 15;
231 
232  // create a linear factor
233  SharedDiagonal model = noiseModel::Unit::Create(2);
234  JacobianFactor::shared_ptr linear_factor(new JacobianFactor(
235  l3, I_2x2, l5, 2.0 * I_2x2, Z_2x1, model));
236 
237  // create a set of values - build with full set of values
238  gtsam::Values full_values, exp_values;
239  full_values.insert(l3, Point2(1.0, 2.0));
240  full_values.insert(l5, Point2(4.0, 3.0));
241  exp_values = full_values;
242  full_values.insert(l1, Point2(3.0, 7.0));
243 
244  LinearContainerFactor actual(linear_factor, full_values);
245 
246  // Verify the keys
247  EXPECT(assert_container_equality({l3, l5}, actual.keys()));
248 
249  // Verify subset of linearization points
250  EXPECT(assert_equal(exp_values, actual.linearizationPoint(), tol));
251 }
252 
253 /* ************************************************************************* */
254 TEST(TestLinearContainerFactor, jacobian_relinearize)
255 {
256  // Create a Between Factor from a Point3. This is actually a linear factor.
257  gtsam::Key key1(1);
258  gtsam::Key key2(2);
259  gtsam::Values linpoint1;
260  linpoint1.insert(key1, gtsam::Point3(-22.4, +8.5, +2.4));
261  linpoint1.insert(key2, gtsam::Point3(-21.0, +5.0, +21.0));
262 
263  gtsam::Point3 measured(1.0, -2.5, 17.8);
264  gtsam::SharedNoiseModel model = gtsam::noiseModel::Isotropic::Sigma(3, 0.1);
265  gtsam::BetweenFactor<gtsam::Point3> betweenFactor(key1, key2, measured, model);
266 
267  // Create a jacobian container factor at linpoint 1
268  gtsam::JacobianFactor::shared_ptr jacobian(new gtsam::JacobianFactor(*betweenFactor.linearize(linpoint1)));
269  gtsam::LinearContainerFactor jacobianContainer(jacobian, linpoint1);
270 
271  // Create a second linearization point
272  gtsam::Values linpoint2;
273  linpoint2.insert(key1, gtsam::Point3(+18.0, -0.25, +1.11));
274  linpoint2.insert(key2, gtsam::Point3(-10.0, +11.2, +0.05));
275 
276  // Check the error at linpoint2 versus the original factor
277  double expected_error = betweenFactor.error(linpoint2);
278  double actual_error = jacobianContainer.error(linpoint2);
279  EXPECT_DOUBLES_EQUAL(expected_error, actual_error, 1e-9 );
280 
281  // Re-linearize around the new point and check the factors
282  gtsam::GaussianFactor::shared_ptr expected_factor = betweenFactor.linearize(linpoint2);
283  gtsam::GaussianFactor::shared_ptr actual_factor = jacobianContainer.linearize(linpoint2);
284  CHECK(gtsam::assert_equal(*expected_factor, *actual_factor));
285 }
286 
287 /* ************************************************************************* */
288 TEST(TestLinearContainerFactor, hessian_relinearize)
289 {
290  // Create a Between Factor from a Point3. This is actually a linear factor.
291  gtsam::Key key1(1);
292  gtsam::Key key2(2);
293  gtsam::Values linpoint1;
294  linpoint1.insert(key1, gtsam::Point3(-22.4, +8.5, +2.4));
295  linpoint1.insert(key2, gtsam::Point3(-21.0, +5.0, +21.0));
296 
297  gtsam::Point3 measured(1.0, -2.5, 17.8);
298  gtsam::SharedNoiseModel model = gtsam::noiseModel::Isotropic::Sigma(3, 0.1);
299  gtsam::BetweenFactor<gtsam::Point3> betweenFactor(key1, key2, measured, model);
300 
301  // Create a hessian container factor at linpoint 1
302  gtsam::HessianFactor::shared_ptr hessian(new gtsam::HessianFactor(*betweenFactor.linearize(linpoint1)));
303  gtsam::LinearContainerFactor hessianContainer(hessian, linpoint1);
304 
305  // Create a second linearization point
306  gtsam::Values linpoint2;
307  linpoint2.insert(key1, gtsam::Point3(+18.0, -0.25, +1.11));
308  linpoint2.insert(key2, gtsam::Point3(-10.0, +11.2, +0.05));
309 
310  // Check the error at linpoint2 versus the original factor
311  double expected_error = betweenFactor.error(linpoint2);
312  double actual_error = hessianContainer.error(linpoint2);
313  EXPECT_DOUBLES_EQUAL(expected_error, actual_error, 1e-9 );
314 
315  // Re-linearize around the new point and check the factors
316  gtsam::GaussianFactor::shared_ptr expected_factor = gtsam::HessianFactor::shared_ptr(new gtsam::HessianFactor(*betweenFactor.linearize(linpoint2)));
317  gtsam::GaussianFactor::shared_ptr actual_factor = hessianContainer.linearize(linpoint2);
318  CHECK(gtsam::assert_equal(*expected_factor, *actual_factor));
319 }
320 
321 /* ************************************************************************* */
322 TEST(TestLinearContainerFactor, Rekey) {
323  // Make an example factor
324  auto nonlinear_factor =
325  std::make_shared<gtsam::BetweenFactor<gtsam::Point3>>(
326  gtsam::Symbol('x', 0), gtsam::Symbol('l', 0), gtsam::Point3(0, 0, 0),
327  gtsam::noiseModel::Isotropic::Sigma(3, 1));
328 
329  // Linearize and create an LCF
330  gtsam::Values linearization_pt;
331  linearization_pt.insert(gtsam::Symbol('x', 0), gtsam::Point3(0, 0, 0));
332  linearization_pt.insert(gtsam::Symbol('l', 0), gtsam::Point3(0, 0, 0));
333 
334  LinearContainerFactor lcf_factor(
335  nonlinear_factor->linearize(linearization_pt), linearization_pt);
336 
337  // Define a key mapping
338  std::map<gtsam::Key, gtsam::Key> key_map;
339  key_map[gtsam::Symbol('x', 0)] = gtsam::Symbol('x', 4);
340  key_map[gtsam::Symbol('l', 0)] = gtsam::Symbol('l', 4);
341 
342  // Rekey (Calls NonlinearFactor::rekey() which should probably be overriden)
343  // This of type boost_ptr<NonlinearFactor>
344  auto lcf_factor_rekeyed = lcf_factor.rekey(key_map);
345 
346  // Cast back to LCF ptr
347  LinearContainerFactor::shared_ptr lcf_factor_rekey_ptr =
348  std::static_pointer_cast<LinearContainerFactor>(lcf_factor_rekeyed);
349  CHECK(lcf_factor_rekey_ptr);
350 
351  // For extra fun lets try linearizing this LCF
352  gtsam::Values linearization_pt_rekeyed;
353  for (auto key : linearization_pt.keys()) {
354  linearization_pt_rekeyed.insert(key_map.at(key), linearization_pt.at(key));
355  }
356 
357  // Check independent values since we don't want to unnecessarily sort
358  // The keys are just in the reverse order wrt the other container
359  CHECK(assert_equal(linearization_pt_rekeyed.keys()[1], lcf_factor_rekey_ptr->keys()[0]));
360  CHECK(assert_equal(linearization_pt_rekeyed.keys()[0], lcf_factor_rekey_ptr->keys()[1]));
361 }
362 
363 /* ************************************************************************* */
364 TEST(TestLinearContainerFactor, Rekey2) {
365  // Make an example factor
366  auto nonlinear_factor =
367  std::make_shared<gtsam::BetweenFactor<gtsam::Point3>>(
368  gtsam::Symbol('x', 0), gtsam::Symbol('l', 0), gtsam::Point3(0, 0, 0),
369  gtsam::noiseModel::Isotropic::Sigma(3, 1));
370 
371  // Linearize and create an LCF
372  gtsam::Values linearization_pt;
373  linearization_pt.insert(gtsam::Symbol('x', 0), gtsam::Point3(0, 0, 0));
374  linearization_pt.insert(gtsam::Symbol('l', 0), gtsam::Point3(0, 0, 0));
375 
376  LinearContainerFactor lcf_factor(
377  nonlinear_factor->linearize(linearization_pt), linearization_pt);
378 
379  // Define a key mapping with only a single key remapped.
380  // This should throw an exception if there is a bug.
381  std::map<gtsam::Key, gtsam::Key> key_map;
382  key_map[gtsam::Symbol('x', 0)] = gtsam::Symbol('x', 4);
383 
384  // Cast back to LCF ptr
385  LinearContainerFactor::shared_ptr lcf_factor_rekey_ptr =
386  std::static_pointer_cast<LinearContainerFactor>(
387  lcf_factor.rekey(key_map));
388  CHECK(lcf_factor_rekey_ptr);
389 }
390 
391 /* ************************************************************************* */
392 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
393 /* ************************************************************************* */
394 
key1
const Symbol key1('v', 1)
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::LinearContainerFactor::rekey
NonlinearFactor::shared_ptr rekey(const std::map< Key, Key > &rekey_mapping) const override
Definition: LinearContainerFactor.cpp:168
TEST
TEST(TestLinearContainerFactor, generic_jacobian_factor)
Definition: testLinearContainerFactor.cpp:30
Pose2.h
2D Pose
l3
Point3 l3(2, 2, 0)
gtsam::Values::keys
KeyVector keys() const
Definition: Values.cpp:219
gtsam::HessianFactor
A Gaussian factor using the canonical parameters (information form)
Definition: HessianFactor.h:99
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
gtsam::NoiseModelFactor::error
double error(const Values &c) const override
Definition: NonlinearFactor.cpp:138
LinearContainerFactor.h
Wrap Jacobian and Hessian linear factors to allow simple injection into a nonlinear graph.
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
g1
Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0))
gtsam::HybridValues::retract
HybridValues retract(const VectorValues &delta) const
Definition: HybridValues.cpp:79
b
Scalar * b
Definition: benchVecAdd.cpp:17
gtsam::JacobianFactor
Definition: JacobianFactor.h:91
poseA1
Pose2 poseA1(0.0, 0.0, 0.0)
gtsam::JacobianFactor::clone
GaussianFactor::shared_ptr clone() const override
Definition: JacobianFactor.h:190
Eigen::Upper
@ Upper
Definition: Constants.h:211
measured
Point2 measured(-17, 30)
HessianFactor.h
Contains the HessianFactor class, a general quadratic factor.
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::LinearContainerFactor::linearizationPoint
const std::optional< Values > & linearizationPoint() const
Definition: LinearContainerFactor.h:86
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
gtsam::HessianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
A shared_ptr to this class.
Definition: HessianFactor.h:108
gtsam::HessianFactor::error
double error(const VectorValues &c) const override
Definition: HessianFactor.cpp:332
Point3.h
3D Point
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:44
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
landmark2
Point2 landmark2(7.0, 1.5)
TestableAssertions.h
Provides additional testing facilities for common data structures.
gtsam::HessianFactor::clone
GaussianFactor::shared_ptr clone() const override
Definition: HessianFactor.h:187
poseA2
Pose2 poseA2(2.0, 0.0, 0.0)
gtsam::NoiseModelFactor::linearize
std::shared_ptr< GaussianFactor > linearize(const Values &x) const override
Definition: NonlinearFactor.cpp:152
ceres::Matrix
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
Definition: gtsam/3rdparty/ceres/eigen.h:42
gtsam::Values::retract
Values retract(const VectorValues &delta) const
Definition: Values.cpp:99
landmark1
Point2 landmark1(5.0, 1.5)
gtsam.examples.PlanarManipulatorExample.delta
def delta(g0, g1)
Definition: PlanarManipulatorExample.py:45
key2
const Symbol key2('v', 2)
gtsam::VectorValues
Definition: VectorValues.h:74
tol
const double tol
Definition: testLinearContainerFactor.cpp:22
BetweenFactor.h
g2
Pose3 g2(g1.expmap(h *V1_g1))
gtsam::GaussianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:764
A2
static const double A2[]
Definition: expn.h:7
Symbol.h
gtsam::LinearContainerFactor::isHessian
bool isHessian() const
Definition: LinearContainerFactor.cpp:141
gtsam::HybridValues::insert
void insert(Key j, const Vector &value)
Definition: HybridValues.cpp:85
VectorValues.h
Factor Graph Values.
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
gtsam::LinearContainerFactor::isJacobian
bool isJacobian() const
Definition: LinearContainerFactor.cpp:136
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
g
void g(const string &key, int i)
Definition: testBTree.cpp:41
TestResult
Definition: TestResult.h:26
key
const gtsam::Symbol key('X', 0)
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
gtsam::LinearContainerFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: LinearContainerFactor.h:44
gtsam::Values::zeroVectors
VectorValues zeroVectors() const
Definition: Values.cpp:261
gtsam
traits
Definition: SFMdata.h:40
x2
gtsam::Key x2
Definition: testLinearContainerFactor.cpp:24
gtsam::Values
Definition: Values.h:65
gtsam::LinearContainerFactor::error
double error(const Values &c) const override
Definition: LinearContainerFactor.cpp:77
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition: Factor.h:143
CHECK
#define CHECK(condition)
Definition: Test.h:108
l1
gtsam::Key l1
Definition: testLinearContainerFactor.cpp:24
std
Definition: BFloat16.h:88
gtsam::assert_container_equality
bool assert_container_equality(const std::map< size_t, V2 > &expected, const std::map< size_t, V2 > &actual)
Definition: TestableAssertions.h:237
gtsam::noiseModel::Diagonal::shared_ptr
std::shared_ptr< Diagonal > shared_ptr
Definition: NoiseModel.h:321
G
JacobiRotation< float > G
Definition: Jacobi_makeGivens.cpp:2
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:156
A1
static const double A1[]
Definition: expn.h:6
gtsam::Z_2x1
static const Eigen::MatrixBase< Vector2 >::ConstantReturnType Z_2x1
Definition: Vector.h:46
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:41
gtsam::JacobianFactor::error
double error(const VectorValues &c) const override
Definition: JacobianFactor.cpp:487
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::JacobianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: JacobianFactor.h:97
gtsam::assert_inequal
bool assert_inequal(const Matrix &A, const Matrix &B, double tol)
Definition: Matrix.cpp:61
gtsam::noiseModel::Isotropic::Sigma
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
Definition: NoiseModel.cpp:625
gtsam::LinearContainerFactor
Definition: LinearContainerFactor.h:29
gtsam::LinearContainerFactor::hasLinearizationPoint
bool hasLinearizationPoint() const
Casting syntactic sugar.
Definition: LinearContainerFactor.h:141
gtsam::LinearContainerFactor::linearize
GaussianFactor::shared_ptr linearize(const Values &c) const override
Definition: LinearContainerFactor.cpp:104
gtsam::Factor::size
size_t size() const
Definition: Factor.h:160
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
main
int main()
Definition: testLinearContainerFactor.cpp:392
x1
gtsam::Key x1
Definition: testLinearContainerFactor.cpp:24
diag_model2
const gtsam::noiseModel::Diagonal::shared_ptr diag_model2
Definition: testLinearContainerFactor.cpp:21
gtsam::Pose2
Definition: Pose2.h:39
gtsam::Symbol
Definition: inference/Symbol.h:37
gtsam::BetweenFactor
Definition: BetweenFactor.h:40

gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:16:41