点云目标检测训练数据预处理---平面拟合与坐标转换(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

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

相关推荐
步木木25 分钟前
Anaconda和Pycharm的区别,以及如何选择两者
ide·python·pycharm
星始流年27 分钟前
解决PyInstaller打包PySide6+QML应用的资源文件问题
python·llm·pyspider
南玖yy28 分钟前
Python网络爬虫:从入门到实践
爬虫·python
The Future is mine1 小时前
Python计算经纬度两点之间距离
开发语言·python
九月镇灵将1 小时前
GitPython库快速应用入门
git·python·gitpython
兔子的洋葱圈2 小时前
【django】1-2 django项目的请求处理流程(详细)
后端·python·django
独好紫罗兰2 小时前
洛谷题单3-P5719 【深基4.例3】分类平均-python-流程图重构
开发语言·python·算法
27669582922 小时前
美团民宿 mtgsig 小程序 mtgsig1.2 分析
java·python·小程序·美团·mtgsig·mtgsig1.2·美团民宿
橘子在努力2 小时前
【橘子大模型】关于PromptTemplate
python·ai·llama
SheepMeMe2 小时前
蓝桥杯2024省赛PythonB组——日期问题
python·算法·蓝桥杯