Ceres 常用的优化手段

0. 简介

我们知道，在SLAM中，最主要也最关键的就是如何对我们设计出来的非线性问题进行求解，并计算出最优解。而在这个过程中我们用的最多的就是Ceres，之前作者在《​​SLAM本质剖析-Ceres​​》。而我们这一篇文章就来讨论一下如何使用Ceres来完成常见的优化。

1. Ceres中点到点的距离残差

下面的部分就是我们最常用的两个点到点的计算公式，其中​​Eigen_MAKE_ALIGNED_OPERATOR_NE​​W是一个Eigen库中的宏，用于启用对齐操作，以提高程序性能。

​​PointToPointError​​​函数用于构造函数，它初始化了​​m_point_lidar​​​（点云中的点），​​m_point_cam​​​（相机中的点）和​​m_w​​​（权重）变量。接下来是重载运算符，它接收一个旋转和平移向量（​​R_t​​​）作为参数，并计算出旋转矩阵（​​R​​）。它通过旋转矩阵和平移向量来计算投影点，并通过计算相机中的点和点云中的点的投影点的距离来计算残差，最后将残差乘以权重。

​​Create​​​函数用于创建costfunction。它接收点云中的点，相机中的点和权重作为参数，并创建一个自动微分代价函数，返回一个costfunction。最后是​​m_point_lidar​​​，​​m_point_cam​​​和​​m_w​​变量的定义，它们代表了点云中的点，相机中的点和权重。

// 点到点的距离损失
struct PointToPointError
{
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW 
    PointToPointError(const Eigen::Vector3d& point_lidar,
                    const Eigen::Vector3d& point_cam,
                    const double& w):
                    m_point_lidar(point_lidar),
                    m_point_cam(point_cam),
                    m_w(w){}//m_point_lidar代表了点云中的点，m_point_cam代表了相机中的点，m_w代表了权重

    template<typename T>
    bool operator()(const T* const R_t,T* residual) const{
        T rot[3*3];
        ceres::AngleAxisToRotationMatrix(R_t, rot);//将旋转向量转换为旋转矩阵
        Eigen::Matrix<T, 3, 3> R = Eigen::Matrix<T, 3, 3>::Identity();//初始化旋转矩阵
        for (int i = 0; i < 3; ++i) {
            for (int j = 0; j < 3; ++j) {
                R(i, j) = rot[i+j*3];//将旋转矩阵赋值给R
            }
        }
        Eigen::Map<const Eigen::Matrix<T,3,1>> trans(R_t+3);//将平移向量赋值给trans
        residual[0] = (m_point_cam.template cast<T>() - (R * m_point_lidar.template cast<T>() + trans)).norm();//通过获取相机中的点和点云中的点的投影点，计算残差
        residual[0] *= m_w;//乘以权重
        return true;
    }

    static ceres::CostFunction* Create(const Eigen::Vector3d& point_lidar,
                                    const Eigen::Vector3d& point_cam,
                                    const double& w){
        return (new ceres::AutoDiffCostFunction<PointToPointError, 1, 6>(
            new PointToPointError(point_lidar, point_cam, w)));//创建costfunction
    }

    const Eigen::Vector3d m_point_lidar;
    const Eigen::Vector3d m_point_cam;
    const double m_w;
};

// 点到点的距离损失，方法2，主要是通过
struct LidarDistanceFactor {

    LidarDistanceFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d closed_point_)
        : curr_point(curr_point_), closed_point(closed_point_) {}// curr_point代表当前点，closed_point代表前一帧平面上的一个点

    template <typename T>
    bool operator()(const T* q, const T* t, T* residual) const {
        Eigen::Quaternion<T> q_w_curr{q[3], q[0], q[1], q[2]};// 当前帧到世界坐标系的旋转
        Eigen::Matrix<T, 3, 1> t_w_curr{t[0], t[1], t[2]};// 当前帧到世界坐标系的平移
        Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};// 当前点
        Eigen::Matrix<T, 3, 1> point_w;// 当前点在世界坐标系下的位置
        point_w = q_w_curr * cp + t_w_curr;// 当前点在世界坐标系下的位置

        residual[0] = point_w.x() - T(closed_point.x());// 误差为当前点在世界坐标系下的位置的x坐标减去前一帧平面上的一个点的x坐标
        residual[1] = point_w.y() - T(closed_point.y());
        residual[2] = point_w.z() - T(closed_point.z());
        return true;
    }

    static ceres::CostFunction* Create(const Eigen::Vector3d curr_point_,
                                       const Eigen::Vector3d closed_point_) {
        return (new ceres::AutoDiffCostFunction<LidarDistanceFactor, 3, 4, 3>(
            new LidarDistanceFactor(curr_point_, closed_point_)));
    }

    Eigen::Vector3d curr_point;
    Eigen::Vector3d closed_point;
};
// ceres::CostFunction *cost_function = LidarDistanceFactor::Create(curr_point, center);
// problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);

2. Ceres中点到线的距离残差

​​struct LidarEdgeFactor​​​是一个Ceres的代价函数类，用于计算激光雷达数据的优化问题中的边界残差。它计算了当前点到前一帧的边的投影点的距离作为误差残差。该类包含了一个构造函数，一个重载的运算符"()"，以及一个静态的​​Create​​函数。

构造函数定义了四个变量：​​curr_point​​​表示当前点；​​last_point_a​​​代表前一帧的边上的第一个点；​​last_point_b​​代表前一帧的边上的第二个点；s代表当前点在前一帧边上的投影的位置。重载的运算符"()"定义了代价函数的计算过程，其中：

• q和t分别代表优化变量中的旋转和平移量
• residual数组存储了残差

该函数通过插值的方式计算前一帧到当前帧的旋转和平移，并通过矩阵运算将当前点变换到前一帧空间，最后计算当前点到前一帧边的投影点的距离。静态的​​Create​​​函数用于创建​​LidarEdgeFactor​​​类的实例，并返回一个指向​​ceres::AutoDiffCostFunction​​​的指针，它接受​​curr_point​​​、​​last_point_a​​​、​​last_point_b​​​和​​s​​作为参数。

struct LidarEdgeFactor {
    //计算3D点到直线的距离
    LidarEdgeFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_,
                    Eigen::Vector3d last_point_b_, double s_)
        : curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_), s(s_) {// curr_point代表当前点，last_point_a代表前一帧的边上的第一个点，last_point_b代表前一帧的边上的第二个点，s代表当前点在前一帧边上的投影的位置
    }

    template <typename T>
    bool operator()(const T* q, const T* t, T* residual) const {

        Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};// 当前点
        Eigen::Matrix<T, 3, 1> lpa{T(last_point_a.x()), T(last_point_a.y()), T(last_point_a.z())};// 前一帧边上的第一个点
        Eigen::Matrix<T, 3, 1> lpb{T(last_point_b.x()), T(last_point_b.y()), T(last_point_b.z())};// 前一帧边上的第二个点

        // Eigen::Quaternion<T> q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]};
        Eigen::Quaternion<T> q_last_curr{q[3], q[0], q[1], q[2]};// 前一帧到当前帧的旋转
        Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)};// 单位四元数
        q_last_curr = q_identity.slerp(T(s), q_last_curr);// slerp插值
        Eigen::Matrix<T, 3, 1> t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};// 前一帧到当前帧的平移

        Eigen::Matrix<T, 3, 1> lp;// 前一帧边上的投影点
        lp = q_last_curr * cp + t_last_curr;// 前一帧边上的投影点

        Eigen::Matrix<T, 3, 1> nu = (lp - lpa).cross(lp - lpb);// 当前点到前一帧边上的投影点的向量与前一帧边上的向量的叉乘
        Eigen::Matrix<T, 3, 1> de = lpa - lpb;// 前一帧边上的向量

        residual[0] = nu.x() / de.norm();// 误差为当前点到前一帧边上的投影点的向量与前一帧边上的向量的叉乘与前一帧边上的向量的模的比值
        residual[1] = nu.y() / de.norm();
        residual[2] = nu.z() / de.norm();

        return true;
    }

    static ceres::CostFunction* Create(const Eigen::Vector3d curr_point_,
                                       const Eigen::Vector3d last_point_a_,
                                       const Eigen::Vector3d last_point_b_, const double s_) {
        return (new ceres::AutoDiffCostFunction<LidarEdgeFactor, 3, 4, 3>( //残差维数，第一个变量维数，第二个变量维数
            new LidarEdgeFactor(curr_point_, last_point_a_, last_point_b_, s_)));
    }

    Eigen::Vector3d curr_point, last_point_a, last_point_b;
    double s;
};
// ceres::CostFunction* cost_function = LidarEdgeFactor::Create(curr_point, point_a, point_b, 1.0);
// problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);

3. Ceres中点到面的残差

下面两个为点到面的方程，这部分主要的是第二种方法用的比较多，我们下面来主要梳理一下第二种方法。这里主要命名了一个名为LidarPlaneNormFactor函数，。它的作用是在BA（Bundle Adjustment）中对激光雷达平面的法向量进行误差优化。

该类构造函数接受三个参数：curr_point_代表当前点，plane_unit_norm_代表前一帧平面的法向量，negative_OA_dot_norm_代表前一帧平面的法向量与前一帧平面上的一个点的点乘。

在该类中有一个模板函数operator()，它定义了误差函数：对于当前帧的位姿（q为旋转四元数，t为平移向量），误差为前一帧平面的法向量与当前点在世界坐标系下的位置的点乘。

最后，该类还提供了一个静态函数Create，用于创建残差块。

//计算激光点到平面的残差，以三个点为平面
struct LidarPlaneFactor {
    LidarPlaneFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_j_,
                     Eigen::Vector3d last_point_l_, Eigen::Vector3d last_point_m_, double s_)
        : curr_point(curr_point_), last_point_j(last_point_j_), last_point_l(last_point_l_),
          last_point_m(last_point_m_), s(s_) {// curr_point代表当前点，last_point_j代表前一帧的平面上的第一个点，last_point_l代表前一帧的平面上的第二个点，last_point_m代表前一帧的平面上的第三个点，s代表当前点在前一帧平面上的投影的位置
        ljm_norm = (last_point_j - last_point_l).cross(last_point_j - last_point_m);// 前一帧平面上的向量
        ljm_norm.normalize();// 前一帧平面上的向量的模
    }

    template <typename T>
    bool operator()(const T* q, const T* t, T* residual) const {

        Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};// 当前点
        Eigen::Matrix<T, 3, 1> lpj{T(last_point_j.x()), T(last_point_j.y()), T(last_point_j.z())};// 前一帧平面上的第一个点
        // Eigen::Matrix<T, 3, 1> lpl{T(last_point_l.x()), T(last_point_l.y()),
        // T(last_point_l.z())};
        // Eigen::Matrix<T, 3, 1> lpm{T(last_point_m.x()), T(last_point_m.y()),
        // T(last_point_m.z())};
        Eigen::Matrix<T, 3, 1> ljm{T(ljm_norm.x()), T(ljm_norm.y()), T(ljm_norm.z())};// 前一帧平面上的向量

        // Eigen::Quaternion<T> q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]};
        Eigen::Quaternion<T> q_last_curr{q[3], q[0], q[1], q[2]};// 前一帧到当前帧的旋转
        Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)};// 单位四元数
        q_last_curr = q_identity.slerp(T(s), q_last_curr);// 前一帧到当前帧的旋转
        Eigen::Matrix<T, 3, 1> t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};// 前一帧到当前帧的平移

        Eigen::Matrix<T, 3, 1> lp;// 前一帧边上的投影点
        lp = q_last_curr * cp + t_last_curr;// 当前点在前一帧的位置

        residual[0] = (lp - lpj).dot(ljm);// 误差为当前点到前一帧平面上的投影点的向量与前一帧平面上的向量的点乘

        return true;
    }

    static ceres::CostFunction* Create(const Eigen::Vector3d curr_point_,
                                       const Eigen::Vector3d last_point_j_,
                                       const Eigen::Vector3d last_point_l_,
                                       const Eigen::Vector3d last_point_m_, const double s_) {
        return (new ceres::AutoDiffCostFunction<LidarPlaneFactor, 1, 4, 3>(
            new LidarPlaneFactor(curr_point_, last_point_j_, last_point_l_, last_point_m_, s_)));
    }

    Eigen::Vector3d curr_point, last_point_j, last_point_l, last_point_m;
    Eigen::Vector3d ljm_norm;
    double s;
};

// 计算激光点到平面的残差，以平面的法向量为参数
struct LidarPlaneNormFactor {

    LidarPlaneNormFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d plane_unit_norm_,
                         double negative_OA_dot_norm_)
        : curr_point(curr_point_), plane_unit_norm(plane_unit_norm_),
          negative_OA_dot_norm(negative_OA_dot_norm_) {}// curr_point代表当前点，plane_unit_norm代表前一帧平面的法向量，negative_OA_dot_norm代表前一帧平面的法向量与前一帧平面上的一个点的点乘

    template <typename T>
    bool operator()(const T* q, const T* t, T* residual) const {
        Eigen::Quaternion<T> q_w_curr{q[3], q[0], q[1], q[2]};// 当前帧到世界坐标系的旋转
        Eigen::Matrix<T, 3, 1> t_w_curr{t[0], t[1], t[2]};// 当前帧到世界坐标系的平移
        Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};// 当前点
        Eigen::Matrix<T, 3, 1> point_w;
        point_w = q_w_curr * cp + t_w_curr;// 当前点在世界坐标系下的位置

        Eigen::Matrix<T, 3, 1> norm(T(plane_unit_norm.x()), T(plane_unit_norm.y()),
                                    T(plane_unit_norm.z()));// 前一帧平面的法向量
        residual[0] = norm.dot(point_w) + T(negative_OA_dot_norm);// 误差为前一帧平面的法向量与当前点在世界坐标系下的位置的点乘
        return true;
    }

    static ceres::CostFunction* Create(const Eigen::Vector3d curr_point_,
                                       const Eigen::Vector3d plane_unit_norm_,
                                       const double negative_OA_dot_norm_) {
        return (new ceres::AutoDiffCostFunction<LidarPlaneNormFactor, 1, 4, 3>(
            new LidarPlaneNormFactor(curr_point_, plane_unit_norm_, negative_OA_dot_norm_)));// 创建残差块
    }

    Eigen::Vector3d curr_point;
    Eigen::Vector3d plane_unit_norm;
    double negative_OA_dot_norm;
};
// ceres::CostFunction* cost_function = 
//     LidarPlaneNormFactor::Create(curr_point, norm, negative_OA_dot_norm);//点到平面的距离： 点与平面单位法向量的点乘
// problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);

4. Ceres中线特征误差

这里定义线特征的误差为一个结构体LineFeatureProjFactor。构造函数​​LineFeatureProjFactor​​用于初始化sp_，ep_，norm_vec_，direct_vec_，K_。在构造函数内部，通过计算获得KK_的值。