Create axis, plane and pyramid geometries in Open3D format : param K : calibration matrix (camera intrinsics) : param R : rotation matrix : param t : translation : param width : image width : param height: image height : param scale : camera model sca
(K, R, t, width, height, scale=1, color=None)
| 134 | # Auxiliary funcions |
| 135 | # |
| 136 | def draw_camera(K, R, t, width, height, scale=1, color=None): |
| 137 | """ Create axis, plane and pyramid geometries in Open3D format |
| 138 | : param K : calibration matrix (camera intrinsics) |
| 139 | : param R : rotation matrix |
| 140 | : param t : translation |
| 141 | : param width : image width |
| 142 | : param height: image height |
| 143 | : param scale : camera model scale |
| 144 | : param color : color of the image plane and pyramid lines |
| 145 | : return : camera model geometries (axis, plane and pyramid) |
| 146 | """ |
| 147 | |
| 148 | # default color |
| 149 | if color is None: |
| 150 | color = [0.8, 0.2, 0.8] |
| 151 | |
| 152 | # camera model scale |
| 153 | s = 1 / scale |
| 154 | |
| 155 | # intrinsics |
| 156 | Ks = np.array([[K[0, 0] * s, 0, K[0,2]], |
| 157 | [ 0, K[1, 1] * s, K[1,2]], |
| 158 | [ 0, 0, K[2,2]]]) |
| 159 | Kinv = np.linalg.inv(Ks) |
| 160 | |
| 161 | # 4x4 transformation |
| 162 | T = np.column_stack((R, t)) |
| 163 | T = np.vstack((T, (0, 0, 0, 1))) |
| 164 | |
| 165 | # axis |
| 166 | axis = create_coordinate_frame(T, scale=scale*0.5) |
| 167 | |
| 168 | # points in pixel |
| 169 | points_pixel = [ |
| 170 | [0, 0, 0], |
| 171 | [0, 0, 1], |
| 172 | [width, 0, 1], |
| 173 | [0, height, 1], |
| 174 | [width, height, 1], |
| 175 | ] |
| 176 | |
| 177 | # pixel to camera coordinate system |
| 178 | points = [scale * Kinv @ p for p in points_pixel] |
| 179 | |
| 180 | # image plane |
| 181 | width = abs(points[1][0]) + abs(points[3][0]) |
| 182 | height = abs(points[1][1]) + abs(points[3][1]) |
| 183 | plane = open3d.geometry.TriangleMesh.create_box(width, height, depth=1e-6) |
| 184 | plane.paint_uniform_color(color) |
| 185 | plane.transform(T) |
| 186 | plane.translate(R @ [points[1][0], points[1][1], scale]) |
| 187 | |
| 188 | # pyramid |
| 189 | points_in_world = [(R @ p + t) for p in points] |
| 190 | lines = [ |
| 191 | [0, 1], |
| 192 | [0, 2], |
| 193 | [0, 3], |
no test coverage detected