RANSAC算法PCL实例

算法简介

附上一个上古时期的博客：https://www.cnblogs.com/xrwang/archive/2011/03/09/ransac-1.html
RANSAC从样本中随机选出一个样本的子集，使用最小方差估计算法对这个子集计算模型参数，然后计算所有样本与该模型的偏差，再用一个预先设定好的阈值与偏差比较，当偏差小于阈值时，该样本点属于模型内样本点(inliers)，否则为外样本点(outliers)，记录下当前inliers的个数，然后重复这个过程。每重复一次，都记录当前最佳的模型参数，所谓最佳，就是inliers的个数最多，此时对应的inliers个数为best_ninliers。每次迭代的末尾，都会根据期望的误差率，best_ninliers，总样本数，当前迭代次数计算一个迭代结束的评判因子，据此决定是否结束迭代。

RANSAC理论上可以剔除outliers的影响，并得到全局最优的参数估计，但是RANSAC存在几个问题：

• 阈值的设置，
• RANSAC的迭代次数是运行期决定的，不能预知确切的迭代次数
• 只能从一个特定数据集中估计一个模型，存在多个模型时，RANSAC不能找到别的模型

来自wiki的伪码

Given:
    data – A set of observations.
    model – A model to explain observed data points.
    n – Minimum number of data points required to estimate model parameters.
    k – Maximum number of iterations allowed in the algorithm.
    t – Threshold value to determine data points that are fit well by model.
    d – Number of close data points required to assert that a model fits well to data.

Return:
    bestFit – model parameters which best fit the data (or null if no good model is found)

iterations = 0
bestFit = null
bestErr = something really large

while iterations < k do
    maybeInliers := n randomly selected values from data
    maybeModel := model parameters fitted to maybeInliers
    alsoInliers := empty set
    for every point in data not in maybeInliers do
        if point fits maybeModel with an error smaller than t
             add point to alsoInliers
    end for
    if the number of elements in alsoInliers is > d then
        // This implies that we may have found a good model
        // now test how good it is.
        betterModel := model parameters fitted to all points in maybeInliers and alsoInliers
        thisErr := a measure of how well betterModel fits these points
        if thisErr < bestErr then
            bestFit := betterModel
            bestErr := thisErr
        end if
    end if
    increment iterations
end while

return bestFit

伪代码很清晰，也很好理解

一个实例

用RANSAC来对点进行直线拟合，使用PCL库

#include <iostream>
#include <pcl/console/parse.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <unordered_set>

struct Color
{

    float r, g, b;

    Color(float setR, float setG, float setB)
        : r(setR), g(setG), b(setB)
    {}
};

void renderPointCloud(pcl::visualization::PCLVisualizer::Ptr& viewer, const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, std::string name, Color color)
{

    viewer->addPointCloud<pcl::PointXYZ> (cloud, name);
    viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, name);
    viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, color.r, color.g, color.b, name);
}

pcl::visualization::PCLVisualizer::Ptr initScene()
{
    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer ("2D Viewer"));
    viewer->setBackgroundColor (0, 0, 0);
    viewer->initCameraParameters();
    viewer->setCameraPosition(0, 0, 15, 0, 1, 0);
    viewer->addCoordinateSystem (1.0);
    return viewer;
}

pcl::PointCloud<pcl::PointXYZ>::Ptr CreateData()
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
    // Add inliers
    float scatter = 0.6;
    for(int i = -5; i < 5; i++)
    {
        double rx = 2*(((double) rand() / (RAND_MAX))-0.5);
        double ry = 2*(((double) rand() / (RAND_MAX))-0.5);
        pcl::PointXYZ point;
        point.x = i+scatter*rx;
        point.y = i+scatter*ry;
        point.z = 0;

        cloud->points.push_back(point);
    }
    // Add outliers
    int numOutliers = 10;
    while(numOutliers--)
    {
        double rx = 2*(((double) rand() / (RAND_MAX))-0.5);
        double ry = 2*(((double) rand() / (RAND_MAX))-0.5);
        pcl::PointXYZ point;
        point.x = 5*rx;
        point.y = 5*ry;
        point.z = 0;

        cloud->points.push_back(point);

    }
    cloud->width = cloud->points.size();
    cloud->height = 1;

    return cloud;

}

std::unordered_set<int> Ransac(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, int maxIterations, float distanceTol)
{
    std::unordered_set<int> inliersResult;
    srand(time(NULL));

    // For max iterations
        while( maxIterations--)
        {
            std::unordered_set<int> inliers;
            while(inliers.size() < 2)
                inliers.insert(rand() % (cloud -> points.size()));

            float x1, x2, y1, y2;
            auto itr = inliers.begin();
            x1=cloud -> points[*itr].x;
            y1=cloud -> points[*itr].y;
            itr ++;
            x2=cloud -> points[*itr].x;
            y2=cloud -> points[*itr].y;

            float a = y1 - y2;
            float b = x2 - x1;
            float c = x1 * y2 - x2 * y1;

            for (int index = 0; index < cloud->points.size(); index++)
            {
                if(inliers.count(index) > 0)//if the point is already in the line
                    continue;

                pcl::PointXYZ point = cloud->points[index];
                float x3 = point.x;
                float y3 = point.y;

                float d = fabs(a * x3 + b * y3 + c)/sqrt(a * a + b * b);

                if(d <= distanceTol)
                    inliers.insert(index);
            }

            if(inliers.size() > inliersResult.size())
            {
                inliersResult = inliers;
            }
        }
    return inliersResult;
}

int
main(int argc, char** argv)
{
    // Create viewer
    pcl::visualization::PCLVisualizer::Ptr viewer = initScene();
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = CreateData();

    std::unordered_set<int> inliers = Ransac(cloud, 5, 1.0);

    pcl::PointCloud<pcl::PointXYZ>::Ptr  cloudInliers(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudOutliers(new pcl::PointCloud<pcl::PointXYZ>());

    for(int index = 0; index < cloud->points.size(); index++)
    {
        pcl::PointXYZ point = cloud->points[index];
        if(inliers.count(index))
            cloudInliers->points.push_back(point);
        else
            cloudOutliers->points.push_back(point);
    }

    if(inliers.size())
    {
        renderPointCloud(viewer,cloudInliers,"inliers",Color(0,1,0));
        renderPointCloud(viewer,cloudOutliers,"outliers",Color(1,0,0));
    }

    while (!viewer->wasStopped ())
    {
        viewer->spinOnce ();
    }


    return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)

add_definitions(-std=c++11)
project(random_sample_consensus)

find_package(PCL 1.2 REQUIRED)

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

add_executable (random_sample_consensus random.cpp)
target_link_libraries (random_sample_consensus ${PCL_LIBRARIES})

运行结果：
代码很简单，重点部分在Ransac()函数中，可以比较清楚的解释RANSAC的每一步。
设置的阈值是1.0，迭代次数是5，运行了4次，得到了4个不同的结果，有好有坏，可以增加迭代次数来提高估计的准确性。