gtsam  4.1.0
gtsam
factorTesting.h
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 #pragma once
21 
24 
25 namespace gtsam {
26 
38  const Values& values, double delta = 1e-5) {
39 
40  // We will fill a vector of key/Jacobians pairs (a map would sort)
41  std::vector<std::pair<Key, Matrix> > jacobians;
42 
43  // Get size
44  const Eigen::VectorXd e = factor.whitenedError(values);
45  const size_t rows = e.size();
46 
47  // Loop over all variables
48  const double one_over_2delta = 1.0 / (2.0 * delta);
49  for(Key key: factor) {
50  // Compute central differences using the values struct.
51  VectorValues dX = values.zeroVectors();
52  const size_t cols = dX.dim(key);
53  Matrix J = Matrix::Zero(rows, cols);
54  for (size_t col = 0; col < cols; ++col) {
55  Eigen::VectorXd dx = Eigen::VectorXd::Zero(cols);
56  dx[col] = delta;
57  dX[key] = dx;
58  Values eval_values = values.retract(dX);
59  const Eigen::VectorXd left = factor.whitenedError(eval_values);
60  dx[col] = -delta;
61  dX[key] = dx;
62  eval_values = values.retract(dX);
63  const Eigen::VectorXd right = factor.whitenedError(eval_values);
64  J.col(col) = (left - right) * one_over_2delta;
65  }
66  jacobians.push_back(std::make_pair(key,J));
67  }
68 
69  // Next step...return JacobianFactor
70  return JacobianFactor(jacobians, -e);
71 }
72 
73 namespace internal {
74 // CPPUnitLite-style test for linearization of a factor
75 bool testFactorJacobians(const std::string& name_,
76  const NoiseModelFactor& factor, const gtsam::Values& values, double delta,
77  double tolerance) {
78 
79  // Create expected value by numerical differentiation
80  JacobianFactor expected = linearizeNumerically(factor, values, delta);
81 
82  // Create actual value by linearize
83  boost::shared_ptr<JacobianFactor> actual = //
84  boost::dynamic_pointer_cast<JacobianFactor>(factor.linearize(values));
85  if (!actual) return false;
86 
87  // Check cast result and then equality
88  bool equal = assert_equal(expected, *actual, tolerance);
89 
90  // if not equal, test individual jacobians:
91  if (!equal) {
92  for (size_t i = 0; i < actual->size(); i++) {
93  bool i_good = assert_equal((Matrix) (expected.getA(expected.begin() + i)),
94  (Matrix) (actual->getA(actual->begin() + i)), tolerance);
95  if (!i_good) {
96  std::cout << "Mismatch in Jacobian " << i+1 << " (base 1), as shown above" << std::endl;
97  }
98  }
99  }
100 
101  return equal;
102 }
103 }
104 
110 #define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance) \
111  { EXPECT(gtsam::internal::testFactorJacobians(name_, factor, values, numerical_derivative_step, tolerance)); }
112 
113 } // namespace gtsam
numericalDerivative.h
Some functions to compute numerical derivatives.
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::Values::zeroVectors
VectorValues zeroVectors() const
Return a VectorValues of zero vectors for each variable in this Values.
Definition: Values.cpp:216
gtsam::VectorValues
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:74
gtsam::NoiseModelFactor
A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density Templated on...
Definition: NonlinearFactor.h:154
gtsam::equal
bool equal(const T &obj1, const T &obj2, double tol)
Call equal on the object.
Definition: Testable.h:83
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::linearizeNumerically
JacobianFactor linearizeNumerically(const NoiseModelFactor &factor, const Values &values, double delta=1e-5)
Linearize a nonlinear factor using numerical differentiation The benefit of this method is that it do...
Definition: factorTesting.h:37
gtsam::VectorValues::dim
size_t dim(Key j) const
Return the dimension of variable j.
Definition: VectorValues.h:128
NonlinearFactor.h
Non-linear factor base classes.
gtsam::Values::retract
Values retract(const VectorValues &delta) const
Add a delta config to current config and returns a new config.
Definition: Values.cpp:109
gtsam::JacobianFactor
A Gaussian factor in the squared-error form.
Definition: JacobianFactor.h:91
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
equals with an tolerance, prints out message if unequal
Definition: Matrix.cpp:42
gtsam::NoiseModelFactor::whitenedError
Vector whitenedError(const Values &c) const
Vector of errors, whitened This is the raw error, i.e., i.e.
Definition: NonlinearFactor.cpp:90