Merge pull request #571 from VictorLamoine/intersections

Add threePlanesIntersection
PointCloudLibrary · Mar 15, 2014 · 7525218 · 7525218
2 parents 4c1675c + 380ff1b
commit 7525218
Show file tree

Hide file tree

Showing 3 changed files with 104 additions and 0 deletions.
diff --git a/common/include/pcl/common/intersections.h b/common/include/pcl/common/intersections.h
@@ -86,7 +86,27 @@ namespace pcl
                               const Eigen::Vector4f &fplane_b,
                               Eigen::VectorXf &line,
                               double angular_tolerance = 0.1);
+
+  /** \brief Determine the point of intersection of three non-parallel planes by solving the equations.
+    * \note If using nearly parralel planes you can lower the determinant_tolerance value. This can
+    * lead to inconsistent results.
+    * If the three planes intersects in a line the point will be anywhere on the line.
+    * \param[in] plane_a are the coefficients of the first plane in the form ax + by + cz + d = 0
+    * \param[in] plane_b are the coefficients of the second plane
+    * \param[in] plane_c are the coefficients of the third plane
+    * \param[in] determinant_tolerance is a limit to determine whether planes are parallel or not
+    * \param[out] intersection_point, the three coordinates x, y, z of the intersection point
+    * \return true if succeeded/planes aren't parallel
+    */
+  PCL_EXPORTS bool
+  threePlanesIntersection (const Eigen::Vector4f &plane_a, 
+                           const Eigen::Vector4f &plane_b,
+                           const Eigen::Vector4f &plane_c,
+                           Eigen::Vector3f &intersection_point,
+                           double determinant_tolerance = 1e-6);
+
 }
 /*@}*/
 #endif  //#ifndef PCL_INTERSECTIONS_H_
 
+
diff --git a/common/src/intersections.cpp b/common/src/intersections.cpp
@@ -113,3 +113,48 @@ pcl::planeWithPlaneIntersection (const Eigen::Vector4f &plane_a,
   line[5] = line_direction[2];
   return true;
 } 
+
+bool 
+pcl::threePlanesIntersection (const Eigen::Vector4f &plane_a, 
+                              const Eigen::Vector4f &plane_b,
+                              const Eigen::Vector4f &plane_c,
+                              Eigen::Vector3f &intersection_point,
+                              double determinant_tolerance)
+{
+  // Check if some planes are parallel
+  Eigen::Matrix3f normals_in_lines;
+
+  for (int i = 0; i < 3; i++)
+  {
+    normals_in_lines (i, 0) = plane_a[i];
+    normals_in_lines (i, 1) = plane_b[i];
+    normals_in_lines (i, 2) = plane_c[i];
+  }
+
+  double determinant = normals_in_lines.determinant ();
+  if (fabs (determinant) < determinant_tolerance)
+  {
+    // det ~= 0
+    PCL_ERROR ("Two or more planes are parralel.\n");
+    return (false);
+  }
+
+  // Left part of the 3 equations
+  Eigen::Matrix3f left_member;
+
+  for (int i = 0; i < 3; i++)
+  {
+    left_member (0, i) = plane_a[i];
+    left_member (1, i) = plane_b[i];
+    left_member (2, i) = plane_c[i];
+  }
+
+  // Right side of the 3 equations
+  Eigen::Vector3f right_member;
+  right_member << -plane_a[3], -plane_b[3], -plane_c[3];
+
+  // Solve the system
+  intersection_point = left_member.fullPivLu ().solve (right_member);
+  return (true);
+}
+
diff --git a/test/common/test_plane_intersection.cpp b/test/common/test_plane_intersection.cpp
@@ -301,6 +301,44 @@ TEST (PCL, planeWithPlaneIntersection)
 
 }
 
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+TEST (PCL, threePlanesIntersection)
+{
+  // Testing 2 parallel planes
+  Eigen::Vector4f plane_a (1.0f, 0.0f, 0.0f, -0.5f);
+  Eigen::Vector4f plane_b (1.0f, 0.0f, 0.0f, 0.5f);
+  Eigen::Vector4f plane_c (0.0f, 0.0f, 1.0f, -0.5f);
+  Eigen::Vector3f point (1.0f, 2.0f, 3.0f);
+
+  std::cout << std::endl;
+  EXPECT_FALSE (threePlanesIntersection (plane_a, plane_b, plane_c, point, 1e-6));
+  std::cout << std::endl;
+  EXPECT_FALSE (threePlanesIntersection (plane_a, plane_b, plane_c, point, 1e-3));
+  EXPECT_FLOAT_EQ (1.0f, point (0));
+  EXPECT_FLOAT_EQ (2.0f, point (1));
+  EXPECT_FLOAT_EQ (3.0f, point (2));
+
+  //perfect box
+  plane_b << 0.0f, 1.0f, 0.0f, 0.5f;
+
+  std::cout << std::endl;
+  EXPECT_TRUE (threePlanesIntersection (plane_a, plane_b, plane_c, point));
+  EXPECT_FLOAT_EQ (0.5f, point (0));
+  EXPECT_FLOAT_EQ (-0.5f, point (1));
+  EXPECT_FLOAT_EQ (0.5f, point (2));
+
+  //general planes
+  plane_a << 1.4564f, 0.5465f, -0.1325f, 0.4685f;
+  plane_b << -1.5619f, 5.5461f, 5.4569f, 2.9414f;
+  plane_c << 0.9852f, 654.55f, -0.1546f, -45.1516f;
+
+  std::cout << std::endl;
+  EXPECT_TRUE (threePlanesIntersection (plane_a, plane_b, plane_c, point));
+  EXPECT_NEAR (-0.413977f, point (0), 1e-4);
+  EXPECT_NEAR (0.0694323f, point (1), 1e-4);
+  EXPECT_NEAR (-0.728082f, point (2), 1e-4);
+}
+
 //* ---[ */
 int
 main (int argc, char** argv)
@@ -310,3 +348,4 @@ main (int argc, char** argv)
 }
 /* ]--- */
 
+