使用Ceres-Sovler进行四元数求解优化：LocalParameterization-＞Manifold

使用Ceres-Sovler进行四元数求解优化：LocalParameterization->Manifold

代码示例

代码中q是待优化的单位四元数；在2.1版本及以前，cere支持调用ceres提供的LocalParameterization方法和QuaternionParameterization即可。

    / 上略 /
    // Build the problem.
    double q[4];
	ceres::Problem problem;
	/****************************************/
	/* 省略调用AddResidualBlock() q是参数之一*/
	/****************************************/
    ceres::LocalParameterization* quaternion_parameterization = new ceres::QuaternionParameterization;
    problem.SetParameterization(q, quaternion_parameterization);
    / 下略 //

2.2版本开始，LocalParameterization方法不可用，需要用Manifold替代：

    / 上略 /
    // Build the problem.
    double q[4];
	ceres::Problem problem;
	/****************************************/
	/* 省略调用AddResidualBlock() q是参数之一*/
	/****************************************/
    ceres::Manifold* quaternion_manifold = new ceres::QuaternionManifold;
    problem.SetManifold(q, quaternion_manifold);
    / 下略 //

为什么四元数求解需要特殊对待？LocalParameterization和Manifold是什么？

在最优化问题求解时，迭代的每一步都要更新所有的参数。绝大多数参数的更新都比较简单，直接加上或者减去更新量$dv$即可，例如从一堆二维数据点拟合直线时的直线参数更新；而四元数（单位四元数）作为姿态参数的一种表达形式，其更新不能简单的加减：因为两个单位四元数的和一般不能保持是单位四元数。所以，四元数的更新应该用乘法完成。LocalParameterization或Manifold就是做这个的，当你添加了一个四元数作为待优化参数后，还应该告诉解算器，这个参数需要特殊处理，其“加”“减”不能直接进行，四元数所在的Manifold上的“加减”按照下面的原则：

$$\begin{array}{r}\begin{array}{rl}⊞(x,\Delta )& =\exp \left(\Delta \right)\otimes x\\ \boxminus (y,x)& =\log \left(y\otimes x^{-1}\right)\end{array}\end{array}$$

其中，$\otimes$是四元数乘法；$⊞$是“加法”，$\boxminus$是“减法”；对$\Delta \in {\mathbb{R}}^3$，

$$\begin{array}{r}\exp (\Delta )=\left[\begin{array}{c}\cos \left(\Vert \Delta \Vert \right)\\ \frac{\sin \left(|\Delta \Vert \right)}{\Vert \Delta \Vert }\Delta \end{array}\right]\end{array}$$
以及，对于单位四元数$q$，
$$\log (q)=\frac{atan2\left(\sqrt{1-q_0^2},q_0\right)}{\sqrt{1-q_0^2}}\left[\begin{array}{ccc}{q}_1,& q_2,& q_3\end{array}\right]$$