[Fix] Add tolerance threshold to correct planar patch detection for n…

…oise-free planes (issue #6550) (#6608)

The coplanar patches for noise-free plane are not merged. The reason is normal deviation diff threshold is 1, and the dot product of direction normal (plane patches) would be too. So the robust planar test will fail on this.

And also the box size need to be larger than zero, but for ideal points, it maybe calculated to zero. So we need to set a tolerance on this too.

cpp/open3d/geometry/PointCloudPlanarPatchDetection.cpp

@@ -27,6 +27,8 @@ namespace geometry {
 
 namespace {
 
+double tolerance = 1e-6;
+
 /// \brief Planar patch container
 struct PlanarPatch {
     double GetSignedDistanceToPoint(const Eigen::Vector3d& point) const {
@@ -400,9 +402,13 @@ class PlaneDetector {
                 (rect.bottom_left(1) + rect.top_right(1)) / 2. * rect.B.col(1);
 
         // Scale basis to fit points
-        const double width = (rect.top_right.x() - rect.bottom_left.x());
-        const double height = (rect.top_right.y() - rect.bottom_left.y());
-        const double depth = (rect.top_right.z() - rect.bottom_left.z());
+        const double _mini = std::min(tolerance, max_point_dist_);
+        const double width =
+                std::max(rect.top_right.x() - rect.bottom_left.x(), _mini);
+        const double height =
+                std::max(rect.top_right.y() - rect.bottom_left.y(), _mini);
+        const double depth =
+                std::max(rect.top_right.z() - rect.bottom_left.z(), _mini);
 
         std::shared_ptr<OrientedBoundingBox> obox =
                 std::make_shared<OrientedBoundingBox>();
@@ -590,6 +596,7 @@ class PlaneDetector {
         // Use lower bound of the spread around the median as an indication
         // of how similar the point normals associated with the patch are.
         GetMinMaxRScore(normal_similarities, min_normal_diff_, tmp, 3);
+        min_normal_diff_ = std::min(min_normal_diff_, 1.0 - tolerance);
         // Use upper bound of the spread around the median as an indication
         // of how close the points associated with the patch are to the patch.
         GetMinMaxRScore(point_distances, tmp, max_point_dist_, 3);
@@ -937,7 +944,6 @@ void ExtractPatchesFromPlanes(
             Eigen::Vector3d(0.4660, 0.6740, 0.1880),
             Eigen::Vector3d(0.3010, 0.7450, 0.9330),
             Eigen::Vector3d(0.6350, 0.0780, 0.1840)};
-
     for (size_t i = 0; i < planes.size(); i++) {
         if (!planes[i]->IsFalsePositive()) {
             // create a patch by delimiting the plane using its perimeter points