CODEWITHSUNDEEP
pythondata-visualizationpoint-cloudsbounding-boxopen3d
Kevin Hu
Kevin Hu

Reputation: 95

How to draw bounding boxes and update them real time in python

I am currently learning open3d for visualizing point cloud data. I managed to visualize a sequence of point clouds read from a series of .pcd files (one file for one point cloud) at real-time using the non-blocking visualization introduced here: Non-blocking visualization

By following the documentation I was able to update geometry of type "point cloud". Here is how I did the visualization:

    import open3d as o3d
    import numpy as np
    import time

    geometry = o3d.geometry.PointCloud()
    geometry.points = o3d.utility.Vector3dVector(pt_clouds[0])
    o3d.utility.set_verbosity_level(o3d.utility.VerbosityLevel.Debug)

    vis = o3d.visualization.Visualizer()
    vis.create_window()
    vis.add_geometry(geometry)

    for pt_cloud in pt_clouds: #pt_clouds are the point cloud data from several .pcf files

        geometry.points = o3d.utility.Vector3dVector(pt_cloud)
        vis.update_geometry(geometry)
        vis.poll_events()
        vis.update_renderer()
        time.sleep(1 / 20)

    vis.destroy_window()

However, apart from the point clouds I also have a set of bounding boxes for each point cloud given by the center coordinates [cx, cy, cz], rotation around z axis "rot_z" and [length, width, height] of the bounding boxes. I want to find a way to render the bounding box along with the point clouds and update them every frame (for each frame, there are one point cloud + a different number of bounding boxes to be rendered and the old rendering of last frame needs to be cleared and updated).

Is there a way to do this? If open3d cannot do this, what is the common way/library to do this in python?

Any advice will be grateful, thanks in advance!

Upvotes: 1

Views: 13549

Answers (1)

Aaron Berger
Aaron Berger

Reputation: 73

For visualizing the bounding boxes, you'll have to convert the boxes from

[cx, cy, cz, rot_z, length, width, height] to an array of points that represent the 8 corners of the box.

You can use a function like this, that takes an array containing [x,y,z,h,w,l,r], and returns an [8, 3] matrix that represents the [x, y, z] for each 8 corners of the box:

def box_center_to_corner(box_center):
    # To return
    corner_boxes = np.zeros((8, 3))

    translation = box[0:3]
    h, w, l = size[3], size[4], size[5]
    rotation = box[6]

    # Create a bounding box outline
    bounding_box = np.array([
        [-l/2, -l/2, l/2, l/2, -l/2, -l/2, l/2, l/2],
        [w/2, -w/2, -w/2, w/2, w/2, -w/2, -w/2, w/2],
        [-h/2, -h/2, -h/2, -h/2, h/2, h/2, h/2, h/2]])

    # Standard 3x3 rotation matrix around the Z axis
    rotation_matrix = np.array([
        [np.cos(rotation), -np.sin(rotation), 0.0],
        [np.sin(rotation), np.cos(rotation), 0.0],
        [0.0, 0.0, 1.0]])

    # Repeat the [x, y, z] eight times
    eight_points = np.tile(translation, (8, 1))

    # Translate the rotated bounding box by the
    # original center position to obtain the final box
    corner_box = np.dot(
        rotation_matrix, bounding_box) + eight_points.transpose()

    return corner_box.transpose()

Once you've converted your bounding boxes to [8, 3] matrices representing the corners, you can display them in Open3D by using the LineSet object:

# Our lines span from points 0 to 1, 1 to 2, 2 to 3, etc...
lines = [[0, 1], [1, 2], [2, 3], [0, 3],
         [4, 5], [5, 6], [6, 7], [4, 7],
         [0, 4], [1, 5], [2, 6], [3, 7]]

# Use the same color for all lines
colors = [[1, 0, 0] for _ in range(len(lines))]

line_set = o3d.geometry.LineSet()
line_set.points = o3d.utility.Vector3dVector(corner_box)
line_set.lines = o3d.utility.Vector2iVector(lines)
line_set.colors = o3d.utility.Vector3dVector(colors)

# Create a visualization object and window
vis = o3d.visualization.Visualizer()
vis.create_window()

# Display the bounding boxes:
vis.add_geometry(corner_box)

In terms of updating the bounding boxes each time step, refer to the documentation for the Open3D Visualizer object, where there are methods for clearing all geometry (clear_geometries()), clearing a specific geometry (remove_geometry(geometry)), etc.

Hope this answer wasn't too late. Good luck!

Upvotes: 7

Related Questions