pcl源码分析之随机采样一致性直线拟合

---

前言

直线拟合是点云处理的常用方法之一，这里将pcl里随机采样一致性直线拟合的源码分析一下，加深对原理以及代码的理解。

---

一、应用实例

随机采样一致性直线拟合实例在之前的文章里已经写过。

二、SampleConsensusModelLine类

这里主要看其构造函数，随机采样的样本数量sample_size_ = 2，直线方程参数数量 model_size_ = 6

      SampleConsensusModelLine (const PointCloudConstPtr &cloud, bool random = false) 
        : SampleConsensusModel<PointT> (cloud, random)
      {
        model_name_ = "SampleConsensusModelLine";
        sample_size_ = 2;
        model_size_ = 6;
      }

三、RandomSampleConsensus类

这里实际上和平面拟合是一样的，主要用到了computeModel函数，该函数的解析已经在平面拟合博文里写了，这里不再重复。
直线拟合，平面拟合的流程基本一致，唯一不同的是计算方程参数，毕竟一个是平面，一个是直线，计算直线方程在函数computeModelCoefficients中实现。

1.computeModelCoefficients 函数

该函数功能是计算直线参数

template <typename PointT> bool
pcl::SampleConsensusModelLine<PointT>::computeModelCoefficients (
      const Indices &samples, Eigen::VectorXf &model_coefficients) const
{
  // Make sure that the samples are valid
  if (!isSampleGood (samples))
  {
    PCL_ERROR ("[pcl::SampleConsensusModelLine::computeModelCoefficients] Invalid set of samples given!\n");
    return (false);
  }
 //这里model_size_=6
  model_coefficients.resize (model_size_);
  //两点就可以确定一条直线
  //对于直线方程：t = (x - x0) / A = (y - y0) / B = (z - z0) / C
  //因此这里将第一个点作为（x0, y0, z0）
  model_coefficients[0] = (*input_)[samples[0]].x;
  model_coefficients[1] = (*input_)[samples[0]].y;
  model_coefficients[2] = (*input_)[samples[0]].z;
  //将第二个点减去第一个点的向量作为方向向量
  model_coefficients[3] = (*input_)[samples[1]].x - model_coefficients[0];
  model_coefficients[4] = (*input_)[samples[1]].y - model_coefficients[1];
  model_coefficients[5] = (*input_)[samples[1]].z - model_coefficients[2];

  // This precondition should hold if the samples have been found to be good
  //C/C++ 中的assert是一个宏，用于在运行时检查一个条件是否为真，如果条件不满足，
  //则运行时将终止程序的执行并输出一条错误信息。
  //这里实际上是在判断A * A + B * B + C * C 是否大于零，如果等于零，那后面直线向量做归一化就会出错
  //毕竟分母为零，是会出问题的
  assert (model_coefficients.template tail<3> ().squaredNorm () > 0.0f);
  //直线向量归一化
  model_coefficients.template tail<3> ().normalize ();
  PCL_DEBUG ("[pcl::SampleConsensusModelLine::computeModelCoefficients] Model is (%g,%g,%g,%g,%g,%g).\n",
             model_coefficients[0], model_coefficients[1], model_coefficients[2],
             model_coefficients[3], model_coefficients[4], model_coefficients[5]);
  return (true);
}

2.countWithinDistance函数

该函数功能是判定内点（点到直线距离在一个阈值范围内）

template <typename PointT> std::size_t
pcl::SampleConsensusModelLine<PointT>::countWithinDistance (
      const Eigen::VectorXf &model_coefficients, const double threshold) const
{
  // Needs a valid set of model coefficients
  //直线方程有6个参数，这里就是判定model_coefficients的size是否是6
  if (!isModelValid (model_coefficients))
    return (0);

  double sqr_threshold = threshold * threshold;

  std::size_t nr_p = 0;

  // Obtain the line point and direction
  //直线方程的3个点
  Eigen::Vector4f line_pt  (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
  //直线方程的向量
  Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
  //向量归一化
  line_dir.normalize ();

  // Iterate through the 3d points and calculate the distances from them to the line
  for (std::size_t i = 0; i < indices_->size (); ++i)
  {
    // Calculate the distance from the point to the line
    // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1)
    //这里，p2和p1是直线上的两个点，p0是当前给出的点
    //根据上面的公式：
    //P2-P1 = line_dir（两点相减，其实就是向量，只不过这里归一化了）
    //P1-P0 = line_pt - (*input_)[(*indices_)[i]].getVector4fMap ()
    //因此||(P2-P1) x (P1-P0)|| = (line_pt - (*input_)[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm ()
    //因为p2和p1是直线上的两个点，故P2-P1 = line_dir，且归一化了，则||P2-P1|| = 1
    double sqr_distance = (line_pt - (*input_)[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm ();

    if (sqr_distance < sqr_threshold)
      nr_p++;
  }
  return (nr_p);
}

点到直线距离，写详细点，可以这样写

//点到线距离
float DisPoint2Line(pcl::PointXYZ point, pcl::ModelCoefficients::Ptr coefficients)
{
	Eigen::Vector3f AB;
	AB << coefficients->values[3], coefficients->values[4], coefficients->values[5];

	Eigen::Vector3f AP;
	AP << coefficients->values[0] - point.x, coefficients->values[1] - point.y, coefficients->values[2] - point.z;

	Eigen::Vector3f ABxAP;
	ABxAP = AB.cross(AP);

	float S = ABxAP.norm();
	float d = AB.norm();

	float dis = S / d;

	return dis;
}

3.selectWithinDistance函数

该函数和countWithinDistance类似，就不再做解释了

template <typename PointT> void
pcl::SampleConsensusModelLine<PointT>::selectWithinDistance (
      const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers)
{
  // Needs a valid set of model coefficients
  if (!isModelValid (model_coefficients))
    return;

  double sqr_threshold = threshold * threshold;

  inliers.clear ();
  error_sqr_dists_.clear ();
  inliers.reserve (indices_->size ());
  error_sqr_dists_.reserve (indices_->size ());

  // Obtain the line point and direction
  Eigen::Vector4f line_pt  (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
  Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
  line_dir.normalize ();

  // Iterate through the 3d points and calculate the distances from them to the line
  for (std::size_t i = 0; i < indices_->size (); ++i)
  {
    // Calculate the distance from the point to the line
    // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1)
    double sqr_distance = (line_pt - (*input_)[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm ();

    if (sqr_distance < sqr_threshold)
    {
      // Returns the indices of the points whose squared distances are smaller than the threshold
      inliers.push_back ((*indices_)[i]);
      error_sqr_dists_.push_back (sqr_distance);
    }
  }
}

---

总结

总结了一下pcl里随机采样一致性直线拟合的源码和原理，后续有新的发现再更新。