随机抽样一致（RANSAC）原理分析

template <typename NUMTYPE>
bool RANSAC_Template<NUMTYPE>::execute(
    const CMatrixDynamic<NUMTYPE>& data, const TRansacFitFunctor& fit_func,
    const TRansacDistanceFunctor& dist_func,
    const TRansacDegenerateFunctor& degen_func, const double distanceThreshold,
    const unsigned int minimumSizeSamplesToFit,
    std::vector<size_t>& out_best_inliers,
    CMatrixDynamic<NUMTYPE>& out_best_model, const double p,
    const size_t maxIter) const
{
    MRPT_START

    ASSERT_(minimumSizeSamplesToFit >= 1);

    // 作者参考 http://www.csse.uwa.edu.au/~pk/ 的 matlab代码
    const size_t D = data.rows();  //  维度
    const size_t Npts = data.cols();

    ASSERT_(D >= 1);
    ASSERT_(Npts > 1);

    const size_t maxDataTrials =
        100;  // 原数据集随机抽样的最大尝试次数。

    out_best_model.setSize(
        0, 0);  // 初始化
    out_best_inliers.clear();

    size_t trialcount = 0;
    size_t bestscore = std::string::npos;  //内点的个数 npos will mean "none" 原文件是初始化为零
    size_t N = 1;  // 整个流程迭代次数

    std::vector<size_t> ind(minimumSizeSamplesToFit);//随机采样的数据

    while (N > trialcount)
    {
        // Select at random s datapoints to form a trial model, M.
        // In selecting these points we have to check that they are not in
        // a degenerate configuration.
        bool degenerate = true;
        size_t count = 1;
        std::vector<CMatrixDynamic<NUMTYPE>> MODELS;
        ////////////////////////1随机从数据集中随机抽出s个样本数据，拟合多个模型////////////////////////////
        while (degenerate)
        {
            // 在1到npts范围内生成s个随机指标
            ind.resize(minimumSizeSamplesToFit);

            // The +0.99... is due to the floor rounding afterwards when
            // converting from random double samples to size_t
            getRandomGenerator().drawUniformVector(
                ind, 0.0, Npts - 1 + 0.999999);

            // 测试这些点是否是退化,如果为true，那么不能退化
            degenerate = degen_func(data, ind);

            if (!degenerate)
            {
            //通过随机采样的数据点拟合模型，这里拟合了很多模型
                fit_func(data, ind, MODELS);

            //模型为空则继续此过程
                degenerate = MODELS.empty();
            }

            // 循环次数判断，最大为100次 
            if (++count > maxDataTrials)
            {
                MRPT_LOG_WARN("Unable to select a nondegenerate data set");
                break;
            }
        }

       //运行到这里已经有很多模型了，下面计算点与模型之间的距离，返回的bestModelIdx是元素的索引。
//////////////////////////////////////2计算数据集中所有数据与模型M的投影误差，若误差小于阈值，加入内点集 I ；///////////////////////////////
        unsigned int bestModelIdx = 1000;
        std::vector<size_t> inliers;
        if (!degenerate)
        {
            dist_func(
                data, MODELS, NUMTYPE(distanceThreshold), bestModelIdx,
                inliers);
            ASSERT_(bestModelIdx < MODELS.size());
        }
        
        // Find the number of inliers to this model.
        const size_t ninliers = inliers.size();
        bool update_estim_num_iters =
            (trialcount == 0);  // 即使是内点为零，也要更新
/////////////////////////////////////////////3如果当前内点集 I 元素个数大于最优内点集 I_best , 则更新 I_best = I，同时更新迭代次数k ;///////////////////////////////////
        if (ninliers > bestscore ||
            (bestscore == std::string::npos && ninliers != 0))//内点的个数大于0，将找到的最优模型赋值到全局变量
        {
            bestscore = ninliers;  // Record data for this model

            out_best_model = MODELS[bestModelIdx];
            out_best_inliers = inliers;
            update_estim_num_iters = true;
        }
/////////////////////////////////////////////////4如果迭代次数大于k,则退出 ; 否则迭代次数加1，并重复上述步骤；/////////////////////////////
        if (update_estim_num_iters)
        {
            // 更新n试验次数，以确保我们用概率p选择一个没有异常值的数据集。
            double fracinliers = ninliers / static_cast<double>(Npts);
            double pNoOutliers =
                1 -
                pow(fracinliers, static_cast<double>(minimumSizeSamplesToFit));

            pNoOutliers = std::max(
                std::numeric_limits<double>::epsilon(),
                pNoOutliers);  // 避免负无穷
            pNoOutliers = std::min(
                1.0 - std::numeric_limits<double>::epsilon(),
                pNoOutliers);  // 避免除数为零
            N = static_cast<size_t>(log(1 - p) / log(pNoOutliers));
            MRPT_LOG_DEBUG(format(
                "Iter #%u Estimated number of iters: %u  pNoOutliers = %f  "
                "#inliers: %u\n",
                (unsigned)trialcount, (unsigned)N, pNoOutliers,
                (unsigned)ninliers));
        }

        ++trialcount;

        MRPT_LOG_DEBUG(format(
            "trial %u out of %u \r", (unsigned int)trialcount,
            (unsigned int)ceil(static_cast<double>(N))));

        // Safeguard against being stuck in this loop forever
        if (trialcount > maxIter)
        {
            MRPT_LOG_WARN(format(
                "Warning: maximum number of trials (%u) reached\n",
                (unsigned)maxIter));
            break;
        }
    }

    if (out_best_model.rows() > 0)
    {  // We got a solution
        MRPT_LOG_INFO(
            format("Finished in %u iterations.\n", (unsigned)trialcount));
        return true;
    }
    else
    {
        MRPT_LOG_WARN("Finished without any proper solution!");
        return false;
    }

    MRPT_END
}