RANSAC平面拟合理论和代码---PCL源码笔记

RANSAC平面拟合理论和代码—PCL源码笔记

RANSAC平面拟合的原理,首先知道如何定义平面,求平面的方程,求平面的法向量,以及求点到平面的距离。 其次,需要了解RANSAC的原理和公式。

一、平面相关定义

我们知道 A x + B y + C z + d = 0 Ax+By+Cz+d=0 Ax+By+Cz+d=0平面方程的定义。 我们知道,三个点是可以形成一个平面的,因此如果给定三个点,假设 p 0 , p 1 , p 2 p_0, p_1, p_2 p0,p1,p2, 求这三个点所形成的平面,可根据以下步骤:

  1. 求两点之间的向量 p 1 p 0 ⃗ , p 2 p 0 ⃗ \vec{p_1p_0}, \vec{p_2 p_0} p1p0 ,p2p0
    p 1 p 0 = p 1 − p 0 = ( l , m , n ) p 2 p 0 = p 2 − p 0 = ( o , p , q ) p_1 p_0 = p_1 - p_0 = (l, m, n)\\ p_2 p_0 = p_2 - p_0 = (o, p, q) p1p0=p1p0=(l,m,n)p2p0=p2p0=(o,p,q)

  2. 求两向量的向量积,即 p 1 p 0 ⃗ × p 2 p 0 ⃗ \vec{p_1p_0} \times \vec{p_2 p_0} p1p0 ×p2p0
    p 1 p 0 ⃗ × p 2 p 0 ⃗ = ( m q − n p , n o − l q , l p − m o ) \vec{p_1p_0} \times \vec{p_2 p_0} = (mq-np, no-lq, lp-mo) p1p0 ×p2p0 =(mqnp,nolq,lpmo)

  3. 所得向量积即为平面方程 A x + B y + C z + d = 0 Ax+By+Cz+d=0 Ax+By+Cz+d=0法向量
    A = m q − n p B = n o − l q C = l p − m o d = − ( A x 0 + B y 0 + C z 0 ) A = mq-np \\ B = no-lq \\ C = lp-mo \\ d = - (Ax_0+ By_0 + Cz_0) A=mqnpB=nolqC=lpmod=(Ax0+By0+Cz0)

  4. 点到平面的距离
    先来看一下向量在轴上的投影:
    设A,B 两点 x x x 轴上的投影分别为 A 1 , B 1 A_1, B_1 A1,B1, 则向量 A 1 B 1 ⃗ \vec{A_1B_1} A1B1 的长度 ∣ ∣ A 1 B 1 ⃗ ∣ ∣ ||\vec{A_1B_1}|| A1B1 成为向量 A B ⃗ \vec{AB} AB x x x轴上的投影,记为 P r j x A 1 B 1 ⃗ Prj_x \vec{A_1B_1} PrjxA1B1 。设 A 1 B 1 ⃗ \vec{A_1 B_1} A1B1 。设 A B ⃗ \vec{AB} AB x x x轴之间夹角为 α \alpha α, 则

    P r j x A B ⃗ = ∣ ∣ A 1 B 1 ⃗ ∣ ∣ = ∣ A B ⃗ ∣ ∣ c o s α Prj_x \vec{AB} = ||\vec{A_1B_1}|| = |\vec{AB}|| cos \alpha PrjxAB =A1B1 =AB cosα

    点到平面的距离计算
    向量法计算点到平面的距离,就是把点和平面放在直角坐标系下进行计算。这样,点和平面均可用坐标来表示(如图1所示)。

    假设平面方程为:
    A x + B y + C z + d = 0 Ax+By+Cz+d=0 Ax+By+Cz+d=0

    设向量 n ⃗ = ( A , B , C ) \vec{n} = (A, B, C) n =(A,B,C) 为 平面的法向量,平面外一点 M 1 M_1 M1,坐标为 ( x 1 , y 1 , z 1 ) (x_1, y_1, z_1) (x1,y1,z1), 在平面上取一点 M 0 M_0 M0,则点 M 1 M_1 M1到平面的距离 d d d为:

    d = P r j n ⃗ M 0 M 1 ⃗ = ∣ ∣ M 0 M 1 ⃗ ∣ ∣ c o s α d= Prj_{\vec{n}} \vec{M_0 M_1} = ||\vec{M_0 M_1}|| cos \alpha d=Prjn M0M1 =M0M1 cosα

    其中 α \alpha α为向量 n ⃗ \vec{n} n 与向量 M 0 M 1 ⃗ \vec{M_0 M_1} M0M1 的夹角,

    cos ⁡ α = M 0 M 1 ⃗ ⋅ n ⃗ ∣ ∣ M 0 M 1 ⃗ ∣ ∣ ⋅ ∣ ∣ n ⃗ ∣ ∣ \cos \alpha = \frac{\vec{M_0 M_1} \cdot \vec{n}} {||\vec{M_0 M_1}|| \cdot ||\vec{n}||} cosα=M0M1 n M0M1 n

    因此,
    d = M 0 M 1 ⃗ ⋅ n ⃗ ∣ ∣ n ⃗ ∣ ∣ d= \frac{\vec{M_0 M_1} \cdot \vec{n} }{||\vec{n}||} d=n M0M1 n

二、RANSAC原理

理解完平面的定义,法向量求解和点到平面的距离,我们就可以继续探讨RANSAC拟合平面的关键算法: RANSAC算法。

RANSAC算法有以下步骤:

  • 随机选取三个点,根据平面方程 A x + B y + C z + d = 0 Ax+By+Cz+d=0 Ax+By+Cz+d=0, 计算模型参数。
  • 用剩余的点计算到该平面方程的距离,将距离与所设定的阈值做比较,如果小于,则该点为内点;否则为外点。统计该参数模型下内点的个数。
  • 继续执行上面两步,若当前模型的内点数量大于已经保存的最大内点数量,则变更新的模型参数,保留模型参数始终是内点数量最多的模型参数。
  • 重复上述三步,不断迭代,直到达到迭代阈值,找到内点个数最多的模型参数,最后用内点再次对模型参数进行估计,从而得到最终的模型参数。

上面有个阈值需要关注,迭代阈值。 我们如何知道迭代的次数呢?其实,这个值是可以估算出来的:

假设内点在数据中的占比为 p p p,那么每次计算模型使用 k k k个点的情况下,选取点为内点的概率为 p k p^k pk,选取的点为外点的概率为 1 − p k 1-p^k 1pk, 因此尝试 S S S次失败的概率为:
1 − P = ( 1 − p k ) S 1-P = (1-p^k)^S 1P=(1pk)S
都为内点的概率为:
P = 1 − ( 1 − p k ) S P = 1- (1-p^k)^S P=1(1pk)S

通过上面的式子,得到最终需要尝试的次数为:
S = log ⁡ ( 1 − P ) log ⁡ ( 1 − p k ) S = \frac{\log(1-P)}{\log(1-p^k)} S=log(1pk)log(1P)

但是,通常一开始我们无法知道内点在数据中的占比 p p p这个先验,因此可以使用自适应迭代次数的方法。也就是一开始设定一个无穷大的迭代次数,然后每次更新模型参数估计的时候,用当前的内点比值当成 p p p来估算出迭代次数。 又或者设置一个最大的迭代数,但是往往可能无法达到最优解。

三、PCL平面拟合

PCL官方使用RANSAC你和平面的模板。

pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.01);

seg.setInputCloud (cloud);
seg.segment (*inliers, *coefficients);

步骤如下:

1. 定义平面模型参数,和内点索引。
2. 声明分割模型,并设置模型的类别PLANE,拟合模型的方法RANSAC,距离阈值0.01。
3. 设置输入,并获得结果。

1. segment函数

主要关注一下segment这个模板函数,在PCL源码中的sac_segmentation.hpp文件里。下面进行了精简,方便理解。

template <typename PointT> void
pcl::SACSegmentation<PointT>::segment (PointIndices &inliers, ModelCoefficients &model_coefficients){

    // Copy the header information
    inliers.header = model_coefficients.header = input_->header;
    ... 

    // 计算模型
    if (!sac_->computeModel (0))
    {
        PCL_ERROR ("[pcl::%s::segment] Error segmenting the model! No solution found.\n", getClassName ().c_str ());
        deinitCompute ();
        inliers.indices.clear (); model_coefficients.values.clear ();
        return;
    }

    // 获取内点
    sac_->getInliers (inliers.indices);

    // 获取模型参数
    Eigen::VectorXf coeff (model_->getModelSize ());
    sac_->getModelCoefficients (coeff);

    // 如果用户需要优化模型参数
    if (optimize_coefficients_)
    {
        Eigen::VectorXf coeff_refined (model_->getModelSize ());
        model_->optimizeModelCoefficients (inliers.indices, coeff, coeff_refined);
        model_coefficients.values.resize (coeff_refined.size ());
        memcpy (&model_coefficients.values[0], &coeff_refined[0], coeff_refined.size () * sizeof (float));
        // Refine inliers
        model_->selectWithinDistance (coeff_refined, threshold_, inliers.indices);
    }else
    {
        model_coefficients.values.resize (coeff.size ());
        memcpy (&model_coefficients.values[0], &coeff[0], coeff.size () * sizeof (float));
    }

    ...

}

2. computeModel函数

上面的平面拟合计算主要在sac_->computeModel (0)这一句。也就是在computeModel这个模板函数,在PCL源码ransac.hpp中实现。为了方便阅读理解,精简了源码。

template <typename PointT> bool
pcl::RandomSampleConsensus<PointT>::computeModel (int)
{
  // Warn and exit if no threshold was set
  if (threshold_ == std::numeric_limits<double>::max())
  {
    PCL_ERROR ("[pcl::RandomSampleConsensus::computeModel] No threshold set!\n");
    return (false);
  }

  iterations_ = 0;
  std::size_t n_best_inliers_count = 0;
  double k = std::numeric_limits<double>::max();

  Indices selection;
  Eigen::VectorXf model_coefficients (sac_model_->getModelSize ());

  const double log_probability  = std::log (1.0 - probability_);
  const double one_over_indices = 1.0 / static_cast<double> (sac_model_->getIndices ()->size ());

  unsigned skipped_count = 0;

  // suppress infinite loops by just allowing 10 x maximum allowed iterations for invalid model parameters!
  const unsigned max_skip = max_iterations_ * 10;

  int threads = threads_;

    // Iterate
    while (true) // infinite loop with four possible breaks
    {
      // Get X samples which satisfy the model criteria
      // 随机获取样本
      {
        sac_model_->getSamples (iterations_, selection); // The random number generator used when choosing the samples should not be called in parallel
      }

      if (selection.empty ())
      {
        PCL_ERROR ("[pcl::RandomSampleConsensus::computeModel] No samples could be selected!\n");
        break;
      }

      // Search for inliers in the point cloud for the current plane model M
      // 计算模型参数
      if (!sac_model_->computeModelCoefficients (selection, model_coefficients)) // This function has to be thread-safe
      {
        //++iterations_;
        unsigned skipped_count_tmp;

        skipped_count_tmp = ++skipped_count;
        if (skipped_count_tmp < max_skip)
          continue;
        else
          break;
      }

      // Select the inliers that are within threshold_ from the model
      //sac_model_->selectWithinDistance (model_coefficients, threshold_, inliers);
      //if (inliers.empty () && k > 1.0)
      //  continue;

      // 计算点到平面的距离
      std::size_t n_inliers_count = sac_model_->countWithinDistance (model_coefficients, threshold_); // This functions has to be thread-safe. Most work is done here

      std::size_t n_best_inliers_count_tmp;

      n_best_inliers_count_tmp = n_best_inliers_count;

      if (n_inliers_count > n_best_inliers_count_tmp) // This condition is false most of the time, and the critical region is not entered, hopefully leading to more efficient concurrency
      {
          // Better match ?
          if (n_inliers_count > n_best_inliers_count)
          {
            n_best_inliers_count = n_inliers_count; // This write and the previous read of n_best_inliers_count must be consecutive and must not be interrupted!
            n_best_inliers_count_tmp = n_best_inliers_count;

            // Save the current model/inlier/coefficients selection as being the best so far
            model_              = selection;
            model_coefficients_ = model_coefficients;

            // Compute the k parameter (k=std::log(z)/std::log(1-w^n))
            // 计算迭代次数 k 
            const double w = static_cast<double> (n_best_inliers_count) * one_over_indices;
            double p_no_outliers = 1.0 - std::pow (w, static_cast<double> (selection.size ()));
            p_no_outliers = (std::max) (std::numeric_limits<double>::epsilon (), p_no_outliers);       // Avoid division by -Inf
            p_no_outliers = (std::min) (1.0 - std::numeric_limits<double>::epsilon (), p_no_outliers);   // Avoid division by 0.
            k = log_probability / std::log (p_no_outliers);
          } 
      }

      int iterations_tmp;
      double k_tmp;

      iterations_tmp = ++iterations_;

      k_tmp = k;

      if (iterations_tmp > k_tmp)
        break;
      if (iterations_tmp > max_iterations_)
      {
        PCL_DEBUG ("[pcl::RandomSampleConsensus::computeModel] RANSAC reached the maximum number of trials.\n");
        break;
      }
    } // while



  if (model_.empty ())
  {
    PCL_ERROR ("[pcl::RandomSampleConsensus::computeModel] RANSAC found no model.\n");
    inliers_.clear ();
    return (false);
  }

  // Get the set of inliers that correspond to the best model found so far
  sac_model_->selectWithinDistance (model_coefficients_, threshold_, inliers_);
  return (true);
}

3. computeModelCoefficients函数

然后可以看到,computeModelCoefficients 函数,就是用于计算平面参数的,即平面法向量,和上面的理论一一对应:

template <typename PointT> bool
pcl::SampleConsensusModelPlane<PointT>::computeModelCoefficients (
      const Indices &samples, Eigen::VectorXf &model_coefficients) const
{
  // Need 3 samples
  if (samples.size () != sample_size_)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelPlane::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
    return (false);
  }

  pcl::Array4fMapConst p0 = (*input_)[samples[0]].getArray4fMap ();
  pcl::Array4fMapConst p1 = (*input_)[samples[1]].getArray4fMap ();
  pcl::Array4fMapConst p2 = (*input_)[samples[2]].getArray4fMap ();

  // Compute the segment values (in 3d) between p1 and p0
  Eigen::Array4f p1p0 = p1 - p0;
  // Compute the segment values (in 3d) between p2 and p0
  Eigen::Array4f p2p0 = p2 - p0;

  // Avoid some crashes by checking for collinearity here
  Eigen::Array4f dy1dy2 = p1p0 / p2p0;
  if ( p2p0.isZero() || ((dy1dy2[0] == dy1dy2[1]) && (dy1dy2[2] == dy1dy2[1])) )          // Check for collinearity
  {
    return (false);
  }

  // Compute the plane coefficients from the 3 given points in a straightforward manner
  // calculate the plane normal n = (p2-p1) x (p3-p1) = cross (p2-p1, p3-p1)
  model_coefficients.resize (model_size_);
  model_coefficients[0] = p1p0[1] * p2p0[2] - p1p0[2] * p2p0[1];
  model_coefficients[1] = p1p0[2] * p2p0[0] - p1p0[0] * p2p0[2];
  model_coefficients[2] = p1p0[0] * p2p0[1] - p1p0[1] * p2p0[0];
  model_coefficients[3] = 0.0f;

  // Normalize
  model_coefficients.normalize ();

  // ... + d = 0
  model_coefficients[3] = -1.0f * (model_coefficients.template head<4>().dot (p0.matrix ()));

  PCL_DEBUG ("[pcl::SampleConsensusModelPlane::computeModelCoefficients] Model is (%g,%g,%g,%g).\n",
             model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3]);
  return (true);
}

4. countWithinDistance函数

得到平面方程之后,需要计算点到平面的距离 countWithinDistance函数。

template <typename PointT> std::size_t
pcl::SampleConsensusModelPlane<PointT>::countWithinDistance (
      const Eigen::VectorXf &model_coefficients, const double threshold) const
{
  // Needs a valid set of model coefficients
  if (!isModelValid (model_coefficients))
  {
    PCL_ERROR ("[pcl::SampleConsensusModelPlane::countWithinDistance] Given model is invalid!\n");
    return (0);
  }
  return countWithinDistanceStandard (model_coefficients, threshold);
}


pcl::SampleConsensusModelPlane<PointT>::countWithinDistanceStandard (
      const Eigen::VectorXf &model_coefficients, const double threshold, std::size_t i) const
{
  std::size_t nr_p = 0;
  // Iterate through the 3d points and calculate the distances from them to the plane
  for (; i < indices_->size (); ++i)
  {
    // Calculate the distance from the point to the plane normal as the dot product
    // D = (P-A).N/|N|
    Eigen::Vector4f pt ((*input_)[(*indices_)[i]].x,
                        (*input_)[(*indices_)[i]].y,
                        (*input_)[(*indices_)[i]].z,
                        1.0f);
    if (std::abs (model_coefficients.dot (pt)) < threshold)
    {
      nr_p++;
    }
  }
  return (nr_p);
}

值得注意的是,这里只有dot product,而公式:
d = M 0 M 1 ⃗ ⋅ n ⃗ ∣ ∣ n ⃗ ∣ ∣ d= \frac{\vec{M_0 M_1} \cdot \vec{n} }{||\vec{n}||} d=n M0M1 n

是有 n ⃗ ∣ ∣ n ⃗ ∣ ∣ \frac{\vec{n}}{||\vec{n}||} n n ,而代码中没有是因为computeModelCoefficients函数中已经normalize法向量了。

Conclusion

至此,RANSAC 平面拟合的理论和相关的PCL源码学习到此了。但是RANSAC经过了那么多年的发展,已经衍生了很多优化版本。以后有机会会继续学习的。

Reference

  1. PCL平面分割, https://pcl.readthedocs.io/projects/tutorials/en/master/planar_segmentation.html#planar-segmentation
  2. 点到平面距离, https://baike.baidu.com/item/%E7%82%B9%E5%88%B0%E5%B9%B3%E9%9D%A2%E8%B7%9D%E7%A6%BB/10690055?fr=aladdin
  • 5
    点赞
  • 47
    收藏
    觉得还不错? 一键收藏
  • 2
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 2
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值