多传感器融合（1）——3D激光里程计

整体流程介绍

前端里程计方案

前端里程计方案——基于直接匹配

ICP系列——点到点ICP

这部分的需要了解，增加的式子为两坨点云的质心坐标，两坨点云分别减去自身的质心，实际上就是消除了两坨点云位置上的影响，剩下的只有姿态上的差异。

---

NDT系列——经典NDT

这里需要注意的是：我们直接使用上一帧计算出的均值和方差，带入到当前帧中进行计算概率，概率值越大说明匹配的程度越高

 

NDT系列——其他NDT

里程计工程框架实现

第一章作业扩展匹配算法NDT 和 ICP

首先在调用算法选择上，在提供一个icp的接口，仿照NDT的形式进行书写

---

 在配置文件中加入icp所需要的参数

---

具体编写icp算法文件：

---

ICPRegistration::ICPRegistration(const YAML::Node& node)
    :icp_ptr_(new pcl::IterativeClosestPoint()) {
    //导入icp的基本参数
    float max_dist = node["max_dist"].as();
    float trans_eps = node["trans_eps"].as();
    float eculi_eps = node["eculi_eps"].as();
    int max_iter = node["max_iter"].as();

    SetRegistrationParam(max_dist, trans_eps, eculi_eps, max_iter);
}

ICPRegistration::ICPRegistration(float max_dist, float trans_eps, float eculi_eps, int max_iter)
    :icp_ptr_(new pcl::IterativeClosestPoint()) {

    SetRegistrationParam(max_dist, trans_eps, eculi_eps, max_iter);
}


bool ICPRegistration::SetRegistrationParam(float max_dist, float trans_eps, float eculi_eps, int max_iter) {
    //设置最大对应点的欧式距离，只有对应点之间的距离小于该设置值的对应点才作为ICP计算的点对。
    //默认值为：1.7976931348623157e+308，基本上对所有点都计算了匹配点。 
    icp_ptr_->setMaxCorrespondenceDistance(max_dist);
    // 设置前后两次迭代的点对的欧式距离均值的最大容差，迭代终止条件之三，默认值为：-std::numeric_limits::max () 
    icp_ptr_->setEuclideanFitnessEpsilon(eculi_eps);
    //设置前后两次迭代的转换矩阵的最大容差（epsilion），
    //一旦两次迭代小于这个最大容差，则认为已经收敛到最优解，迭代停止。迭代停止条件之二，默认值为：0 
    icp_ptr_->setTransformationEpsilon(trans_eps);
    //设置最大的迭代次数。迭代停止条件之一 
    icp_ptr_->setMaximumIterations(max_iter);

    LOG(INFO) << "ICP 的匹配参数为：" << std::endl
              << "max_dist: " << max_dist << ", "
              << "eculi_eps: " << eculi_eps << ", "
              << "trans_eps: " << trans_eps << ", "
              << "max_iter: " << max_iter 
              << std::endl << std::endl;

    return true;
}

bool ICPRegistration::SetInputTarget(const CloudData::CLOUD_PTR& input_target) {
    icp_ptr_->setInputTarget(input_target);

    return true;
}

bool ICPRegistration::ScanMatch(const CloudData::CLOUD_PTR& input_source, 
                                const Eigen::Matrix4f& predict_pose, 
                                CloudData::CLOUD_PTR& result_cloud_ptr,
                                Eigen::Matrix4f& result_pose) {
    //上一帧点云
    icp_ptr_->setInputSource(input_source);
    //计算需要的刚体变换以便将输入的点云匹配到目标点云
    icp_ptr_->align(*result_cloud_ptr, predict_pose);
    //得到变换位姿
    result_pose = icp_ptr_->getFinalTransformation();

    return true;
}
}

 icp算法的调用：

当上面选择的是icp算法的时候，下面使用的就为icp算法。

registration_ptr_->ScanMatch(filtered_cloud_ptr, predict_pose, result_cloud_ptr_, current_frame_.pose);

扩展接口的形式：

采用多态的形式，设置接口的父类：

---

有新的算法加入时，用子类继承父类，这样可以保证整个程序的接口不变，只对算法部分进行替换即可，对于扩展来说很是方便

icp子类

ndt子类