Ceres Solver是谷歌2010就开始用于解决优化问题的C++库,2014年开源.在Google地图,Tango项目,以及著名的SLAM系统OKVIS和Cartographer的优化模块中均使用了Ceres Solver.

  有关为何SLAM问题可以建模为最小二乘问题,进而使用最优化方法来求解,可以理解这一段话:

  • Maximum likelihood estimation (MLE) is a well-known estimation method used in many robotic and computer vision applications. Under Gaussian assumption, the MLE converts to a nonlinear least squares (NLS) problem. Efficient solutions to NLS exist and they are based on iteratively solving sparse linear systems until convergence.

  在SLAM领域优化问题还可以使用g2o来求解.不过Ceres提供了自动求导功能,虽然是数值求导,但可以避免复杂的雅克比计算,目前来看Ceres相对于g2o的缺点仅仅是依赖的库多一些(g2o仅依赖Eigen).但是提供了可以直接对数据进行操作的能力,相对于g2o应用在视觉SLAM中,更加倾向于通用的数值优化,最重要的是提供的官方资料比较全(看g2o简直受罪...).详细的介绍可以参考google的文档:http://ceres-solver.org/features.html

  优化问题的本质是调整优化变量,使得优化变量建模得出的估计值不断接近观测数据(使得目标函数下降),是最大似然框架下对优化变量的不断调整,得到优化变量建模得出的估计值在观测条件下的无偏估计过程.

  这里已知的是固定的观测数据z,以及需要调整的初始估计值x0.通常会建模成观测数据和估计值之间的最小二乘问题(但并不一定是最好的建模方式):

   $\mathop{\arg\min}_{x_{i,j}} \ \ \frac{1}{2}\sum_{i=1}^{m}\sum_{j=1}^{n}\left \|z_{i,j}-h(x{_{i,j}})) \right \| ^{2}$

一些概念:

  • ObjectiveFunction:目标函数;
  • ResidualBlock:残差(代价函数的二范数,有时不加区分),多个ResidualBlock组成完整的目标函数;
  • CostFunction:代价函数,观测数据与估计值的差,观测数据就是传感器获取的数据,估计值是使用别的方法获取(例如初始化,ICP,PnP或者匀速模型...)的从优化变量通过建模得出的观测值;例如从对极几何得到的相机位姿,三角化得到的地图点可以作为优化变量的初始值,但是需要利用坐标系变换和相机模型转化为2D平面上的像素坐标估计值,与实际测量得到的观测值之间构建最小二乘问题;
  • ParameterBlock:优化变量;
  • LossFunction:核函数,用来减小Outlier的影响,对应g2o中的edge->setRobustKernel()

求解步骤:

以寻找y=(10-x)的最小值为例

1.定义一个Functor(拟函数/函数对象)类,其中定义的是CostFunction. 需要重载函数调用运算符,从而可以像使用函数一样使用该类对象.(与普通函数相比,能够在类中存储状态,更加灵活)

  operator()的形参,前面几个对应 problem.AddResidualBlock(cost_function, NULL, &x);中最后一部分优化变量,最后一个对应残差

struct CostFunctor {
template <typename T>
bool operator()(const T* const x, T* residual) const {
residual[] = T(10.0) - x[];
return true;
}
};

这里模板参数T通常为double,在需要求residual的雅克比时,T=Jet

2. 建立非线性最小二乘问题,并使用Ceres Solver求解

int main(int argc, char** argv) {
google::InitGoogleLogging(argv[]); // The variable to solve for with its initial value.
double initial_x = 5.0;
double x = initial_x; // Build the problem.
Problem problem; // Set up the only cost function (also known as residual). This uses
// auto-differentiation to obtain the derivative (jacobian).
CostFunction* cost_function =
new AutoDiffCostFunction<CostFunctor, , >(new CostFunctor);
problem.AddResidualBlock(cost_function, NULL, &x); // Run the solver!
Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
options.minimizer_progress_to_stdout = true;
Solver::Summary summary;
Solve(options, &problem, &summary); std::cout << summary.BriefReport() << "\n";
std::cout << "x : " << initial_x
<< " -> " << x << "\n";
return ;
}

3. 参数选择

  在做Bundle Adjustment过程中,建立好优化问题后,需要对优化求解器进行一些参数设置:

Solver::Options options;
options.gradient_tolerance = 1e-;
options.function_tolerance = 1e-;
...
  • 梯度阈值 gradient_tolerance.
  • 相邻两次迭代之间目标函数之差 function_tolerance.
  • 梯度下降策略 trust_region_strategy 可选levenberg_marquardt,dogleg.
  • 线性增量方程 HΔx=g 求解方法 linear_solver 可选sparse_schur,dense_schur,sparse_normal_cholesky,视觉SLAM中主要采用稀疏Schur Elimination/ Marginalization的方法(也就是消元法),将地图点的增量边缘化,先求出相机位姿的增量,可以极大地较少计算量,避免H矩阵直接求逆.
  • 稀疏线性代数库 sparse_linear_algebra_library 可选suite_sparse,cx_sparse(ceres编译时需额外编译),cx_sparse相对suite_sparse,更精简速度较慢,但是不依赖BLAS和LAPACK.这个通常选择suite_sparse即可.
  • 稠密线性代数库 dense_linear_algebra_library 可选eigen,lapack.
  • 边缘化次序 ParameterBlockOrdering 设置那些优化变量在求解增量方程时优先被边缘化,一般会将较多的地图点先边缘化,不设置ceres会自动决定边缘化次序,这在SLAM里面常用于指定Sliding Window的范围.
  • 多线程 这里的设置根据运行平台会有较大不同,对计算速度的影响也是最多的.分为计算雅克比时的线程数num_threads,以及求解线性增量方程时的线程数num_linear_solver_threads.
  • 迭代次数 max_num_iterations,有时迭代多次均不能收敛,可能是初值不理想或者陷入了平坦的区域等等原因,需要设定一个最大迭代次数.

总结:

设置options细节较多,可以参考官方文档去设定:

  http://ceres-solver.org/nnls_solving.html#solver-options

通过实验发现除了多线程以及linear_solver_type,别的对优化性能和结果影响不是很大:

  实验代码在:https://github.com/xushangnjlh/xushang_SLAM;

对于93个相机位姿 61203个地图点 287451个观测数据的SFM优化问题:

  测试数据集http://grail.cs.washington.edu/projects/bal/final.html

边缘化的影响:

  • 边缘化所有地图点Total Time: 3.6005s 其中Linear solver:1.8905s
  • 边缘化2/3地图点Total Time: 5.4723s 其中Linear solver:3.7504s
  • 边缘化1/3地图点Total Time: 7.0223s 其中Linear solver:5.3335s

线程影响(在四核CPU上测试):

  • num_threads=1 Total Time:5.9113s
  • num_threads=2 Total Time:4.2645s
  • num_threads=4 Total Time:3.5785s
  • num_threads=8 Total Time:3.6472s

运行结果截图:

Ceres优化的更多相关文章

  1. ROSCon 2016视频和幻灯片发布 ROS机器人操作系统重要参考资料

    ROSCon 2016视频和幻灯片发布 By Tully Foote on 十月19,2016 7:28 AM 全部PPT下载地址:http://pan.baidu.com/s/1gf2sn2F RO ...

  2. cartographer 分析

    原文链接:http://blog.csdn.net/zyh821351004/article/details/52421005 cartographer与karto的比较 1. 两者采取的都是图优化框 ...

  3. VINS-mono详细解读

    VINS-mono详细解读 极品巧克力 前言 Vins-mono是香港科技大学开源的一个VIO算法,https://github.com/HKUST-Aerial-Robotics/VINS-Mono ...

  4. ROSCon 2017通知 Announcing ROSCon 2017: September 21st and 22nd in Vancouver

    ROSCon 2017通知:9月21日和22日在温哥华 我们很高兴地宣布,2017年ROSCon将在举行9月21-22日,2017年温哥华会议中心在加拿大温哥华.2017年IROS将在同一地点9月24 ...

  5. ROS会议 ROSCon 2017

    ----ROSCon2012-2017----来源链接:https://roscon.ros.org           近三年ROSCon(2015-2017)都会将会议视频录像和文档公开~以下为机 ...

  6. VINS(八)初始化

    首先通过imu预积分陀螺仪和视觉特征匹配分解Fundamental矩阵获取rotationMatrix之间的约束关系,联立方程组可以求得外参旋转矩阵: 接下来会检测当前frame_count是否达到W ...

  7. SLAM方向公众号、知乎、博客上有哪些大V可以关注?

    一.公众号 泡泡机器人:泡泡机器人由一帮热爱探索并立志推广机器人同时定位与地图构建(SLAM)技术的极客创办而成,通过原创文章.公开课等方式分享SLAM领域的数学理论.编程实践和学术前沿. ​ 经典文 ...

  8. cartographer 3D scan matching 理解

    cartographer 3D scan matching没有论文和其它资料,因此尝试通过源码理解其处理方法,理解不当之处还请指正. 目录: 0.2D 匹配方法简介 1.real time corre ...

  9. vins_fusion学习笔记

    Vins-Fusion源码:https://github.com/HKUST-Aerial-Robotics/VINS-Fusion 摘要 应项目需要,侧重学习stereo+gps融合 转载几篇写的比 ...

随机推荐

  1. 如何在Windows中安装GitHub

    1.使用Git 在Git官网下载Git安装包,双击开始安装即可.安装完毕之后,在Windows中会具有一个Git Bash命令行工具以及一个Git GUI客户端工具. 点击Download即可下载. ...

  2. RabbitMQ Windows环境搭建

    1.0 RabbitMQ是用Erlang语言编写,因此安装RabbitMQ,首先要安装Erlang. Erlang的官网:http://www.erlang.org/ 文件:otp_win64_19. ...

  3. ListView实现下拉刷新(二)隐藏头布局

    一.问题分析 在上一篇中,我们将头布局加到了ListView上.但是没有隐藏他.你可能会想,隐藏还不简单,直接给它设置为GONE属性不就可以了吗,在需要的时候再设定为可见.没错,这正是ListView ...

  4. MS17-010复现

    很早之前做的了,今天整理看到了,正好腾到blog上. ########################分割线############################## MS-17-010 攻击者向 ...

  5. ECMAScript6 Generator & async

    Generator Generator函数是一个状态机,执行后返回一个遍历器对象.调用遍历器对象的.next()函数获取下一个状态. Generator是一个普通的函数,函数内部使用yield关键字定 ...

  6. redis make报错

    解压redis后,第一次make出现报错: [root@localhost redis-3.2.5]# make cd src && make allmake[1]: Entering ...

  7. Python书单

    gitbook.jb51 1.Python基础教程 2.流畅的Python:总有论坛的人吐槽它翻译的还是不太友好,其实真正的差别没有那么大,重要的还是有所收获 3.<Python进阶>译本 ...

  8. 【译】为什么要写super(props)

    译注: 原文地址 https://overreacted.io/why-do-we-write-super-props/ 正文 我听说Hooks是新的热点.好笑的是,我想通过描述一些关于class组件 ...

  9. Python(一)数据结构和算法的20个练习题问答

    数据结构和算法 Python 提供了大量的内置数据结构,包括列表,集合以及字典.大多数情况下使用这些数据结构是很简单的. 但是,我们也会经常碰到到诸如查询,排序和过滤等等这些普遍存在的问题. 因此,这 ...

  10. oracle 12如何解锁账户锁定状态及修改忘记的密码

    有两种方法,大同小异吧,感觉命令真是个好东西,哈哈哈哈,挽救了我安了4次才安好的oracle!!! 方法一: 1.如果忘记密码,找到忘记密码的是哪个用户身份,如果用户被锁定,可以使用下面说的方法解除锁 ...