使用PCLPY进行点云图像融合
pclpy简介
pclpy是点云库(PCL)的Python绑定。使用CppHeaderParser和pybind11从头文件生成。
这个库正在积极开发中，api可能会发生变化。所包含的模块确实可以工作，但测试还不完整。目前只支持Windows和python 3.6 x64。
许多其他python库尝试绑定PCL。最流行的是python-pcl，它使用Cython。虽然Cython非常强大，但绑定c++模板并不是它的强项(PCL大量使用模板)。python-pcl有大量的代码重复，维护和添加特性都非常难，而且对PCL的类和点类型绑定不完整。使用pybind11绑定，我们可以直接使用c++，模板、boost::smart_ptr和缓冲区协议都比较容易实现。
GitHub：https://github.com/davidcaron/pclpy
pypi: https://pypi.org/project/pclpy/
pybind11：https://pybind11.readthedocs.io/en/stable/
下载：
Release下载：https://github.com/davidcaron/pclpy/releases
安装
建议使用Anconda创建多个虚拟环境进行测试
conda create -n pclpy11 python=3.6 # 创建新的虚拟环境
conda activate pclpy11 # 激活环境
0.12.0版下载安装：
conda install -c conda-forge -c davidcaron pclpy`
注意：最新版的0.12.0里面移除了可视化模块，所以想要可视化看点云效果的，可以下载0.12.0之前的版本：
比如下载安装0.11.0版本，注意使用pip下载，使用国内镜像下载更快：
pip install pclpy==0.11.0 -i https://pypi.tuna.tsinghua.edu.cn/simple
测试
from pclpy import pcl
# 读取pcd格式点云数据
pc = pcl.PointCloud.PointXYZ()
reader = pcl.io.PCDReader()
reader.read("bunny.pcd", pc)
# 可视化点云
viewer = pcl.visualization.CloudViewer("viewer")
viewer.showCloud(pc, "hi")
while not viewer.wasStopped(10):
pass
点云与图像融合
点云、图像之间的坐标变换相关基础知识参考链接：一文看懂自动驾驶中的坐标变换
使用文件
本文的点云和图像的融合用到了相机内外参、激光雷达外参的yaml文件，相机采集的图像以及对应的激光雷达采集的点云数据.pcd文件。
内外参yaml文件内容
外参文件内容如下所示，包含了旋转矩阵的四元数rotation以及偏置translation。
# From front camera to vechicle coordinate
header:
frame_id: vechicle
child_frame_id: front
transform:
rotation:
x: -0.716810
y: 0.000349
z: -0.005084
w: 0.697250
translation:
x: 0.046892
y: 1.845396
z: 1.189992
内参文件内容如下所示，包含相机所采集图像的宽、高，以及矩阵的失真系数D和相机内参矩阵K。
height: 1080
width: 1920
distortion_model: plumb_bob
D: [-0.3713391106016114, 0.1439459925562498, -9.793218981776926e-05, 0.0014091097954690595, 0.0]
K: [2029.1089355290503, 0.0, 961.105118299655, 0.0, 2029.404084256603, 585.5222597066615, 0.0, 0.0, 1.0]
坐标变换程序
参数加载以及利用激光雷达外参将点云映射到车身坐标系。
# 加载外参文件
config = YamlReader(lidar_extrinsic_config_file)
# 读取rotation矩阵
rotation = config.read(["transform", "rotation"])
quat = [rotation['x'], rotation['y'], rotation['z'], rotation['w']]
R_matrix = R.from_quat(quat).as_matrix()
E_lidar = R.from_quat(quat).as_euler('ZYX', degrees='True') # 计算欧拉角
translation = config.read(["transform", "translation"]) # 读取translation矩阵
T_matrix = [translation["x"], translation["y"], translation["z"]]
# 点云坐标从lidar坐标系变换到Vehicle坐标系
points = origin_cloud.to_list()
new_points = []
for point in points:
new_points.append(np.dot(R_matrix, point) + T_matrix)
vehicle_cloud = pcl.PointCloud()
vehicle_cloud.from_list(new_points)
# 彩色点云可视化
vs = pcl.pcl_visualization.PCLVisualizering
viewer = pcl.pcl_visualization.PCLVisualizering()
visualcolor1 = pcl.pcl_visualization.PointCloudColorHandleringCustom(origin_cloud, 0, 250, 0) # 设置颜色
pcl.pcl_visualization.PCLVisualizering.AddPointCloud_ColorHandler(viewer, origin_cloud, visualcolor1, id=b'cloud1', viewport=0) # 添加点云及标签
visualcolor2 = pcl.pcl_visualization.PointCloudColorHandleringCustom(vehicle_cloud, 0, 0, 250) # 设置颜色
pcl.pcl_visualization.PCLVisualizering.AddPointCloud_ColorHandler(viewer, vehicle_cloud, visualcolor2, id=b'cloud2', viewport=0) # 添加点云及标签
viewer.Spin()
可视化结果如图所示：
绿色的是原始点云的可视化结果，蓝色的是使用激光雷达外参将点云坐标转换到车身坐标系后的点云数据可视化结果。
同理，使用相机外参对点云进行逆映射到相机坐标系，可视化结果如下：
红色的为映射到相机坐标系下的点云可视化结果。
利用相机内参筛选出可以映射到图像上的点云，程序如下，可视化结果如下图。
# 图像进行畸变矫正
undistort_image = copy.deepcopy(origin_image)
undistort_image = cv2.undistort(origin_image, K_arr, D_arr, None)
# 点云映射到图像
points = img_cloud.to_list()
pixel_points = [] # 筛选出可以映射到图像上的点云
uvz_list = []
for point in points:
p_result = np.dot(K_arr, point)
z = point[2]
u = int(p_result[0] / z)
v = int(p_result[1] / z)
if 0 <= u < width and 0 <= v < height and z >= 0:
pixel_points.append(point)
uvz_list.append((u, v, z))
pixel_cloud = pcl.PointCloud()
pixel_cloud.from_list(pixel_points)
将可以映射到图像上的点云的深度信息给到图像，并进行可视化
final_image = cloud_2_image(undistort_image, pixel_cloud, uvz_list)
cv2.imshow("Image window3", final_image)
cv2.waitKey(0)
利用图像的色彩信息对点云进行着色：
程序及所用文件下载链接：使用PCLPY进行点云图像融合代码及所用文件