Loading...
Searching...
No Matches
factorTesting.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/numericalDerivative.h>
#include <string>
#include <vector>
namespace gtsam {
inline JacobianFactor linearizeNumerically(const NoiseModelFactor& factor,
const Values& values,
double delta = 1e-5) {
// We will fill a vector of key/Jacobians pairs (a map would sort)
std::vector<std::pair<Key, Matrix> > jacobians;
// Get size
const Vector e = factor.whitenedError(values);
const size_t rows = e.size();
// Loop over all variables
const double one_over_2delta = 1.0 / (2.0 * delta);
for (Key key : factor) {
// Compute central differences using the values struct.
VectorValues dX = values.zeroVectors();
const size_t cols = dX.dim(key);
Matrix J = Matrix::Zero(rows, cols);
for (size_t col = 0; col < cols; ++col) {
Vector dx = Vector::Zero(cols);
dx(col) = delta;
dX[key] = dx;
Values eval_values = values.retract(dX);
const Vector left = factor.whitenedError(eval_values);
dx(col) = -delta;
dX[key] = dx;
eval_values = values.retract(dX);
const Vector right = factor.whitenedError(eval_values);
J.col(col) = (left - right) * one_over_2delta;
}
jacobians.emplace_back(key, J);
}
// Next step...return JacobianFactor
return JacobianFactor(jacobians, -e);
}
namespace internal {
// CPPUnitLite-style test for linearization of a factor
inline bool testFactorJacobians(const std::string& name_,
const NoiseModelFactor& factor,
const gtsam::Values& values, double delta,
double tolerance) {
// Create expected value by numerical differentiation
JacobianFactor expected = linearizeNumerically(factor, values, delta);
// Create actual value by linearize
auto actual =
boost::dynamic_pointer_cast<JacobianFactor>(factor.linearize(values));
if (!actual) return false;
// Check cast result and then equality
bool equal = assert_equal(expected, *actual, tolerance);
// if not equal, test individual jacobians:
if (!equal) {
for (size_t i = 0; i < actual->size(); i++) {
bool i_good =
assert_equal((Matrix)(expected.getA(expected.begin() + i)),
(Matrix)(actual->getA(actual->begin() + i)), tolerance);
if (!i_good) {
std::cout << "Mismatch in Jacobian " << i + 1
<< " (base 1), as shown above" << std::endl;
}
}
}
return equal;
}
} // namespace internal
#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance) \
{ EXPECT(gtsam::internal::testFactorJacobians(name_, factor, values, numerical_derivative_step, tolerance)); }
} // namespace gtsam
numericalDerivative.h
Some functions to compute numerical derivatives.
NonlinearFactor.h
Non-linear factor base classes.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
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:43
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:39
gtsam::equal
bool equal(const T &obj1, const T &obj2, double tol)
Call equal on the object.
Definition Testable.h:84
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
gtsam::JacobianFactor
A Gaussian factor in the squared-error form.
Definition JacobianFactor.h:91
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
gtsam::VectorValues::dim
size_t dim(Key j) const
Return the dimension of variable j.
Definition VectorValues.h:130
gtsam::NoiseModelFactor
A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density Templated on...
Definition NonlinearFactor.h:174
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:109
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
gtsam::Values::retract
Values retract(const VectorValues &delta) const
Add a delta config to current config and returns a new config.
Definition Values.cpp:99
gtsam::Values::zeroVectors
VectorValues zeroVectors() const
Return a VectorValues of zero vectors for each variable in this Values.
Definition Values.cpp:272
JacobianFactor
In Gaussian factors, the error function returns either the negative log-likelihood,...
NoiseModelFactor
noise model to the factor, and calculates the error by asking the user to implement the method