Merge pull request PointCloudLibrary#1698 from sand-box/intersection_fix

Fix issue PointCloudLibrary#1695. Test for parallel planes in planePlaneIntersection.
SergioRAgostinho · Aug 26, 2016 · d34e5a9 · d34e5a9
2 parents a7bcb98 + 8e8a778
commit d34e5a9
Showing 1 changed file with 3 additions and 4 deletions.
diff --git a/common/include/pcl/common/impl/intersections.hpp b/common/include/pcl/common/impl/intersections.hpp
@@ -89,12 +89,11 @@ pcl::planeWithPlaneIntersection (const Eigen::Matrix<Scalar, 4, 1> &plane_a,
   plane_a_norm.normalize ();
   plane_b_norm.normalize ();
 
-  // Test if planes are parallel (test_cos == 1)
+  // Test if planes are parallel
   double test_cos = plane_a_norm.dot (plane_b_norm);
-  double upper_limit = 1 + angular_tolerance;
-  double lower_limit = 1 - angular_tolerance;
+  double tolerance_cos = 1 - sin (fabs (angular_tolerance));
 
-  if ((test_cos > lower_limit) && (test_cos < upper_limit))
+  if (fabs (test_cos) > tolerance_cos)
   {
       PCL_DEBUG ("Plane A and Plane B are parallel.\n");
       return (false);