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);
}

自动求导的实现相当巧妙,但凡事有两面性,自动求导也有缺点: