Ceres中的LocalParameterization
参考：
[1] ceres-solver
[2]《A Tutorial on Graph-Based SLAM》
[3]《流形与几何初步》
[4]《Quaternion kinematics for the error-state Kalman filter》
对于四元数或者旋转矩阵这种使用过参数化表示旋转的方式，它们是不支持广义的加法（因为使用普通的加法就会打破其 constraint，比如旋转矩阵加旋转矩阵得到的就不再是旋转矩阵），所以我们在使用ceres对其进行迭代更新的时候就需要自定义其更新方式了，具体的做法是实现一个参数本地化的子类，需要继承于LocalParameterization，LocalParameterization是纯虚类，所以我们继承的时候要把所有的纯虚函数都实现一遍才能使用该类生成对象.
除了不支持广义加法要自定义参数本地化的子类外，如果你要对优化变量做一些限制也可以如法炮制，比如ceres中slam2d example中对角度范围进行了限制.
这里以四元数为例子，解释如何实现参数本地化，需要注意的是，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 x 的自由度（可能有冗余），比如四元数的自由度是4，旋转矩阵的自由度是9
2、LocalSize()
表示 Δ x \Delta x Δx 所在的正切空间（tangent space）的自由度，那么这个自由度是多少呢？下面进行解释，
正切空间是流形（manifold）中概念，对流形感兴趣的可以参考[2]，参考论文[3]，我们可以这么理解manifold：
A manifold is a mathematical space that is not necessarily Euclidean on a global scale, but can be seen as Euclidean on a local scale
上面的意思是说， S O 3 SO3 SO3 空间是属于非欧式空间的，但是没关系，我们只要保证旋转的局部是欧式空间，就可以使用流形进行优化了. 比如用四元数表示的3D旋转是属于非欧式空间的，那么我们取四元数的向量部分作为优化过程中的微小增量（因为是小量，所以不存在奇异性）. 为什么使用四元数的向量部分？这部分可以参考[4]或者之前写的关于四元数的博客. 这样一来，向量部分就位于欧式空间了，也就得到了正切空间自由度是3.
3、Plus()
实现了优化变量的更新，即使用GN法得到的 Δ x ~ ∗ \Delta \tilde { \mathbf { x } } ^ { * } Δx~∗ 更新原来优化变量 x ˘ \breve { \mathbf { x } } x˘，使用的是 ⊞ ⊞ ⊞ 广义加法运算符.
x ∗ = x ˘ ⊞ Δ x ~ ∗ \mathbf { x } ^ { * } = \breve { \mathbf { x } } \mathbb { ⊞ } \Delta \tilde { \mathbf { x } } ^ { * } x∗=x˘⊞Δx~∗
⊞ ⊞ ⊞运算符，首先将四元数的向量部分（与旋转向量相差一个系数2）变成一个完整的四元数（纯虚四元数的指数），即得到过参数化的增量，然后将该增量应用到待估计变量上.
4、ComputeJacobian()
参考[2]中的公式24，使用链式法则我们知道 J ~ i j \tilde { \mathbf { J } } _ { i j } J~ij 由两部分构成，第一部分 ∂ e i j ( x ˘ ) ∂ x ˘ i \frac { \partial \mathbf { e } _ { i j } ( \breve { \mathbf { x } } ) } { \partial \breve { \mathbf { x } } _ { i } } ∂x˘i∂eij(x˘) 是对原始过参数化的优化变量（比如，四元数）的导数，这个很容易求得，直接借助ceres的AutoDiffCostFunction() 计算即可，或者自己计算雅可比矩阵，实现一个costfunction，关键是第二部分是如何求得的呢？
J ~ i j = ∂ e i j ( x ˘ ⊞ Δ x ~ ) ∂ Δ x ~ ∣ Δ x ~ = 0 = ∂ e i j ( x ˘ ) ∂ x ˘ i ⋅ x ˘ i ⊞ Δ x ~ i ∂ Δ x ~ i ∣ Δ x ~ = 0 \tilde { \mathbf { J } } _ { i j } = \left. \frac { \partial \mathbf { e } _ { i j } ( \breve { \mathbf { x } } \mathbb { ⊞ } \boldsymbol { \Delta } \tilde { \mathbf { x } } ) } { \partial \Delta \tilde { \mathbf { x } } } \right| _ { \Delta \tilde { \mathbf { x } } = 0 }= \frac { \partial \mathbf { e } _ { i j } ( \breve { \mathbf { x } } ) } { \partial \breve { \mathbf { x } } _ { i } } \cdot \left. \frac { \breve { \mathbf { x } } _ { i } \mathbb { ⊞ } \boldsymbol { \Delta } \tilde { \mathbf { x } } _ { i } } { \partial \mathbf { \Delta } \tilde { \mathbf { x } } _ { i } } \right| _ { \Delta \tilde { \mathbf { x } } = 0 } J~ij=∂Δx~∂eij(x˘⊞Δx~)∣∣∣∣Δx~=0=∂x˘i∂eij(x˘)⋅∂Δx~ix˘i⊞Δx~i∣∣∣∣Δx~=0
现在求解 ∂ x ˘ i ⊞ Δ x ~ i ∂ Δ x ~ i ∣ Δ x ~ = 0 \left. \frac { \partial {\breve { \mathbf { x } } _ { i } \mathbb { ⊞ } \boldsymbol { \Delta } \tilde { \mathbf { x } } _ { i } }} { \partial \boldsymbol { \Delta } \tilde { \mathbf { x } } _ { i } } \right| _ { \boldsymbol { \Delta } \tilde { \mathbf { x } } = \mathbf { 0 } } ∂Δx~i∂x˘i⊞Δx~i∣∣∣Δx~=0，以四元数为例：
∂ q ˘ i ⊞ Δ q ~ i ∂ Δ q ~ i = ∂ q ˘ i ⊗ e Δ q ~ i ∂ Δ q ~ i ≈ ∂ q ˘ i ⊗ [ 1 Δ q ~ i ] ∂ Δ q ~ i = ∂ [ q ˘ i ] L [ 1 Δ q ~ i ] ∂ Δ q ~ i \frac { \partial {\breve { \mathbf { q } } _ { i } \mathbb { ⊞ } \boldsymbol { \Delta } \tilde { \mathbf { q } } _ { i } }} { \partial \boldsymbol { \Delta } \tilde { \mathbf { q } } _ { i } }= \frac { \partial {\breve { \mathbf { q } } _ { i } \mathbb {\otimes } e^{\boldsymbol { \Delta } \tilde { \mathbf { q } } _ { i } }}} { \partial \boldsymbol { \Delta } \tilde { \mathbf { q } } _ { i } } \approx \frac { \partial {\breve { \mathbf { q } } _ { i } \mathbb { \otimes } \begin{bmatrix} 1 \\ \boldsymbol { \Delta } \tilde { \mathbf { q } } _ { i } \\ \end{bmatrix} }} { \partial \boldsymbol { \Delta } \tilde { \mathbf { q } } _ { i } }= \frac { \partial { [\breve { \mathbf { q } } _ { i }]_L \mathbb { } \begin{bmatrix} 1 \\ \boldsymbol { \Delta } \tilde { \mathbf { q } } _ { i } \\ \end{bmatrix} }} { \partial \boldsymbol { \Delta } \tilde { \mathbf { q } } _ { i } } ∂Δq~i∂q˘i⊞Δq~i=∂Δq~i∂q˘i⊗eΔq~i≈∂Δq~i∂q˘i⊗[1Δq~i]=∂Δq~i∂[q˘i]L[1Δq~i]
注意上面符号的变化，先从 ⊞ ⊞ ⊞ 变成四元数乘法 ⊗ \otimes ⊗，最后变成普通的乘法，进一步得到，
[ q w − q x − q y − q z q x q w − q z q y q y q z q w − q x q z − q y q x q w ] ∂ [ 1 Δ q ~ i ] ∂ Δ q ~ i = [ q w − q x − q y − q z q x q w − q z q y q y q z q w − q x q z − q y q x q w ] [ 0 0 0 1 0 0 0 1 0 0 0 1 ] = [ − q x − q y − q z q w − q z q y q z q w − q x − q y q x q w ] \left[ \begin{array} { l l l l } { q _ { w } } & { - q _ { x } } & { - q _ { y } } & { - q _ { z } } \\ { q _ { x } } & { q _ { w } } & { - q _ { z } } & { q _ { y } } \\ { q _ { y } } & { q _ { z } } & { q _ { w } } & { - q _ { x } } \\ { q _ { z } } & { - q _ { y } } & { q _ { x } } & { q _ { w } } \end{array} \right] \frac { \partial { \begin{bmatrix} 1 \\ \boldsymbol { \Delta } \tilde { \mathbf { q } } _ { i } \\ \end{bmatrix} }} { \partial \boldsymbol { \Delta } \tilde { \mathbf { q } } _ { i } }= \left[ \begin{array} { l l l l } { q _ { w } } & { - q _ { x } } & { - q _ { y } } & { - q _ { z } } \\ { q _ { x } } & { q _ { w } } & { - q _ { z } } & { q _ { y } } \\ { q _ { y } } & { q _ { z } } & { q _ { w } } & { - q _ { x } } \\ { q _ { z } } & { - q _ { y } } & { q _ { x } } & { q _ { w } } \end{array} \right] \left[ \begin{array} { l l l l } { 0} & { 0 } & {0 } \\ { 1 } & { 0} & {0 } \\ { 0} & { 1 } & { 0}\\ { 0} & {0 } & {1 } \end{array} \right] = \left[ \begin{array} { l l l l } { - q _ { x } } & { - q _ { y } } & { - q _ { z } } \\ { q _ { w } } & { - q _ { z } } & { q _ { y } } \\ { q _ { z } } & { q _ { w } } & { - q _ { x } } \\ { - q _ { y } } & { q _ { x } } & { q _ { w } } \end{array} \right] ⎣⎢⎢⎡qwqxqyqz−qxqwqz−qy−qy−qzqwqx−qzqy−qxqw⎦⎥⎥⎤∂Δq~i∂[1Δq~i]=⎣⎢⎢⎡qwqxqyqz−qxqwqz−qy−qy−qzqwqx−qzqy−qxqw⎦⎥⎥⎤⎣⎢⎢⎡010000100001⎦⎥⎥⎤=⎣⎢⎢⎡−qxqwqz−qy−qy−qzqwqx−qzqy−qxqw⎦⎥⎥⎤
最后得到雅可比矩阵是4*3维的，代码里使用一个size为12的数组存储.
<完>
@leatherwang