非线性优化工具学习记录 ceres 与 g2o

Ceres

• Ceres库主要用于求解无约束或者有界约束的最小二乘问题。

构建过程

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

• 构建优化问题（ceres::Problem）

• 构建代价函数（ceres::CostFunction）或残差（residual）

• 添加代价函数、核函数：通过 ceres::Problem::AddResidualBlock 添加代价函数（cost function）、核函数（loss function）和输入参数块（即待优化状态量块）， 核函数的意义主要是为了降低野点（outliers）对于解的影响

• 配置求解器（ceres::Solver::Options）

• 运行求解器（ceres::Solve(options, &problem, &summary)）

Example

构建 ceres 优化结构体

• operator() 重载用于计算残差，其中添加残差块时对于优化的参数输入类型为 数组指针 或者 vector 指针

struct ICP2dCOST {
    ICP2dCOST(Vec2d x, Vec2d y) : x_(x), y_(y){}

    template <typename T>
    bool operator()(const T* const pose, T* residuals) const {
        // pose是需要进行配准的scan的位姿
        
        T pw[2];

        pw[0] = T(x_[0]) * ceres::cos(pose[2]) - T(x_[1]) * ceres::sin(pose[2]) + pose[0];
        pw[1] = T(x_[0]) * ceres::sin(pose[2]) + T(x_[1]) * ceres::cos(pose[2]) + pose[1];

        residuals[0] =  pw[0] - T(y_[0]);
        residuals[1] =  pw[1] - T(y_[1]);
        return true;
    }

    const Vec2d x_, y_;
};


struct Point2PlaneCOST {
    Point2PlaneCOST(Vec2d x, Vec3d line_coeffs) : x_(x), line_coeffs_(line_coeffs){}

    template <typename T>
    bool operator()(const T* const pose, T* residuals) const {
        // pose是需要进行配准的scan的位姿
        
        T pw[2];

        pw[0] = T(x_[0]) * ceres::cos(pose[2]) - T(x_[1]) * ceres::sin(pose[2]) + pose[0];
        pw[1] = T(x_[0]) * ceres::sin(pose[2]) + T(x_[1]) * ceres::cos(pose[2]) + pose[1];

        residuals[0] =  line_coeffs_[0] * pw[0] + line_coeffs_[1] * pw[1] + line_coeffs_[2];

        return true;
    }

    const Vec2d x_;
    const Vec3d line_coeffs_;
};

配置基本流程

ceres::Problem problem;    // 构建优化问题
ceres::Solver::Options options;   // 相关配置
options.linear_solver_type = ceres::DENSE_QR;   // 解方程用的方法
options.max_num_iterations = 5;  // 优化时最大迭代次数
options.minimizer_progress_to_stdout = true;  // 是否打印优化信息

ceres::Solver::Summary summary;    // 优化结果

problem.AddResidualBlock(
    new ceres::AutoDiffCostFunction<ICP2dCOST, 2, 3>(
        new ICP2dCOST(pd, Vec2d(target_cloud_->points[nn_idx[0]].x, target_cloud_->points[nn_idx[0]].y))
    ),             // cost function  自动求导
    nullptr,     // 核函数
    pose           // 优化变量
);

G2O

• 先看上半部分。SparseOptimizer 是最终要维护的优化器。它是一个Optimizable Graph，从而也是一个Hyper Graph。一个 SparseOptimizer 含有很多个顶点 （都继承自 Base Vertex）和很多个边（继承自 BaseUnaryEdge（单顶点）, BaseBinaryEdge（双顶点）或BaseMultiEdge（多顶点））。这些 Base Vertex 和 Base Edge 都是抽象的基类，而实际用的顶点和边，都是它们的派生类。用 SparseOptimizer.addVertex 和 SparseOptimizer.addEdge 向一个图中添加顶点和边，最后调用 SparseOptimizer.optimize 完成优化。

• 在优化之前，需要指定求解器和迭代算法。从图中下半部分可以看到，一个 SparseOptimizer 拥有一个 Optimization Algorithm，继承自Gauss-Newton, Levernberg-Marquardt, Powell’s dogleg 之一。同时，这个 Optimization Algorithm 拥有一个Solver，它含有两个部分。一个是 SparseBlockMatrix ，用于计算稀疏的雅可比和海塞； 一个是用于计算迭代过程中最关键的一步 ** H Δx = − b**。这就需要一个线性方程的求解器。而这个求解器，可以从 PCG, CSparse, Choldmod 三者选一。

综上所述，在g2o中选择优化方法一共需要三个步骤：

• 选择一个线性方程求解器，从 PCG, CSparse, Choldmod中选，来自 g2o/solvers 文件夹中。

• 选择一个 BlockSolver。

• 选择一个迭代策略，从GN, LM, Doglog中选。

Example

顶点，继承自 BaseVertex

• ❤️, SE2> 表示该顶点维护的变量有三个维度，类型是 SE2

• _estimate 为顶点的估计值

• EIGEN_MAKE_ALIGNED_OPERATOR_NEW 是一个宏，用于确保Eigen库中的对象正确对齐。

• setToOriginImpl 用于设置顶点的估计值 _estimate

• oplusImpl 用于更新顶点的估计值，也就是增加时的公式

// 定义顶点
class VertexSE2 : public g2o::BaseVertex<3, SE2> {
   public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    void setToOriginImpl() override { _estimate = SE2(); }
    void oplusImpl(const double* update) override {
        _estimate.translation()[0] += update[0];
        _estimate.translation()[1] += update[1];
        _estimate.so2() = _estimate.so2() * SO2::exp(update[2]);
    }

    bool read(std::istream& is) override { return true; }
    bool write(std::ostream& os) const override { return true; }
};

// 实例化顶点
auto* v = new VertexSE2();
v->setId(0);   // 设置顶点的 ID
v->setEstimate(current_pose);  // 设置顶点的初始估计值 _estimate
optimizer.addVertex(v);   // 向优化器添加顶点

v->estimate(); // 获得 _estimate

边，BaseUnaryEdge, BaseBinaryEdge或BaseMultiEdge

• <2, Vec2d, VertexSE2> 表示残差（_error） 是 2 维，用 Vec2d 类型进行表示，该边连接一个顶点 VertexSE2

• computeError 定义 _error 的计算公式

• 自动求导，或自行设置雅可比矩阵

• linearizeOplus 设置雅可比矩阵

• setMeasurement 用于设置这个边约束的观测值。

• setInformation 设置约束的信息矩阵。在优化问题中，信息矩阵通常是观测值的协方差矩阵的逆。单位矩阵的逆还是单位矩阵，所以这里设置的信息矩阵表示所有的观测值都有相同的权重，且没有相关性。不同问题不同设置方式。

// 定义边
class EdgeSE2Point2Point : public g2o::BaseUnaryEdge<2, Vec2d, VertexSE2> {
   public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    EdgeSE2Point2Point(const Vec2d& point)
        : point_(point){}

    void computeError() override {
        VertexSE2* v = (VertexSE2*)_vertices[0];
        SE2 pose = v->estimate();
        Vec2d pw = pose * point_;
        _error = pw - measurement();
    }

    bool read(std::istream& is) override { return true; }
    bool write(std::ostream& os) const override { return true; }

   private:
    Vec2d point_;
};

// 实例化边
auto* edge = new EdgeSE2Point2Point(pd);
edge->setId(j);  // 设置边的序号
edge->setVertex(0, v);   // 设置该边的顶点以及序号
edge->setMeasurement(Vec2d(target_cloud_->points[nn_idx[0]].x, target_cloud_->points[nn_idx[0]].y));  // 设置边约束的观测值。
edge->setInformation(Mat2d::Identity());  // 设置约束的信息矩阵
optimizer.addEdge(edge);

// 优化
optimizer.setVerbose(true);   // 打印信息
optimizer.initializeOptimization();
optimizer.optimize(5);    // 优化多少次