本文，從 ceres::QuaternionParameterization 入手，在理解 ceres::QuaternionParameterization 的基礎上形成對 ceres::LocalParameterization 的認識。
ceres-solver 中對 LocalParameterization 的解釋如下。
Sometimes the parameters \(\mathbf{x}\) can overparameterize a problem. In that case it is desirable to choose a parameterization to remove the null directions of the cost. More generally, if \(\mathbf{x}\) lies on a manifold of a smaller dimension than the ambient space that it is embedded in, then it is numerically and computationally more effective to optimize it using a parameterization that lives in the tangent space of that manifold at each point.
簡單地說。LocalParameterization 是在優化 Manifold 上的變量時需要考慮的，Manifold 上變量是 over parameterized，即 Manifold 上變量的維度大於其自由度。這會導致 Manifold 上變量各個量之間存在約束，如果直接對這些量求導、優化，那么這就是一個有約束的優化，實現困難。為了解決這個問題，在數學上是對 Manifold 在當前變量值處形成的切空間（Tangent Space）求導，在切空間上優化，最后投影回 Manifold。
對於 SLAM 問題，廣泛遇到的 Manifold 是旋轉，旋轉僅需使用 3 個量（so(3)）即可表達，但是實際應用中因為涉及到萬向鎖的問題，在更高維度表達旋轉。四元數就是在維度 4 表達 3 個自由度的三維空間的旋轉。
QuaternionParameterization 的方法 int GlobalSize() 返回 4，表示 Manifold 空間（四元數）是 4 維的，方法 int LocalSize() 返回 3，表示切空間是 3 維的。
QuaternionParameterization 的方法 bool ComputeJacobian(const double* x, double* jacobian) 計算得到一個 4x3 的矩陣。這些由 ComputeJacobian 計算得到的矩陣在 ceres 代碼中被稱作 "global_to_local"，含義是 Manifold 上變量對 Tangent Space 上變量的導數。在 ceres::CostFunction 處提供 residuals 對 Manifold 上變量的導數（\({\partial \mathbf{e} \over \partial \mathbf{X}^{(G)}}\)），乘以這個矩陣（\({\partial \mathbf{X}^{(G)} \over \partial \mathbf{X}^{(L)}}\)）之后變成對 Tangent Space 上變量的導數（\({\partial \mathbf{e} \over \partial \mathbf{X}^{(L)}}\)）。
QuaternionParameterization 的方法 bool Plus(const double* x, const double* delta, double* x_plus_delta) 將優化得到的增量（delta，在 Tangent Space）加到待優化變量（x，在 Manifold）中形成優化結果（x_plus_delta，在 Manifold），完成一次優化迭代。
注意。1. 在 ceres 源碼中沒有明確說明之處都認為矩陣 raw memory 存儲方式是 Row Major 的，這與 Eigen 默認的 Col Major 是相反的。2. ceres 默認的 Quaternion raw memory 存儲方式是 w, x, y, z，而 Eigen Quaternion 的存儲方式是 x, y, z, w，這就導致在 ceres 代碼中除 ceres::QuaternionParameterization 之外還有 ceres::EigenQuaternionParameterization 。3. ceres 中 Quaternion 是 Hamilton Quaternion，遵循 Hamilton 乘法法則。
1. ceres::QuaternionParameterization 與 AutoDiff 的配合
首先，將方法 QuaternionParameterization::ComputeJacobian 的源碼摘抄如下。
bool QuaternionParameterization::ComputeJacobian(const double* x,
double* jacobian) const {
jacobian[0] = -x[1]; jacobian[1] = -x[2]; jacobian[2] = -x[3]; // NOLINT
jacobian[3] = x[0]; jacobian[4] = x[3]; jacobian[5] = -x[2]; // NOLINT
jacobian[6] = -x[3]; jacobian[7] = x[0]; jacobian[8] = x[1]; // NOLINT
jacobian[9] = x[2]; jacobian[10] = -x[1]; jacobian[11] = x[0]; // NOLINT
return true;
}
按照 Row Major 的存儲方式，按照 Manifold 上變量（x）的存儲方式 w, x, y, z，可以將以上代碼轉換為數學公式。
需要理解這個矩陣的由來。參考文獻 [1] 的公式 (18)，以上矩陣對應 \([\mathbf{q}]_R\) 的右側三列。如果將 Manifold 上變量 \(\mathbf{X}^{(G)}\) 寫作 \(\mathbf{q}\)，將 Tangent Space 上變量寫作 \(\mathbf{\theta}\)，那么以上矩陣 \({\partial \mathbf{X}^{(G)} \over \partial \mathbf{X}^{(L)}} = {\partial \mathbf{\theta} \otimes \mathbf{q} \over \partial \mathbf{\theta}}\)。這是一個左擾動形式，檢查方法 QuaternionParameterization::Plus，源碼摘抄如下。
bool QuaternionParameterization::Plus(const double* x,
const double* delta,
double* x_plus_delta) const {
const double norm_delta =
sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);
if (norm_delta > 0.0) {
const double sin_delta_by_delta = (sin(norm_delta) / norm_delta);
double q_delta[4];
q_delta[0] = cos(norm_delta);
q_delta[1] = sin_delta_by_delta * delta[0];
q_delta[2] = sin_delta_by_delta * delta[1];
q_delta[3] = sin_delta_by_delta * delta[2];
QuaternionProduct(q_delta, x, x_plus_delta);
} else {
for (int i = 0; i < 4; ++i) {
x_plus_delta[i] = x[i];
}
}
return true;
}
可以看到，QuaternionProduct(q_delta, x, x_plus_delta);，優化得到的增量 q_delta 是左乘原變量 x，得到新變量 x_plus_delta，這與 QuaternionParameterization::ComputeJacobian 中的左擾動雅克比相對應。
實驗，將 QuaternionParameterization::ComputeJacobian 計算的雅克比改為參考文獻 [1] 的公式 (18) \([\mathbf{q}]_L\) 的右三列，將 QuaternionParameterization::Plus 中增量加到原變量過程改為右乘，即改變之后 QuaternionProduct(x, q_delta, x_plus_delta);。隨后將這個右擾動形式的 QuaternionParameterization 與 AutoDiff 結合，解決實際問題，可以看到得到的結果在誤差允許范圍內與左擾動結果一致。
結論。QuaternionParameterization 的 Plus 與 ComputeJacobian 共同決定使用左擾動或使用右擾動形式。
得到以上關於 QuaternionParameterization 的好處是在不使用 AutoDiff 或 NumericDiff，用戶代碼提供雅克比時，能夠提供指導。按照已有資料，一般都是對 so(3) 求導，很難找到對四元數求導。如果使用 ceres::QuaternionParameterization，ceres 期望用戶代碼提供對四元數的導數，用戶提供這種導數比較麻煩。
如果用戶自行提供一種 QuaternionParameterization，獲得對 parameterization 的控制。用戶就可以在 CostFunction 中提供對 so(3) 的求導，定義 QuaternionParameterization::ComputeJacobian 返回矩陣 \(\begin{bmatrix}\mathbf{I}_{3\times3} \\ (\mathbf{0}_{3\times1})^T\end{bmatrix}\)。矩陣 \(\begin{bmatrix}\mathbf{I}_{3\times3} \\ (\mathbf{0}_{3\times1})^T\end{bmatrix}\) 將用戶返回的 \(N(e)\times4\) 的矩陣，截取為 \(N(e)\times3\) 的矩陣。注，\(N(e)\) 表示誤差 residuals 的個數。並且 QuaternionParameterization::Plus 中增量的加法應與 CostFunction 中對 so(3) 導數的左右擾動一致。在這個過程中，用戶在 CostFunction 中返回的對 so(3) 導數應當是最后一列留 0，前三列填充值。（需要注意，ceres::QuaternionParameterization::Plus 操作的是半軸角，而不是軸角，如果直接套用，會使得計算出的雅克比差一個系數 2。）
當然以上這段文字描述的操作可以實現是有前提條件的，“所有用戶對 Manifold 變量的導數在進入 ceres 之后會先經過 LocalParameterization::ComputeJacobian 轉換為對 Tangent Space 變量的導數，再於其他處使用”。在當前對 ceres 的使用范圍內，以上假設沒有被打破。.
現在證明使用 AutoDiff 計算得到的誤差對四元數的導數乘以 ceres::QuaternionParameterization::ComputeJacobian 返回矩陣的結果就是誤差對 so(3) 的左擾動導數。
Quaternion 在誤差計算中最廣泛的用法是將一個三維點坐標從某個坐標系轉換到另一個坐標系，如將世界坐標系下的坐標轉換到相機坐標系下的坐標，\({}^{C}\mathbf{X}=\mathbf{R}\{{}^C\mathbf{q}_W\}{}^W\mathbf{X}+{}^C\mathbf{t}_W\)。按照鏈式法則，僅需證明 \({\partial {}^{C}\mathbf{X} \over \partial {}^C\mathbf{q}_W}{\partial \mathbf{\theta} \otimes {}^C\mathbf{q}_W \over \partial \mathbf{\theta}} = {\partial {}^{C}\mathbf{X} \over \partial \mathbf{\theta}}\)。
參考文獻 [1] 公式 (115)，得到 \({}^{C}\mathbf{X}\)。
參考《視覺 SLAM 十四講》 4.3.5，知道對 so(3) 的左擾動導數是 \(-({}^C\mathbf{X})^{\wedge}\)，這與以上推導得到的結果是基本對應的，差了一個位移 \({}^C\mathbf{t}_W\)。這個差異可以解釋，使用 se(3) 的擾動模型是 \(\Delta\mathbf{R}(\mathbf{R}\mathbf{p}+\mathbf{t})+\Delta \mathbf{t}\)，所以得到的對 so(3) 的導數中含有位移。而以上公式中的擾動模型是 \((\Delta\mathbf{R})\mathbf{R}\mathbf{p}+\mathbf{t}+\Delta\mathbf{t}\)，應當應用《視覺 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 是如何計算的。
2. ceres::LocalParameterization 的其他用法
以上內容已對 ceres::QuaternionParameterization 中最重要的兩個函數 ceres::QuaternionParameterization::ComputeJacobian 與 ceres::QuaternionParameterization::Plus 進行解釋。ceres::QuaternionParameterization 繼承於 ceres::LocalParameterization，在了解 ceres::QuaternionParameterization 運行原理的情況下，可以根據自身需求繼承實現自己的 ceres::LocalParameterization。
一個非常好用的簡單例子是“固定模長優化”。如前面的解釋 ceres::LocalParameterization 用於 Manifold 上變量的優化，在 Manifold 上變量之間存在相互約束，所以可以看做是存在限制條件的優化。“固定模長優化”是對待優化變量進行了模長限制，最常見的“固定模長優化”是重力加速度方向優化，參考 VINS [2] 的 Algorithm 1，初始化過程中的重力加速度方向優化。
在實現過程中，可以在 CostFunction 中計算對 gravity 三維變量的導數，在 ceres::LocalParameterization::ComputeJacobian 中確定主方向，計算文中的 \(\mathbf{b}_1, \mathbf{b}_2\)，將對 gravity 三維變量的導數投影到 \(\mathbf{b}_1, \mathbf{b}_2\) 這兩個方向上。優化出來的 \(\mathbf{b}_1, \mathbf{b}_2\) 兩個方向上的增量，在 ceres::LocalParameterization::Plus 中合並到 gravity 三維坐標中，並且在合並過程中，注意歸一化，實現固定模長。這個方法，經過本人實驗，是成功的。VINS 代碼沒有細看，粗看其中函數 void RefineGravity(map<double, ImageFrame> &all_image_frame, Vector3d &g, VectorXd &x)
並沒有使用 ceres::LocalParameterization 處理。