jacobian_test.hpp

// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide
// SPDX-License-Identifier: MIT
#pragma once
 
#include <random>
#include <vector>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
 
namespace gtsam_points {
 
template <typename GenerateFactor, typename GenerateValues>
bool test_factor_jacobian(std::mt19937& mt, const GenerateFactor& generate_factor, const GenerateValues& generate_values) {
  auto factor = generate_factor(mt);
  gtsam::Values values = generate_values(mt);
 
  std::vector<gtsam::Matrix> Hs_a(factor->keys().size());
  std::vector<gtsam::Matrix> Hs_d(factor->keys().size());
  gtsam::Vector x0 = factor->unwhitenedError(values, Hs_a);
 
  const double eps = 1e-6;
 
  for (int i = 0; i < factor->keys().size(); i++) {
    const auto key = factor->keys()[i];
    gtsam::VectorValues delta = values.zeroVectors();
    const int D = delta[key].size();
 
    gtsam::Matrix Hd(x0.size(), D);
    for (int j = 0; j < D; j++) {
      delta[key].setZero();
      delta[key][j] = eps;
 
      gtsam::Vector xi = factor->unwhitenedError(values.retract(delta));
      Hd.col(j) = (xi - x0) / eps;
    }
 
    Hs_d[i] = Hd;
  }
 
  bool ok = true;
  for (int i = 0; i < factor->keys().size(); i++) {
    ok &= ((Hs_a[i] - Hs_d[i]).array().abs() < 1e-3).all();
  }
 
  if (ok) {
    std::cout << "OK (x0=" << x0.transpose() << ")" << std::endl;
  } else {
    std::cout << "Failed (x0=" << x0.transpose() << ")" << std::endl;
 
    for (int i = 0; i < factor->keys().size(); i++) {
      std::cout << "*** key " << i << " ***" << std::endl;
      std::cerr << "--- Ha ---" << std::endl << Hs_a[i] << std::endl;
      std::cerr << "--- Hd ---" << std::endl << Hs_d[i] << std::endl;
      std::cerr << "--- err ---" << std::endl << Hs_a[i] - Hs_d[i] << std::endl;
    }
  }
 
  return ok;
}
 
}  // namespace gtsam_points