PCL 两个平面的交线

Σίσυφος1900

已于 2023-06-26 17:09:28 修改

阅读量633

点赞数

分类专栏： PCL C++ 数学之美文章标签： 3d 算法平面

于 2023-04-14 14:38:02 首次发布

本文链接：https://blog.csdn.net/weixin_39354845/article/details/130153151

版权

PCL 同时被 3 个专栏收录

56 篇文章 17 订阅

订阅专栏

C++

48 篇文章 6 订阅

订阅专栏

数学之美

24 篇文章 4 订阅

订阅专栏

该代码段展示了在PCL库中使用模板函数计算两个平面交点的算法。通过归一化平面法向量，检测平面是否平行，然后利用Lagrange乘子法构造线性方程组来求解交点。最终，通过Eigen矩阵运算求得交点所在的直线参数表示。

摘要由CSDN通过智能技术生成

一、原理

Intersection Of Two Planes john krumn

PCL 中实现


template <typename Scalar> bool 
pcl::planeWithPlaneIntersection (const Eigen::Matrix<Scalar, 4, 1> &plane_a, 
                                 const Eigen::Matrix<Scalar, 4, 1> &plane_b,
                                 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line,
                                 double angular_tolerance)
{
  typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
  typedef Eigen::Matrix<Scalar, 4, 1> Vector4;
  typedef Eigen::Matrix<Scalar, 5, 1> Vector5;
  typedef Eigen::Matrix<Scalar, 5, 5> Matrix5;

  // Normalize plane normals
  Vector3 plane_a_norm (plane_a.template head<3> ());
  Vector3 plane_b_norm (plane_b.template head<3> ());
  plane_a_norm.normalize ();
  plane_b_norm.normalize ();

  // Test if planes are parallel
  double test_cos = plane_a_norm.dot (plane_b_norm);
  double tolerance_cos = 1 - sin (fabs (angular_tolerance));

  if (fabs (test_cos) > tolerance_cos)
  {
      PCL_DEBUG ("Plane A and Plane B are parallel.\n");
      return (false);
  }

  Vector4 line_direction = plane_a.cross3 (plane_b);
  line_direction.normalized();

  // Construct system of equations using lagrange multipliers with one objective function and two constraints
  Matrix5 langrange_coefs;
  langrange_coefs << 2,0,0, plane_a[0], plane_b[0],  
                     0,2,0, plane_a[1], plane_b[1],
                     0,0,2, plane_a[2], plane_b[2],
                     plane_a[0], plane_a[1], plane_a[2], 0, 0,
                     plane_b[0], plane_b[1], plane_b[2], 0, 0;

  Vector5 b;
  b << 0, 0, 0, -plane_a[3], -plane_b[3];

  line.resize(6);
  // Solve for the lagrange multipliers
  line.template head<3>() = langrange_coefs.colPivHouseholderQr().solve(b).template head<3> ();
  line.template tail<3>() = line_direction.template head<3>();
  return (true);
}

应用：


int  IntersectionWithTwoPlanes(Eigen::Vector4f  & plane_a, Eigen::Vector4f  &plane_b, Eigen::VectorXf  &line)
{
	pcl::planeWithPlaneIntersection(plane_a, plane_b, line, 1e-6);
	return 0;
}