使用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的比较详细的资料的话，希望联系我，因为官方的文档感觉有好多细节没说明白（小小吐槽下）。