Export oriented (around Z axis) scene bbox to meshes Args: scene_bbox: (N x 7 numpy array): xyz pos of center and 3 lengths (dx,dy,dz) and heading angle around Z axis. Y forward, X right, Z upward. heading angle of positive X is 0, heading angle of pos
(scene_bbox, out_filename, colors=None)
| 176 | |
| 177 | |
| 178 | def write_oriented_bbox(scene_bbox, out_filename, colors=None): |
| 179 | """Export oriented (around Z axis) scene bbox to meshes |
| 180 | Args: |
| 181 | scene_bbox: (N x 7 numpy array): xyz pos of center and 3 lengths (dx,dy,dz) |
| 182 | and heading angle around Z axis. |
| 183 | Y forward, X right, Z upward. heading angle of positive X is 0, |
| 184 | heading angle of positive Y is 90 degrees. |
| 185 | out_filename: (string) filename |
| 186 | """ |
| 187 | |
| 188 | def heading2rotmat(heading_angle): |
| 189 | pass |
| 190 | rotmat = np.zeros((3, 3)) |
| 191 | rotmat[2, 2] = 1 |
| 192 | cosval = np.cos(heading_angle) |
| 193 | sinval = np.sin(heading_angle) |
| 194 | rotmat[0:2, 0:2] = np.array([[cosval, -sinval], [sinval, cosval]]) |
| 195 | return rotmat |
| 196 | |
| 197 | def convert_oriented_box_to_trimesh_fmt(box): |
| 198 | ctr = box[:3] |
| 199 | lengths = box[3:6] |
| 200 | trns = np.eye(4) |
| 201 | trns[0:3, 3] = ctr |
| 202 | trns[3, 3] = 1.0 |
| 203 | trns[0:3, 0:3] = heading2rotmat(box[6]) |
| 204 | box_trimesh_fmt = trimesh.creation.box(lengths, trns) |
| 205 | return box_trimesh_fmt |
| 206 | |
| 207 | if colors is not None: |
| 208 | if colors.shape[0] != len(scene_bbox): |
| 209 | colors = [colors for _ in range(len(scene_bbox))] |
| 210 | colors = np.array(colors).astype(np.uint8) |
| 211 | assert colors.shape[0] == len(scene_bbox) |
| 212 | assert colors.shape[1] == 4 |
| 213 | |
| 214 | scene = trimesh.scene.Scene() |
| 215 | for idx, box in enumerate(scene_bbox): |
| 216 | box_tr = convert_oriented_box_to_trimesh_fmt(box) |
| 217 | if colors is not None: |
| 218 | box_tr.visual.main_color[:] = colors[idx] |
| 219 | box_tr.visual.vertex_colors[:] = colors[idx] |
| 220 | for facet in box_tr.facets: |
| 221 | box_tr.visual.face_colors[facet] = colors[idx] |
| 222 | scene.add_geometry(box_tr) |
| 223 | |
| 224 | mesh_list = trimesh.util.concatenate(scene.dump()) |
| 225 | # save to ply file |
| 226 | trimesh.io.export.export_mesh(mesh_list, out_filename, file_type="ply") |
| 227 | |
| 228 | return |
| 229 | |
| 230 | |
| 231 | def write_oriented_bbox_camera_coord(scene_bbox, out_filename): |
nothing calls this directly
no test coverage detected