| 13 | edges = [[0,1], [1,3], [3,2], [2,0], [0,4], [4,5], [5,1], [5,7], [7,6], [6,4], [6,2], [7,3]] |
| 14 | |
| 15 | def get_image_point(loc, K, w2c): |
| 16 | # Calculate 2D projection of 3D coordinate |
| 17 | |
| 18 | # Format the input coordinate (loc is a carla.Position object) |
| 19 | point = np.array([loc[0], loc[1], loc[2], 1]) |
| 20 | # transform to camera coordinates |
| 21 | point_camera = np.dot(w2c, point) |
| 22 | |
| 23 | # New we must change from UE4's coordinate system to an "standard" |
| 24 | # (x, y ,z) -> (y, -z, x) |
| 25 | # and we remove the fourth componebonent also |
| 26 | point_camera = [point_camera[1], -point_camera[2], point_camera[0]] |
| 27 | |
| 28 | depth = point_camera[2] |
| 29 | |
| 30 | # now project 3D->2D using the camera matrix |
| 31 | point_img = np.dot(K, point_camera) |
| 32 | # normalize |
| 33 | point_img[0] /= point_img[2] |
| 34 | point_img[1] /= point_img[2] |
| 35 | |
| 36 | return point_img[0:2], depth |
| 37 | |
| 38 | def point_in_canvas_wh(pos): |
| 39 | """Return true if point is in canvas""" |