pcl采样一致性模型介绍

采样一致性可以简单高效的检测出一些数学模型，主要是针对具有数学表达式的目标模型。pcl中sample consensus模块中不仅包含各种的采样一致性估计方法，也包含了一些已经编写好的数学模型，下面主要介绍一下pcl中的采样一致性模型。

1、pcl::SampleConsensusModelCircle2D< PointT > Class Template Reference

SampleConsensusModelCircle2D 定义了在 X-Y 平面上进行 2D 圆分割的模型。包含圆心和半径三个参数。
模型系数定义为：
center.x : 圆心的 X 坐标
center.y : 圆心的 Y 坐标
radius : 圆的半径

   // Center (x, y)
   model_coefficients[0] = static_cast<float> ((m[0] * u[0] -  m[1] * v[0]  - uvdif[1] )             / (m[0] - m[1]));
   model_coefficients[1] = static_cast<float> ((m[0] * m[1] * uvdif[0] +  m[0] * v[1] - m[1] * u[1]) / (m[0] - m[1]));
  
   // Radius
   model_coefficients[2] = static_cast<float> (sqrt ((model_coefficients[0] - p0[0]) * (model_coefficients[0] - p0[0]) +
                                                     (model_coefficients[1] - p0[1]) * (model_coefficients[1] - p0[1])));
   PCL_DEBUG ("[pcl::SampleConsensusModelCircle2D::computeModelCoefficients] Model is (%g,%g,%g).\n",
              model_coefficients[0], model_coefficients[1], model_coefficients[2]);

上述源码表示计算圆心和半径的方法。

2、pcl::SampleConsensusModelCircle3D< PointT > Class Template Reference

SampleConsensusModelCircle3D 定义了一个用于 3D 圆分割的模型。
模型系数定义为：
center.x : 圆心的 X 坐标
center.y : 圆心的 Y 坐标
center.z : 圆心的 Z 坐标
radius : 圆的半径
normal.x : 法线方向的 X 坐标
normal.y : 法线方向的 Y 坐标
normal.z : 法线方向的 Z 坐标

   Eigen::Vector3d common_helper_vec = helper_vec01.cross (helper_vec12);
  
   double commonDividend = 2.0 * common_helper_vec.squaredNorm ();
  
   double alpha = (helper_vec12.squaredNorm () * helper_vec01.dot (helper_vec02)) / commonDividend;
   double beta =  (helper_vec02.squaredNorm () * helper_vec10.dot (helper_vec12)) / commonDividend;
   double gamma = (helper_vec01.squaredNorm () * helper_vec20.dot (helper_vec21)) / commonDividend;
  
   Eigen::Vector3d circle_center = alpha * p0 + beta * p1 + gamma * p2;
  
   Eigen::Vector3d circle_radiusVector = circle_center - p0;
   double circle_radius = circle_radiusVector.norm ();
   Eigen::Vector3d circle_normal = common_helper_vec.normalized (); 
  
   model_coefficients[0] = static_cast<float> (circle_center[0]);
   model_coefficients[1] = static_cast<float> (circle_center[1]);
   model_coefficients[2] = static_cast<float> (circle_center[2]);
   model_coefficients[3] = static_cast<float> (circle_radius);
   model_coefficients[4] = static_cast<float> (circle_normal[0]);
   model_coefficients[5] = static_cast<float> (circle_normal[1]);
   model_coefficients[6] = static_cast<float> (circle_normal[2]);

上述源码表示计算圆心和半径以及法线方向的方法，总共七个参数。

3、pcl::SampleConsensusModelCone< PointT, PointNT > Class Template Reference

SampleConsensusModelCone 定义了一个用于 3D 圆锥分割的模型。
模型系数定义为：
apex.x : 圆锥顶点的 X 坐标
apex.y : 圆锥顶点的 Y 坐标
apex.z : 圆锥顶点的 Z 坐标
axis_direction.x : 圆锥轴方向的 X 坐标
axis_direction.y : 圆锥轴方向的 Y 坐标
axis_direction.z : 圆锥轴方向的 Z 坐标
opening_angle : 圆锥的开口角度

   //compute axis (normal of plane defined by: { apex+(p1-apex)/(||p1-apex||), apex+(p2-apex)/(||p2-apex||), apex+(p3-apex)/(||p3-apex||)}
   Eigen::Vector4f ap1 = p1 - apex;
   Eigen::Vector4f ap2 = p2 - apex;
   Eigen::Vector4f ap3 = p3 - apex;
  
   Eigen::Vector4f np1 = apex + (ap1/ap1.norm ());
   Eigen::Vector4f np2 = apex + (ap2/ap2.norm ());
   Eigen::Vector4f np3 = apex + (ap3/ap3.norm ());
  
   Eigen::Vector4f np1np2 = np2 - np1;
   Eigen::Vector4f np1np3 = np3 - np1;
  
   Eigen::Vector4f axis_dir = np1np2.cross3 (np1np3);
   axis_dir.normalize ();
  
   // normalize the vector (apex->p) for opening angle calculation
   ap1.normalize ();
   ap2.normalize ();
   ap3.normalize ();
  
   //compute opening angle
   float opening_angle = ( std::acos (ap1.dot (axis_dir)) + std::acos (ap2.dot (axis_dir)) + std::acos (ap3.dot (axis_dir)) ) / 3.0f;
  
   model_coefficients.resize (model_size_);
   // model_coefficients.template head<3> ()    = line_pt.template head<3> ();
   model_coefficients[0] = apex[0];
   model_coefficients[1] = apex[1];
   model_coefficients[2] = apex[2];
   // model_coefficients.template segment<3> (3) = line_dir.template head<3> ();
   model_coefficients[3] = axis_dir[0];
   model_coefficients[4] = axis_dir[1];
   model_coefficients[5] = axis_dir[2];
   // cone radius
   model_coefficients[6] = opening_angle;

上述代码表示计算圆锥顶点坐标，圆锥轴方向和圆锥的开口角度的方法。

4、pcl::SampleConsensusModelCylinder< PointT, PointNT > Class Template Reference

SampleConsensusModelCylinder 定义了一个用于 3D 圆柱分割的模型。
模型系数定义为：
point_on_axis.x ：位于圆柱轴上的点的 X 坐标
point_on_axis.y ：位于圆柱轴上的点的 Y 坐标
point_on_axis.z ：位于圆柱轴上的点的 Z 坐标
axis_direction.x ：圆柱体轴方向的X坐标
axis_direction.y ：圆柱体轴方向的Y坐标
axis_direction.z ：圆柱体轴方向的Z坐标
radius : 圆柱体的半径

   // point_on_axis, axis_direction
   Eigen::Vector4f line_pt  = p1 + n1 + sc * n1;
   Eigen::Vector4f line_dir = p2 + tc * n2 - line_pt;
   line_dir.normalize ();
  
   model_coefficients.resize (model_size_);
   // model_coefficients.template head<3> ()    = line_pt.template head<3> ();
   model_coefficients[0] = line_pt[0];
   model_coefficients[1] = line_pt[1];
   model_coefficients[2] = line_pt[2];
   // model_coefficients.template segment<3> (3) = line_dir.template head<3> ();
   model_coefficients[3] = line_dir[0];
   model_coefficients[4] = line_dir[1];
   model_coefficients[5] = line_dir[2];
   // cylinder radius
   model_coefficients[6] = static_cast<float> (sqrt (pcl::sqrPointToLineDistance (p1, line_pt, line_dir)));

上述为计算圆柱轴上的点、圆柱体轴方向和圆柱体的半径的部分代码。

5、pcl::SampleConsensusModelLine< PointT > Class Template Reference

SampleConsensusModelLine 定义了一个用于 3D 线分割的模型。
模型系数定义为：
point_on_line.x : 线上一个点的 X 坐标
point_on_line.y : 线上一个点的 Y 坐标
point_on_line.z : 线上一个点的 Z 坐标
line_direction.x : 直线方向的 X 坐标
line_direction.y : 直线方向的 Y 坐标
line_direction.z : 直线方向的 Z 坐标

   model_coefficients.resize (model_size_);
   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];
  
   model_coefficients.template tail<3> ().normalize ();

根据采样点计算直线的方向和一个直线上的点。

6、pcl::SampleConsensusModelNormalParallelPlane< PointT, PointNT > Class Template Reference

SampleConsensusModelNormalParallelPlane 定义了一个使用附加表面法线约束的 3D 平面分割模型。

基本上，这意味着检查内点不仅涉及“到模型的距离”标准，而且还涉及平面法线和内点法线之间的额外“最大角度偏差”。 此外，平面法线必须平行于用户指定的轴。 这意味着平面本身将垂直于该轴，类似于 SACMODEL_PERPENDICULAR_PLANE 。
模型系数定义为：
a : 平面法线的 X 坐标（归一化）
b : 平面法线的 Y 坐标（归一化）
c : 平面法线的 Z 坐标（归一化）
d : 平面方程的第四个 Hessian 分量
请记住，需要指定一个大于 0 的角度进行轴角约束！
此外，可以指定更多的约束，例如：
一个轴，我们需要沿着它搜索垂直于 (setAxis) 的平面；
平面法线与上述给定轴之间的角度公差阈值（setEpsAngle）；
我们期望飞机与原点的距离（setDistanceFromOrigin）；
距离公差，作为与上述给定距离的最大允许偏差（setEpsDist）。

     // Obtain the plane normal
     Eigen::Vector4f coeff = model_coefficients;
     coeff[3] = 0.0f;
     coeff.normalize ();
  
     if (std::abs (axis_.dot (coeff)) < cos_angle_)
     {
       PCL_DEBUG ("[pcl::SampleConsensusModelNormalParallelPlane::isModelValid] Angle between plane normal and given axis is too large.\n");
       return  (false);
     }

根据夹角进行判断。
class SampleConsensusModelNormalParallelPlane : public SampleConsensusModelNormalPlane<PointT, PointNT>
这个类继承自SampleConsensusModelNormalPlane一些函数并没有重写，直接调用基类的函数成员。

7、pcl::SampleConsensusModelNormalPlane< PointT, PointNT > Class Template Reference

SampleConsensusModelNormalPlane 定义了一个使用附加表面法线约束的 3D 平面分割模型。
基本上，这意味着检查内点不仅涉及“到模型的距离”标准，而且还涉及平面法线和内点法线之间的额外“最大角度偏差”。
模型系数定义为：
a : 平面法线的 X 坐标（归一化）
b : 平面法线的 Y 坐标（归一化）
c : 平面法线的 Z 坐标（归一化）
d : 平面方程的第四个 Hessian 分量
要设置内点估计过程中表面法线的影响，请设置法线权重（0.0-1.0）

template <typename PointT, typename PointNT>
   class SampleConsensusModelNormalPlane : public SampleConsensusModelPlane<PointT>, public SampleConsensusModelFromNormals<PointT, PointNT>

继承自SampleConsensusModelPlane和SampleConsensusModelFromNormals，计算平面参数的方法在基类中，SampleConsensusModelNormalPlane类重写了一些添加限制的方法。

8、pcl::SampleConsensusModelNormalSphere< PointT, PointNT > Class Template Reference

SampleConsensusModelNormalSphere 使用附加的表面法线约束定义了一个用于 3D 球体分割的模型。
基本上，这意味着检查内点不仅涉及“到模型的距离”标准，而且还涉及球体法线和内点法线之间的额外“最大角度偏差”。
模型系数定义为：
center.x : 球体中心的 X 坐标
center.y ：球体中心的 Y 坐标
center.z : 球体中心的 Z 坐标
radius : 球体的半径

template <typename PointT, typename PointNT>
   class SampleConsensusModelNormalSphere : public SampleConsensusModelSphere<PointT>, public SampleConsensusModelFromNormals<PointT, PointNT>

继承自SampleConsensusModelSphere和SampleConsensusModelFromNormals，和上述类的结构相同。

9、pcl::SampleConsensusModelParallelLine< PointT > Class Template Reference

SampleConsensusModelParallelLine 使用附加角度约束定义 3D 线分割模型。
检查内点不仅涉及“到模型的距离”标准，还涉及线方向和用户指定轴之间的附加“最大角度偏差”。
模型系数定义为：
point_on_line.x : 线上一个点的 X 坐标
point_on_line.y : 线上一个点的 Y 坐标
point_on_line.z : 线上一个点的 Z 坐标
line_direction.x : 直线方向的 X 坐标
line_direction.y : 直线方向的 Y 坐标
line_direction.z : 直线方向的 Z 坐标

template <typename PointT>
   class SampleConsensusModelParallelLine : public SampleConsensusModelLine<PointT>

继承自SampleConsensusModelLine，计算模型参数的方法在基类中，和上述类结构相同。

10、pcl::SampleConsensusModelParallelPlane< PointT > Class Template Reference

SampleConsensusModelParallelPlane 使用附加的角度约束定义了一个用于 3D 平面分割的模型。

该平面必须在用户指定的角度阈值 (setEpsAngle) 内平行于用户指定的轴 (setAxis)。 换句话说，平面法线必须（几乎）垂直于指定的轴。
a : 平面法线的 X 坐标（归一化）
b : 平面法线的 Y 坐标（归一化）
c : 平面法线的 Z 坐标（归一化）
d : 平面方程的第四个 Hessian 分量

template <typename PointT>
   class SampleConsensusModelParallelPlane : public SampleConsensusModelPlane<PointT>

基类SampleConsensusModelPlane中包含计算平面参数的方法。

11、pcl::SampleConsensusModelPerpendicularPlane< PointT > Class Template Reference

SampleConsensusModelPerpendicularPlane 定义了一个使用附加角度约束的 3D 平面分割模型。
平面必须垂直于用户指定的轴 (setAxis)，直到用户指定的角度阈值 (setEpsAngle)。 换句话说，平面法线必须（几乎）平行于指定的轴。 模型系数定义为：
a : 平面法线的 X 坐标（归一化）
b : 平面法线的 Y 坐标（归一化）
c : 平面法线的 Z 坐标（归一化）
d : 平面方程的第四个 Hessian 分量

template <typename PointT>
   class SampleConsensusModelPerpendicularPlane : public SampleConsensusModelPlane<PointT>

基类SampleConsensusModelPlane中包含计算平面参数的方法，同上。

12、pcl::SampleConsensusModelPlane< PointT > Class Template Reference

SampleConsensusModelPlane 定义了一个用于 3D 平面分割的模型。
模型系数定义为：
a : 平面法线的 X 坐标（归一化）
b : 平面法线的 Y 坐标（归一化）
c : 平面法线的 Z 坐标（归一化）
d : 平面方程的第四个 Hessian 分量

   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 ()));

计算平面参数的方法。

13、pcl::SampleConsensusModelSphere< PointT > Class Template Reference

SampleConsensusModelSphere 定义了一个用于 3D 球体分割的模型。
模型系数定义为：
center.x : 球体中心的 X 坐标
center.y ：球体中心的 Y 坐标
center.z : 球体中心的 Z 坐标
radius : 球体的半径

   // Center (x , y, z)
   model_coefficients.resize (model_size_);
   model_coefficients[0] = 0.5f * m12 / m11;
   model_coefficients[1] = 0.5f * m13 / m11;
   model_coefficients[2] = 0.5f * m14 / m11;
   // Radius
   model_coefficients[3] = std::sqrt (model_coefficients[0] * model_coefficients[0] +
                                      model_coefficients[1] * model_coefficients[1] +
                                      model_coefficients[2] * model_coefficients[2] - m15 / m11);

计算球形和半径。

14、pcl::SampleConsensusModelStick< PointT > Class Template Reference

SampleConsensusModelStick 定义了一个用于 3D 棒分割的模型。
棒是用户给定最小/最大宽度的线。 模型系数定义为：
point_on_line.x : 线上一个点的 X 坐标
point_on_line.y : 线上一个点的 Y 坐标
point_on_line.z : 线上一个点的 Z 坐标
line_direction.x : 直线方向的 X 坐标
line_direction.y : 直线方向的 Y 坐标
line_direction.z : 直线方向的 Z 坐标
line_width : 线的宽度

   model_coefficients.resize (model_size_);
   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[4] = (*input_)[samples[1]].y;
   model_coefficients[5] = (*input_)[samples[1]].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];
  
 //  model_coefficients.template segment<3> (3).normalize ();
   // We don't care about model_coefficients[6] which is the width (radius) of the stick

第七个参数没有计算。

除此之外，还有两个关于配准的类

pcl::SampleConsensusModelRegistration2D< PointT > Class Template Reference
pcl::SampleConsensusModelRegistration< PointT > Class Template Reference

以上两个类采用采样一致性的方法进行点到点的配准，在此不作过多介绍。