Converting raw point cloud to depth

[SkurSIv]

I am trying to get a depth information from the raw point cloud. I followed the code example in scripts (viewer/kitti360Viewer3DRaw.py, helpers/project.py, devkits/common/loadCalibration.py):

• Get Cam0ToVelo from calib_cam_to_velo.txt with loadCalibrationRigid();
• Get CamToPose from calib_cam_to_velo.txt with loadCalibrationCameraToPose();
• Calculate CamkToCam0, then CamToVelo and VeloToCam;
• Multiply VeloToCam to R_rect to get VeloToRect;
• Project points to the camera coordinate system with VeloToRect;
• Use CameraPerspectie.cam2image() to get u, v and depth.

And as a result I am getting the non-linear displacement between the depth points and the corresponding image points.
I tried to calculate rectification and projection matrices (R_rect, P) with OpenCV from the data in perspective.txt (K1, D1, K2, D2, R2, T2, S, S_rect). I found out that R_rect from perspective.txt is very close to R_rect_cv, but P_rect from perspective.txt is very different from P_rect_cv. When I tried the algorithm above with R_rect_cv and P_rect_cv, I have got the linear displacement between the depth and the corresponding image points.
So what is the correct way of using the calibration information for getting the depth from the raw point clouds?
My projection code:
`

camera = CameraPerspective(kitti360Path, sequence, cam_id)

velo = Kitti360Viewer3DRaw(mode='velodyne', seq=seq, kitti360Path=kitti360Path)

fileCameraToVelo = os.path.join(kitti360Path, 'calibration', 'calib_cam_to_velo.txt')
TrCam0ToVelo = loadCalibrationRigid(fileCameraToVelo)

fileCameraToPose = os.path.join(kitti360Path, 'calibration', 'calib_cam_to_pose.txt')
TrCamToPose = loadCalibrationCameraToPose(fileCameraToPose)

TrVeloToCam = {}
for k, v in TrCamToPose.items():
    # Tr(cam_k -> velo) = Tr(cam_k -> cam_0) @ Tr(cam_0 -> velo)
    TrCamkToCam0 = np.linalg.inv(TrCamToPose['image_00']) @ TrCamToPose[k]
    TrCamToVelo = TrCam0ToVelo @ TrCamkToCam0
    # Tr(velo -> cam_k)
    TrVeloToCam[k] = np.linalg.inv(TrCamToVelo)
TrVeloToRect = np.matmul(camera.R_rect, TrVeloToCam['image_%02d' % cam_id])

file_count = get_file_count(os.path.join(kitti360Path, 'data_2d_raw_rect', sequence, 'image_00/data_rect'))
for frame in range(0, file_count):
    points = velo.loadVelodyneData(frame)
    pointsCam = np.matmul(TrVeloToRect, points.T).T
    pointsCam = pointsCam[:, :3]
    u, v, depth = camera.cam2image(pointsCam.T)
    u = u.astype(np.int)
    v = v.astype(np.int)
    depthMap = np.zeros((camera.height, camera.width))
    mask = np.logical_and(np.logical_and(np.logical_and(u >= 0, u < camera.width), v >= 0), v < camera.height)
    mask = np.logical_and(np.logical_and(mask, depth > 0), depth < 30)
    depthMap[v[mask], u[mask]] = depth[mask]

[SkurSIv]

Problem is solved, it is crucial to set points[:, 3] = 1 in a loop. My mistake. So the only question remains is why the projection matrix P calculated from (K1, D1, K2, D2, R2, T2, S, S_rect) is different from the one provided in perspective.txt

[LLHEVER]

Hello @SkurSIv
I am also confused about this problem at present, could you please share your final solution code？
Thank you very much!

[SkurSIv]

Hello @SkurSIv I am also confused about this problem at present, could you please share your final solution code？ Thank you very much!

Hi!
As I said above, the displacement problem was solved with adding points[:, 3] = 1 in the loop over frames. The corrected code is below. For now I still do not have the answer for the question why the calculated P is different from the provided one.

camera = CameraPerspective(kitti360Path, sequence, cam_id)
velo = Kitti360Viewer3DRaw(mode='velodyne', seq=seq, kitti360Path=kitti360Path)
fileCameraToVelo = os.path.join(kitti360Path, 'calibration', 'calib_cam_to_velo.txt')
TrCam0ToVelo = loadCalibrationRigid(fileCameraToVelo)
fileCameraToPose = os.path.join(kitti360Path, 'calibration', 'calib_cam_to_pose.txt')
TrCamToPose = loadCalibrationCameraToPose(fileCameraToPose)

TrVeloToCam = {}
for k, v in TrCamToPose.items():
    TrCamkToCam0 = np.linalg.inv(TrCamToPose['image_00']) @ TrCamToPose[k]
    TrCamToVelo = TrCam0ToVelo @ TrCamkToCam0
    TrVeloToCam[k] = np.linalg.inv(TrCamToVelo)
TrVeloToRect = np.matmul(camera.R_rect, TrVeloToCam['image_%02d' % cam_id])

file_count = get_file_count(os.path.join(kitti360Path, 'data_2d_raw', sequence, 'image_00/data_rect'))
for frame in trange(0, file_count):
    points = velo.loadVelodyneData(frame)
    points[:, 3] = 1

    pointsCam = np.matmul(TrVeloToRect, points.T).T
    pointsCam = pointsCam[:, :3]

    u, v, depth = camera.cam2image(pointsCam.T)
    u = u.astype(int)
    v = v.astype(int)

    depthMap = np.zeros((camera.height, camera.width))
    mask = np.logical_and(np.logical_and(np.logical_and(u >= 0, u < camera.width), v >= 0), v < camera.height)
    mask = np.logical_and(mask, depth > 0)
    depthMap[v[mask], u[mask]] = depth[mask]