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 =
      std::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