点云目标检测训练数据预处理---平面拟合与坐标转换(python实现)

在做centerpoint训练之前，需要先对点云数据进行标注，然后制作kittti数据集。不用nuScenes或者waymo数据集的理由也很简单，因为麻烦，没有kitti数据集直观。

kitti数据集的格式如下，可以看到数据集中只有航向角，意味着数据集默认地平面为xoy面或与xoy面平行（本人在交通领域就业，所以采集的点云场景一般都是路面场景，激光雷达的安装位置和公路上的监控摄像头位置差不多)

但是一般激光雷达的安装角度未知，如果不将点云数据中的地平面转换成xoy面或与xoy面平行，做数据标注的时候会比较吃力，且训练时在z方向的范围也不方便设置。使用训练出来的模型去预测目标，3Dbox会比较奇怪，就像下图这样。

对于这种情况，我的思路是：

1、首先拟合地平面的平面方程，用open3d的segment_plane方法，效果如下，蓝色是原始点云，红色是拟合出的平面，下方小框里是输出的平面方程

2、拟合得到了平面方程，也就知道了平面法向量，而xoy面的平面法向量也是已知的：(0,0,1) 。

接下来就是高中数学知识了。

转换后的效果如下，彩色的是原始点云，白色的是转换后的点云。

附上代码，代码中有详细注释，应该比较好懂，需要安装open3d（pip install open3d 即可）

python 复制代码

import open3d as o3d
import numpy as np
import copy

# 读取点云文件
pcd = o3d.io.read_point_cloud("not_xoy.pcd")

# distance_threshold：距离阈值，需要用可视化软件(比如 cloud compare)去看两个点之间的距离，取稍大一点的值即可
plane_model, inliers = pcd.segment_plane(distance_threshold=0.25,
                                        ransac_n=3,
                                        num_iterations=1000)

[a, b, c, d] = plane_model
print(f"Fitted plane equation: {a:.2f}x + {b:.2f}y + {c:.2f}z + {d:.2f} = 0")

''' 平面拟合后的可视化
inlier_cloud = pcd.select_by_index(inliers)
outlier_cloud = pcd.select_by_index(inliers, invert=True)
#设置原始点云为蓝色
outlier_cloud.paint_uniform_color([0,0,1])
#设置拟合的平面点云为红色
inlier_cloud.paint_uniform_color([1,0,0])
# 可视化
vis = o3d.visualization.Visualizer()
vis.create_window()
vis.add_geometry(outlier_cloud)
vis.add_geometry(inlier_cloud)
vis.run()
vis.destroy_window()
'''

# xoy 平面法向量为(0,0,1)
# 上述平面拟合得到的法向量为(a,b,c)
target_normal = np.array([0, 0, 1])
normal_vector = np.array([a, b, c])
# 1：计算旋转轴（叉积）和旋转角度（点积）
rotation_axis = np.cross(normal_vector, target_normal)  # 旋转轴
rotation_axis_norm = np.linalg.norm(rotation_axis)      # 旋转轴的长度

# 如果旋转轴长度为0，说明法向量与目标法向量已经重合，无需旋转
if rotation_axis_norm == 0:
    rotation_matrix = np.eye(3)  # 旋转矩阵就是单位矩阵
else:
    # 旋转轴的单位向量
    rotation_axis_unit = rotation_axis / rotation_axis_norm
    
    # 计算旋转角度
    cos_theta = np.dot(normal_vector, target_normal) / (np.linalg.norm(normal_vector) * np.linalg.norm(target_normal))
    theta = np.arccos(cos_theta)  # 旋转角度

    # Rodrigues' 旋转公式计算旋转矩阵
    K = np.array([[0, -rotation_axis_unit[2], rotation_axis_unit[1]],
                  [rotation_axis_unit[2], 0, -rotation_axis_unit[0]],
                  [-rotation_axis_unit[1], rotation_axis_unit[0], 0]])  # 旋转轴的反对称矩阵
    
    rotation_matrix = np.eye(3) + np.sin(theta) * K + (1 - np.cos(theta)) * np.dot(K, K)

# 输出旋转矩阵
print("旋转矩阵:")
print(rotation_matrix)

# 初始化变换矩阵为单位矩阵，变换矩阵为旋转矩阵+平移向量
transformation_matrix = np.eye(4)

# 平移向量(地平面与xoy面平行即可，可以不用平移)
# translation_vector = np.asarray([0.0, 0.0, d])
translation_vector = np.asarray([0.0, 0.0, 0.0])

# 赋值
transformation_matrix[:3, :3] = rotation_matrix
transformation_matrix[:3, 3] = translation_vector

# 应用变换矩阵
pcd_transformed = copy.deepcopy(pcd)
pcd_transformed.transform(transformation_matrix)
 
# 保存转换后的点云
o3d.io.write_point_cloud("transformed_output.pcd", pcd_transformed)

多说一句：平面拟合使用的segment_plane方法中的distance_threshold参数，一定要自己观察两点之间的距离再设置，因为每个激光雷达参数性能不一样，距离分辨率也不一样。我用的cloud compare中Tools---point list picking

然后选右上方第二个工具，再选择平面中的两个点就可以显示两点间的距离