PCL 两个平面的交线

一、原理

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;
}