How to visualize vectors on camera image from Nuscenes dataset?

[datvuthanh]

Hi Tsinghua-MARS-Lab,

Thank you for your work. Currently, I want to visualize vector on camera image from nuscenes dataset. This is my pseudo code:

Assume that, i obtained vector points from sample vector line (N,2)

        import copy
        for cam_type, img_info in img_infos.items():
            img_filename = img_info['data_path']
            img = imageio.imread(img_filename)

            cam2lidar_rt = np.eye(4)
            cam2lidar_rt[:3, :3] = img_info['sensor2lidar_rotation']
            cam2lidar_rt[:3, -1] = img_info['sensor2lidar_translation']
            lidar2cam_rt = np.linalg.inv(cam2lidar_rt)

            lidar2ego_rt = np.eye(4)
            lidar2ego_rt[:3, :3] = img_metas['lidar2ego_rots']
            lidar2ego_rt[:3, -1] = img_metas['lidar2ego_trans']
            ego2lidar_rt = np.linalg.inv(lidar2ego_rt)

            ego2cam_rt = lidar2cam_rt @ ego2lidar_rt

            intrinsic = img_info['cam_intrinsic']

            viewpad = np.eye(4)
            viewpad[:intrinsic.shape[0],
                    :intrinsic.shape[1]] = intrinsic

            lidar2img = (viewpad @ lidar2cam_rt)
            ego2img = (viewpad @ ego2cam_rt)

            for vector in vectors_gt:
                pts = np.array(vector['pts'])
                # squeeze to N,2
                pts = pts.squeeze()
                if pts.shape[1] == 2:
                    zeros = np.randn((pts.shape[0], 1))
                    pts = np.concatenate([pts, zeros], axis=1)
                pts_ego_4d = np.concatenate([pts, np.ones([len(pts), 1])], axis=-1)

                ego2img_rt = copy.deepcopy(ego2img).reshape(4, 4)
                uv = pts_ego_4d @ ego2img_rt.T

                uv = remove_nan_values(uv)
                depth = uv[:, 2]

                uv = uv[:, :2] / uv[:, 2].reshape(-1, 1)

                h, w, c = img.shape

                is_valid_x = np.logical_and(0 <= uv[:, 0], uv[:, 0] < w - 1)
                is_valid_y = np.logical_and(0 <= uv[:, 1], uv[:, 1] < h - 1)
                is_valid_z = depth > 0
                is_valid_points = np.logical_and.reduce([is_valid_x, is_valid_y, is_valid_z])
                if is_valid_points.sum() == 0:
                    print('no valid points')

                uv = np.round(uv[is_valid_points]).astype(np.int32)

                line = np.round(uv).astype(int)  # type: ignore
                test_img = img.copy()
                for i in range(len(line) - 1):

                    if (not is_valid_points[i]) or (not is_valid_points[i + 1]):
                        continue
                        
                    x1 = line[i][0]
                    y1 = line[i][1]
                    x2 = line[i + 1][0]
                    y2 = line[i + 1][1]

                    # Use anti-aliasing (AA) for curves
                    test_img = cv2.line(test_img, pt1=(x1, y1), pt2=(x2, y2), color=(255,0,0), thickness=2, lineType=cv2.LINE_AA)

My script code return no valid points, thank you.

[yuantianyuan01]

Sorry for late response. I suggest checking whether ego2img_rt is correct or not. I don't think you can directly take the inverse of the 4x4 transformation matrix. A example should be like

cam2ego_rotation = Quaternion(cs_record['rotation']).rotation_matrix
cam2ego_translation = np.array(cs_record['translation'])

ego2cam_rotation = cam2ego_rotation.T
ego2cam_translation = ego2cam_rotation.dot(-cam2ego_translation)

transform_matrix = np.eye(4) #ego2cam
transform_matrix[:3, :3] = ego2cam_rotation
transform_matrix[:3, 3] = ego2cam_translation