7. 【自动驾驶和机器人中的SLAM技术】基于点面ICP的iekf-lio系统

本文链接：https://blog.csdn.net/Walking_roll/article/details/135280290

1. 理论推导

在这里插入图片描述

2.代码实现

由于第7章已经有个icp_3d类了，并且有了点面ICP功能，这里直接在其基础上添加实现基于点面ICP的IEKF LIO系统对观测方程的处理方式，即：
void Icp3d::ComputeResidualAndJacobians(const SE3& input_pose, Mat18d& HTVH, Vec18d& HTVr)
在这里插入图片描述

对观测方程的处理代码实现（核心）：

void Icp3d::ComputeResidualAndJacobians(const SE3& input_pose, Mat18d& HTVH, Vec18d& HTVr) 
{
    LOG(INFO) << "aligning with point to plane";
    assert(target_ != nullptr && source_ != nullptr);
    SE3 pose = input_pose;

    int cnt_pts = source_->points.size();
    std::vector<int> index(cnt_pts);
    for (int i = 0; i < index.size(); ++i) 
        index[i] = i;

    std::vector<bool> effect_pts(index.size(), false);                    // 用于标记有效点
    std::vector<Eigen::Matrix<double, 1, 18>> jacobians(index.size());    // 用于存储雅可比矩阵
    std::vector<double> errors(index.size());                             // 用于存储残差
    // gauss-newton 迭代
    // 最近邻，可以并发
    std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&](int idx) {
        // 并发遍历到点云中的某个点，不是按顺序遍历的
        auto q = ToVec3d(source_->points[idx]);
        Vec3d qs = pose * q;  // 雷达系转换到IMU系：P_I = R_IL * P_L + T_IL

        std::vector<int> nn;
        kdtree_->GetClosestPoint(ToPointType(qs), nn, 5);  // 这里取5个最近邻
        if (nn.size() > 3) {
            // convert to eigen
            std::vector<Vec3d> nn_eigen;
            for (int i = 0; i < nn.size(); ++i) {
                nn_eigen.emplace_back(ToVec3d(target_->points[nn[i]]));
            }

            Vec4d n;
            if (!math::FitPlane(nn_eigen, n)) {
                // 失败的不要
                effect_pts[idx] = false;
                return;
            }

            double dis = n.head<3>().dot(qs) + n[3];
            if (fabs(dis) > options_.max_plane_distance_) {
                // 点离的太远了不要
                effect_pts[idx] = false;
                return;
            }

            effect_pts[idx] = true;

            // build residual
            Eigen::Matrix<double, 1, 18> J;
            J.setZero();
            J.block<1, 3>(0, 0) = n.head<3>().transpose();
            J.block<1, 3>(0, 6) = -n.head<3>().transpose() * pose.so3().matrix() * SO3::hat(q);
            

            jacobians[idx] = J;
            errors[idx] = dis;
        } else {
            effect_pts[idx] = false;
        }
    });

    double total_res = 0;
    int effective_num = 0;

    const double info_ratio = 0.01;  // 每个点反馈的info因子

    auto H_and_err = std::accumulate(
        index.begin(), index.end(), std::pair<Mat18d, Vec18d>(Mat18d::Zero(), Vec18d::Zero()),
        [&jacobians, &errors, &effect_pts, &total_res, &effective_num](const std::pair<Mat18d, Vec18d>& pre,
                                                                        int idx) -> std::pair<Mat18d, Vec18d> {
            if (!effect_pts[idx]) {
                return pre;
            } else {
                total_res += errors[idx] * errors[idx];
                effective_num++;
                return std::pair<Mat18d, Vec18d>(pre.first + jacobians[idx].transpose() * jacobians[idx] * info_ratio,
                                                pre.second - jacobians[idx].transpose() * errors[idx] * info_ratio);
            }
        });

        HTVH = H_and_err.first;
        HTVr = H_and_err.second;
        
     LOG(INFO) << "effective: " << effective_num;
}