SLAM中的Ransac的OpenCV源代码

本文主要记录一下比较基础的内容：RANSAC原理及OpenCV相关源代码

RANSAC

1. RANSAC的基础内容：以下内容来源于：1
   RANSAC是“RANdom SAmple Consensus（随机抽样一致）”的缩写。它可以从一组包含“局外点”的观测数据集中，通过迭代方式估计数学模型的参数。它是一种不确定的算法——它有一定的概率得出一个合理的结果；为了提高概率必须提高迭代次数。

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

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

   3. RANSAC与最小二乘法的区别：简单的最小二乘法不能找到适应于inliers的直线，因为最小二乘法尽量去适应包括outliers在内的所有点。但是RANSAC可以得到一个仅仅用inliers计算出的模型，且概率较高。但是，RANSAC并不能保证结果一定正确，为了保证算法有足够高的合理概率，必须小心的选择算法的参数

   4. 概述：RANSAC算法的输入是一组观测数据，一个可以解释或者适应于观测数据的参数化模型，一些可信的参数（这里的参数包括：适用于模型的最少数据个数；算法的迭代次数；用于决定数据是否适用于模型的阈值；判定模型是否适用于数据集的数据数目）
      （给出了迭代次数的计算方法，但是阈值和数据数目依赖于数据集，仅能针对特定的问题和数据集通过实验来确定）。
      RANSAC通过反复选择数据中的一组随机子集来达成目标。被选取的子集被假设为局内点，并用下述方法进行验证：
      1.有一个模型适应于假设的局内点，即所有的未知参数都能从假设的局内点计算得出。
      2.用1中得到的模型去测试所有的其它数据，如果某个点适用于估计的模型，认为它也是局内点。
      3.如果有足够多的点被归类为假设的局内点，那么估计的模型就足够合理。
      4.然后，用所有假设的局内点去重新估计模型，因为它仅仅被初始的假设局内点估计过。
      5.最后，通过估计局内点与模型的错误率来评估模型。
      这个过程被重复执行固定的次数，每次产生的模型要么因为局内点太少而被舍弃，要么因为比现有的模型更好而被选用。

   5. 其中，核心是随机性和假设性：随机性用于减少计算。假设性，是指随机抽出来的数据都认为是正确的，并以此去计算其他点，获得其他满足变换关系的点，然后利用投票机制，选出获票最多的那一个变换。

   6. 优点与缺点
      RANSAC的优点是它能鲁棒的估计模型参数。例如，它能从包含大量局外点的数据集中估计出高精度的参数
      RANSAC的缺点是它计算参数的迭代次数没有上限；如果设置迭代次数的上限，得到的结果可能不是最优的结果，甚至可能得到错误的结果。RANSAC只有一定的概率得到可信的模型，概率与迭代次数成正比。RANSAC的另一个缺点是它要求设置跟问题相关的阀值。
      RANSAC只能从特定的数据集中估计出一个模型，如果存在两个（或多个）模型，RANSAC不能找到别的模型。

2. OpenCV中的实现

   1. 涉及到了下述几个类：

      1. PnPRansacCallback：这个类里面包含runKernel() 和computeError() 两个函数，前者用于根据已知的点并使用PnP方法求解得对应的模型矩阵，返回得到的模型的数目；后者用于计算重投影误差
      2. RANSACPointSetRegistrator：这个类里面包含findInliers()，getSubset()和run()函数。findInliers() 里面，根据绑定的callback计算得到的重投影误差，将误差值小于阈值的那些点均认为是“真的”inliers；getSubset() 使用了随机数生成器，每次返回modelPoints个点；run() 函数是最重要的，它完成了下述工作：根据输入的迭代次数进行迭代，在每一次迭代过程中，如果得到的数目大于modelPoints那么就使用getSubsets()函数生成modelPoints个点，否则就不用随机生成。之后，调用PnPRansacCallback类中的runKernel()函数，得到一组对应的模型，对于每个模型，选择出具有最大好点数目（好点是指inliers，最大好点数目就代表能够包含最多的inliers）的模型

   2. 涉及到下述几个函数：

      1. createRANSACPointSetRegistrator()函数：创建RANSACPointSetRegistrator对象
      2. Mat::checkVector()函数: 就是检查这个matrix是否是一个vector向量，返回值为该向量的维度/通道数

   3. PnPRansacCallback类——runKernel函数：注意，此时调用的cb，即callback中的runkernel函数，即solvepnp.cpp中的类PnPRansacCallback的函数。本函数根据给定的点来计算_model矩阵，并且返回发现了的模型的数目。此时，使用将m1和m2两个矩阵使用solvePnP函数来求解，得到rvec, tvec。将两个按行合并，即A=[B C]。之后会依据得到的_model来计算符合要求的inliers

      int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const
      {
          Mat opoints = _m1.getMat(), ipoints = _m2.getMat();
      
          bool correspondence = solvePnP( _m1, _m2, cameraMatrix, distCoeffs,
                                              rvec, tvec, useExtrinsicGuess, flags );
      
          Mat _local_model;
          hconcat(rvec, tvec, _local_model);
          _local_model.copyTo(_model);
      
          return correspondence;
      }
      

   4. PnPRansacCallback类——computeError()函数，使用最后的那个for循环来计算重投影误差总和，并将其存入_err变量

      	void computeError( InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err ) const
      {
          Mat m1 = _m1.getMat(), m2 = _m2.getMat(), model = _model.getMat();
          const Point3f* from = m1.ptr();
          const Point3f* to   = m2.ptr();
          const double* F = model.ptr();
      
          int count = m1.checkVector(3);
          CV_Assert( count > 0 );
      
          _err.create(count, 1, CV_32F);
          Mat err = _err.getMat();
          float* errptr = err.ptr();
      
          for(int i = 0; i < count; i++ )
          {
              const Point3f& f = from[i];
              const Point3f& t = to[i];
      
              double a = F[0]*f.x + F[1]*f.y + F[ 2]*f.z + F[ 3] - t.x;
              double b = F[4]*f.x + F[5]*f.y + F[ 6]*f.z + F[ 7] - t.y;
              double c = F[8]*f.x + F[9]*f.y + F[10]*f.z + F[11] - t.z;
      
              errptr[i] = (float)(a*a + b*b + c*c);
          }
      }```
      
      

   5. RANSACPointSetRegistrator类——constructor

      RANSACPointSetRegistrator(const Ptr& _cb=Ptr(),
                                int _modelPoints=0, double _threshold=0, double _confidence=0.99, int _maxIters=1000)
      : cb(_cb), modelPoints(_modelPoints), threshold(_threshold), confidence(_confidence), maxIters(_maxIters)
      {
          checkPartialSubsets = false;
      }
      

   6. RANSACPointSetRegistrator类——findInliers()函数
      因为得到的重投影误差值为距离的平方，所以需要对阈值进行平方，然后比较。对于那些小于阈值的点，就将其存入maskptr[i]中，同时统计成功的数目（这里的成功是指得到的重投影误差值小于阈值的平方）

      int findInliers( const Mat& m1, const Mat& m2, const Mat& model, Mat& err, Mat& mask, double thresh ) const
      {
          cb->computeError( m1, m2, model, err );
          mask.create(err.size(), CV_8U);
      
          CV_Assert( err.isContinuous() && err.type() == CV_32F && mask.isContinuous() && mask.type() == CV_8U);
          const float* errptr = err.ptr();
          uchar* maskptr = mask.ptr();
          float t = (float)(thresh*thresh);
          int i, n = (int)err.total(), nz = 0;
          for( i = 0; i < n; i++ )
          {
              int f = errptr[i] <= t;
              maskptr[i] = (uchar)f;
              nz += f;
          }
          return nz;
      }
      

   7. RANSACPointSetRegistrator类——getSubset()函数：
      一共执行maxAttempts次。每一次，对于model中的所有点，随机挑选一些点，这些点的数目似乎并没有

      for(; iters < maxAttempts; iters++)
              {
                  for( i = 0; i < modelPoints && iters < maxAttempts; )
                  {
                      int idx_i = 0;
                      for(;;)
                      {
                          idx_i = idx[i] = rng.uniform(0, count);
                          for( j = 0; j < i; j++ )
                              if( idx_i == idx[j] )
                                  break;
                          if( j == i )
                              break;
                      }
                      for( k = 0; k < esz1; k++ )
                          ms1ptr[i*esz1 + k] = m1ptr[idx_i*esz1 + k];
                      for( k = 0; k < esz2; k++ )
                          ms2ptr[i*esz2 + k] = m2ptr[idx_i*esz2 + k];
                      if( checkPartialSubsets && !cb->checkSubset( ms1, ms2, i+1 ))
                      {
                          // we may have selected some bad points;
                          // so, let's remove some of them randomly
                          i = rng.uniform(0, i+1);
                          iters++;
                          continue;
                      }
                      i++;
                  }
                  if( !checkPartialSubsets && i == modelPoints && !cb->checkSubset(ms1, ms2, i))
                      continue;
                  break;
              }
      
              return i == modelPoints && iters < maxAttempts;
      

   8. RANSACPointSetRegistrator类——run()函数

      bool run(InputArray _m1, InputArray _m2, OutputArray _model, OutputArray _mask) const
      

      对应于1中的run参数，可以更进一步了解各参数的含义
      run()函数中的关键代码。使用findInliers()（见第四条）可以得到对应的mask数组。使用runKernel()函数(见第二条)返回得到的候选模型数目，并从这些候选中选出具有最多inliers数目的模型

      for( iter = 0; iter < niters; iter++ )
      {
          int i, nmodels;
          if( count > modelPoints )
          {
              bool found = getSubset( m1, m2, ms1, ms2, rng, 10000 );
              if( !found )
              {
                  if( iter == 0 )
                      return false;
                  break;
              }
          }
      
          nmodels = cb->runKernel( ms1, ms2, model );
          if( nmodels <= 0 )
              continue;
          CV_Assert( model.rows % nmodels == 0 );
          Size modelSize(model.cols, model.rows/nmodels);
      
          for( i = 0; i < nmodels; i++ )
          {
              Mat model_i = model.rowRange( i*modelSize.height, (i+1)*modelSize.height );
              int goodCount = findInliers( m1, m2, model_i, err, mask, threshold );
      
              if( goodCount > MAX(maxGoodCount, modelPoints-1) )
              {
                  std::swap(mask, bestMask);
                  model_i.copyTo(bestModel);
                  maxGoodCount = goodCount;
                  niters = RANSACUpdateNumIters( confidence, (double)(count - goodCount)/count, modelPoints, niters );
              }
          }
      }
      

      下面的代码就是对选出的这个模型进行更新，将得到的model模型存入_model，然后输出，返回true；否则，直接释放_model空间，返回false

      if( maxGoodCount > 0 )
      {
          if( bestMask.data != bestMask0.data )
          {
              if( bestMask.size() == bestMask0.size() )
                  bestMask.copyTo(bestMask0);
              else
                  transpose(bestMask, bestMask0);
          }
          bestModel.copyTo(_model);
          result = true;
      }
      else
          _model.release();
      

   9. RANSACUpdateNumIters()函数
      其中最关键的代码是return中的语句。

      return denom >= 0 || -num >= maxIters*(-denom) ? maxIters : cvRound(num/denom);
      

   10. createRANSACPointSetRegistrator()函数
       创建RANSAC点集配准器，从而获取“较为正确的outliers和inliers”。调用RANSACPointSetRegistrator里面的findInliers()，getSubset()和run()函数，执行RANSAC算法。该算法将会返回一个RANSACPointSetRegistrator对象

       Ptr createRANSACPointSetRegistrator(const Ptr& _cb,
                                                                int _modelPoints, double _threshold,
                                                                double _confidence, int _maxIters)
       {
           return Ptr(
               new RANSACPointSetRegistrator(_cb, _modelPoints, _threshold, _confidence, _maxIters));
       }
       

   11. Mat::checkVector()函数

       int Mat::checkVector(int _elemChannels, int _depth, bool _requireContinuous) const
       {
       return data && (depth() == _depth || _depth <= 0) &&
           (isContinuous() || !_requireContinuous) &&
           ((dims == 2 && (((rows == 1 || cols == 1) && channels() == _elemChannels) ||
                           (cols == _elemChannels && channels() == 1))) ||
           (dims == 3 && channels() == 1 && size.p[2] == _elemChannels && (size.p[0] == 1 || size.p[1] == 1) &&
            (isContinuous() || step.p[1] == step.p[2]*size.p[2]))) ? (int)(total()*channels()/_elemChannels) : -1;
       }
       

3. 铺垫了这么久，现在来看一下solvepnp.cpp中的solvePnPRansac()函数，它在昨天的博客中2 使用了
   OpenCV中的具体实现方式，可以查看D:\opencv\sources\modules\calib3d\src中的solvepnp.cpp文件。本函数首先使用RANSAC算法获得较为鲁棒的outliers和inliers，然后再使用它们来计算PnP
   本函数同findHomography函数一样，都可以在有少量数据量的情况下较为鲁棒。其中，findHomography函数可以在D:\opencv\sources\modules\calib3d\src的fundam.cpp文件中找到
   solvePnPRansac()函数接口如下：输入为outliers集合，inliers集合，相机参数和校正参数；输出为旋转矩阵、平移向量以及“RANSAC方法认为正确的inliers集合”

   bool solvePnPRansac(InputArray _opoints, InputArray _ipoints,
                           InputArray _cameraMatrix, InputArray _distCoeffs,
                           OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess,
                           int iterationsCount, float reprojectionError, double confidence,
                           OutputArray _inliers, int flags)
   

   接下来就是从输入参数中获取信息，初始化变量
   与RANSAC相关的代码有：

   1. 设置一个ransac_kernel_method变量，默认值为SOLVEPNP_EPNP，当 std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));时，该向量被设置为SOLVEPNP_P3P。该变量将会传入solvePnP()函数，指定使用的PnP函数
   2. 绑定callback回调函数，将其绑定到PnPRansacCallback类上，从而可以调用上述runKernel() 和*computeError()*函数

      Ptr cb; // pointer to callback
          cb = makePtr( cameraMatrix, distCoeffs, ransac_kernel_method, useExtrinsicGuess, rvec, tvec);
      

   3. 调用createRANSACPointSetRegistrator()函数，生成一个RANSACPointSetRegistrator对象，并调用其上的run()函数，得到最优的模型

      // call Ransac
          int result = createRANSACPointSetRegistrator(cb, model_points,
              param1, param2, param3)->run(opoints, ipoints, _local_model, _mask_local_inliers);
      
          if( result <= 0 || _local_model.rows <= 0)
          {
              _rvec.assign(rvec);    // output rotation vector
              _tvec.assign(tvec);    // output translation vector
      
              if( _inliers.needed() )
                  _inliers.release();
      
              return false;
          }
      

   4. 接下来就是用由RANSAC算法得到的掩码mask，根据该mask，可以获得原先outliers和inliers中的“真的inliers”。最后，就可以使用这些opoints_inliers, ipoints_inliers来求解PnP问题，得到更准确的rvec, tvec

      result = solvePnP(opoints_inliers, ipoints_inliers, cameraMatrix,
                    distCoeffs, rvec, tvec, useExtrinsicGuess,
                    (flags == SOLVEPNP_P3P || flags == SOLVEPNP_AP3P) ? SOLVEPNP_EPNP : flags) ? 1 : -1;