多传感器融合作业1：

1。C++多态实现样文件的切换：

front_end.cpp中的initRegistration，匹配的子类选择，基类指针用哪个子类实例化可以由配置参数决定。

2。使用pcl中的ICP方法：

定义一个icp匹配方法对象，并初始化:

pcl::NormalDistributionsTransform<CloudData::POINT, CloudData::POINT>::Ptr icp_ptr_;

在ndt_registration.cpp中修改icp参数：

bool ICPRegistration::SetRegistrationParam(float max_cordistance, int max_iter, double trans_eps, int euc_fitnessEpsilon) {

    //设置最大对应点的欧式距离，只有对应点之间的距离小于该设置值的对应点才作为ICP计算的点对，默认值为：1.7976931348623157e+308，基本上对所有点都计算了匹配点
    icp_ptr_->setMaxCorrespondenceDistance(max_cordistance);
     //设置最大迭代次数，是第一个条件
    icp_ptr_->setMaximumIterations(max_iter);
    //设置前后两次迭代的转换矩阵的最大容差，一旦两次迭代小于这个最大容查，则认为已经收敛到最优解，迭代停止。是迭代终止的第二个条件，默认值为：0
    icp_ptr_->setTransformationEpsilon(trans_eps);
    //设置前后两次迭代的点对的欧式距离均值的最大容差，是迭代终止的第三个条件，默认值为：-std::numeric_limits::max()
    icp_ptr_->setEuclideanFitnessEpsilon(euc_fitnessEpsilon); 
   
    /*LOG(INFO) << "NDT 的匹配参数为：" << std::endl
              << "res: " << res << ", "
              << "step_size: " << step_size << ", "
              << "trans_eps: " << trans_eps << ", "
              << "max_iter: " << max_iter 
              << std::endl << std::endl;
*/
    return true;
}

在yaml文件中添加ICP参数：

ICP:
    max_cordistance : 0.05
    max_iter : 50
    trans_eps : 1e-8
    euc_fitnessEpsilon : 1

记得修改构造函数的参数：

ICPRegistration::ICPRegistration(float max_cordistance, int max_iter, double trans_eps, int euc_fitnessEpsilon)
    :icp_ptr_(new pcl::NormalDistributionsTransform<CloudData::POINT, CloudData::POINT>()) {

    SetRegistrationParam(max_cordistance, max_iter, trans_eps, euc_fitnessEpsilon);
}