RANSAC 配准算法_ransac点云配准算法

1. 简介

先讲一下背景吧。

点云配准（Point Cloud Registration）指的是输入两幅点云 (source 和 target) ，输出一个变换使得变换后的source和target的重合程度尽可能高。

点云配准可以分为粗配准（Coarse Registration）和精配准（Fine Registration）两步。粗配准指的是在两幅点云之间的变换完全未知的情况下进行较为粗糙的配准，目的主要是为精配准提供较好的变换初值；精配准则是给定一个初始变换，进一步优化得到更精确的变换。

而今天我们所要讲的内容就是RANSAC 粗配准算法。RANSAC即Random Sample Consensus，随机采样一致算法。

2. RANSAC步骤

RANSAC算法的核心思想是随机性和假设性。 请记住这句话。

随机性用于减少计算，循环次数是利用正确数据出现的概率。

而假设性，就是说随机抽出来的数据都认为是正确的，并以此去计算其他点，获得其他满足变换关系的点，然后利用投票机制，选出获票最多的那一个变换。

RANSAC广义上讲，是随机选择一个小的数据点子集，然后对其进行拟合，查看有多少其他点匹配到拟合的模型上，迭代这个过程直至有较大的概率找到我们想要拟合的模型。

在点云配准这个应用中，使用 RANSAC 方法，就是在源点云中随机找不共线的三个点，去“蒙”他们和目标点云中的哪三个点相对应，然后计算变换矩阵，并验证是否为最优的变换矩阵。简要流程如下：

1. 在源点云中随机选择 3 个点。
2. 分别查找他们在目标点云中特征向量最接近的点。这里的“接近”是欧式距离意义下的，查询数据结构使用 KD 树。当然，这里最接近的点会很多，我们为了在最快的时间内得到最终更好的匹配，通常会加一些约束，比如每组点的连线形成一个三角形，三角形的边不能太短、两个三角形的对应边长不能差太多、优先选择特征向量远离均值的点等等；
3. 假设2中最接近的点是正确的，计算两组点之间的变换矩阵。本质是求最小二乘法意义下的最优变换矩阵。
4. 求出源点云在此变换之后的点云，计算其与目标点云的重合度（定义为源点云中阈值半径内存在目标点的点的个数）。
5. 回到1，重复若干次，返回重合度最高的变换。

以上，RANSAC其实是一种不确定的算法，即有一定的概率得出一个合理的结果，当然也会出现错误的结果。他为了提高时间效率不会去遍历每一个可能性，只是采取随机采样的方式，得到这些随机采样中的最好的结果，这个最好的结果也许是错误的结果，这里面就涉及到概率。如果要提高概率，一个要提高迭代的次数，在一个就是减少数据集离群点的比例。

因此，问题来了，在这算法流程中存在两个重要的参数需要设置，迭代次数（采样次数）和阈值。

迭代的次数我们应该选择多大呢？这个值是否可以事先知道应该设为多少呢？还是只能凭经验决定呢？

我们要剔除离群点 ，需要事先设定阈值，当模型具有明显的物理意义时，这个阈值还比较容易设定，但是若模型比较抽象时，阈值就不那么容易设定了，而且固定阈值不适用于样本动态变化的应用。那我们该怎么设置这个阈值呢？

3. RANSAC原理

我们先从一个小例子开始。

假设我们主观上看出了两个图像之间的点匹配，并且我们认为可以用一些参数去表示两者之间的变换，那么我们该如何去得到这个变换参数呢？

通常的策略是用最小二乘法去估计点之间的关系，但是会存在一个问题，如下图所示：

像这种离群点其实是不能很好拟合我们的模型的。能够拟合模型的点称为inliers，离群点称为outlier。

最小二乘估计对这些离群点很敏感，因此一些outlier可能会对结果进行扭曲，如下图所示。

而且，最小二乘估计只能估计出一个模型，如果存在两个或多个模型，也会对结果进行扭曲。

因此，为了解决以上问题，需要将估计视为一个两阶段过程：
– 将数据点分类为outlier或inlier
– 将模型拟合到inlier，同时忽略outlier

RANSAC可以解决以上问题，

比如我们要拟合的模型是一条直线，一条直线两个点就可以确定，因为我们需要随机采样两个点，如下图所示。

我们给一个阈值去评估这个模型，在这个阈值内，有4个点圈进来了。

如果换其他的采样点呢？

此时 ，有6个点圈进来了。

我们再一次迭代，

此时，有19个点圈进来了。

继续迭代。

此时，有13个点圈进来了。

在这几次迭代中 ，第三次圈进来的点数最多，我们可以认为第三次是目前这几次迭代中的最优解。

那么，整个算法流程如下，

那么，回到我们最开始的问题，我们怎么去选择迭代次数N呢？

注：outlier的概率e通常是一个先验值，P 是我们希望RANSAC得到正确模型的概率。如果事先不知道e的值，可以使用自适应迭代次数的方法。也就是一开始设定一个无穷大的迭代次数，然后每次更新模型参数估计的时候，用当前的outlier比值当成e来估算出迭代次数。

那么我们怎么去选择阈值呢？ 通常来说，我们是根据经验去选择。好吧，确实太抽象了，这个就需要多做实验，多try。

4. RANSAC的优缺点

RANSAC的优点是它能鲁棒的估计模型参数。例如，它能从包含大量局外点的数据集中估计出高精度的参数。

RANSAC的缺点是它计算参数的迭代次数没有上限；如果设置迭代次数的上限，得到的结果可能不是最优的结果，甚至可能得到错误的结果。RANSAC只有一定的概率得到可信的模型，概率与迭代次数成正比。RANSAC的另一个缺点是它要求设置跟问题相关的阀值。

RANSAC只能从特定的数据集中估计出一个模型，如果存在两个（或多个）模型，RANSAC不能找到别的模型。

RANSAC可以对局外点进行剔除，这一点是比较好的，但它也并不是完美的，虽然缺点有这么多，但是我们还是离不开它。

5. 代码实现

PCL 库中以随机采样一致性算法( RANSAC) 为核心，实现了五种类似于RANSAC的随机参数估计算法，例如随机采样一致性估计（RANSAC ) 、最大似然一致性估计 （MLESAC ) 、最小中值方差一致性估计 ( LMEDS )等，所有的估计参数算法都符合一致性准则。利用RANSAC可以实现点云分割，目前 PCL 中支持的几何模型分割有 空间平面、直线、二维或三维圆、圆球、锥体等 。 RANSAC的另一应用就是点云的配准对的剔除。

下面用opencv进行代码示例。

#include <opencv2/opencv.hpp>
#include <iostream>
#include <opencv2/xfeatures2d.hpp>
#include <opencv2/features2d/features2d.hpp>
void extracte_orb(cv::Mat input,std::vector<cv::KeyPoint> &keypoint,cv::Mat &descriptor){
    cv::Ptr<cv::ORB> f2d = cv::ORB::create(500);
    f2d->detect(input,keypoint);
    cv::Mat image_with_kp;
    f2d->compute(input,keypoint,descriptor);
    cv::drawKeypoints(input, keypoint, image_with_kp, cv::Scalar::all(-1),cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
    cv::imwrite("orb"+std::to_string(random())+".png",image_with_kp);
}

void match_two_image(cv::Mat image1,cv::Mat image2, std::vector<cv::KeyPoint> keypoint1,std::vector<cv::KeyPoint> keypoint2,cv::Mat descriptor1,cv::Mat descriptor2){
    cv::BFMatcher matcher(cv::NORM_HAMMING);
    std::vector<cv::DMatch> matches;
    matcher.match(descriptor1,descriptor2, matches);
    cv::Mat good_matches_image;
    cv::drawMatches(image1, keypoint1, image2, keypoint2,
                    matches, good_matches_image, cv::Scalar::all(-1), cv::Scalar::all(-1),
                    std::vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS);
    cv::imwrite("good_matches_image.png",good_matches_image);
    {
        std::vector <cv::KeyPoint> RAN_KP1, RAN_KP2;
        std::vector<cv::Point2f> keypoints1, keypoints2;
        for (int i = 0; i < matches.size(); i++) {
            keypoints1.push_back(keypoint1[matches[i].queryIdx].pt);
            keypoints2.push_back(keypoint2[matches[i].trainIdx].pt);
            RAN_KP1.push_back(keypoint1[matches[i].queryIdx]);
            RAN_KP2.push_back(keypoint2[matches[i].trainIdx]);
        }

        std::vector<uchar> RansacStatus;
        cv::findFundamentalMat(keypoints1, keypoints2, RansacStatus, cv::FM_RANSAC);
        std::vector <cv::KeyPoint> ransac_keypoints1, ransac_keypoints2;
        std::vector <cv::DMatch> ransac_matches;
        int index = 0;
        for (size_t i = 0; i < matches.size(); i++)
        {
            if (RansacStatus[i] != 0)
            {
                ransac_keypoints1.push_back(RAN_KP1[i]);
                ransac_keypoints2.push_back(RAN_KP2[i]);
                matches[i].queryIdx = index;
                matches[i].trainIdx = index;
                ransac_matches.push_back(matches[i]);
                index++;
            }
        }
        cv::Mat after_ransac_sift_match;
        cv::drawMatches(image1, ransac_keypoints1, image2, ransac_keypoints2,
                        ransac_matches, after_ransac_sift_match, cv::Scalar::all(-1), cv::Scalar::all(-1),
                        std::vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS);
        cv::imwrite("after_ransac_orb_match.png",after_ransac_sift_match);
    }
}

int main(int argc, char *argv[])
{
    cv::Mat image1 = cv::imread(argv[1]);
    cv::Mat image2 = cv::imread(argv[2]);
    std::vector<cv::KeyPoint> keypoint1,keypoint2;
    cv::Mat descriptor1, descriptor2;
    extracte_orb(image1,keypoint1,descriptor1);
    extracte_orb(image2,keypoint2,descriptor2);
    match_two_image(image1,image2,keypoint1,keypoint2,descriptor1,descriptor2);
    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68

OpenCV RANSAC的效果展示

6. 参考

Robust Estimation : RANSAC
04-随机采样一致性算法RANSAC
RANSAC