RANSAC原理 by MRPT

ransac模板

使用该函数的关键是要根据不同情况设置fit_func、dist_func和degen_func。

template <typename NUMTYPE>
bool RANSAC_Template<NUMTYPE>::execute(
	const CMatrixTemplateNumeric<NUMTYPE>	  &data,  //数据
	TRansacFitFunctor			fit_func,     //拟合方程
	TRansacDistanceFunctor  	dist_func,   //距离方程
	TRansacDegenerateFunctor 	degen_func,  //退化方程
	const double   				distanceThreshold,  ///距离阈值
	const unsigned int			minimumSizeSamplesToFit,  //拟合方程需要的最小数据样本数量
	mrpt::vector_size_t			&out_best_inliers,   //最终得到的内点集
	CMatrixTemplateNumeric<NUMTYPE> &out_best_model,  //最终模型
	bool						verbose, 
	const double                p,  //置信度概率
	const size_t				maxIter  //最大迭代次数
	)

ransac程序流程：

（1）根据随机生成的均匀样本集拟合模型，M可能代表一系列模型
（2）在上面得到了模型，计算点与模型之间的距离，返回内点的索引。返回的是拥有最多内点的模型
（3）计算内点率，更新N,即需要执行的trials。 （这一步可以关注一下，提前终止收敛时间。）

{
	// Highly inspired on http://www.csse.uwa.edu.au/~pk/

	const size_t D = size(data,1);  //  dimensionality
	const size_t Npts = size(data,2); //列数

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

	const size_t maxDataTrials = 100; // Maximum number of attempts to select a non-degenerate data set.

	out_best_model.setSize(0,0);  // Sentinel value allowing detection of solution failure.
	out_best_inliers.clear();

    size_t trialcount = 0;
    size_t bestscore = std::string::npos; // npos will mean "none"
    size_t N = 1;     // Dummy initialisation for number of trials.

    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< CMatrixTemplateNumeric<NUMTYPE> >  MODELS;

        while (degenerate)
        {
            // Generate s random indicies in the range 1..npts
            ind.resize( minimumSizeSamplesToFit );

			// The +0.99... is due to the floor rounding afterwards when converting from random double samples to size_t
			//随机生成均匀样本
			randomGenerator.drawUniformVector(ind,0.0, Npts-1+0.999999 );

            // Test that these points are not a degenerate configuration.
            degenerate = degen_func(data, ind);

            if (!degenerate)
            {
                // Fit model to this random selection of data points.
                // Note that M may represent a set of models that fit the data
                //根据随机生成的均匀样本集拟合模型，M可能代表一系列模型
                fit_func(data,ind,MODELS);

                // Depending on your problem it might be that the only way you
                // can determine whether a data set is degenerate or not is to
				// try to fit a model and see if it succeeds.  If it fails we
                // reset degenerate to true.
                //拟合失败则设置degerate为true
                degenerate = MODELS.empty();
			}

            // Safeguard against being stuck in this loop forever
            if (++count > maxDataTrials)
            {
                if (verbose) printf_debug("[RANSAC] Unable to select a nondegenerate data set\n");
                break;
            }
        }

        // Once we are out here we should have some kind of model...
        // Evaluate distances between points and model returning the indices
        // of elements in x that are inliers.  Additionally, if M is a cell
        // array of possible models 'distfn' will return the model that has
        // the most inliers.  After this call M will be a non-cell objec
        // representing only one model.
        //在上面得到了模型，计算点与模型之间的距离，返回内点的索引。distfn返回的是拥有最多内点的模型
        unsigned int bestModelIdx = 1000;
        mrpt::vector_size_t   inliers;
        if (!degenerate)
        {
			dist_func(data,MODELS, 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); // Always update on the first iteration, regardless of the result (even for ninliers=0)

        //找出具有最大内点数的模型，存为out_best_model
        if (ninliers > bestscore || (bestscore==std::string::npos && ninliers!=0))
        {
            bestscore = ninliers;  // Record data for this model

            out_best_model    = MODELS[bestModelIdx];
            out_best_inliers  = inliers;
            update_estim_num_iters=true;
        }

        if (update_estim_num_iters)
        {
            // Update estimate of N, the number of trials to ensure we pick,
            // with probability p, a data set with no outliers.
            //更新N,即需要执行的trials
            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);  // Avoid division by -Inf
            pNoOutliers = std::min(1.0 - std::numeric_limits<double>::epsilon() , pNoOutliers); // Avoid division by 0.
            // Number of
            N = log(1-p)/log(pNoOutliers);
			if (verbose)
				printf_debug("[RANSAC] Iter #%u Estimated number of iters: %u  pNoOutliers = %f  #inliers: %u\n", (unsigned)trialcount ,(unsigned)N,pNoOutliers, (unsigned)ninliers);
        }

        ++trialcount;

        if (verbose)
            printf_debug("[RANSAC] 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)
        {
			printf_debug("[RANSAC] Warning: maximum number of trials (%u) reached\n", (unsigned)maxIter);
            break;
        }
    }

    if (size(out_best_model,1)>0)
    {  // We got a solution
		if (verbose)
			printf_debug("[RANSAC]  Finished in %u iterations.\n",(unsigned)trialcount );
    	return true;
    }
    else
    {
		if (verbose)
			printf_debug("[RANSAC] Warning: Finished without any proper solution.\n");
    	return false;
    }

	MRPT_END
}