QuaternionParameterization
中表示四元数中四个量在内存中的存储顺序是[w, x, y, z],而Eigen内部四元数在内存中的存储顺序是[x, y, z, w],但是其构造顺序是[w, x, y, z](不要被这个假象给迷惑),所以就要使用另一种参数本地化类,即EigenQuaternionParameterization
,下面就以QuaternionParameterization
为例子说明,如下:
class CERES_EXPORT QuaternionParameterization : public LocalParameterization {
public:
virtual ~QuaternionParameterization() {}
virtual bool Plus(const double* x,
const double* delta,
double* x_plus_delta) const;
virtual bool ComputeJacobian(const double* x,
double* jacobian) const;
virtual int GlobalSize() const { return 4; }
virtual int LocalSize() const { return 3; }
};
1、GlobalSize()
表示参数 x
x 的自由度(可能有冗余),比如四元数的自由度是4,旋转矩阵的自由度是9
表示 Δx
Δx 所在的正切空间(tangent space)的自由度,那么这个自由度是3
上面的意思是说,SO3空间是属于非欧式空间的,但是没关系,我们只要保证旋转的局部是欧式空间,就可以使用流形进行优化了. 比如用四元数表示的3D旋转是属于非欧式空间的,那么我们取四元数的向量部分作为优化过程中的微小增量(因为是小量,所以不存在奇异性). 为什么使用四元数的向量部分?这部分可以参考《Quaternion kinematics for the error-state Kalman filter》. 这样一来,向量部分就位于欧式空间了,也就得到了正切空间自由度是3.
可理解为右扰动。
《视觉 SLAM 十四讲》 4.3.4,在 so(3) 上进行扰动。总结,如果是对整个 se(3) 做一个 Parameterization(参考郑帆的博客《On-Manifold Optimization Demo using Ceres Solver》),那么就需要使用《视觉 SLAM 十四讲》 4.3.5 的导数模型,如果仅仅只是 QuaternionParameterization,那么仅需要使用《视觉 SLAM 十四讲》 4.3.4 的导数模型。在确定 Parameterization 的导数模型时,也应该关注在 CostFunction 中 residual 是如何计算的。
ref:https://blog.csdn.net/hzwwpgmwy/article/details/86490556