Program Listing for File ceres_local_parameterization.hpp

↰ Return to documentation for file (sophus/ceres_local_parameterization.hpp)

#pragma once

#include <ceres/local_parameterization.h>

#include <sophus/ceres_typetraits.hpp>

namespace Sophus {

template <template <typename, int = 0> class LieGroup>
class LocalParameterization : public ceres::LocalParameterization {
 public:
  using LieGroupd = LieGroup<double>;
  using Tangent = typename LieGroupd::Tangent;
  using TangentMap = typename Sophus::Mapper<Tangent>::ConstMap;
  static int constexpr DoF = LieGroupd::DoF;
  static int constexpr num_parameters = LieGroupd::num_parameters;

  bool Plus(double const* T_raw, double const* delta_raw,
            double* T_plus_delta_raw) const override {
    Eigen::Map<LieGroupd const> const T(T_raw);
    TangentMap delta = Sophus::Mapper<Tangent>::map(delta_raw);
    Eigen::Map<LieGroupd> T_plus_delta(T_plus_delta_raw);
    T_plus_delta = T * LieGroupd::exp(delta);
    return true;
  }

  bool ComputeJacobian(double const* T_raw,
                       double* jacobian_raw) const override {
    Eigen::Map<LieGroupd const> T(T_raw);
    Eigen::Map<Eigen::Matrix<double, num_parameters, DoF,
                             DoF == 1 ? Eigen::ColMajor : Eigen::RowMajor>>
        jacobian(jacobian_raw);
    jacobian = T.Dx_this_mul_exp_x_at_0();
    return true;
  }

  int GlobalSize() const override { return LieGroupd::num_parameters; }

  int LocalSize() const override { return LieGroupd::DoF; }
};

}  // namespace Sophus