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
}