手写RANSAC实现点云粗配准

使用RANSAC算法实现点云粗配准

摘要

点云配准是3D数据处理中的一个关键步骤，其目的是将来自不同视角或时间的点云数据对齐到同一个坐标系下。配准算法通常分为粗配准（Coarse Registration）和精配准（Fine Registration）。精配准算法（如ICP）对初始位置非常敏感，需要一个较好的初始变换。本文将详细介绍如何使用RANSAC（随机抽样一致性）算法来解决点云的粗配准问题，为后续的精配准提供一个鲁棒的初始位姿。

一、 RANSAC 算法原理

1.1 核心思想

RANSAC（Random Sample Consensus）是一种迭代算法，用于从一组包含大量“局外点”（Outliers）的观测数据中，估计出某个数学模型的参数。其核心思想是，数据中包含正确的“局内点”（Inliers）和错误的“局外点”，局内点能够被某个模型所拟合，而局外点则不能。算法通过随机抽样来寻找最优模型，具有很强的抗噪声能力。

1.2 算法流程

RANSAC算法的输入是一组观测数据、一个用于拟合数据的参数化模型以及一些置信度参数。其目标是通过迭代找到最优的模型及其对应的局内点集。

基本步骤如下：

1. 随机采样：从整个数据集中随机选择拟合模型所需的最小样本子集。对于3D刚体变换，需要至少3对不共线的对应点。
2. 模型拟合：使用这个最小样本子集来计算模型的参数。在点云配准中，就是计算一个刚体变换矩阵（旋转矩阵 $R$ 和平移向量 $t$）。
3. 一致性检查 (共识)：将数据集中所有其他点应用上一步计算出的模型，并检查它们与模型的拟合程度。如果一个点与模型的误差小于预设的阈值，则将其归类为“局内点”（Inlier）。
4. 模型评估：如果本次迭代中找到的局内点数量足够多（超过某个阈值），则认为这个模型是一个不错的候选模型。
5. 模型优化：使用当前找到的所有局内点，重新计算模型参数，得到一个更精确的模型。
6. 迭代与更新：重复以上步骤固定的次数。在每次迭代结束后，保留拥有最多局内点数量的最佳模型。

最终，算法输出的是具有最高共识（最多局内点）的模型参数。

二、 基于RANSAC的点云配准流程

在点云配准任务中，我们通常已经通过特征匹配（如FPFH、SHOT等）得到了一组包含错误匹配的初始对应点对。RANSAC的目标就是从这些嘈杂的对应点对中，筛选出正确的匹配并计算出最佳的变换矩阵。

具体流程如下：

1. 随机选取三对点：在所有初始对应点对中，随机选取3对点。这3对点构成了计算变换矩阵的最小样本集。
2. 计算变换矩阵：基于这3对对应点，使用SVD（奇异值分解）等方法求解出一个刚体变换矩阵 $T$（包含旋转 $R$ 和平移 $t$）。
3. 验证并统计局内点：将此变换矩阵 $T$ 应用于源点云中的所有点，并计算变换后的点与目标点云中对应点之间的距离误差。若误差小于设定的阈值（distance_threshold），则将该对应点对标记为“局内点”，并统计局内点的总数。
4. 循环迭代：重复以上步骤，直到达到预设的最大迭代次数。在每次迭代后，都记录下当前模型（变换矩阵 $T$）的局内点数量。
5. 确定最佳模型：选择在所有迭代中产生最多局内点的变换矩阵作为最终的最佳模型。所有被这个最佳模型识别为“局内点”的对应点对，被认为是正确的匹配。

这个最佳变换矩阵就是我们需要的粗配准结果，它可以作为ICP等精配准算法的优秀初始值。

三、 RANSAC算法的优点

1. 鲁棒性强：RANSAC对包含大量局外点（错误匹配）的数据集具有极强的鲁棒性，这是其最核心的优势。
2. 无需良好初值：与ICP等算法不同，RANSAC不需要一个接近最终结果的初始变换估计，可以直接从一个混乱的对应点集中找到正确的变换关系。
3. 适用性广：RANSAC不依赖于点云的局部几何特征，因此可用于特征稀疏或重复性结构较多的场景。

四、 C++与PCL代码实现

下面是使用PCL（Point Cloud Library）库实现RANSAC粗配准的C++代码示例。

代码逻辑解析

这段代码的核心逻辑是模拟RANSAC的过程：

• 随机采样：在源点云中随机选择3个点，并要求它们之间有足够的距离，以避免退化配置（例如三点共线）。
• 寻找对应关系：在目标点云中，通过几何约束（点对之间的距离）来寻找与源点云中3个采样点可能对应的点。这是一种简化的、不依赖特征描述子的对应关系查找方法。
• 模型估计与评估：使用找到的3对对应点计算变换矩阵，然后通过一个**适应度分数（Fitness Score）**来评估这个变换的好坏。适应度分数是通过计算变换后的源点云与目标点云之间的平均距离得到的，分值越小，代表配准效果越好。
• 迭代更新：在多次迭代中，保留适应度分数最低的那个变换矩阵作为最终结果。

完整代码

// RansacRegistration.cpp
//
// 这是一个使用RANSAC思想进行点云粗配准的示例。
// 它通过随机采样和几何约束来找到最佳的刚体变换。
//

#include <iostream>
#include <vector>
#include <string>
#include <ctime>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/registration/transformation_estimation_svd.h>
#include <pcl/common/transforms.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>

using namespace std;
using namespace pcl;

// 定义点云类型
typedef PointXYZ PointT;
typedef PointCloud<PointT> PointCloudT;

// 计算两点之间的欧氏距离
double pointDistance(const PointT& p1, const PointT& p2)
{
    return sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2) + pow(p1.z - p2.z, 2));
}

// 检查随机选取的三个点是否分布得足够开，避免退化情况
bool threePointsDistanceCheck(const PointT& p1, const PointT& p2, const PointT& p3, double min_dist)
{
    if (pointDistance(p1, p2) < min_dist) return false;
    if (pointDistance(p1, p3) < min_dist) return false;
    if (pointDistance(p2, p3) < min_dist) return false;
    return true;
}

// 点云可视化函数
void visualize_pcd(PointCloudT::Ptr pcd_src, PointCloudT::Ptr pcd_tgt, PointCloudT::Ptr pcd_final)
{
    pcl::visualization::PCLVisualizer viewer("RANSAC Registration Viewer");
    viewer.setBackgroundColor(0, 0, 0); // 设置背景为黑色

    // 定义源点云、目标点云和配准后点云的颜色
    pcl::visualization::PointCloudColorHandlerCustom<PointT> src_h(pcd_src, 0, 255, 0);     // 绿色
    pcl::visualization::PointCloudColorHandlerCustom<PointT> tgt_h(pcd_tgt, 255, 0, 0);     // 红色
    pcl::visualization::PointCloudColorHandlerCustom<PointT> final_h(pcd_final, 0, 0, 255); // 蓝色

    // 添加点云到可视化窗口
    viewer.addPointCloud(pcd_src, src_h, "source cloud");
    viewer.addPointCloud(pcd_tgt, tgt_h, "target cloud");
    viewer.addPointCloud(pcd_final, final_h, "final cloud");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "source cloud");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "target cloud");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "final cloud");

    while (!viewer.wasStopped())
    {
        viewer.spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }
}

int main(int argc, char** argv)
{
    // --- 1. 加载源点云和目标点云 ---
    PointCloudT::Ptr cloud_src(new PointCloudT);
    PointCloudT::Ptr cloud_tgt(new PointCloudT);

    if (pcl::io::loadPCDFile<PointT>("e:\\data\\office1.pcd", *cloud_src) == -1) {
        PCL_ERROR("Couldn't read source file.\n");
        return -1;
    }
    if (pcl::io::loadPCDFile<PointT>("e:\\data\\office2.pcd", *cloud_tgt) == -1) {
        PCL_ERROR("Couldn't read target file.\n");
        return -1;
    }
    
    // --- 2. RANSAC配准参数初始化 ---
    int n_iterations = 2000;       // 最大迭代次数
    double min_sample_distance = 1.0; // 随机采样点之间的最小距离 (单位：米)
    double correspondence_distance_threshold = 0.1; // 对应点对距离容忍度 (单位：米)
    double fitness_score_threshold = 2.0; // 评估模型的距离阈值 (单位：米)

    int n_src_points = cloud_src->points.size();
    int n_tgt_points = cloud_tgt->points.size();

    double min_fitness_score = std::numeric_limits<double>::max();
    Eigen::Matrix4f final_transformation = Eigen::Matrix4f::Identity();

    srand((unsigned int)time(0));

    // --- 3. RANSAC主循环 ---
    for (int iter = 0; iter < n_iterations; ++iter)
    {
        cout << "--- Iteration: " << iter + 1 << " ---" << endl;

        // --- 3.1 在源点云中随机选择3个点 ---
        vector<int> src_indices;
        while (src_indices.size() < 3) {
            int rand_idx = rand() % n_src_points;
            // 避免重复选择
            if (find(src_indices.begin(), src_indices.end(), rand_idx) == src_indices.end()) {
                src_indices.push_back(rand_idx);
            }
        }

        // 检查三点是否共线或距离过近
        if (!threePointsDistanceCheck(cloud_src->points[src_indices[0]], cloud_src->points[src_indices[1]], cloud_src->points[src_indices[2]], min_sample_distance)) {
            continue;
        }

        // 计算源点云中采样点之间的距离
        double dist_src_01 = pointDistance(cloud_src->points[src_indices[0]], cloud_src->points[src_indices[1]]);
        double dist_src_02 = pointDistance(cloud_src->points[src_indices[0]], cloud_src->points[src_indices[2]]);
        double dist_src_12 = pointDistance(cloud_src->points[src_indices[1]], cloud_src->points[src_indices[2]]);

        // --- 3.2 在目标点云中寻找满足几何约束的对应点 ---
        vector<int> tgt_indices;
        // 随机选择第一个目标点
        tgt_indices.push_back(rand() % n_tgt_points);

        // 寻找第二个和第三个目标点
        vector<int> potential_p1_indices, potential_p2_indices;
        for (int i = 0; i < n_tgt_points; ++i) {
            if (abs(pointDistance(cloud_tgt->points[tgt_indices[0]], cloud_tgt->points[i]) - dist_src_01) < correspondence_distance_threshold) {
                potential_p1_indices.push_back(i);
            }
        }
        if (potential_p1_indices.empty()) continue;
        tgt_indices.push_back(potential_p1_indices[rand() % potential_p1_indices.size()]);
        
        for (int i = 0; i < n_tgt_points; ++i) {
            if (abs(pointDistance(cloud_tgt->points[tgt_indices[0]], cloud_tgt->points[i]) - dist_src_02) < correspondence_distance_threshold &&
                abs(pointDistance(cloud_tgt->points[tgt_indices[1]], cloud_tgt->points[i]) - dist_src_12) < correspondence_distance_threshold) {
                potential_p2_indices.push_back(i);
            }
        }
        if (potential_p2_indices.empty()) continue;
        tgt_indices.push_back(potential_p2_indices[rand() % potential_p2_indices.size()]);

        // --- 3.3 计算变换矩阵 ---
        PointCloudT::Ptr src_sample(new PointCloudT);
        PointCloudT::Ptr tgt_sample(new PointCloudT);
        for(int idx : src_indices) src_sample->points.push_back(cloud_src->points[idx]);
        for(int idx : tgt_indices) tgt_sample->points.push_back(cloud_tgt->points[idx]);

        registration::TransformationEstimationSVD<PointT, PointT> TESVD;
        Eigen::Matrix4f transformation;
        TESVD.estimateRigidTransformation(*src_sample, *tgt_sample, transformation);

        // --- 3.4 评估模型 ---
        PointCloudT::Ptr transformed_cloud(new PointCloudT);
        transformPointCloud(*cloud_src, *transformed_cloud, transformation);

        KdTreeFLANN<PointT> kdtree;
        kdtree.setInputCloud(cloud_tgt);
        double fitness_score = 0.0;
        int inlier_count = 0;
        
        for (const auto& point : transformed_cloud->points)
        {
            vector<int> nn_indices(1);
            vector<float> nn_dists(1);
            kdtree.nearestKSearch(point, 1, nn_indices, nn_dists);
            if (nn_dists[0] <= fitness_score_threshold * fitness_score_threshold)
            {
                fitness_score += nn_dists[0];
                inlier_count++;
            }
        }
        
        if (inlier_count > 0) {
            fitness_score = fitness_score / inlier_count;
        } else {
            fitness_score = std::numeric_limits<double>::max();
        }

        // --- 3.5 更新最佳模型 ---
        if (fitness_score < min_fitness_score)
        {
            min_fitness_score = fitness_score;
            final_transformation = transformation;
            cout << "Found a better model with fitness score: " << min_fitness_score << endl;
        }
    }

    // --- 4. 应用最终变换并输出结果 ---
    PointCloudT::Ptr final_cloud(new PointCloudT);
    transformPointCloud(*cloud_src, *final_cloud, final_transformation);
    
    cout << "\nFinal Transformation Matrix:" << endl;
    cout << final_transformation << endl;
    cout << "Min Fitness Score: " << min_fitness_score << endl;

    // 保存配准后的点云
    io::savePCDFileASCII("e:\\OutputCloud.pcd", *final_cloud);

    // --- 5. 可视化结果 ---
    visualize_pcd(cloud_src, cloud_tgt, final_cloud);

    return 0;
}

五、 结果展示

下图展示了配准前后的效果。其中，绿色点云为源点云，红色点云为目标点云，蓝色点云为经过RANSAC粗配准后对齐的源点云。可以看出，即使初始位姿相差很大，RANSAC算法也成功地找到了一个很好的对齐位置。

图：配准结果（绿色: 源点云, 红色: 目标点云, 蓝色: 对齐后的源点云）

六、 总结

RANSAC是一种非常强大且鲁棒的算法，尤其适用于处理包含大量噪声和错误匹配的场景。在点云配准领域，它能够有效地解决粗配准问题，为后续的精配准算法（如ICP）提供一个可靠的初始变换，从而大大提高了整体配准的成功率和准确性。本文所提供的代码是一个基础实现，实际应用中还可以结合特征描述子来生成更可靠的初始对应点集，以进一步提升RANSAC的效率和性能。