优化库：Ceres & g2o

发表于 |

Ceres

使用Ceres求解非线性优化问题，主要分为三个部分：

  • 第一部分：构建代价函数Cost_Functor

    1
    2
    3
    4
    5
    6
    7
    8
    // 定义一个实例化时才知道的类型T
    template <typename T>

    // 运算符()的重载，用来得到残差fi
    bool operator()(const T* const x, T* residual) const {
         residual[0] = T(10.0) - x[0];
         return true;
       }

  • 第二部分：构建最小二乘问题problem

    1
    2
    3
    4
    5
    Problem problem;
    // 使用自动求导，第一个1是输出维度（残差项），第二个1是输入维度（优化项）
    CostFunction* cost_function = new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
    // 添加误差项，NULL表示不使用核函数，x是优化项
    problem.AddResidualBlock(cost_function, NULL, &x);

  • 第三部分：求解器参数配置Solver

    1
    2
    3
    4
    5
    6
    7
    Solver::Options options;
    options.linear_solver_type = ceres::DENSE_QR; //配置增量方程的解法，稠密的QR分解
    options.minimizer_progress_to_stdout = true;//输出到cout
    Solver::Summary summary;//优化信息
    Solve(options, &problem, &summary);//求解

    cout << summary.BriefReport() << "\n";//输出优化的简要信息

使用核函数：数据中往往存在离群点，离群点会对寻优结果造成影响，这时可以使用一些损失核函数来对离群点的影响加以消除，Ceres库中提供的核函数主要有：TrivialLoss 、HuberLoss、 SoftLOneLoss 、 CauchyLoss。 

1
2
// 使用核函数
problem.AddResidualBlock(cost_function, new CauchyLoss(0.5, &x);

without kernel

with kernel

g2o

用g2o优化库来进行优化的步骤如下：

  1. 定义节点和边的类型，通常在默认的基础类型上做修改

    定义顶点，顶点的基类为g2o::BaseVertex<优化变量维度，数据类型>

    1
    class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>

    顶点的更新函数oplusImpl：定义增量加法，因为优化变量和增量之间并不一定是线性叠加的关系，如位姿变换。

    定义边， 本例中的边为一元边，基类为g2o::BaseUnaryEdge<观测值维度，数据类型，连接顶点类型>

    1
    class CurveFittingEdge: public  g2o::BaseUnaryEdge<1, double , CurveFittingVertex>

    误差项计算函数computeError：计算预测值和观测值的误差。估计值是基于当前对优化变量的estimate计算出的，观测值是直接获取的，如本例中的y值。

  1. 构建图模型

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    15
    // vertex
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // camera pose
    pose->setId( index );
    pose->setEstimate( expression );
    optimizer.addVertex ( pose );

    // edge
    g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
    edge->setId ( index );
    edge->setVertex ( 0, point );
    edge->setVertex ( 1, pose );
    edge->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) );     // 导入观测值
    edge->setParameterId ( 0,0 );
    edge->setInformation ( Eigen::Matrix2d::Identity() );      // 设置信息矩阵
    optimizer.addEdge ( edge );

    信息矩阵edge->setInformation(信息矩阵)：因为最终的优化函数是$\sum e_i^T \Sigma^{-1}e_i$，是误差项和信息矩阵乘积的形式。

  2. 优化器配置

    • 矩阵块Block
    • 优化算法solver
    • 图模型optimizer

  3. 执行优化