Fixes #6291 and #6236 (#6305)

* fix 6291 * fix 6236 * address review comments
isl-org · Aug 15, 2023 · 1012819 · 1012819
1 parent a89031c
commit 1012819
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 11 deletions.
diff --git a/cpp/open3d/geometry/PointCloudSegmentation.cpp b/cpp/open3d/geometry/PointCloudSegmentation.cpp
@@ -66,23 +66,24 @@ class RANSACResult {
 };
 
 // Calculates the number of inliers given a list of points and a plane model,
-// and the total distance between the inliers and the plane. These numbers are
-// then used to evaluate how well the plane model fits the given points.
+// and the total squared point-to-plane distance.
+// These numbers are then used to evaluate how well the plane model fits the
+// given points.
 RANSACResult EvaluateRANSACBasedOnDistance(
         const std::vector<Eigen::Vector3d> &points,
         const Eigen::Vector4d plane_model,
         std::vector<size_t> &inliers,
-        double distance_threshold,
-        double error) {
+        double distance_threshold) {
     RANSACResult result;
 
+    double error = 0;
     for (size_t idx = 0; idx < points.size(); ++idx) {
         Eigen::Vector4d point(points[idx](0), points[idx](1), points[idx](2),
                               1);
         double distance = std::abs(plane_model.dot(point));
 
         if (distance < distance_threshold) {
-            error += distance;
+            error += distance * distance;
             inliers.emplace_back(idx);
         }
     }
@@ -93,7 +94,7 @@ RANSACResult EvaluateRANSACBasedOnDistance(
         result.inlier_rmse_ = 0;
     } else {
         result.fitness_ = (double)inlier_num / (double)points.size();
-        result.inlier_rmse_ = error / std::sqrt((double)inlier_num);
+        result.inlier_rmse_ = std::sqrt(error / (double)inlier_num);
     }
     return result;
 }
@@ -204,10 +205,9 @@ std::tuple<Eigen::Vector4d, std::vector<size_t>> PointCloud::SegmentPlane(
             continue;
         }
 
-        double error = 0;
         inliers.clear();
         auto this_result = EvaluateRANSACBasedOnDistance(
-                points_, plane_model, inliers, distance_threshold, error);
+                points_, plane_model, inliers, distance_threshold);
 #pragma omp critical
         {
             if (this_result.fitness_ > result.fitness_ ||

diff --git a/cpp/open3d/geometry/RGBDImageFactory.cpp b/cpp/open3d/geometry/RGBDImageFactory.cpp
@@ -18,7 +18,9 @@ std::shared_ptr<RGBDImage> RGBDImage::CreateFromColorAndDepth(
         bool convert_rgb_to_intensity /* = true*/) {
     std::shared_ptr<RGBDImage> rgbd_image = std::make_shared<RGBDImage>();
     if (color.height_ != depth.height_ || color.width_ != depth.width_) {
-        utility::LogError("Unsupported image format.");
+        utility::LogError(
+                "RGB image size ({} {}) and depth image size ({} {}) mismatch.",
+                color.height_, color.width_, depth.height_, depth.width_);
     }
     rgbd_image->depth_ =
             *depth.ConvertDepthToFloatImage(depth_scale, depth_trunc);
@@ -55,7 +57,9 @@ std::shared_ptr<RGBDImage> RGBDImage::CreateFromSUNFormat(
         bool convert_rgb_to_intensity /* = true*/) {
     std::shared_ptr<RGBDImage> rgbd_image = std::make_shared<RGBDImage>();
     if (color.height_ != depth.height_ || color.width_ != depth.width_) {
-        utility::LogError("Unsupported image format.");
+        utility::LogError(
+                "RGB image size ({} {}) and depth image size ({} {}) mismatch.",
+                color.height_, color.width_, depth.height_, depth.width_);
     }
     for (int v = 0; v < depth.height_; v++) {
         for (int u = 0; u < depth.width_; u++) {
@@ -75,7 +79,9 @@ std::shared_ptr<RGBDImage> RGBDImage::CreateFromNYUFormat(
         bool convert_rgb_to_intensity /* = true*/) {
     std::shared_ptr<RGBDImage> rgbd_image = std::make_shared<RGBDImage>();
     if (color.height_ != depth.height_ || color.width_ != depth.width_) {
-        utility::LogError("Unsupported image format.");
+        utility::LogError(
+                "RGB image size ({} {}) and depth image size ({} {}) mismatch.",
+                color.height_, color.width_, depth.height_, depth.width_);
     }
     for (int v = 0; v < depth.height_; v++) {
         for (int u = 0; u < depth.width_; u++) {