手写ICP-SVD

ICP简介

Iterative Closest Point迭代最近点算法；

分别在待匹配的目标点云P和源点云Q中，按照一定的约束条件，找到最邻近点（pi，qi），然后计算出最优匹配参数R和t，使得误差函数最小。误差函数为E(R,t)为

其中R和t分别为两帧点云的旋转与平移矩阵；通过不断的迭代求解出两帧点云相对平移与旋转

流程如下：

ICP求解过程

1、构造误差函数

2、化简

 将误差函数拆解成两部分，一部分是只与旋转有关，另一部分与旋转和平移有关，所以可以先求旋转再求平移

3、旋转矩阵求解

 

SVD分解参考

奇异值分解（SVD） - 知乎

 

4、平移矩阵求解

 

 代码实现

1、获取对应点

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)
    {
        //并且设置存储点的编号及点的距离的Vector对象
        std::vector<int>corr_ind;
        std::vector<float> corr_sq_dis;
        //在input_target_kdtree中找input_source查找点i的1个最近点
         input_target_kdtree_->nearestKSearch(input_source->at(i),1,corr_ind,corr_sq_dis);
        //如果最近点的距离大于阈值那么放弃该点
        if(corr_sq_dis.at(0)>MAX_CORR_DIST_SQR)
            continue;
        //目标点云对应点
        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;
}

细节解释

//在input_target_kdtree中找input_source查找点i的1个最近点
         input_target_kdtree_->nearestKSearch(input_source->at(i),1,corr_ind,corr_sq_dis);
        //如果最近点的距离大于阈值那么放弃该点
        if(corr_sq_dis.at(0)>MAX_CORR_DIST_SQR)
            continue;

KD-tree最近邻查找

最邻近点和kd树详解 - 灰信网（软件开发博客聚合）

2、获取变换矩阵(旋转和平移矩阵)

void ICPSVDRegistration::GetTransform(
    const std::vector<Eigen::Vector3f> &xs,
    const std::vector<Eigen::Vector3f> &ys,
    Eigen::Matrix4f &transformation_
) {
    const size_t N = xs.size();
    //点云中心
    Eigen::Vector3f mu_x(0,0,0);
    Eigen::Vector3f mu_y(0,0,0);
    //x-x'
    Eigen::MatrixXf A (3,N),B(3,N);
    for(auto i=0;i<N;i++)
    {
        A.block<3,1>(0,i)=xs[i];
        B.block<3,1>(0,i)=ys[i];
    }
     mu_x = A.rowwise().mean();
     mu_y = B.rowwise().mean();
    A.colwise()   -=  mu_x;
    B.colwise()   -=  mu_y;
    Eigen::Matrix3f H,R;
    Eigen::Vector3f t;
    //PPT  P15
    H=B*A.transpose();
    LOG(INFO)<<"H:"<<H<<std::endl;
    //svd分解
    Eigen::JacobiSVD<Eigen::MatrixXf>svd(H,Eigen::ComputeFullU | Eigen::ComputeFullV);
    R=svd.matrixV()*svd.matrixU().transpose();
    //求解t平移矩阵，ppt18业
    t=mu_x-R*mu_y;
    transformation_.setZero();
    transformation_.block<3,3>(0,0)=R;
    transformation_.block<3,1>(0,3)=t;
    transformation_(3,3)=1;
    // TODO: find centroids of mu_x and mu_y:


}

细节解释

 //点云中心
    Eigen::Vector3f mu_x(0,0,0);
    Eigen::Vector3f mu_y(0,0,0);

    mu_x = A.rowwise().mean();
    mu_y = B.rowwise().mean();

 A.colwise()   -=  mu_x;
 B.colwise()   -=  mu_y;

 H=B*A.transpose();

Eigen::JacobiSVD<Eigen::MatrixXf>svd(H,Eigen::ComputeFullU | Eigen::ComputeFullV);
R=svd.matrixV()*svd.matrixU().transpose();

 t=mu_x-R*mu_y;

 

 3、匹配

bool ICPSVDRegistration::ScanMatch(
    const CloudData::CLOUD_PTR& input_source, 
    const Eigen::Matrix4f& predict_pose, 
    CloudData::CLOUD_PTR& result_cloud_ptr,
    Eigen::Matrix4f& result_pose
) {
    input_source_ = input_source;

    // pre-process input source:
    CloudData::CLOUD_PTR transformed_input_source(new CloudData::CLOUD());
    pcl::transformPointCloud(*input_source_, *transformed_input_source, predict_pose);

    // init estimation:
    transformation_.setIdentity();
    
    //
    // TODO: first option -- implement all computing logic on your own
    //
    // do estimation:
    int curr_iter = 0;
    while (curr_iter < max_iter_) {
        // TODO: apply current estimation:
        CloudData::CLOUD_PTR curr_input_source(new CloudData::CLOUD());
        pcl::transformPointCloud(*transformed_input_source,*curr_input_source,transformation_);
        // TODO: get correspondence:获取对应点
        std::vector<Eigen::Vector3f> xs;
        std::vector<Eigen::Vector3f> ys;
        // TODO: do not have enough correspondence -- break:
        //如果没有足够多的对应点直接break
        if(GetCorrespondence(curr_input_source,xs,ys)<3)
        {
            break;
        }
        // TODO: update current transform:
        //根据svd求解出的变换矩阵，更新变换矩阵
        Eigen::Matrix4f delta_transformation;
        GetTransform(xs,ys,delta_transformation);
        // TODO: whether the transformation update is significant:
        //当变换矩阵的平移矩阵和旋转矩阵大于给定数值时才满足要求
        if(!IsSignificant(delta_transformation,trans_eps_))
            break;
        // TODO: update transformation:
        transformation_=delta_transformation*transformation_;
        //迭代次数加1，当超过迭代次数时退出
        ++curr_iter;
    }

    // set output:
    result_pose = transformation_ * predict_pose;
    Eigen::Quaternionf qr(result_pose.block<3,3>(0,0));// 首先将变换矩阵拆分为一个旋转矩阵（实际使用四元数表示）
    qr.normalize();//归一化
    Eigen::Vector3f t=result_pose.block<3,1>(0,3);
    result_pose.block<3,3>(0,0)=qr.toRotationMatrix();//将四元数转换成旋转矩阵
    result_pose.block<3,1>(0,3)=t;
    //result_pose是两个点云的变换矩阵，此函数通过变换矩阵，变换两个点云
    pcl::transformPointCloud(*input_source_, *result_cloud_ptr, result_pose);
    
    return true;
}