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 #include <string>
25 #include <vector>
26 
27 namespace gtsam {
28 
40  const Values& values,
41  double delta = 1e-5) {
42  // We will fill a vector of key/Jacobians pairs (a map would sort)
43  std::vector<std::pair<Key, Matrix> > jacobians;
44 
45  // Get size
46  const Vector e = factor.whitenedError(values);
47  const size_t rows = e.size();
48 
49  // Loop over all variables
50  const double one_over_2delta = 1.0 / (2.0 * delta);
51  for (Key key : factor) {
52  // Compute central differences using the values struct.
53  VectorValues dX = values.zeroVectors();
54  const size_t cols = dX.dim(key);
55  Matrix J = Matrix::Zero(rows, cols);
56  for (size_t col = 0; col < cols; ++col) {
57  Vector dx = Vector::Zero(cols);
58  dx(col) = delta;
59  dX[key] = dx;
60  Values eval_values = values.retract(dX);
61  const Vector left = factor.whitenedError(eval_values);
62  dx(col) = -delta;
63  dX[key] = dx;
64  eval_values = values.retract(dX);
65  const Vector right = factor.whitenedError(eval_values);
66  J.col(col) = (left - right) * one_over_2delta;
67  }
68  jacobians.emplace_back(key, J);
69  }
70 
71  // Next step...return JacobianFactor
72  return JacobianFactor(jacobians, -e);
73 }
74 
75 namespace internal {
76 // CPPUnitLite-style test for linearization of a factor
77 inline bool testFactorJacobians(const std::string& name_,
78  const NoiseModelFactor& factor,
79  const gtsam::Values& values, double delta,
80  double tolerance) {
81  // Create expected value by numerical differentiation
83 
84  // Create actual value by linearize
85  auto actual =
86  std::dynamic_pointer_cast<JacobianFactor>(factor.linearize(values));
87  if (!actual) return false;
88 
89  // Check cast result and then equality
90  bool equal = assert_equal(expected, *actual, tolerance);
91 
92  // if not equal, test individual jacobians:
93  if (!equal) {
94  for (size_t i = 0; i < actual->size(); i++) {
95  bool i_good =
96  assert_equal((Matrix)(expected.getA(expected.begin() + i)),
97  (Matrix)(actual->getA(actual->begin() + i)), tolerance);
98  if (!i_good) {
99  std::cout << "Mismatch in Jacobian " << i + 1
100  << " (base 1), as shown above" << std::endl;
101  }
102  }
103  }
104 
105  return equal;
106 }
107 } // namespace internal
108 
114 #define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance) \
115  { EXPECT(gtsam::internal::testFactorJacobians(name_, factor, values, numerical_derivative_step, tolerance)); }
116 
117 } // namespace gtsam
col
m col(1)
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::HybridValues::retract
HybridValues retract(const VectorValues &delta) const
Definition: HybridValues.cpp:79
gtsam::JacobianFactor
Definition: JacobianFactor.h:91
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
rows
int rows
Definition: Tutorial_commainit_02.cpp:1
gtsam::linearizeNumerically
JacobianFactor linearizeNumerically(const NoiseModelFactor &factor, const Values &values, double delta=1e-5)
Definition: factorTesting.h:39
gtsam::NoiseModelFactor::linearize
std::shared_ptr< GaussianFactor > linearize(const Values &x) const override
Definition: NonlinearFactor.cpp:150
numericalDerivative.h
Some functions to compute numerical derivatives.
gtsam::NoiseModelFactor::whitenedError
Vector whitenedError(const Values &c) const
Definition: NonlinearFactor.cpp:107
J
JacobiRotation< float > J
Definition: Jacobi_makeJacobi.cpp:3
gtsam.examples.PlanarManipulatorExample.delta
def delta(g0, g1)
Definition: PlanarManipulatorExample.py:45
gtsam::VectorValues
Definition: VectorValues.h:74
left
static char left
Definition: blas_interface.hh:62
gtsam::internal::testFactorJacobians
bool testFactorJacobians(const std::string &name_, const NoiseModelFactor &factor, const gtsam::Values &values, double delta, double tolerance)
Definition: factorTesting.h:77
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::VectorValues::dim
size_t dim(Key j) const
Definition: VectorValues.h:133
key
const gtsam::Symbol key('X', 0)
NonlinearFactor.h
Non-linear factor base classes.
right
static char right
Definition: blas_interface.hh:61
gtsam
traits
Definition: SFMdata.h:40
gtsam::NoiseModelFactor
Definition: NonlinearFactor.h:197
gtsam::Values
Definition: Values.h:65
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
internal
Definition: BandTriangularSolver.h:13
cols
int cols
Definition: Tutorial_commainit_02.cpp:1
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::equal
bool equal(const T &obj1, const T &obj2, double tol)
Definition: Testable.h:85
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:31