bhomaidan90
bhomaidan90

Reputation: 204

boxes upper face pose estimation from pointcloud

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)

enter image description here

Upvotes: 0

Views: 60

Answers (0)

Related Questions