在点云处理过程中,我们有时需要根据法向量把点云旋转到指定方向。例如,我们需要把激光雷达点云中地面旋转到与xoy平面平行。本节将详细介绍其中原理和python代码。
1 平面方程
平面方程可以用如下公式表示:
则(A, B, C)为平面的一个法向量,推导方式请参考博客:python三维点云投影(一)_Coding的叶子的博客-CSDN博客_点云投影。
2 法向量旋转
如下图所示,假设向量n0是原始法向量,n1是目标向量方向。我们的目标是将n0旋转到n1方向。n0的坐标为(x0, y0, z0),n1的坐标为(x1, y1, z1),原点坐标为O(0, 0, 0)。这三个坐标构成了一个平面,并且旋转轴垂直于该平面,且经过坐标原点,即该平面的法向量。通过这三个点计算出平面方程如下:
即:
则该平面的一个法向量为,也就是旋转轴的向量。
向量n0到向量n1的旋转角度theta为这两个向量之间的夹角,即:
3 open3d点云旋转
点云旋转的方法已在博客:点云旋转平移(三)—python open3d点云旋转_Coding的叶子的博客-CSDN博客_python 点云旋转详细介绍,包括欧拉角旋转、轴角旋转、四元数旋转等。这里采用轴角的方式进行旋转,其中旋转轴向量的模长为旋转角度大小。根据第2节中的旋转轴向量和旋转角度即可得到open3d中所需要的轴向量,计算公式如下:
4 参考代码
点云可视化的方式种类较多,如open3d、mayavi、pcl、matplotib、cloudcompare等,在之前的博客里都有介绍。这里采用matplotlib的方式进行点云可视化。如果还需要将点云进一步平移,可以参考本专栏之前的博客。
# -*- coding: utf-8 -*-
"""
乐乐感知学堂公众号
@author: https://blog.csdn.net/suiyingy
"""
import numpy as np
import open3d as o3d
import matplotlib.pyplot as plt
from copy import deepcopy
def viz_matplot(points):
x = points[:, 0] # x position of point
y = points[:, 1] # y position of point
z = points[:, 2] # z position of point
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.scatter(x, # x
y, # y
z, # z
c=z, # height data for color
cmap='rainbow',
marker=".")
ax.axis()
def pcd_rotate_normal(pointcloud, n0, n1):
"""
Parameters
----------
pointcloud : open3d PointCloud, 输入点云
n0 : array, 1x3, 原始法向量
n1 : array, 1x3, 目标法向量
Returns
-------
pcd : open3d PointCloud, 旋转后点云
"""
pcd = deepcopy(pointcloud)
n0_norm2 = np.sqrt(sum(n0 ** 2))
n1_norm2 = np.sqrt(sum(n1 ** 2))
theta = np.arccos(sum(n0 * n1) / n0_norm2 / n1_norm2)
r_axis = np.array([n1[2]*n0[1]-n0[2]*n1[1], n0[2]*n1[0]-n1[2]*n0[0], n0[0]*n1[1]-n1[0]*n0[1]])
r_axis = r_axis * theta / np.sqrt(sum(r_axis ** 2))
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(p)
R = pcd.get_rotation_matrix_from_axis_angle(r_axis.T)
pcd.rotate(R)
return pcd
if __name__ == '__main__':
#生成平面点云
x = np.arange(301).reshape(-1, 1).repeat(100, 0) / 100.
y = np.arange(301).reshape(-1, 1).repeat(100, 1).T.reshape(-1, 1) / 100.
#平面方程
z = (12 - 4*x -4*y) / 3
p = np.concatenate((x, y, z), 1)
p = p[np.where(p[:,2]>=0)]
viz_matplot(p)
#原始法向量
n0 = np.array([4, 4, 3])
#目标法向量
n1 = np.array([0, 0, 1])
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(p)
pcd = pcd_rotate_normal(pcd, n0, n1)
p = np.array(pcd.points)
#旋转后z的值应该基本相等
print('min Z: ', np.min(p[:, -1]), 'max Z: ', np.max(p[:, -1]))
viz_matplot(p)
plt.show()
5 旋转效果
旋转前和旋转后的图片如下图所示。目标法向量为(0,0,1),即于xoy平面垂直,旋转后的平面应与xoy平面平行。
6 【python三维深度学习】python三维点云从基础到深度学习_Coding的叶子的博客-CSDN博客_python 三维点云
更多三维、二维感知算法和金融量化分析算法请关注“乐乐感知学堂”微信公众号,并将持续进行更新。