Draw the camera pose in the form of a cone. Args: camera_pose (numpy.ndarray): 4x4 camera pose from camera to world. camera_size (float): Size of the camera cone. Defaults to 0.5. return_points (bool): Whether to return the points of the camera cone. Defaults
(camera_pose, camera_size=0.5, return_points=False)
| 127 | |
| 128 | |
| 129 | def draw_camera(camera_pose, camera_size=0.5, return_points=False): |
| 130 | """Draw the camera pose in the form of a cone. |
| 131 | |
| 132 | Args: |
| 133 | camera_pose (numpy.ndarray): 4x4 camera pose from camera to world. |
| 134 | camera_size (float): Size of the camera cone. Defaults to 0.5. |
| 135 | return_points (bool): Whether to return the points of the camera cone. |
| 136 | Defaults to False. |
| 137 | |
| 138 | Returns: |
| 139 | numpy.ndarray | :obj:`LineSet`: |
| 140 | if return_points is True, return the points of the camera cone. |
| 141 | Otherwise, return the camera cone as an open3d.LineSet. |
| 142 | """ |
| 143 | # camera_pose : 4*4 camera to world |
| 144 | point = np.array([[0, 0, 0], [-camera_size, -camera_size, camera_size * 2], |
| 145 | [camera_size, -camera_size, camera_size * 2], |
| 146 | [-camera_size, camera_size, camera_size * 2], |
| 147 | [camera_size, camera_size, camera_size * 2]]) |
| 148 | pc = o3d.geometry.PointCloud(points=o3d.utility.Vector3dVector(point)) |
| 149 | pc.transform(camera_pose) |
| 150 | if return_points: |
| 151 | return pc.points |
| 152 | color = (100 / 255.0, 149 / 255.0, 237 / 255.0) |
| 153 | lines_pcd = o3d.geometry.LineSet() |
| 154 | lines_pcd.lines = o3d.utility.Vector2iVector([[0, 1], [0, 2], [0, |
| 155 | 3], [0, 4], |
| 156 | [1, 2], [1, 3], [2, 4], |
| 157 | [3, 4]]) |
| 158 | lines_pcd.points = pc.points |
| 159 | lines_pcd.paint_uniform_color(color) |
| 160 | return lines_pcd |