diff --git a/cpp/open3d/geometry/PointCloudSegmentation.cpp b/cpp/open3d/geometry/PointCloudSegmentation.cpp index 8e4f65f626b..5603f303ca0 100644 --- a/cpp/open3d/geometry/PointCloudSegmentation.cpp +++ b/cpp/open3d/geometry/PointCloudSegmentation.cpp @@ -64,23 +64,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 &points, const Eigen::Vector4d plane_model, std::vector &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); } } @@ -91,7 +92,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; } @@ -202,10 +203,9 @@ std::tuple> 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 index 1a2b5f0ff09..1f2819ebe3a 100644 --- a/cpp/open3d/geometry/RGBDImageFactory.cpp +++ b/cpp/open3d/geometry/RGBDImageFactory.cpp @@ -18,7 +18,9 @@ std::shared_ptr RGBDImage::CreateFromColorAndDepth( bool convert_rgb_to_intensity /* = true*/) { std::shared_ptr rgbd_image = std::make_shared(); 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::CreateFromSUNFormat( bool convert_rgb_to_intensity /* = true*/) { std::shared_ptr rgbd_image = std::make_shared(); 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::CreateFromNYUFormat( bool convert_rgb_to_intensity /* = true*/) { std::shared_ptr rgbd_image = std::make_shared(); 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++) {