Transform point cloud from ZED frame to World frame

[anhtuduong]

Transformation

Point cloud taken from ZED camera is based on the camera's frame.
We need to transform every point cloud to world's frame as it is the base frame.

Current transformation from camera's frame to world's frame:

w_R_c = np.matrix([[0, -0.499, 0.866], [-1, 0, 0], [0, -0.866, -0.499]])
x_c = np.array([-0.9, 0.24, -0.35])
base_offset = np.array([0.5, 0.35, 1.75])

# Transform point cloud to world frame
def transform_pointcloud(point_cloud):
    """ @brief Transform point_cloud to world frame
        @param point_cloud (list): list of point_cloud
        @return point_cloud (list): list of point_cloud
    """
    point_cloud = np.array(point_cloud)
    for i in range(len(point_cloud)):
        point_cloud[i] = point_cloud[i].dot(w_R_c.T) + x_c + base_offset
    
    log.debug('point_cloud transformed')
    return point_cloud

Cleaning point cloud

Point cloud taken from ZED camera is included the point cloud of the surface of the table, as it extracts every pixels of the 2D bounding box.

The point is to filter all the point cloud that belong to the object, not the surface of the table.

---

Tasks:

• Testing transformation to world frame on a single object
• Testing clean point cloud for one object

• Implement general method for transformation between frames
• Implement general method for cleaning all point cloud