Ceres用法及Ceres-Sophus在位姿图优化问题的应用

最新推荐文章于 2024-04-09 10:17:48 发布

牛犇犇子木

最新推荐文章于 2024-04-09 10:17:48 发布

阅读量2.7k

点赞数 7

文章标签： slam 算法

本文链接：https://blog.csdn.net/ljtx200888/article/details/114278683

版权

本文介绍了Ceres Solver用于非线性优化的基本步骤，包括构建Cost Function、添加Residual Block和配置优化参数。Ceres与g2o在优化方式上的差异在于它不直接支持信息矩阵操作。同时，文章详细阐述了Ceres-Sophus在位姿图优化问题中的应用，涉及残差和雅克比矩阵的推导，并提供了代码实现的思路。讨论了如何在Ceres中设置固定节点的问题，参考了相关博客资源。

摘要由CSDN通过智能技术生成

（一）Ceres-Solver的一般用法

简述：

Ceres Solver is an open source C++ library for modeling and solving large, complicated optimization problems.

使用 Ceres Solver 求解非线性优化问题，主要包括以下几部分：

构建代价函数（cost function）或残差（residual）
构建优化问题(ceres::Problem)：通过 AddResidualBlock 添加代价函数(cost function)、损失函数(loss function核函数) 和待优化状态量
配置求解器(ceres::Solver::Options)
运行求解器(ceres::Solve(options, &problem, &summary))

注意：与g2o优化中直接在边上添加information（信息矩阵）不同，Ceres无法直接实现此功能。由于Ceres只接受最小二乘优化就，即 $min(r^{^{T}}r)$ ;若要对残差加权，使用马氏距离即 $min (r^{^{T}}\Sigma r)$ ,则要对信息矩阵（协方差矩阵） $\Sigma$ 做Cholesky分解： $LL^{^{T}}=\Sigma$ ，则 $r^{^{T}}\Sigma r=r^{^{T}}(LL^{^{T}})r=(L^{^{T}}r)^{T}(L^{T}r)$ ，令 $r'=L^{T}r$ ,最终得到纯种的最小二乘优化： $min(r'^{^{T}}r')$ 。

Ceres优化步骤：

Step1：构建Cost Function

（1）AutoDiffCostFunction（自动求导）

构造 代价函数结构体或代价函数类（例如：struct CostFunctor），在其结构体内对模板括号() 重载用来定义残差；
在重载的 () 函数形参中，最后一个为残差，前面几个为待优化状态量，注意：形参类型必须是模板类型。

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

利用ceres::CostFunction *cost_function=new ceres::AutoDiffCostFunction<functype,dimension1,dimension2>(new costFunc)，获得Cost Function

模板参数分别是：代价结构体仿函数类型名，残差维度，第一个优化变量维度，第二个....；函数参数（new costFunc）是代价结构体（或类）对象，即此对象重载的 () 函数的函数指针

（2）NumericDiffCostFunction（数值求导）

数值求导法 也是构造 代价函数结构体，也需要重载括号()来定义残差，但在重载括号() 时没有用模板；

struct CostFunctorNum {
    bool operator()(const double *const x, double *residual) const {
        residual[0] = 10.0 - x[0]; // r(x) = 10 - x
        return true;