48 lines
1.3 KiB
C++
48 lines
1.3 KiB
C++
|
#ifndef SOPHUS_TEST_LOCAL_PARAMETERIZATION_SE3_HPP
|
||
|
|
#define SOPHUS_TEST_LOCAL_PARAMETERIZATION_SE3_HPP
|
||
|
|
|
||
|
|
#include <ceres/local_parameterization.h>
|
||
|
|
#include <sophus/se3.hpp>
|
||
|
|
|
||
|
|
namespace Sophus {
|
||
|
|
namespace test {
|
||
|
|
|
||
|
|
class LocalParameterizationSE3 : public ceres::LocalParameterization {
|
||
|
|
public:
|
||
|
|
virtual ~LocalParameterizationSE3() {}
|
||
|
|
|
||
|
|
// SE3 plus operation for Ceres
|
||
|
|
//
|
||
|
|
// T * exp(x)
|
||
|
|
//
|
||
|
|
virtual bool Plus(double const* T_raw, double const* delta_raw,
|
||
|
|
double* T_plus_delta_raw) const {
|
||
|
|
Eigen::Map<SE3d const> const T(T_raw);
|
||
|
|
Eigen::Map<Vector6d const> const delta(delta_raw);
|
||
|
|
Eigen::Map<SE3d> T_plus_delta(T_plus_delta_raw);
|
||
|
|
T_plus_delta = T * SE3d::exp(delta);
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Jacobian of SE3 plus operation for Ceres
|
||
|
|
//
|
||
|
|
// Dx T * exp(x) with x=0
|
||
|
|
//
|
||
|
|
virtual bool ComputeJacobian(double const* T_raw,
|
||
|
|
double* jacobian_raw) const {
|
||
|
|
Eigen::Map<SE3d const> T(T_raw);
|
||
|
|
Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> jacobian(
|
||
|
|
jacobian_raw);
|
||
|
|
jacobian = T.Dx_this_mul_exp_x_at_0();
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
virtual int GlobalSize() const { return SE3d::num_parameters; }
|
||
|
|
|
||
|
|
virtual int LocalSize() const { return SE3d::DoF; }
|
||
|
|
};
|
||
|
|
} // namespace test
|
||
|
|
} // namespace Sophus
|
||
|
|
|
||
|
|
#endif
|