局部优化的RANSAC算法

最近了解到RANSAC算法是个好东西，进一步调查了相关文献，找到了一篇优秀的论文："Locally Optimized RANSAC". 大家可以在google中搜索并且下载下来，好好读一读应该会有收获的。

为了便于以后应用，我把RANSAC算法包装成简洁的模板形式，与大家共享：

 

//RANSAC: 模版化的 RANSAC 方法
#pragma once
//#include "xsort.h"     //XSortPro

//pessimistic estimate of fraction of inliers for RANSAC
#define RANSAC_INLIER_FRAC 0.25

//模版化的 RANSAC 方法: TData, TModel
//TData:  .SetSampled(bool), .IsSampled()
//TModel: .FixModel(TData**, int), .TestData(TData*), operator=
class RSData{//数据
public:
 float ferr; //该数据的模型测试误差
public:
 //设置抽样状态
 void SetSampled(bool sampled);
 //是否抽样过
 bool IsSampled() const;
 //比较：用于数据好坏排序
 bool operator<(const RSData& r){
  return (fabs(ferr)<fabs(r.ferr));
 };
};
class RSModel{//模型
public:
 void operator=(const RSModel& r);
 //FixModel: 模型参数估计, 返回误差总和
 double FixModel(RSData** pData, int N);
 //TestData: 测试指定的数据，返回误差
 double TestData(RSData* pdata) const;
};

template <class TData>
int SampleData(TData** pData,int N, TData** pSample,int m)
{
 int k,i;
 for(i=0; i<m; ++i)
 {
  do
  {
   k = rand() % N;
  }
  while( pData[k]->IsSampled() );
  
  pData[k]->SetSampled(true);
  pSample[i]= pData[k];
 }

 //reset back!
 for(i=0; i<m; ++i)
  pSample[i]->SetSampled(false);
 return 0;
}

template <class TData, class TModel>
int GetInlierData(TData** pData,int N, TModel& mp, double toler)
{
 int inum= 0;
 for(int k=0; k<N; ++k){
  mp.TestData(pData[k]);
  if(fabs(pData[k]->ferr)<toler){
   ++inum;
  }
 }
 return inum;
}
template <class TData, class TModel>
int GetInlierData(TData** pData,int N, TModel& mp, double toler, TData** pInlier)
{
 int inum= 0;
 for(int k=0; k<N; ++k){
 //! mp.TestData(pData[k]);
  if(fabs(pData[k]->ferr)<toler){
   pInlier[inum++]= pData[k];
  }
 }
 return inum;
}

//refer to paper: Locally Optimized RANSAC.
/*The following methods of local optimization have been tested.
1. Standard. The standard implementation of RANSAC without any local optimization.

2. Take all data points with error smaller than θ and use a linear algorithm to
hypothesize new model parameters.
3. Iterative. Take all data points with error smaller that K · θ and use linear algorithm
to compute new model parameters. Reduce the threshold and iterate until the threshold is θ.

4. Inner RANSAC. A new sampling procedure is executed. Samples are selected only
form Ik data points consistent with the hypothesised model of k-th step of RANSAC.
New models are verified against whole set of data points. As the sampling is running on
inlier data, there is no need for the size of sample to be minimal. On the contrary, the
size of the sample is selected to minimize the error of the model parameter estimation.
The number of repetitions is set to ten in the experiments presented.

5. Inner RANSAC with iteration. This method is similar to the previous one, the dif-
ference being that each sample of the inner RANSAC is processed by method 3.*/
//LocalRANSAC: 简单的局部优化算法，并没完全按照上述论文中的想法实现
template <class TData, class TModel>
int LocalRANSAC(TData** pData,int N, TData** pInlier,int inum,
    int m,int mi,int tms, TModel& mp, double toler)
{// 局部优化算法1: Locally Optimized RANSAC.
 TModel tmp;
 TData** pSample= pInlier+N;
 int md= inum-m; 
 
 //第一次模型估计使用全部的inliers, 期望得到更好的结果！
 int k= 0;
 for(k=0; k<inum; ++k)
  pSample[k]= pInlier[k];
 int imax= inum;
 int m2= inum;
 double es;

 k= 0;
 do{
  //模型估计
  es= tmp.FixModel(pSample,m2);
  if(es>=0){
   //Get inliers for all data!
   inum= GetInlierData(pData,N, tmp, toler);
   if(imax<inum){
    imax= inum;
    mp= tmp;
   }
  }
#ifdef _DEBUG
  else{//retry for debug!
   es= tmp.FixModel(pSample,m2);
  }
#endif

  //随机抽样in inliers，更新抽样个数！
  m2= m + rand()%md;
  SampleData(pInlier,inum, pSample,m2);
 }
 while(k<tms && imax<mi);
 return imax;
}

//LSortRANSAC: 引入数据好坏的排序策略，不采用随机抽样而采用贪婪抽样
template <class TData, class TModel>
int LSortRANSAC(TData** pData,int N, TData** pInlier,int inum,
    int m,int mi,int tms, TModel& mp, double toler)
{// 局部优化算法2: Local sorted RANSAC
 TModel tmp;
 //按照数据好坏排序，下面这个排序函数原定义在xsort.h中，请大家自己提供。有问题的话，可以和我联系。

//参见拙文：http://blog.csdn.net/xlh9718/article/details/17102951
 XSortPro(pInlier, inum);

 double es;
 int imax= inum;
 int step= (inum-m)/tms;
 step= max(step, 1);
 for(int k=m; k<inum; k+=step){
  es= tmp.FixModel(pInlier, k);
  if(es>=0){
   inum= GetInlierData(pData,N, tmp, toler);
   if(imax<inum){
    imax= inum;
    mp= tmp;
   }
  }
#ifdef _DEBUG
  else{//retry for debug!
   es= tmp.FixModel(pInlier, k);
  }
#endif
 }
 return imax;
}

//m: minimum data number to fix the model para
//mi: if inliers>mi, then return success at once!
//toler: inlier if |ferr=TModel.TestData(dat)|<toler; else outlier.
//prp_bad: the probability that no samples of all inliers were drawn, =0.01 is ok
//return:  maximum inlier
template <class TData, class TModel>
int RansacMethod(TData** pData,int N, int m, int mi, TModel& mp, double toler, double prp_bad)
{
 ASSERT(N>mi && mi>m);
 TData** pInlier= new TData*[N*2];
 TData** pSample= pInlier+N; 
 int k= 0;
 for(; k<N; ++k)//全部设为可抽样状态
  pData[k]->SetSampled(false);

 static unsigned int seed= 0;
#ifdef _DEBUG
 static unsigned int debug_seed= 99;
 seed= debug_seed;
#endif
 if(seed==0)
  seed= (unsigned int)time(0);
 srand(seed);
 TModel tmp;
 k= 0;

 int tms= 10;
 int inum= 0;
 int imax= 0;
 double mfrac= RANSAC_INLIER_FRAC;
 double es,prp(1.0);
 while(prp>prp_bad){
  //极小抽样 in all data
  SampleData(pData,N, pSample,m);

  //模型估计
  es= tmp.FixModel(pSample,m);
  if(es<0){
#ifdef _DEBUG
   //retry for debug!
   es= tmp.FixModel(pSample,m);
#endif
   ++k;
   continue;
  }

  //Get inliers for all data
  inum= GetInlierData(pData,N, tmp, toler);//tmp.TestData

  if(imax<inum){
   imax= inum;
   mp= tmp;

   if(imax>m){//Improved RANSAC
    imax= GetInlierData(pData,N, tmp,toler, pInlier);
    ASSERT(imax==inum);
   // imax= LocalRANSAC(pData,N, pInlier,inum, m,mi,tms, mp, toler);
    imax= LSortRANSAC(pData,N, pInlier,inum, m,mi,tms, mp, toler);
   }

   if(imax>mi)
    break;
   mfrac = (double)imax / (double)N;
  }
  prp= pow(1.0 - pow(mfrac, m), k);
  ++k;
 }

 delete[] pInlier;
 return imax;
}
//参数同上RansacMethod
template <class TData, class TModel>
int RANSACMethod(TData* pData,int N, int m, int mi, TModel& mp, double toler, double prp_bad)
{
 if(N<mi)
  return false;
 TData** ppData= new TData*[N];
 for(int k=0; k<N; ++k)
  ppData[k]= pData+k;

 int imax= RansacMethod(ppData,N, m,mi, mp, toler, prp_bad);
 delete[] ppData;
 return imax;
}

///模拟测试函数/
class RData: public RSData{
public:
 int  tag;
 void SetSampled(bool sampled){
  tag= sampled;
 };
 bool IsSampled() const{
  return (tag&1);
 };
};
class RModel: public RSModel{
public:
 void operator=(const RModel& r){
 };
 double FixModel(RData** pData, int N){
  double es= 0;
  for(int k=0; k<N; ++k){
   es+= TestData(pData[k]);
  }
  return es;
 };
 double TestData(RData* p) const{
  double ferr= (rand()%100)*0.02;
  p->ferr= (float)ferr;
  return ferr;
 };
};

inline int TestRANSAC()
{
 int N= 16;
 RData* pData= new RData[N];
 int mi= (int)(N*0.5);
 int m(4);

 RModel mp;
 double toler= 1.01;
 double prp_bad= 0.01;

 int imax= RANSACMethod(pData,N, m,mi,mp, toler,prp_bad);
// ASSERT(imax==N);
 delete[] pData;
 return imax;
}

 

实际应用时，只需自定义两个象RSData,RSModel那样的class，并实现其中的成员函数，然后就可以调用RANSACMethod函数了。