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

在做centerpoint训练之前，需要先对点云数据进行标注，然后制作kittti数据集。
kitti数据集的格式如下，可以看到数据集中只有航向角，意味着数据集默认地平面为xoy面或者其他的面，点云视角需要调整至俯视视角
但是一般路口激光雷达的安装角度未知(车载激光雷达点云数据中的地面一般是和XOY面平行的，所以可以不用换做拉平处理)，需要将点云数据中的地平面转换成xoy面，
而CenterPoint不直接在原始的、杂乱的 3D 点云上操作，它将3D空间离散化成一个规则的网格（通常是长方体区域），这个网格的平面与地面平行。所以如果要做centerpoint训练，所有点云数据都需要做拉平操作。

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

2、拟合得到了平面方程，也就知道了平面法向量，而xoy面的平面法向量也是已知的：(0,0,1) 。
接下来就是高中数学知识了。
转换后的效果如下，彩色的是原始点云，白色的是转换后的点云。

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

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

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