自动驾驶 python 使用Open3d在点云中实现第一人称视角导航

Open3D是一个开源库，支持快速开发处理3D数据的软件。Open3D前端在C++和Python中公开了一组精心挑选的数据结构和算法。后端经过高度优化，并设置为并行化。

本人最近在工作上需要实现一个需求，就是使用车载雷达采集的点云数据，复现第一人称视角，简单地来说，就是开着车采集完点云数据，然后在电脑上使用open3d通过设定每一帧的视角，复现驾驶的过程。

直接贴代码，由于数据敏感的原因，只能把实现的最简单的代码贴上来供大家参考。

# This demo code realize that first-person navigation in pointcloud.

import open3d as o3d

import numpy as np

from scipy.spatial.transform import Rotation as R

odometry_file_path = "YOUR_ODOMETRY_FILE_PATH"

pcd_file_path = "YOUR_PCD_FILE_PATH"

with open(odometry_file_path, 'r') as f:

content = f.readlines()

position_lis = content

whole_pcd = o3d.io.read_point_cloud(pcd_file_path)

vis = o3d.visualization.Visualizer()

vis.create_window()

vis.add_geometry(whole_pcd)

for idx in range(len(position_lis) - 1):

# odometry of mine format is: timestamp, ox, oy, oz, ow, pos_x, pos_y, pos_z

# [ox, oy, oz, ow] is quaternion

origin_pos = [float(position_lis[idx].split(',')[5]), float(position_lis[idx].split(',')[6]), float(position_lis[idx].split(',')[7])]

ox, oy, oz, ow = [float(position_lis[idx].split(',')[1]), float(position_lis[idx].split(',')[2]), float(position_lis[idx].split(',')[3]), float(position_lis[idx].split(',')[4])]

# get rotation matrix through quaternion

quaternion = np.array([ow, ox, oy, oz])

R = o3d.geometry.get_rotation_matrix_from_quaternion(quaternion)

view_control = vis.get_view_control()

front_vector = R[:, 0].tolist()

up_vector = R[:, 2].tolist()

# set up/lookat/front vector to vis

view_control.set_front(front_vector)

view_control.set_up(up_vector)

view_control.set_lookat(origin_pos)

# because I want get first person view, so set zoom value with 0.001, if set 0, there will be nothing on screen.

view_control.set_zoom(0.001)

vis.update_geometry(whole_pcd)

vis.poll_events()

vis.update_renderer()

vis.destroy_window()

在实现过称中，主要需要注意的是四元数(quaternion)的构成是[ow, ox, oy, oz]，然后使用四元数得到3x3的旋转矩阵，旋转矩阵每一列分别代表当前帧“相机坐标系”（或者说lidar坐标系）的三个轴，随后使用open3d的接口函数set_front/set_up/set_lookat/set_zoom来设置当前帧的视角，其中，set_front设置的是垂直指向屏幕外的向量，set_up设置的是指向屏幕上方的向量，当设置了这两个向量之后，当前视角坐标系就可以确定了（参考链接），set_lookat也就是当前帧lidar所处的位置，是以世界坐标系为统一坐标系计算出来的（可以理解为，不管哪一帧，在整个地图，只有一个共同的原点），set_zoom是控制视角远近的，因为要实现第一人称视角，所以设置为0.001，因为设置0的话屏幕会出现空白的现象。

其他的code简单看一下open3d的说明文档应该很容易了解实现什么功能。如果有小伙伴有open3d的比较详细的资料的话，希望联系我，因为官方的文档感觉有好多细节没说明白（小小吐槽下）。

推荐阅读

 您阅读本篇文章共花了：