convertir 2d point in 3d point C++ code example

Example: kitti convert velodyne point to 2d point

def get_2d_pt_from_vel(velo_pt):
    vld = velo_pt.T.reshape(4, 1)
    pt3d = vld[:, vld[-1, :] > 0].copy()
    pt3d[-1, :] = 1
    pt3d_cam = R0 @ Tr @ pt3d
    mask = pt3d_cam[2, :] >= 0  # Z >= 0
    pt2d_cam = P2 @ pt3d_cam[:, mask]
    pt2d = (pt2d_cam / pt2d_cam[2, :])[:-1, :].T
    return pt2d

velo_pts = np.fromfile(os.path.join(velo_file_location), 
					   dtype=np.float32, count=-1).reshape([-1, 4])
velo_pts = list(velo_pts)

for v in velo_pts:
	pt2d = get_2d_pt_from_vel(v)