pythonhistogrampoint-cloudsopen3d

How to erode cloud edges on x,y directions to sharpen edges


I have a noisy pointcloud of a flat surface that I want to estimate its dimensions, so I want to sharpen the edges, to do so I decided to use a statistical approach using mean of histogram to filter points on the edges as follows:

enter image description here

import open3d as o3d
import numpy as np
import matplotlib.pyplot as plt

def filter_cloud_borders(pcl:o3d.geometry.PointCloud, visu:bool=True) -> o3d.geometry.PointCloud:
    #
    min_bbox = pcl.get_minimal_oriented_bounding_box()
    # Box Pose
    box_pose = np.eye(4)
    box_pose[:3, :3] = min_bbox.R
    box_pose[:3, 3] = min_bbox.center
    # align it to center
    pcl.transform(np.linalg.inv(box_pose))
    pcl_pts = np.asarray(pcl.points)

    # compute the histogram of X and Y values separately
    hist_x, bins_x = np.histogram(pcl_pts[:, 0], bins=500)
    hist_y, bins_y = np.histogram(pcl_pts[:, 1], bins=500)

    # compute the mean of the histogram values for X and Y separately
    mean_hist_x = np.mean(hist_x)
    mean_hist_y = np.mean(hist_y)

    # find indices where histogram values are greater than the mean for X and Y
    ## TODO: adjust these limits properly to avoid over cropping
    crop_factor = 0.9
    indices_greater_than_mean_x = np.where(hist_x > crop_factor*mean_hist_x)[0]
    indices_greater_than_mean_y = np.where(hist_y > crop_factor*mean_hist_y)[0]

    # Filter point cloud points based on X and Y values
    filtered_points = pcl_pts[
        (pcl_pts[:, 0] > bins_x[indices_greater_than_mean_x[0]]) & 
        (pcl_pts[:, 0] < bins_x[indices_greater_than_mean_x[-1]]) &
        (pcl_pts[:, 1] > bins_y[indices_greater_than_mean_y[0]]) &
        (pcl_pts[:, 1] < bins_y[indices_greater_than_mean_y[-1]])
    ]

    # apply transformation to the filtered points
    transformed_points = np.hstack((filtered_points, np.ones((len(filtered_points), 1))))  # Convert points to homogeneous coordinates
    transformed_points = np.dot(box_pose, transformed_points.T).T[:, :3]  # Apply transformation

    new_cloud = o3d.geometry.PointCloud()
    new_cloud.points = o3d.utility.Vector3dVector(transformed_points)
    new_cloud.estimate_normals()
    #
    # pcl.transform(box_pose)
    #
    if(visu):
        plt.plot(hist_x)
        plt.axhline(y=mean_hist_x, color="g", linestyle="-")
        plt.axvline(x=indices_greater_than_mean_x[0], color="m", linestyle="-")
        plt.axvline(x=indices_greater_than_mean_x[-1], color="r", linestyle="-")
        plt.show()

        plt.plot(hist_y)
        plt.axhline(y=mean_hist_y, color="g", linestyle="-")
        plt.axvline(x=indices_greater_than_mean_y[0], color="m", linestyle="-")
        plt.axvline(x=indices_greater_than_mean_y[-1], color="r", linestyle="-")
        plt.show()
        # Origin
        origin = o3d.geometry.TriangleMesh.create_coordinate_frame(
            size=0.1, origin=[0, 0, 0])
        # colors
        min_bbox.color = [0.3, 0.4, 0.5]
        new_cloud.paint_uniform_color([0.5, 0.3, 0.7])
        ## Visualization
        o3d.visualization.draw_geometries([origin, pcl, new_cloud, min_bbox])
    
    return new_cloud

enter image description here

This approach doesn't behave the same always as for different runs I get different histogram distributions using same pointcloud! this might be caused by the minimal bounding box is not the same (especially orientation) for each run! but not sure.

Can you please tell me how can I solve that please?

enter image description here


Solution

  • The solution is to get the rotation of the box from the points rather than the rotation which might be random:

    import numpy as np
    import open3d as o3d
    
    def pose_from_obbox(obbox:o3d.geometry.OrientedBoundingBox) -> np,ndarray:
        vertices = np.asarray(obbox.get_box_points())
        """
        Reference: https://www.open3d.org/docs/latest/cpp_api/classopen3d_1_1geometry_1_1_oriented_bounding_box.html#a8e63a202d0b2bf72014e12c42dd9908b
        ///      ------- x
        ///     /|
        ///    / |
        ///   /  | z
        ///  y
        ///      0 ------------------- 1
        ///       /|                /|
        ///      / |               / |
        ///     /  |              /  |
        ///    /   |             /   |
        /// 2 ------------------- 7  |
        ///   |    |____________|____| 6
        ///   |   /3            |   /
        ///   |  /              |  /
        ///   | /               | /
        ///   |/                |/
        /// 5 ------------------- 4
        """
        # # Define edges of the cube
        edges = [
            (0, 1), (0, 2), (0, 3)
        ]
        # Calculate edge lengths
        edge_lengths = np.linalg.norm(vertices[np.array(edges)[:, 0]] - vertices[np.array(edges)[:, 1]], axis=1)
    
        # Find longest and shortest edges
        longest_edge_idx = np.argmax(edge_lengths)
        shortest_edge_idx = np.argmin(edge_lengths)
    
        # Define x-axis direction vector
        x_axis = vertices[edges[longest_edge_idx][1]] - vertices[edges[longest_edge_idx][0]]
        x_axis = x_axis / np.linalg.norm(x_axis)
    
        # Define z-axis direction vector
        z_axis = vertices[edges[shortest_edge_idx][1]] - vertices[edges[shortest_edge_idx][0]]
        z_axis = z_axis / np.linalg.norm(z_axis)
    
        # Calculate y-axis direction vector
        y_axis = np.cross(z_axis, x_axis)
        y_axis = y_axis / np.linalg.norm(y_axis)
    
        # Build transformation matrix
        center = np.mean(vertices, axis=0)
        
        pose = np.eye(4)
        pose[:3, 3] = center
        pose[:3, 0] = x_axis
        #
        if z_axis[2]<0:
            pose[:3, 1] = - y_axis
            pose[:3, 2] = - z_axis
        else:
            pose[:3, 1] = y_axis
            pose[:3, 2] = z_axis        
        return pose