ICP（二）手动实现ICP

一、ICP在PCL库中的使用

参数设置：

1. setMaximumIterations， 最大迭代次数，icp是一个迭代的方法，最多迭代这些次；
2. setEuclideanFitnessEpsilon， 设置收敛条件是均方误差和小于阈值， 停止迭代
3. setTransformtionEpsilon, 设置两次变化矩阵之间的最小差值（一般设置为1e-10即可，差值较小则停止迭代）；
4. setMaxCorrespondenaceDistance,设置对应点对之间的最大距离（此值对配准结果影响较大）

配准：

    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>::Ptr icp_ptr_;
    icp_ptr_->SetInputTarget(input_target);          //  输入目标点云
    icp_ptr_->setInputSource(input_source);          //  输入待配准点云
    icp_ptr_->align(*result_cloud_ptr, predict_pose);      // 配准              
    result_pose = icp_ptr_->getFinalTransformation();    // 获取变换矩阵 

二、手动实现ICP

0. 参数设置

    LOG(INFO) << "ICP SVD params:" << std::endl
              << "max_corr_dist: " << max_corr_dist_ << ", "// 认为是近邻点的阈值
              << "trans_eps: " << trans_eps_ << ", " //终止迭代的条件 
              << "euc_fitness_eps: " << euc_fitness_eps_ << ", "
              << "max_iter: " << max_iter_  // 最大迭代次数
              << std::endl << std::endl;

2. 构建kd-tree

pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr input_target_kdtree_;

3. 获取目标点云（在里程计中为上一帧点云）input_target,并将其放入kd-tree中

input_target_kdtree_->setInputCloud(input_target_);

4. 获取源点云（当前帧点云）input_source,和其预测位姿predict_pose
   关于预测位姿的获得方法：匀速运动模型
   有上上一帧点点云的位姿$T_{L_{0}W}$，上一帧点云的位姿为$T_{L_{1}W}$，我们就能获得上一次的$ICP$配准结果为$T_{L_1{L}_0}$，将当前帧的预测位姿设定为$T_{L_{2}W}^{pre}=T_{L_1{L}_0}{T}_{L_{1}W}$
5. 将input_source变换到预测位姿上得到transformed_input_source

    CloudData::CLOUD_PTR transformed_input_source(new CloudData::CLOUD());
    pcl::transformPointCloud(*input_source_, *transformed_input_source, predict_pose);

6. 设置初始迭代步长

transformation_.setIdentity();

7. 将transformed_input_source用迭代的步长transformation_进行更新，获得curr_input_source

  CloudData::CLOUD_PTR curr_input_source(new CloudData::CLOUD());
  pcl::transformPointCloud(*transformed_input_source, *curr_input_source, transformation_);

8. 用最近临搜索kd-tree找到最近邻关联点

std::vector<Eigen::Vector3f> xs, ys;
if (GetCorrespondence(curr_input_source,xs,ys) <  3) break;

size_t ICPSVDRegistration::GetCorrespondence(
    const CloudData::CLOUD_PTR &input_source, 
    std::vector<Eigen::Vector3f> &xs,
    std::vector<Eigen::Vector3f> &ys
) {
    const float MAX_CORR_DIST_SQR = max_corr_dist_ * max_corr_dist_;

    size_t num_corr = 0;

    // TODO: set up point correspondence
    for(size_t  i =0; i < input_source->points.size(); ++i){
                std::vector<int>  corr_ind;    // index
                std::vector<float>  corr_sq_dis;     // correspondence_square_dis 
                input_target_kdtree_->nearestKSearch(
                        input_source->at(i),
                        1,
                        corr_ind, corr_sq_dis
                );       // kdtree  搜索
                
                if(corr_sq_dis.at(0) >  MAX_CORR_DIST_SQR)
                    continue;
                
            // add  correspondence:
            Eigen::Vector3f x(
                        input_target_->at(corr_ind.at(0)).x,
                        input_target_->at(corr_ind.at(0)).y,
                        input_target_->at(corr_ind.at(0)).z
            );
            Eigen::Vector3f y(
                        input_source->at(i).x,
                        input_source->at(i).y,
                        input_source->at(i).z
            );
            xs.push_back(x);
            ys.push_back(y);
            
            ++num_corr;
    }

    return num_corr;
}

8. 计算R，t

Eigen::Matrix4f  delta_transformation;
GetTransform(xs, ys, delta_transformation);

void ICPSVDRegistration::GetTransform(
    const std::vector<Eigen::Vector3f> &xs,
    const std::vector<Eigen::Vector3f> &ys,
    Eigen::Matrix4f &transformation_
) {
    const size_t N = xs.size();

    // find centroids of mu_x and mu_y:
    Eigen::Vector3f mu_x = Eigen::Vector3f::Zero();
    Eigen::Vector3f mu_y = Eigen::Vector3f::Zero();
    for (size_t i = 0; i < N; ++i) {
        mu_x += xs.at(i);
        mu_y += ys.at(i);
    }
    mu_x /= N; 
    mu_y /= N;

    // build H:
    Eigen::Matrix3f H = Eigen::Matrix3f::Zero();
    for (size_t i = 0; i < N; ++i) {
        H += (ys.at(i) - mu_y) * (xs.at(i) - mu_x).transpose();
    }

    // solve R:
    Eigen::JacobiSVD<Eigen::MatrixXf> svd(H, Eigen::ComputeThinU | Eigen::ComputeThinV);
    Eigen::Matrix3f R = svd.matrixV() * svd.matrixU().transpose();

    // solve t:
    Eigen::Vector3f t = mu_x - R * mu_y;

    // set output:
    transformation_.setIdentity();
    transformation_.block<3, 3>(0, 0) = R;
    transformation_.block<3, 1>(0, 3) = t;
}

9. 判断是否达到终止条件

if(!IsSignificant(delta_transformation, trans_eps_)) break;

bool ICPSVDRegistration::IsSignificant(
    const Eigen::Matrix4f &transformation,
    const float trans_eps
) {
    // a. translation magnitude -- norm:
    float translation_magnitude = transformation.block<3, 1>(0, 3).norm();
    // b. rotation magnitude -- angle:
    float rotation_magnitude = fabs(
        acos(
            (transformation.block<3, 3>(0, 0).trace() - 1.0f) / 2.0f
        )
    );

    return (
        (translation_magnitude > trans_eps) || 
        (rotation_magnitude > trans_eps)
    );
}

10. 更新总步长

transformation_ = delta_transformation *  transformation_;

11. 对7～10步进行max_iter_次迭代

迭代结束的条件：

• 达到最大迭代次数max_iter_；
• 迭代步长小与设定阈值trans_eps_。

12. 更在最终配准位姿result_pose

result_pose = transformation_ * predict_pose;

13. 对结果进行归一化并将input_source变换到最终配准的位姿上

    Eigen::Quaternionf  qr(result_pose.block<3,3>(0,0));
    qr.normalize();
    Eigen::Vector3f  t  = result_pose.block<3,1>(0,3);
    result_pose.setIdentity();
    result_pose.block<3,3>(0,0) = qr.toRotationMatrix();
    result_pose.block<3,1>(0,3) = t;
    pcl::transformPointCloud(*input_source_, *result_cloud_ptr, result_pose);

三、效果展示

未完续待~

四、关于ICP的一些问题

1. ICP最少需要几对点进行求解？
   答：三对，刚性变换矩阵中涉及到六个未知数α、β、γ、 tx、ty、tz。要唯一确定这六个未知参数，需要六个线性方程（三个求旋转和三个求平移），即至少需要在待匹配点云重叠区域找到3组对应点对，且3组对应点对不能共线，才可以得到这几个未知数的值，进而完成刚性矩阵的参数估计。通常情况下，人们会选择尽可能多的对应点对，进一步提高刚性变换矩阵的参数估计精度。
2. ICP的优缺点
   （1）ICP 选取的所有点进行配准，速度较慢；
   （2）易受到噪声干扰，陷入局部极值；
   （3）要剔除不合适的点对（点对距离过大、包含边界点的点对）
   （4）基于点对的配准，并没有包含局部形状的信息
   （5）ICP 方法需要配准的两个点云具有一定的重叠度；

参考资料

PCL之——点云配准算法ICP