利用Ransac算法进行平面拟合

RANSAC算法是“Random Sample Consensus”的缩写，它的全称是随机抽样一致性算法。算法可以从一组包含“局外点”的观测数据集中，通过迭代方式估计数学模型的参数。RANSAC算法是不确定的算法——它是由一定的概率得出一个合理的结果，为了提高概率必须提高迭代次数。首先对算法进行学习，然后根据理论在Unity实践中实现平面拟合。

RANSAC的基本假设是：
（1）数据由“局内点”组成，例如：数据的分布可以用一些模型参数来解释；
（2）“局外点”是不能适应该模型的数据；
（3）除此之外的数据属于噪声。

局外点产生的原因有：噪声的极值；错误的测量方法；对数据的错误假设。
RANSAC也做了以下假设：给定一组（通常很小的）局内点，存在一个可以估计模型参数的过程；而该模型能够解释或者适用于局内点。

一、演示

一个简单的例子是从一组观测数据中找出合适的二维直线。假设观测数据中包含局内点和局外点，其中局内点近似的被直线所通过，而局外点远离于直线。

简单的最小二乘法不能找到适应于局内点的直线，原因是最小二乘法尽量去适应包括局外点在内的所有点。相反，RANSAC能得出一个仅仅用局内点计算出模型，并且概率还足够高。但是，RANSAC算法并不能保证结果一定正确，为了保证算法有足够高的合理概率，我们必须小心的选择算法的参数。

二、演示

RANSAC算法的输入是一组观测数据，一个可以解释或者适应于观测数据的参数化模型，一些可信的参数。

RANSAC通过反复选择数据中的一组随机子集来达成目标。被选取的子集被假设为局内点，并用下述方法进行验证：

step1.有一个模型适应于假设的局内点，即所有的未知参数都能从假设的局内点计算得出。
step2.用1中得到的模型去测试所有的其它数据，如果某个点适用于估计的模型，认为它也是局内点。
step3.如果有足够多的点被归类为假设的局内点，那么估计的模型就足够合理。
step4.然后，用所有假设的局内点去重新估计模型，因为它仅仅被初始的假设局内点估计过。
step5.最后，通过估计局内点与模型的错误率来评估模型。

三、算法

算法如下所示：

PlaneModel ransac(const std::vector<Eigen::Vector3d>& data, double threshold, int numIterations)
{
    if (data.size()<PlaneModel::ModelSize)
        throw std::runtime_error("Not enough data");
    
    int bestInliers=-1;
    PlaneModel bestModel;
    
    for (int it=0;it<numIterations;it++)
    {
        // select points
        int found=0;
        std::array<size_t,PlaneModel::ModelSize> indices;
        std::set<size_t> usedIndices;
        while (found < PlaneModel::ModelSize)
        {
			unsigned int randomSeed = std::rand();
            size_t sample = (size_t)(randomSeed % (data.size()-1));
            if (usedIndices.find(sample)!=usedIndices.end())
                continue;
            usedIndices.insert(sample);
            indices[found++]=sample;
        }
        
        // compute model
        PlaneModel m;
        m.compute(data,indices);
        
        int inliers=m.computeInliers(data,threshold);
        if (inliers>bestInliers)
        {
            bestModel=m;
            bestInliers=inliers;
        }
    }
    bestModel.refine(data,threshold);
    return bestModel;
}