PCL 实现 SAC_IA 算法原理源码解析

PCL 实现 SAC_IA 算法原理源码解析

采样一致性算法（SAC_IA）用于点云粗配准中，配准效果还不错，PCL 中也实现了该算法，本文深入 PCL 源码，学习一下 SAC_IA 算法在 PCL 中是如何实现的。

主要函数

SAC_IA 算法实现在头文件 ia_ransac.hpp 中，computeTransformation 实现了旋转矩阵的计算并进行迭代，内部调用了其他的一些功能函数。下面看一下各个函数是如何实现的。

computeTransformation

computeTransformation函数内部实现了迭代过程，并最终求取了迭代过程中误差最小的旋转矩阵。

参数：

• output：输出旋转后的点云
• guess：初始旋转矩阵（猜想矩阵），默认为单位阵

具体流程解析看注释：

template <typename PointSource, typename PointTarget, typename FeatureT> void 
pcl::SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess)
{
    // nr_samples_ 为用户设定的采样点数量，其值 >= 3
    // 定义 随机采样点索引 的容器
    std::vector<int> sample_indices (nr_samples_);
    // 定义 随机采样点对应点索引 的容器
    std::vector<int> corresponding_indices (nr_samples_);
    PointCloudSource input_transformed;	// 定义每次旋转的点云，主要作为临时点云
    float error, lowest_error (0);		// 定义误差

    final_transformation_ = guess;	// 将 guess 赋值给 final_transformation_（最小误差对应的旋转矩阵）
    int i_iter = 0;		// 定义迭代次数
    converged_ = false;
    if (!guess.isApprox (Eigen::Matrix4f::Identity (), 0.01f)) // 如果 guess 不是单位矩阵
    {
        // 将初始点云 input_ 根据 guess 做一次旋转，储存至临时点云 input_transformed
        transformPointCloud (*input_, input_transformed, final_transformation_);
        // 计算旋转后的误差
        lowest_error = computeErrorMetric (input_transformed, static_cast<float> (corr_dist_threshold_));
        i_iter = 1;  // 迭代次数加 1
    }
    
	// 开始迭代
    for (; i_iter < max_iterations_; ++i_iter)
    {
        // 选择随机点，将随机选出的点的索引存储到 sample_indices 中
        selectSamples (*input_, nr_samples_, min_sample_distance_, sample_indices);

        // 在目标云中找到相应的特征，将选择出的对应点的索引存储到 corresponding_indices 中
        findSimilarFeatures (*input_features_, sample_indices, corresponding_indices);

        // 估计从样本到其对应点的变换，计算旋转矩阵 transformation_
        transformation_estimation_->estimateRigidTransformation (*input_, sample_indices, *target_, corresponding_indices, transformation_);

        // 转换点云数据并计算误差
        transformPointCloud (*input_, input_transformed, transformation_);
        error = computeErrorMetric (input_transformed, static_cast<float> (corr_dist_threshold_));

        // 如果新误差更小，则更新 lowest_error 和 final_transformation_
        if (i_iter == 0 || error < lowest_error)
        {
            lowest_error = error;
            final_transformation_ = transformation_;
            converged_=true;
        }
    }

    // 使用 final_transformation_ 对点云进行旋转
    transformPointCloud (*input_, output, final_transformation_);
}

selectSamples

该函数用于选择随机采样点。

template <typename PointSource, typename PointTarget, typename FeatureT> void 
pcl::SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::selectSamples (
    const PointCloudSource &cloud, int nr_samples, float min_sample_distance, 
    std::vector<int> &sample_indices)
{
	// 如果采样点数量大于点云总数，错误返回
    if (nr_samples > static_cast<int> (cloud.points.size ()))
    {
        PCL_ERROR ("[pcl::%s::selectSamples] ", getClassName ().c_str ());
        PCL_ERROR ("The number of samples (%d) must not be greater than the number of points (%lu)!\n", nr_samples, cloud.points.size ());
        return;
    }

    // 反复抽取随机样本，直到达到 nr_samples 个
    // 定义找到一个有效点的查找次数
    int iterations_without_a_sample = 0;	
    // 定义找到一个有效点的查找次数的最大值
    int max_iterations_without_a_sample = static_cast<int> (3 * cloud.points.size ());
    sample_indices.clear ();
    while (static_cast<int> (sample_indices.size ()) < nr_samples)
    {
        // 随机选择一个点
        int sample_index = getRandomIndex (static_cast<int> (cloud.points.size ()));

        // 检查该点是否唯一并且与其他点有一定的距离
        bool valid_sample = true;
        for (size_t i = 0; i < sample_indices.size (); ++i)
        {
            float distance_between_samples = euclideanDistance (cloud.points[sample_index], cloud.points[sample_indices[i]]);

            if (sample_index == sample_indices[i] || distance_between_samples < min_sample_distance)
            {
                valid_sample = false;
                break;
            }
        }

        // 如果该点为有效点，那么就加入 sample_indices
        if (valid_sample)
        {
            sample_indices.push_back (sample_index);
            iterations_without_a_sample = 0;
        }
        else
            ++iterations_without_a_sample;		// 该点不是有效点，查找次数加 1

        // 如果找不到有效样本，则放宽样本间距离要求
        // 一个有效点的查找次数大于等于查找次数的最大值
        if (iterations_without_a_sample >= max_iterations_without_a_sample)
        {
            PCL_WARN ("[pcl::%s::selectSamples] ", getClassName ().c_str ());
            PCL_WARN ("No valid sample found after %d iterations. Relaxing min_sample_distance_ to %f\n", iterations_without_a_sample, 0.5*min_sample_distance);

            min_sample_distance_ *= 0.5f;
            min_sample_distance = min_sample_distance_;
            iterations_without_a_sample = 0;
        }
    }
}

findSimilarFeatures

该函数用于查询相似特征。

• input_features：输入为含有特征的点云
• sample_indices：：待查询近邻特征的索引
• corresponding_indices：保存查找到的对应点的索引

注意：

1. 查找的方式为 kdtree
2. 查找 K 个近邻特征匹配的点，然后从 K 个特征中随机选取一个点

template <typename PointSource, typename PointTarget, typename FeatureT> void 
pcl::SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::findSimilarFeatures (
    const FeatureCloud &input_features, const std::vector<int> &sample_indices, 
    std::vector<int> &corresponding_indices)
{
  std::vector<int> nn_indices (k_correspondences_);
  std::vector<float> nn_distances (k_correspondences_);

  corresponding_indices.resize (sample_indices.size ());
  for (size_t i = 0; i < sample_indices.size (); ++i)
  {
    // 找到最接近输入特征的 k 个特征
    feature_tree_->nearestKSearch (input_features, sample_indices[i], k_correspondences_, nn_indices, nn_distances);

    // 随机选择一个并将其添加到相应的索引中
    int random_correspondence = getRandomIndex (k_correspondences_);
    corresponding_indices[i] = nn_indices[random_correspondence];
  }
}

computeErrorMetric

该函数用于计算对应点之间的误差。

template <typename PointSource, typename PointTarget, typename FeatureT> float 
pcl::SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::computeErrorMetric (
    const PointCloudSource &cloud, float)
{
    std::vector<int> nn_index (1);
    std::vector<float> nn_distance (1);

    const ErrorFunctor & compute_error = *error_functor_;
    float error = 0;

    // 循环遍历 source 中的每一个点
    for (int i = 0; i < static_cast<int> (cloud.points.size ()); ++i)
    {
        // 在 target 点云中查找距离 cloud.points[i] 点的最近点
        tree_->nearestKSearch (cloud, i, 1, nn_index, nn_distance);

        // 误差累加
        error += compute_error (nn_distance[0]);
    }
    return (error);
}

总结

经过以上的分析，总结一下 SAC_IA 算法的主要流程：

1. 在 source 点云中随机选择 n 个采样点，n 个采样点有距离限制

2. 在 target 点云中查找采样点的对应点（特征最相近）

   使用 kdtree 方法查找与特征点对应的 K 个特征，然后从其中随机选择一个特征作为对应点

3. 根据选择的采样点与对应点求取旋转矩阵

4. 根据旋转矩阵计算两幅点云的误差，对比每次迭代结果误差，更新最小误差与最佳变换矩阵

5. 以上步骤不断迭代，直至迭代次数达到设定值

SAC_IA 算法也是基于 RANSAC 方法，SAC_IA 算法相对 RANSAC 方法而言，改进之处在于：

• 随机选择采样点：采样点数量更多，采样点之间的关系有距离限制
• 对应点查找：不是点与点距离的查找，而是特征与特征之间的匹配，特征描述符考虑了点的局部结构，相比计算点与点之间的距离，精度更加准确，但速度会降低

注：

以上代码中，由于直接截取了函数代码，有一些成员变量、函数看不到定义，理解起来有困难。