-
Notifications
You must be signed in to change notification settings - Fork 595
/
local_parameterization_se3.hpp
46 lines (38 loc) · 1.24 KB
/
local_parameterization_se3.hpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
#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<Matrix<double, 6, 7> > 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