Проецирование 3D-облака точек в систему координат 2D-изображения ⇐ Python

[Anonymous]

Я работаю с набором данных OSDAR23 и хочу спроецировать облака точек из файла .pcd на 2D-изображение. Предоставляется следующая информация:

Обратите внимание, что внешние параметры калибровки (pose_wrt_parent в файле аннотации) описывают преобразование (перенос и вращение в кватернионах) из лидарной (опорной) системы координат в неоптическую систему координат камеры. Эта неоптическая система координат ориентирована аналогично системе координат лидара:

• положительный x -ось указывает вперед
• положительная ось Y указывает влево
• положительная ось Z указывает вверх

Напротив, оптическая система координат камеры выровнена. аналогично системе координат изображения:

• положительная ось X указывает вправо
  < li>положительная ось Y указывает вниз
• положительная ось Z указывает вперед

Для расчета 2D координат изображения 3D-лидарной точки необходимо выполнить следующие действия:

• Точка лидара преобразуется в неоптическую систему координат камеры с использованием внешних параметров калибровки (pose_wrt_parent в файле аннотации).
• Полученная трехмерная точка преобразуется в оптическую систему координат камеры с помощью кватерниона вращения (x=0.5; y=-0.5; z=0.5; w=-0.5).
• Полученная 3D-точка преобразуется в систему координат 2D-изображения с использованием параметров внутренней калибровки (intrinsics_pinhole в файле аннотации).

Дополнительно:
"rgb_center": {"type": "sensor", "parent": "base", "pose_wrt_parent": {"translation": [0.0669458, -0.000911152, 2.05432], "quaternion": [0.00193374, -0.00293566, 0.00271082, 0.99999]}}

"rgb_center": {"type": "camera", "uri": "rgb_center", "stream_properties": {"intrinsics_pinhole": {"camera_matrix": [4609.471892628096, 0.0, 1257.158605934, 0.0, 0.0, 4609.471892628096, 820.0498076210201, 0.0, 0.0, 0.0, 1.0, 0.0], "distortion_coeffs": [-0.0914603, 0.605326, 0.0, 0.0, 0.417134], "width_px": 2464, "height_px": 1600}}}


Я пробовал использовать следующий код, но получаю всего несколько точек, и они изолированы друг от друга, что указывает на ошибку. Может ли кто-нибудь мне помочь?
import numpy as np
import matplotlib.pyplot as plt
import cv2
from scipy.spatial.transform import Rotation as R

def read_pcd_file(file_path):
points = []
with open(file_path, 'r') as file:
data_section = False
for line in file:
if line.strip() == "DATA ascii":
data_section = True
continue
if data_section:
values = line.strip().split()
if len(values) >= 3:
x, y, z = map(float, values[:3])
points.append([x, y, z])
return np.array(points)

if __name__ == '__main__':

pose_wrt_parent = {
"translation": [0.0669458, -0.000911152, 2.05432],
"quaternion": [0.00193374, -0.00293566, 0.00271082, 0.99999] # x, y, z, w
}

rotation_optical_camera = [0.5, -0.5, 0.5, -0.5]

intrinsics_pinhole = {
"camera_matrix": [4609.471892628096, 0.0, 1257.158605934,
0.0, 4609.471892628096, 820.0498076210201,
0.0, 0.0, 1.0],
"width_px": 2464,
"height_px": 1600,
"distortion_coeffs": [-0.0914603, 0.605326, 0.0, 0.0, 0.417134]
}

points_lidar = read_pcd_file(r"C:\Users\nokia\Desktop\lidar_camera_obj_det\15_construction_vehicle_15.1\lidar\065_1631531287.599802000.pcd")

# Step 1: Trasformation to non-optical system
translation = np.array(pose_wrt_parent["translation"])
#rotation_non_optical = quaternion_to_rotation_matrix(pose_wrt_parent["quaternion"])
rotation_non_optical = R.from_quat(pose_wrt_parent["quaternion"]).as_matrix() # scalar last order assumed
points_non_optical = (rotation_non_optical @ points_lidar.T).T + translation

# Step 2: Trasformation to optical system
#rotation_optical = quaternion_to_rotation_matrix(rotation_optical_camera)
rotation_optical = R.from_quat(rotation_optical_camera).as_matrix()

points_optical = points_non_optical
points_optical = (rotation_optical @ points_non_optical.T).T

# Step 3: Projection on image plane
camera_matrix = np.array(intrinsics_pinhole["camera_matrix"]).reshape(3, 3)
print(camera_matrix)
distortion_coeffs = np.array(intrinsics_pinhole["distortion_coeffs"])

projected_points, _ = cv2.projectPoints(
points_optical, np.zeros(3), np.zeros(3), camera_matrix, distortion_coeffs
)

image_width, image_height = intrinsics_pinhole["width_px"], intrinsics_pinhole["height_px"]

projected_points = projected_points.squeeze()

valid_points = (projected_points[:, 0] >= 0) & (projected_points[:, 0] < image_width) & \
(projected_points[:, 1] >= 0) & (projected_points[:, 1] < image_height)

projected_points = projected_points[valid_points]
depth_values = points_optical[valid_points, 0]

camera_image = cv2.imread(r"C:\Users\nokia\Desktop\lidar_camera_obj_det\15_construction_vehicle_15.1\rgb_center\065_1631531287.600000072.png")
camera_image = cv2.cvtColor(camera_image, cv2.COLOR_BGR2RGB)

point_radius = 5
for (x, y), depth in zip(projected_points, depth_values):
color = plt.cm.viridis(depth / np.max(depth_values))
color = (int(color[0] * 255), int(color[1] * 255), int(color[2] * 255))
cv2.circle(camera_image, (int(x), int(y)), point_radius, color, -1)

plt.figure(figsize=(12, 8))
#plt.figure()
im = plt.imshow(camera_image)
plt.title("Camera Image with LiDAR Points Overlay")
plt.axis("off")
cbar = plt.colorbar(im, orientation='vertical', fraction=0.046, pad=0.04)
cbar.set_label('Distance') # Etichetta per la colorbar
plt.show()



Подробнее здесь: https://stackoverflow.com/questions/792 ... ate-system