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:
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
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?
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