tr_solver.hpp

Go to the documentation of this file.

// Copyright (C) 2023 Petter Nilsson. MIT License.
 
#pragma once
 
#include <optional>
#include <utility>
 
#include <Eigen/Cholesky>
#include <Eigen/Sparse>
#include <Eigen/SparseCholesky>
 
#include "smooth/version.hpp"
 
SMOOTH_BEGIN_NAMESPACE
 
template<typename D2, typename D3>
auto solve_linear_ldlt(
  const auto & J,
  const Eigen::MatrixBase<D2> & d,
  const Eigen::MatrixBase<D3> & r,
  const double lambda,
  std::optional<std::reference_wrapper<double>> dphi = {})
  -> Eigen::Vector<typename std::decay_t<decltype(J)>::Scalar, std::decay_t<decltype(J)>::ColsAtCompileTime>
{
  using JType             = std::decay_t<decltype(J)>;
  using Scalar            = typename JType::Scalar;
  static constexpr auto N = JType::ColsAtCompileTime;  // num variables
 
  static constexpr bool is_sparse = std::is_base_of_v<Eigen::SparseMatrixBase<JType>, JType>;
 
  using Ht = std::
    conditional_t<is_sparse, Eigen::SparseMatrix<typename JType::Scalar>, Eigen::Matrix<typename JType::Scalar, N, N>>;
 
  using LDLTt = std::conditional_t<is_sparse, Eigen::SimplicialLDLT<Ht>, Eigen::LDLT<Ht>>;
 
  Ht H = J.transpose() * J;
  for (auto i = 0u; i < H.rows(); ++i) { H.coeffRef(i, i) += lambda * d(i) * d(i); }
 
  const LDLTt ldlt(H);
  const Eigen::Vector<Scalar, N> x = ldlt.solve(-J.transpose() * r);
 
  if (dphi.has_value()) {
    const Eigen::Vector<Scalar, N> Dx  = -d.cwiseProduct(x);
    const Eigen::Vector<Scalar, N> d_q = d.cwiseProduct(Dx);
    const Eigen::Vector<Scalar, N> y   = ldlt.solve(d_q);
    dphi->get()                        = -d.cwiseProduct(Dx.normalized()).dot(y);
  }
 
  return x;
}
 
template<typename D2, typename D3>
auto solve_trust_region(
  const auto & J, const Eigen::MatrixBase<D2> & d, const Eigen::MatrixBase<D3> & r, const double Delta) -> std::
  pair<Eigen::Vector<typename std::decay_t<decltype(J)>::Scalar, std::decay_t<decltype(J)>::ColsAtCompileTime>, double>
{
  const double lambda = 1. / Delta;
 
  const auto dx = solve_linear_ldlt(J, d, r, lambda);
 
  return {dx, lambda};
}
 
SMOOTH_END_NAMESPACE