Ceres求解優化問題

1. 簡介

Ceres Solver是專門用于求解非線性最小二乘問題的C++開源庫,研究SLAM方向不過濾波和優化兩個技術路線,因此常用Ceres庫解決實際項目中的優化問題,當然還有g2o同樣可用,但就說明文檔而言,Ceres對新用戶更友好,g2o提供不多的文檔,更多是需要參考其它開源項目使用,所以筆者目前更加喜歡使用Ceres.
本文更多的站在一個SLAMer的角度講解Ceres的基本使用方法,如需要了解更多細節功能,請直接跳轉下面的官方文檔:

Ceres官方文檔-英文
Ceres說明文檔-中文

2. 基本使用流程

2.1 Problem

首先我們使用Ceres目的是解決最小二乘問題,所以必須創建一個最小乘問題的對象

ceres::Problem problem;

然后,要定義一個最小二乘問題無非就是需要兩部分內容:

待優化變量
誤差(cost function)

對應于圖優化其實就是圖頂點和邊,這是使用Ceres最關鍵的部分.

2.2 Cost Function

添加邊在Ceres中就是添加cost function.首先我們需要定義一個Cost Function類.這時候你需要考慮一個問題,自己推導雅可比矩陣還是使用Ceres的自動推導:

自動求導
解析導數

2.2.1 使用自動求導的Cost Function

自動導數只需提供誤差的計算方式即可

struct ExponentialResidual {// 提供一個構造函數方便傳入私有成員的初值ExponentialResidual();// 重載():Ceres會默認這個()函數計算誤差template <typename T>bool operator()(const T* const m, const T* const c, T* residual) const {residual[0] = y_ - exp(m[0] * x_ + c[0]);return true;}private:// 存放計算誤差時需要的私有成員
};

細心的讀者已經發現一個細節,重載的operator()是一個模板函數,Ceres正是使用這個模板類完成自動求導,其實現非常妙~關于自動求導的實現請往下閱讀.
對于使用自動求導的cost Function到此已經完成定義,下一步只需要添加到問題中即可:

double m = 0.0;
double c = 0.0;Problem problem;
for (int i = 0; i < kNumObservations; ++i) {CostFunction* cost_function =new AutoDiffCostFunction<ExponentialResidual, 1, 1, 1>(data[2 * i], data[2 * i + 1]);problem.AddResidualBlock(cost_function, nullptr, &m, &c);
}

這里解析一下AutoDiffCostFunction的模板參數:

第一個模板參數為誤差的維度
后續第n+1個模板參數分別對應第n個頂點(待優化變量)的維度

最后便是使用AddResidualBlock添加Cost Function即可.

2.2.2 使用解析導數的Cost Function

如果使用解析導數的方式,定義Cost Function時除了需要提供誤差的計算方式,當然還需要告知Ceres誤差關于待優化變量的雅可比計算方式.具體使用時需要構造一個繼承SizedCostFunction的子類,并重載Evaluate,Evaluate將提供返回誤差結果和雅可比結果,下面是官方的使用例子:

class Rat43Analytic : public SizedCostFunction<1,4> {public:Rat43Analytic(const double x, const double y) : x_(x), y_(y) {}virtual ~Rat43Analytic() {}virtual bool Evaluate(double const* const* parameters,double* residuals,double** jacobians) const {const double b1 = parameters[0][0];const double b2 = parameters[0][1];const double b3 = parameters[0][2];const double b4 = parameters[0][3];residuals[0] = ...;if (!jacobians) return true;double* jacobian = jacobians[0];if (!jacobian) return true;jacobian[0] = ...;jacobian[1] = ...;jacobian[2] = ...;jacobian[3] = ...;return true;}private:const double x_;const double y_;};

值得注意的是:

SizedCostFunction的模板函數和自動求導中的AutoDiffCostFunction類似,第一個模板參數對應誤差的維度,后面第n個分別對應第n個頂點的維度;
注意jacobians是指針的指針,jacobians表示指向所有頂點參數數組的地址,jacobians[i]表示指向第i個頂點參數數組的地址,jacobians[i][j]表示第i個頂點參數數組的第j個參數.注意上面的寫法,要對jacobians和jacobians[i]加以判斷,這說明Ceres在調用這個Evaluate成員函數時并一定需要計算雅可比,不加這個判斷會導致程序段錯誤.

最后同樣只需要添加至problem即可

param[4] = {b1, b2, b3, b4};
ceres::CostFunction* cost_function = new Rat43Analytic(x, y);
problem.AddResidualBlock(cost_function, nullptr, param);

2.3 待優化變量

不同于g2o,Ceres在實現上沒有將頂點和邊完全分離,具體而言是,在定義邊時已經關聯了頂點,一般情況無須自己定義頂點.但是Ceres考慮到用戶不同的需求,比如SLAM中常見的優化位姿問題,不同于普通向量空間,李群不能使用普通加減法,且由于自由度冗余不適合直接用于優化,因此Ceres向用戶提供了LocalParameterization.

class PoseSE3Parameterization: public ceres::LocalParameterization {
public:PoseSE3Parameterization() {}virtual ~PoseSE3Parameterization() {}virtual int GlobalSize();virtual int LocalSize();virtual bool Plus(const double* x, const double* delta, double* x_plus_delta) const;virtual bool ComputeJacobian(const double* x, double* jacobian) const;
};

使用LocalParameterization最基本需要重載4個成員方法:

GlobalSize需返回全局空間(不知道這么表達是否正確)的維度,對于SO3一般使用李代數表示,所以是4,對于SE3就是7,;
LocalSize需返回局部空間的維度,對于位姿就是流型的切空間維度,即對于so3的維度是3,對于se3的維度6;
Plus需提供全局空間的加法操作,上面提到之所以使用LocalParameterization就是因為這類待優化變量不能使用普通的加法,所以這一步告知Ceres如何對變量進行更新;
ComputeJacobian需提供全局空間關于局部空間的雅可比矩陣,不難猜測Ceres進行優化時先調用誤差中Evaluate計算誤差關于全局空間的雅可比,再調用這里的ComputeJacobian得到全局空間關于局部空間的雅可比,根據鏈式法則兩個直接相乘得到的就是誤差關于局部空間的雅可比.

這里先簡單介紹自定義待優化變量的使用,后文將以SLAM中常見的位姿優化問題為例介紹如何編寫Plus和ComputeJacobian.

到此已經自定義完成待優化變量,后面同樣只需要將其添加到Problem即可,如

problem.AddParameterBlock(param, 7, new PoseSE3Parameterization());

上面的7自然代表就是SE3的維度.

2.4 線性求解器

現在定義好了誤差和待優化變量,即確定了最小二乘問題,理論上可以進行優化了,但事實上還需要解決一個實際計算的問題,那便是如何求解線性方程:
$\boldsymbol{A}\boldsymbol{x} = \boldsymbol{b}$
顯然求 $\boldsymbol{A}$ 的逆是最直接的方法,但直接求逆是十分低效的手段;另外SLAM的優化問題一般具有稀疏性,為了更高效地求解上面的線性方程組,Ceres提供了多個選項.這里就直接引用博客:ceres solver里的線性求解器的解釋:

DENSE_QR: 用于小規模最小二乘問題的求解
DENSE_NORMAL_CHOLESKY&SPARSE_NORMAL_CHOLESKY：cholesky分解，用于具有稀疏性的大規模非線性最小二乘問題求解
DENSE_SCHUR&SPARSE_SCHUR: SCHUR分解，用于BA問題求解
ITERATIVE_SCHUR: 使用共軛梯度schur求解BA問題
CGNR: 使用共軛梯度法求解稀疏方程

用戶只需要通過ceres::Solver::Options選擇哪種求解器即可,例如使用DENSE_QR:

ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;

筆者目前而言沒有深入了解上面各個求解方法在實際應用中所帶來差異,但想在此強調的是要根據實際的優化問題合理選擇所需的求解器,例如SLAM中的Bundle Adjustment問題中 $\boldsymbol{A}$ 具有明顯的稀疏性,使用SCHUR消元再求解可以極大提高計算效率,具體詳看<視覺SLAM十四講-ch10>.

2.5 執行求解

最后只需要將上面構建的最小二乘Problem和配置好的Option傳入Solve函數即可,另外可以輸出優化信息,具體如下:

ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::cout << summary.FullReport() << std::endl;

2.6 其它

2.6.1 損失函數

上面例子中AddResidualBlock的第二個參數為nullptr即使用默認的二范數作為損失函數,為了避免一些異常值對優化的影響可以選用其它損失函數,例如:

problem.AddResidualBlock(cost_function, new CauchyLoss(0.5), param);

2.6.2 堆內存

細心的讀者可能發現,上面的實例代碼中邊和頂點都使用了堆內存來實例化,那計算完之后還需要主動delete嗎?看官方的例程后結論是不需要,說明Ceres::Problem自己在析構時會對其內存進行釋放.

2.7 小結

上面講解了如何使用Ceres解決一個實際的優化問題,根據以上的步驟能夠解決大多數的問題.然而,對于位姿到底如何自定義LocalParameterization?自動求導又為何看起來如此"智能"?下面繼續深入一點討論.

3. 使用Ceres實現位姿優化

3.1 重投影誤差

上面提到對于位姿這種流型空間的優化問題,以重投影誤差(看到這里我相信讀者也是做SLAM或相關,重投影誤差就不再多說了)為例介紹其邊的定義,待優化變量使用了四元數加位移的方法表達SE3,其中四元數放在前面,即 $q, t]^T$ , 增量se3則是 $[\phi, \rho]^T$

bool ReprojectionSe3::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{Eigen::Map<const Eigen::Quaterniond> q_cw(parameters[0]);Eigen::Map<const Eigen::Vector3d> t_cw(parameters[0] + 4);Eigen::Vector3d p_c = q_cw * mP3D + t_cw;double z_inv = 1.0 / p_c.z();residuals[0] = mP2D.x - mK.fx * p_c.x() * z_inv - mK.cx;residuals[1] = mP2D.y - mK.fy * p_c.y() * z_inv - mK.cy;if(jacobians) {if(jacobians[0]) {double z2_inv = z_inv * z_inv;Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor> > J_se3(jacobians[0]);J_se3.setZero();J_se3(0, 0) = mK.fx * p_c.x() * p_c.y() * z2_inv;J_se3(0, 1) =-mK.fx * (1 + p_c.x() * p_c.x() * z2_inv);J_se3(0, 2) = mK.fx * p_c.y() * z_inv;J_se3(0, 3) =-mK.fx * z_inv;J_se3(0, 4) = 0.0;J_se3(0, 5) = mK.fx * p_c.x() * z2_inv;J_se3(1, 0) = mK.fy * (1 + p_c.y() * p_c.y() * z2_inv);J_se3(1, 1) =-mK.fy * p_c.x() * p_c.y() * z2_inv;J_se3(1, 2) =-mK.fy * p_c.x() * z_inv;J_se3(1, 3) = 0.0;J_se3(1, 4) =-mK.fy * z_inv;J_se3(1, 5) = mK.fy * p_c.y() * z2_inv;}}return true;
}

雅可比的計算和<視覺SLAM14講>一模一樣,唯一不同是這里由于旋轉放在位移的前面,所以矩陣的前3列和后3列調換了.

3.2 重載自定義變量的成員方法

1.維度成員方法

int PoseSE3Parameterization::GlobalSize() const { return 7; }
int PoseSE3Parameterization::LocalSize() const { return 6; }

GlobalSize和LocalSize正如前面提到的分別對應SE3的維度和se3點維度,所以分別是7和6.

2.待優化變量更新成員方法

bool PoseSE3Parameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const
{Eigen::Map<const Eigen::Vector3d> trans(x + 4);Eigen::Quaterniond delta_q;Eigen::Vector3d delta_t;GetTransformFromSe3(Eigen::Map<const Eigen::Matrix<double,6,1>>(delta), delta_q, delta_t);Eigen::Map<const Eigen::Quaterniond> quater(x);Eigen::Map<Eigen::Quaterniond> quater_plus(x_plus_delta);Eigen::Map<Eigen::Vector3d> trans_plus(x_plus_delta + 4);quater_plus = delta_q * quater;trans_plus = delta_q * trans + delta_t;return true;
}

Plus操作提供了SE3點更新方法,注意:

x:應的是7維的SE3,即 $q, t]^T$ ;
delta就是解線性方程后得到的se3增量, 即 $[\phi, \rho]^T$ ;
x_plus_delta對應更新后的SE3;
GetTransformFromSe3實現了se3到SE3指數映射過程,具體可以參考F-LOAM

不難發現上面實則實現的左乘的更新操作:
$\boldsymbol{T}_{K+1} = \Delta\boldsymbol{T} * \boldsymbol{T}_k$

3.待優化變量的全局關于局部的雅可比成員方法:

bool PoseSE3Parameterization::ComputeJacobian(const double *x, double *jacobian) const
{Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> j(jacobian);(j.topRows(6)).setIdentity();(j.bottomRows(1)).setZero();return true;
}

看到這里我想大部分人都會疑惑,這一步似乎啥都沒做啊?
的確,這里根本沒做任何"有用"的操作,只是將前6維取出來,這是因為前面邊的雅可比Evaluate中已經完成了誤差關于se3點求導計算,但是維度是2x7,我們只需要在這一步取出前6維即可得到最終的2x6維度.
或許有人會質疑,為啥實現得如此不直觀?
事實上筆者已經從好幾個優秀的開源項目中看到過類似的實現,包括上面的實現其實出自F-LOAM;另一個方面,筆者作為從<視覺SLAM14講>入門的學生來說,直接對se3求導似乎更容易,反而要讓筆者計算關于四元數的雅可比會令我不知所措.
當然按照Ceres設計那樣分別提供 誤差關于SE3點雅可比 以及 SE3關于se3的雅可比 也絕對沒問題,想要了解該實現的歡迎跳轉筆者另一篇博客Ceres姿態優化.

4. 自動求導的背后原理

自動求導的理論依據是引入了二元數(Jet),可以將其理解為一個復數,有一個無窮小的分量:
$\delta$
以平方操作為例:
$x^2 = (a+\delta)^2 = a^2 + 2a\delta + \delta^2$
只保留一階無窮小則
$\approx a^2 + 2a\delta$
可見 $2 a$ 正是 $f (x)$ 在 $x = a$ 處的導數.不加證明地說(事實這個有點類似擾動,它的思想就是代入一個微小的擾動,取其系數代表該擾動對函數的影響程度),這個方法對于其它操作同樣適用,那么只要我們實現了二元數的各種運算操作(包括加減乘除指數冪)即可對大部分計算進行自動求導.
Ceres正是實現了二元數(Jet)這一種類:

template<int N> struct Jet {double a;Eigen::Matrix<double, 1, N> v;
};template<int N> Jet<N> operator+(const Jet<N>& f, const Jet<N>& g) {return Jet<N>(f.a + g.a, f.v + g.v);
}template<int N> Jet<N> operator-(const Jet<N>& f, const Jet<N>& g) {return Jet<N>(f.a - g.a, f.v - g.v);
}template<int N> Jet<N> operator*(const Jet<N>& f, const Jet<N>& g) {return Jet<N>(f.a * g.a, f.a * g.v + f.v * g.a);
}template<int N> Jet<N> operator/(const Jet<N>& f, const Jet<N>& g) {return Jet<N>(f.a / g.a, f.v / g.a - f.a * g.v / (g.a * g.a));
}template <int N> Jet<N> exp(const Jet<N>& f) {return Jet<T, N>(exp(f.a), exp(f.a) * f.v);
}// This is a simple implementation for illustration purposes, the
// actual implementation of pow requires careful handling of a number
// of corner cases.
template <int N>  Jet<N> pow(const Jet<N>& f, const Jet<N>& g) {return Jet<N>(pow(f.a, g.a),g.a * pow(f.a, g.a - 1.0) * f.v +pow(f.a, g.a) * log(f.a); * g.v);
}

自動求導的實現相當巧妙,但凡事有兩面性,自動求導也有缺點: