Reputation: 204
I want to estimate the pose of the upper surface of multiple boxes from the pointcloud (link to pointclouds)
I tried Plane Segmentation, and Minimal Oriented Bounding Box
But due to some outliers (that I didn't manage to remove by statistical outliers removal) I get the pose rotated on the Z direction as you can see in the photo below
can you please tell me how can I improve my pose estimation of the upper surface of the boxes? thanks.
#!/usr/bin/env python3
import os
from random import random
from copy import deepcopy
import open3d as o3d
# TODO: please change this directory
pcls_path = "/home/user/pcls"
origin = o3d.geometry.TriangleMesh.create_coordinate_frame(
size=100,
origin=[0., 0., 0.]
)
boxe_pcls = [origin]
for idx, pcl_path in enumerate(os.listdir(pcls_path)):
if pcl_path.lower().endswith('.ply'):
print(pcl_path)
box_pcl = o3d.io.read_point_cloud(os.path.join(pcls_path, pcl_path))
original_box_pcl = deepcopy(box_pcl)
original_box_pcl.paint_uniform_color([random(), random(), random()])
plane_model, inliers = box_pcl.segment_plane(
distance_threshold=0.05,
ransac_n=5,
num_iterations=100
)
inlier_cloud = box_pcl.select_by_index(inliers)
#
box_pcl.paint_uniform_color([random(), random(), random()])
obox_ = inlier_cloud.get_minimal_oriented_bounding_box()
new_obox = o3d.geometry.OrientedBoundingBox(obox_)
new_obox.extent = [1.5 * obox_.extent[0], 1.5 * obox_.extent[1], 20]
print(f"PointCloud {idx} has {len(inlier_cloud.points)} points")
box_pcl = box_pcl.crop(new_obox)
last_obbox = box_pcl.get_minimal_oriented_bounding_box()
last_obbox.color = [random(), random(), random()]
box_pose = o3d.geometry.TriangleMesh.create_coordinate_frame(
size=100,
origin=last_obbox.center
)
rotation = deepcopy(last_obbox.R)
# reverse rotation on Z axis
if rotation[2, 2] > 0:
rotation[:, 2] *= -1
print(f"box {idx} has a rotation \n{rotation}")
box_pose.rotate(rotation)
# pcls.append(inlier_cloud)
# boxe_pcls.append(box_pcl)
boxe_pcls.append(original_box_pcl)
boxe_pcls.append(last_obbox)
boxe_pcls.append(box_pose)
o3d.visualization.draw_geometries(boxe_pcls)
Upvotes: 0
Views: 60