Create axis, plane and pyramed geometries in Open3D format. :param K: calibration matrix (camera intrinsics) :param R: rotation matrix :param t: translation :param w: image width :param h: image height :param scale: camera model scale :param color: color of the image plan
(
K: npt.NDArray[np.float64],
R: npt.NDArray[np.float64],
t: npt.NDArray[np.float64],
w: int,
h: int,
scale: float = 1,
color: list[float] | None = None,
)
| 129 | |
| 130 | |
| 131 | def draw_camera( |
| 132 | K: npt.NDArray[np.float64], |
| 133 | R: npt.NDArray[np.float64], |
| 134 | t: npt.NDArray[np.float64], |
| 135 | w: int, |
| 136 | h: int, |
| 137 | scale: float = 1, |
| 138 | color: list[float] | None = None, |
| 139 | ) -> list[open3d.geometry.Geometry]: |
| 140 | """Create axis, plane and pyramed geometries in Open3D format. |
| 141 | :param K: calibration matrix (camera intrinsics) |
| 142 | :param R: rotation matrix |
| 143 | :param t: translation |
| 144 | :param w: image width |
| 145 | :param h: image height |
| 146 | :param scale: camera model scale |
| 147 | :param color: color of the image plane and pyramid lines |
| 148 | :return: camera model geometries (axis, plane and pyramid) |
| 149 | """ |
| 150 | |
| 151 | if color is None: |
| 152 | color = [0.8, 0.2, 0.8] |
| 153 | |
| 154 | # intrinsics |
| 155 | K = K.copy() / scale |
| 156 | Kinv = np.linalg.inv(K) |
| 157 | |
| 158 | # 4x4 transformation |
| 159 | T = np.column_stack((R, t)) |
| 160 | T = np.vstack((T, (0, 0, 0, 1))) |
| 161 | |
| 162 | # axis |
| 163 | axis = open3d.geometry.TriangleMesh.create_coordinate_frame( |
| 164 | size=0.5 * scale |
| 165 | ) |
| 166 | axis.transform(T) |
| 167 | |
| 168 | # points in pixel |
| 169 | points_pixel = [ |
| 170 | [0, 0, 0], |
| 171 | [0, 0, 1], |
| 172 | [w, 0, 1], |
| 173 | [0, h, 1], |
| 174 | [w, h, 1], |
| 175 | ] |
| 176 | |
| 177 | # pixel to camera coordinate system |
| 178 | points = [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.translate([points[1][0], points[1][1], scale]) |
| 186 | plane.transform(T) |
| 187 | |
| 188 | # pyramid |