Ceres Solver Document學習筆記

• Ceres Solver Document學習筆記
• • 1. 基本概念
  • 2. 基本方法
  • • 2.1 CostFunction
    • 2.2 AutoDiffCostFunction
    • 2.3 NumericDiffCostFuntion
    • 2.4 LossFunction
    • 2.5 LocalParameterization
    • 2.6 Problem
    • 2.7 Solver
    • 2.8 Covariance

Ceres Solver Document學習筆記

之前在學習Vins-Mono時，在Vins-Mono后端就是使用Ceres實現的滑窗優化，當時對Ceres的了解也僅僅是在簡單使用的層面上，最近項目中有再次用到Ceres相關的內容，因此我把Ceres Solver的Document掃了一遍，這篇博客是相關的筆記。我個人覺得Ceres比較重要的優勢主要有如下幾點：

Ceres中提供了自動求導的功能，在優化算法推到的過程中，最麻煩的也就是求雅克比的過程，而Ceres Solver提供的自動求導功能直接為用戶避免了這個麻煩，因此在工程中使用Ceres后除非需要優化系統性能才會采用手動求導雅克比；

Ceres中提供了局部參數化的功能接口，也就是LocalParameteriztion對象，這對于求解旋轉變換相關問題有極大的幫助；

其他的諸如計算速度，求解器豐富等就不多提及了，下面開始展開細節

1. 基本概念

我看大多數博客都會用如下例子：
$$\underset{\mathbf{x}}{\min }\frac{1}{2}\sum _i{\rho }_i\left({\Vert f_i\left(x_{i_1},\dots ,x_{i_k}\right)\Vert }^2\right)$$$$\text{?s.t.?}{l}_j\le x_j\le u_j$$其中，公式中的各個部分對應的概念是：
${\rho }_i\left({\Vert f_i\left(x_{i_1},\dots ,x_{i_k}\right)\Vert }^2\right)$被稱為ResidualBlock，也就是殘差;
$\left\{x_{i_1},\dots ,x_{i_k}\right\}$被成為ParameterBlock，也就是輸入參數;
$f_i(?)$被稱為CostFunction，也就是代價函數，是根據輸入參數直接計算殘差的模塊；
${\rho }_i(?)$被稱為LossFunction，也就是損失函數或者魯棒核函數，通常用來減少輸入參數中外點影響的模塊；
以上都是Ceres中的定義，在其他不同的工具或者任務中定義可能不相同，Ceres所做的也就是根據輸入參數計算損失，并通過凸優化的方法將損失降到局部最小，關于凸優化中的基本原理就不在此補充，下面主要就Ceres各個模塊的用法進行介紹。

2. 基本方法

2.1 CostFunction

CostFunction負責計算殘差和雅克比矩陣，CostFuntion類代碼如下：

class CostFunction {public:virtual bool Evaluate(double const* const* parameters,double* residuals,double** jacobians) = 0;const vector<int32>& parameter_block_sizes();int num_residuals() const;protected:vector<int32>* mutable_parameter_block_sizes();void set_num_residuals(int num_residuals); };

Evaluate成員函數負責根據輸入的paramters計算residuals和jacobians，如果jacobians為nullptr，通常我們只需要在Evaluate函數中實現residuals的計算，jacobians后面可以通過Ceres提供的自動求導（數值求導）替代，否則，我們還需要在Evaluate函數中實現jacobians的計算，jacobians是一個大小為num_residuals * parameter_block_sizes_的行主數組，具體計算公式為：$$\text{jacobians}[i][r?\text{?parameter?block?sizes}[i]+c]=\frac{?\text{residual}[r]}{?\text{parameters}[i][c]}$$在CostFunction中用成員變量CostFunction::parameter_block_sizes_和CostFunction::num_residuals_分別用于記錄輸入parameters和residuals的尺寸，這兩個信息可以通過繼承該類后使用訪問其設置，或者通過Problem::AddResidualBlock()函數添加。

此外，如果paramters大小和residuals大小在編譯時是已知的，我們就可以使用SizeCostFunction，該函數可以將paramters大小和residuals大小設置為模板參數，用戶只需要在使用的時候給模板參數賦值就可以，如下：

template<int kNumResiduals, int... Ns> class SizedCostFunction : public CostFunction {public:virtual bool Evaluate(double const* const* parameters,double* residuals,double** jacobians) const = 0; };

2.2 AutoDiffCostFunction

該方法就是Ceres中提供的自動求導的方法，如果需要使用自動微分函數，必須在CostFunction中定義模板符號函數operator()，該函數根據paramters計算residuals，如下所示：

class MyScalarCostFunctor {MyScalarCostFunctor(double k): k_(k) {}template <typename T>bool operator()(const T* const x , const T* const y, T* e) const {e[0] = k_ - x[0] * y[0] - x[1] * y[1];return true;}private:double k_; };

然后帶自動微分功能的CostFunction構造函數如下：

CostFunction* cost_function= new AutoDiffCostFunction<MyScalarCostFunctor, 1, 2, 2>(new MyScalarCostFunctor(1.0)); ^ ^ ^| | |Dimension of residual ------+ | |Dimension of x ----------------+ |Dimension of y -------------------+

默認情況下，當AutoDiffCostFunction銷毀時，其對應的CostFunction也會銷毀，如果不希望存在這種從屬關系，可以在實例化AutoDiffCostFunction時，傳入參數DO_NOT_TAKE_OWNERSHIP，如下代碼所示：

MyScalarCostFunctor functor(1.0) CostFunction* cost_function= new AutoDiffCostFunction<MyScalarCostFunctor, 1, 2, 2>(&functor, DO_NOT_TAKE_OWNERSHIP);

AutoDiffCostFunction同時支持在運行時確定殘差維度，此時需要在實例化時傳入參數DYNAMIC，代碼如下所示：

CostFunction* cost_function= new AutoDiffCostFunction<MyScalarCostFunctor, DYNAMIC, 2, 2>(new CostFunctorWithDynamicNumResiduals(1.0), ^ ^ ^runtime_number_of_residuals); <----+ | | || | | || | | |Actual number of residuals ------+ | | |Indicate dynamic number of residuals --------+ | |Dimension of x ------------------------------------+ |Dimension of y ---------------------------------------+

AutoDIffCostFunction在編譯時就需要確定參數維度，但是在一些使用場景中需要在運行時才確定，那么這時就需要使用DynamicAutoDiffCostFunction，具體使用方法如下：

template <typename CostFunctor, int Stride = 4> class DynamicAutoDiffCostFunction : public CostFunction { };struct MyCostFunctor {template<typename T>bool operator()(T const* const* parameters, T* residuals) const {} }DynamicAutoDiffCostFunction<MyCostFunctor, 4>* cost_function =new DynamicAutoDiffCostFunction<MyCostFunctor, 4>(new MyCostFunctor()); cost_function->AddParameterBlock(5); cost_function->AddParameterBlock(10); cost_function->SetNumResiduals(21);

2.3 NumericDiffCostFuntion

NumericDiffCostFunction作用和AutoDiffCostFunction作用是相同的，二者唯一的區別是NumericDiffCostFunction中不再使用模板類型，而是使用double類型代替模板類型。谷歌推薦類型為AutoDiffCostFunction，C++模板的使用使得自動求導效率更高，而數值的差花費更多，容易出現數字錯誤，導致收斂速度變慢。

2.4 LossFunction

LossFunction就是上文提到的代價計算公式中的魯棒核函數${\rho }_i(?)$部分，用于減少輸入異常值對結果的影響，通常魯棒核函數${\rho }_i(?)$要求滿足$$\rho (0)=0$$$${\rho }^{\prime }(0)=1$$$${\rho }^{\prime }(s)<1$$$${\rho }^{\prime \prime }(s)<0$$具體地，在Ceres中提供了如下核函數：
TrivialLoss$$\rho (s)=s$$HuberLoss$$\rho (s)=\{\begin{array}{ll}s& s\le 1\\ 2\sqrt{s}?1& s>1\end{array}$$SoftLOneLoss$$\rho (s)=2(\sqrt{1+s}?1)$$CauchyLoss$$\rho (s)=\log (1+s)$$ArctanLoss$$\rho (s)=\arctan (s)$$TolerantLoss$$\rho (s,a,b)=b\log \left(1+e^{(s?a)/b}\right)?b\log \left(1+e^{?a/b}\right)$$這里我們來補充推導下魯棒核函數的作用原理，魯棒核函數是直接作用于殘差$f_i(\mathbf{x})$上，最小二乘的代價函數就變成如下形式：$$\underset{\mathbf{x}}{\min }\frac{1}{2}\sum _k\rho \left({\Vert f_k(\mathbf{x})\Vert }^2\right)$$將誤差的平方項記作$s_k={\Vert f_k(\mathbf{x})\Vert }^2$，則魯棒核函數進行二階泰勒展開有：$$\frac{1}{2}\rho (s)=\frac{1}{2}\left(\text{?const?}+{\rho }^{\prime }\Delta s+\frac{1}{2}{\rho }^{\prime \prime }{\Delta }^{2}s\right)$$其中$\Delta s_k$的計算如下：$$\begin{array}{rl}\Delta s_k& ={\Vert f_k(\mathbf{x}+\Delta \mathbf{x})\Vert }^2?{\Vert f_k(\mathbf{x})\Vert }^2\\ & \approx {\Vert f_k+{\mathbf{J}}_k\Delta \mathbf{x}\Vert }^2?{\Vert f_k(\mathbf{x})\Vert }^2\\ & =2f_k^{?}{\mathbf{J}}_k\Delta \mathbf{x}+(\Delta \mathbf{x}{)}^{?}{\mathbf{J}}_k^{?}{\mathbf{J}}_k\Delta \mathbf{x}\end{array}$$結合以上兩公式有$$\begin{array}{rl}\frac{1}{2}\rho (s)\approx & \frac{1}{2}({\rho }^{\prime }\left[2f_k^{?}{\mathbf{J}}_k\Delta \mathbf{x}+(\Delta \mathbf{x}{)}^{?}{\mathbf{J}}_k^{?}{\mathbf{J}}_k\Delta \mathbf{x}\right]\\ & +\frac{1}{2}{\rho }^{\prime \prime }{\left[2f_k^{?}{\mathbf{J}}_k\Delta \mathbf{x}+(\Delta \mathbf{x}{)}^{?}{\mathbf{J}}_k^{?}{\mathbf{J}}_k\Delta \mathbf{x}\right]}^2+\text{?const?})\\ \approx & {\rho }^{\prime }{f}_k^{?}{\mathbf{J}}_k\Delta \mathbf{x}+\frac{1}{2}{\rho }^{\prime }(\Delta \mathbf{x}{)}^{?}{\mathbf{J}}_k^{?}{\mathbf{J}}_k\Delta \mathbf{x}+{\rho }^{\prime \prime }(\Delta \mathbf{x}{)}^{?}{\mathbf{J}}_k^{?}{f}_k{f}_k^{?}{\mathbf{J}}_k\Delta \mathbf{x}+\text{const}\\ =& {\rho }^{\prime }{f}_k^{?}{\mathbf{J}}_k\Delta \mathbf{x}+\frac{1}{2}(\Delta \mathbf{x}{)}^{?}{\mathbf{J}}_k^{?}\left({\rho }^{\prime }I+2{\rho }^{\prime \prime }{f}_k{f}_k^{?}\right){\mathbf{J}}_k\Delta \mathbf{x}+\text{const}\end{array}$$對上式求和后再對$\Delta \mathrm{x}$進行求導，并令其等于零，有$$\sum _k{\mathbf{J}}_k^{?}\left({\rho }^{\prime }I+2{\rho }^{\prime \prime }{f}_k{f}_k^{?}\right){\mathbf{J}}_k\Delta \mathbf{x}=?\sum _k{\rho }^{\prime }{\mathbf{J}}_k^{?}{f}_k$$$$\sum _k{\mathbf{J}}_k^{?}W{\mathbf{J}}_k\Delta \mathbf{x}=?\sum _k{\rho }^{\prime }{\mathbf{J}}_k^{?}{f}_k$$熟悉優化算法的同學肯定注意到${\sum }_k{\mathbf{J}}_k^{?}W{\mathbf{J}}_k$部分就是$\mathbf{H}$矩陣，而${\sum }_k{\rho }^{\prime }{\mathbf{J}}_k^{?}{f}_k$部分就是$\mathbf{b}$向量，相對于沒有核函數的優化流程中，加入核函數只是在求解$\Delta \mathbf{x}$過程中在$\mathbf{b}$向量中加入了${\rho }^{\prime }$這一項，以上就是魯棒核函數作用與優化過程的原理推導。

2.5 LocalParameterization

LocalParameterization主要是用于優化過程中的表達形式的轉化，最典型的例子就是就是在Bundle Adjustment或者Pose Graph中對四元數的處理，我們都知道，四元數是對SO3的一種表達，用四元數進行旋轉變換的計算是方便的，但是用四個變量表達三個自由度時就必定存在另一個約束，也就是四元數的模長必須為1，因此如果我們使用四元數進行迭代優化的話，每進行一次迭代就需要對四元數進行一次歸一化操作，這樣顯然很麻煩，更加簡便的方法就是使用se3的表達方式李代數進行迭代優化，但是李代數對于計算旋轉變化的操作又不友好，綜上所述，我們最好的辦法就是在迭代優化和計算旋轉變換的過程中存在這么一個從四元數到李代數的轉換，這也就是Ceres為我們提供的LocalParameterization類，LocalParameterization類其實是一個接口類，像四元數到李代數這樣常見的變換，Ceres已經集成好了具體的實現，如EigenQuaternionParameterization類，該類是實現如下：

class CERES_EXPORT EigenQuaternionParameterization: public ceres::LocalParameterization {public:virtual ~EigenQuaternionParameterization() {}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; } };

除此之外，Ceres還提供了例如HomogeneousVectorParameterization、LineParameterization、ProductParameterization等一系列實現，當然也可以自己繼承LocalParameterization類來實現，我之前在VINS-Mono加入線特征的實驗中就有實現一個直線普呂克坐標的轉換，還是非常方便的。

2.6 Problem

Problem類主要用于構建優化問題，其中Problem::AddResidualBlock()用于向Problem中添加CostFunction和LossFunction，以及一系列Parameter Block，具體用法如下：

ResidualBlockId Problem::AddResidualBlock(CostFunction*, LossFunction*, const vector<double*> parameter_blocks); ResidualBlockId Problem::AddResidualBlock(CostFunction*, LossFunction*, double* x0, double* x1...);

此外Problem::AddParameterBlock()方法用于顯示地向Problem對象中添加Parameter Block，這里要注意的是Problem::AddResidualBlock()是不可以重復添加Parameter Block的，而Problem::AddParameterBlock()是可以的，并且當用戶想設置Parameter Block的LocalParameterization屬性時也是使用該方法添加的，具體如下：

void AddParameterBlock(double* values, int size, LocalParameterization* local_parameterization);

除了這兩個方法之外，Problem::SetParameterBlockConstant()用于設置Parameter Block為常量，也就是在迭代優化時不優化這一塊，這在Bundle Adjustment和Pose Graph中經常使用的一個屬性，最后Problem::SetParameterBlockVariable()用于刪除Parameter Block的常量屬性。除了以上介紹的四個方法外，Problem可以通過Options方法對進行進一步配置，具體的方法介紹就不在此介紹了，需要的同學可以去仔細閱讀下Ceres的官方文檔，在正常使用過程中有以上四個方法就基本足夠了。

2.7 Solver

Solver類主要通過Option方法對求解器進行配置，配置的內容主要包括求解器使用的優化方法和參數、線性求解器、預處理器等，這里不對配置的API進行過多的介紹了，我覺得需要了解的直接查文檔就好，這里主要對各個具體的方法進行簡單回顧

首先是求解器使用的優化方法，優化方法分為Linear Search Method和Trust Region Method，我們要解決的問題是：$$\arg \underset{x}{\min }\frac{1}{2}\Vert F(x){\Vert }^2$$我們對非線性代價函數進行線性化$$F(x+\Delta x)\approx F(x)+J(x)\Delta x$$我們要解決的問題便轉化為$$\underset{\Delta x}{\min }\frac{1}{2}\Vert J(x)\Delta x+F(x){\Vert }^2$$我們通過迭代$x\leftarrow x+\Delta x$來解決該問題，那么解決方法就分為：
Linear Search Method：
該方法的步驟主要如下：

給定一個初始化點$x$；

$\Delta x=?H^{?1}(x)g(x)$；

$\arg {\min }_{\mu }\frac{1}{2}\Vert F(x+\mu \Delta x){\Vert }^2$；

$x=x+\mu \Delta x$；

重復第二步；

其中，$\Delta H(x)=J(x{)}^{T}J(x)$， $g=?J^T(x)F(x)$，$\mu$為迭代步長，這其實就是最經典的Gauss-Newton法。

Trust Region Method：

給定一個初始點$x$和一個可信任區域半徑$\mu$

$\arg {\min }_{\mu }\frac{1}{2}\Vert F(x+\mu \Delta x){\Vert }^2$；

$x=x+\mu \Delta x$；

重復第二步；

其中，$\Delta H(x)=J(x{)}^{T}J(x)$， $g=?J^T(x)F(x)$，$\mu$為迭代步長，這其實就是最經典的
2. 求解帶約束問題
$\arg {\min }_{\Delta x}\frac{1}{2}\Vert J(x)\Delta x+F(x){\Vert }^2$
such that $\Vert D(x)\Delta x{\Vert }^2\le \mu$
$L\le x+\Delta x\le U$
3. 計算增益比$\rho =\frac{\Vert F(x+\Delta x){\Vert }^2?\Vert F(x){\Vert }^2}{\Vert J(x)\Delta x+F(x){\Vert }^2?\Vert F(x){\Vert }^2}$
4. 如果$\rho >?$則$x=x+\Delta x.$
5. 如果$\rho >{\eta }_1$則$\mu =2\mu$
6. 如果$\rho <{\eta }_2$則$\mu =0.5?\mu$
7. 重復第二步

其中，$\mu$為信任區域半徑，在Trust Region Method中，最關鍵的步驟就是解決帶約束的優化問題，也就是$$\begin{array}{rl}\arg \underset{\Delta x}{\min }& \frac{1}{2}\Vert J(x)\Delta x+F(x){\Vert }^2\\ \text{?such?that?}& \Vert D(x)\Delta x{\Vert }^2\le \mu \\ & L\le x+\Delta x\le U\end{array}$$解決該問題，Ceres又提供了兩種方法，即著名的Levenberg-Marquardt法和Dogleg法:
在Levenberg-Marquardt法中，將該帶約束的優化問題轉化為無約束形式：$$\arg \underset{\Delta x}{\min }\frac{1}{2}\Vert J(x)\Delta x+F(x){\Vert }^2+\frac{1}{\mu }\Vert D(x)\Delta x{\Vert }^2$$其中$D(x)$是一個非負對角矩陣，這里我們可以從另外一個角度來理解$D(x)$的作用，在Gauss-Newton法中，當$\Delta H$為不可逆矩陣時，關鍵步驟$\Delta x=?H^{?1}(x)g(x)$就將求解失敗，而如果我們將該式改寫為$$\left(J^T(x)J(x)+uI\right)h=?J^T(x)F(x)$$后，其中$I$為單位對角陣，那么矩陣$\left(J^T(x)J(x)+uI\right)$ 一定可逆，而這里的$I$就等價于公式中的$D(x)$。除此之外，我們再來分析參數$u=1/\mu$，當$u$足夠小時，Levenberg-Marquardt法即退化為Gauss-Newton法，當$u$足夠大時，Levenberg-Marquardt法即退化為更簡單的最速下降法，而在迭代過程中具體更加傾向于那種方法則有增益比$\rho$決定的。
在Dogleg法中，其實也是控制算法在Gauss-Newton法和最速下降法中切換，但是思路更加直接，是直接計算出Gauss-Newton法和最速下降法的結果，然后進行比較，具體流程可以參考我之前總結的一篇博客視覺SLAM總結——視覺SLAM面試題匯總中的第37題。

其次是線性求解器，線性求解器指的是求解求解問題$$H\Delta x=g$$Ceres Solver中主要提供了Dense OR、Dense Normal Cholesky、Sparse Normal Cholesky、Dense Schur、Sparse Schur等方法，其中QR分解就是將矩陣分解為一個三角矩陣和一個正交陣，如下$$\Delta x^{?}=?R^{?1}{Q}^{?}g$$其中$Q$為正交陣，$R$為上三角矩陣。Cholesky分解是將矩陣分解為一個上三角矩陣和一個下三角矩陣，如下：$$\Delta x^{?}=R^{?1}{R}^{??}g$$其中，求解三角陣可以采用對矩陣$J$進行OR分解，然后$J^{?}J=R^{?}{Q}^{?}QR=R^{?}R$即可獲得Cholescky分解形式。Schur求解通常是特指和Bundle Adjustment有關的求解方式，在Bundle Adjustment問題中，$H$矩陣通常具有如下形式：$$H=\left[\begin{array}{cc}B& E\\ E^{?}& C\end{array}\right]$$求解方程變為：$$\left[\begin{array}{cc}B& E\\ E^{?}& C\end{array}\right]\left[\begin{array}{c}\Delta y\\ \Delta z\end{array}\right]=\left[\begin{array}{c}v\\ w\end{array}\right]$$此時求解結果變為$$\Delta z=C^{?1}\left(w?E^{?}\Delta y\right)$$$$\left[B?E{C}^{?1}{E}^{?}\right]\Delta y=v?E{C}^{?1}w$$因為$C$是對角線矩陣，因此求解過程中最復雜的求逆過程就被簡化，是的計算過程復雜度大大減小。

2.8 Covariance

Convariance類主要是用于評估非線性最小二乘求解器返回的解的質量，也就是協方差，對于非線性回歸問題$$y=f(x)+N(0,I)$$我們對其求解獲得$$x^{?}=\arg \underset{x}{\min }\Vert f(x)?y{\Vert }^2$$此時$x^{?}$的協方差估計為$$C\left(x^{?}\right)={\left(J^{\prime }\left(x^{?}\right)J\left(x^{?}\right)\right)}^{?1}$$$J^{\prime }\left(x^{?}\right)$為函數$J$在$x^{?}$處的雅克比，當$J^{\prime }\left(x^{?}\right)$非滿秩時，可以根據Moore-Penrose逆給出$$C\left(x^{?}\right)={\left(J^{\prime }\left(x^{?}\right)J\left(x^{?}\right)\right)}^{?}$$以上公式的推導是默認協方差為單位陣，如果不符合這個條件，非線性回歸問題變為：$$y=f(x)+N(0,S)$$那么最大似然問題被求解為：$$x^{?}=\arg \underset{x}{\min }{f}^{\prime }(x)S^{?1}f(x)$$相應地$x^{?}$的協方差變為：$$C\left(x^{?}\right)={\left(J^{\prime }\left(x^{?}\right)S^{?1}J\left(x^{?}\right)\right)}^{?1}$$如下代碼展示了Convariance類的大致使用方法

在這里插入代碼片double x[3]; double y[2];Problem problem; problem.AddParameterBlock(x, 3); problem.AddParameterBlock(y, 2); <Build Problem> <Solve Problem>Covariance::Options options; Covariance covariance(options);vector<pair<const double*, const double*> > covariance_blocks; covariance_blocks.push_back(make_pair(x, x)); covariance_blocks.push_back(make_pair(y, y)); covariance_blocks.push_back(make_pair(x, y));CHECK(covariance.Compute(covariance_blocks, &problem));double covariance_xx[3 * 3]; double covariance_yy[2 * 2]; double covariance_xy[3 * 2]; covariance.GetCovarianceBlock(x, x, covariance_xx) covariance.GetCovarianceBlock(y, y, covariance_yy) covariance.GetCovarianceBlock(x, y, covariance_xy)

可以看到Ceres提供的接口是可以從求解的過程中直接獲得變量間的殘差，Convariance在我之前接觸的項目中還沒有使用到過，之后有機會要嘗試使用下。

以上就是我學習Ceres Document的一個筆記，因為時間原因寫得比較簡單，有問題歡迎隨時交流～

此外，對其他SLAM算法感興趣的同學可以看考我的博客SLAM算法總結——經典SLAM算法框架總結

總結

以上是生活随笔為你收集整理的Ceres Solver Document学习笔记的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。