kitti数据集点云投影到图片，反投影回点云

import numpy as np
import open3d as o3d
import cv2
import matplotlib.pyplot as plt


def read_calib_file(calib_filepath):
	""" Read a calibration file from KITTI dataset. """
	calib = {}
	with open(calib_filepath, 'r') as f:
		for line in f.readlines():
			if line.strip():  # 添加这行代码来检查是否是空行或者行只包含空白字符
				key, value = line.split(':', 1)
				# 将读取的矩阵字符串转换为NumPy数组
				calib[key] = np.array([float(x) for x in value.split()]).reshape(3, -1)
	return calib


file_path = '/backup/kitti/ImageSets/train.txt'  # 替换为你的文件路径
training_addr = '/backup/kitti/training/image_2/'
with open(file_path, 'r') as file:
	lines = file.readlines()
# 处理每行末尾的换行符
imgs_path = [(training_addr + line.strip() + '.png') for line in lines]

file_path2 = '/backup/kitti/ImageSets/val.txt'
with open(file_path2, 'r') as file:
	lines = file.readlines()
for line in lines:
	imgs_path.append(training_addr + line.strip() + '.png')

file_path3 = '/backup/kitti/ImageSets/test.txt'
testing_addr = '/backup/kitti/testing/image_2/'
with open(file_path3, 'r') as file:
	lines = file.readlines()
# 处理每行末尾的换行符
for line in lines:
	imgs_path.append(testing_addr + line.strip() + '.png')


pointclouds = []
for i in imgs_path:
	pointclouds.append(i.replace('image_2', 'velodyne').replace('png', 'bin'))


calibs_path = []
for i in imgs_path:
	calibs_path.append(i.replace('image_2', 'calib').replace('png', 'txt'))


for idx in range(len(pointclouds)):
	img_path = imgs_path[idx]
	img = cv2.imread(img_path)
	image_height = img.shape[0]
	image_width = img.shape[1]

	# cv2.imshow('test', img)
	# cv2.waitKey(0)
	# cv2.destroyAllWindows()

	pc_path = pointclouds[idx]
	points = np.fromfile(pc_path, dtype=np.float32)
	# 假设每个点由四个浮点数表示（例如，x, y, z, intensity），可以重新整形数组
	points = points.reshape(-1, 4)  # 根据你的数据结构调整

	# 创建一个点云对象
	pcd = o3d.geometry.PointCloud()
	pcd.points = o3d.utility.Vector3dVector(points[:, :3])  # 只取x, y, z坐标
	# 显示点云
	vis = o3d.visualization.Visualizer()
	vis.create_window()
	vis.add_geometry(pcd)
	# 在点云显示中添加一个坐标轴
	coord_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.6, origin=[0, 0, 0])
	vis.add_geometry(coord_frame)
	vis.run()
	vis.destroy_window()

	calib_file_path = calibs_path[idx]
	calib_data = read_calib_file(calib_file_path)

	P2 = calib_data['P2']
	R0_rect = calib_data['R0_rect']
	R0_rect = np.insert(R0_rect, 3, values=[0,0,0], axis=0)
	R0_rect = np.insert(R0_rect, 3, values=[0,0,0,1], axis=1)
	Tr_velo_to_cam = calib_data['Tr_velo_to_cam']
	Tr_velo_to_cam = np.insert(Tr_velo_to_cam, 3, values=[0,0,0,1], axis=0)

	points = points[:, 0:3] # lidar xyz

	velo = np.insert(points, 3, 1,axis=1).T
	velo = np.delete(velo, np.where(velo[0,:]<0),axis=1)
	cam = P2.dot(R0_rect.dot(Tr_velo_to_cam.dot(velo)))
	cam = np.delete(cam, np.where(cam[2, :] < 0), axis=1)
	cam[:2] /= cam[2, :]

	u,v,z = cam
	u_out = np.logical_or(u<0, u>image_width)
	v_out = np.logical_or(v<0, v>image_height)
	outlier = np.logical_or(u_out, v_out)
	cam = np.delete(cam,np.where(outlier), axis=1)
	u,v,z = cam

	plt.imshow(img[:, :, ::-1])
	plt.scatter([u], [v], c=[z], cmap='viridis', alpha=1, s=2)
	plt.show()

	# re-project to pointcloud
	# 反投影到相机坐标系
	x = (u - P2[0, 2]) * z / P2[0, 0]
	y = (v - P2[1, 2]) * z / P2[1, 1]
	cam_coords = np.vstack([x, y, z])

	# 将相机坐标系变换到矫正的相机坐标系
	R0_rect_inv = np.linalg.inv(R0_rect[:3, :3])  # 取R0_rect的逆
	cam_coords = R0_rect_inv.dot(cam_coords)

	# 变换到激光雷达坐标系
	Tr_velo_to_cam_inv = np.linalg.inv(Tr_velo_to_cam)  # 取Tr_velo_to_cam的逆
	velo_coords = Tr_velo_to_cam_inv.dot(np.vstack([cam_coords, np.ones((1, cam_coords.shape[1]))]))

	# 去掉齐次坐标
	velo_coords = velo_coords[:3, :].T

	pcd = o3d.geometry.PointCloud()
	pcd.points = o3d.utility.Vector3dVector(velo_coords[:, :3])  # 只取x, y, z坐标
	# 显示点云
	vis = o3d.visualization.Visualizer()
	vis.create_window()
	vis.add_geometry(pcd)
	# 在点云显示中添加一个坐标轴
	coord_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.6, origin=[0, 0, 0])
	vis.add_geometry(coord_frame)
	vis.run()
	vis.destroy_window()

https://medium.com/@jaimin-k/exploring-kitti-visual-ododmetry-dataset-8ac588246cdc