1. 刚体变换介绍
- 点云平移
假设原始坐标P0为（x0，y0，z0），经过平移变化后变成P坐标（x，y，z），那么这个过程可以看成是P0向量经过了一个平移矩阵T的作用，变成了P向量。数学公式为：P = P0 + T
- 点云旋转
点云旋转通常有四种方式来表达欧拉角、旋转矩阵、旋转向量、四元数（搜索关键词：三维旋转变化、刚体运动）
1）欧拉角
欧拉角定义最为直观，即点云围绕XYZ三个轴分别进行旋转，对应的旋转角度即为α、β、γ。每做一次旋转可以得到一个旋转矩阵，分别对应R x ( α ) 、 R y ( β ) 、 R z ( γ ) Rx(α)、Ry(β)、Rz(γ)Rx(α)、Ry(β)、Rz(γ)。总的旋转矩阵R等于这三个矩阵相乘。这里的相乘是有顺序的，不同的相乘顺序得到的结果不一样，欧拉角计算为按照内旋的方式，也就是Z-Y-X旋转顺序，数学公式为：
2）旋转矩阵
旋转矩阵定义一个三维矩阵R，直接左乘点云坐标完成变换，数学公式为：Pt = R * P。旋转矩阵R相当于上述欧拉角乘积后的表现形式。R必须是一个正交矩阵，并且模长为1。
3）旋转向量
旋转向量的表示方法是用一个向量表示旋转轴（单位向量），和一个角度来表示旋转幅度。旋转向量的方向和选择轴向量一直，其模长表示旋转幅度，所以要求旋转轴是单位向量。旋转向量模长不为1，假设模长为1表示不旋转。
4）四元数
四元数是一个实部加三个虚部形成的代数结构，可以表示三维旋转的复数形式
ps：这里附上欧拉角Rx(α)的推导过程，其他的两个平面的推导是相似的。（手写版推导过程）
总结：这里有4中方式表示旋转，之前都可以进行相互转换，都可以看作是欧拉角或者旋转向量或者旋转矩阵的等效形式。
参考资料：
1. 点云旋转平移（一）—基础知识介绍
2. 刚体运动中的坐标变换-旋转矩阵、旋转向量、欧拉角及四元数
3. 旋转矩阵及左右乘的意义，看这一篇就够了
2. Open3d点云平移
open3d中点云的平移函数为：pcd.translate((tx, ty, tz), relative=True)。当relative为True时，（tx, ty, tz）表示点云平移的相对尺度，也就是平移了多少距离。当relative为False时，（tx, ty, tz）表示点云中心（质心）平移到的指定位置。质心可以坐标可以通过pcd.get_center()得到。
参考代码：
def open3d_translate(): pcd_path = r"E:\Study\Machine Learning\Dataset3d\points_pcd\cat.pcd" pcd = open3d.io.read_point_cloud(pcd_path) print('pcd center point: ', pcd.get_center()) pcd_translate = deepcopy(pcd) pcd_translate.translate((50, 50, 50), relative=True) print('pcd_translate center point: ', pcd_translate.get_center()) open3d.visualization.draw_geometries([pcd, pcd_translate], # 点云列表 window_name="Rabbit", # 窗口名称 width=800, height=600)
ps：使用translate进行点云平移后，原始点云数据会发生变化。如果要用到平移之前的点云，那么需要复制一份原始点云进行平移变换。这里输出：
pcd center point: [ 1.05603759e-02 -5.09065193e+00 2.88664306e+01] pcd_translate center point: [50.01056038 44.90934807 78.86643059]
可以看见，新的质心不是简单的平移得到的。
结果展示：
3. Open3d点云旋转
open3d中点云的旋转函数为：pcd.rotate(R, center=(20, 0, 0))
1）第一个参数R是旋转矩阵。open3d中点云的旋转仍然是通过矩阵运算来完成的，因而需要先获取旋转矩阵。旋转矩阵可以自己进行定义，也可以根据上述介绍的欧拉角、旋转向量和四元数计算得到，open3d提供了这种计算的函数。
2）第二个参数center是旋转中心，即围绕哪个点进行旋转。如果不指定center的值，默认为点云质心，围绕质心旋转后的点云质心保持不变。质心可以坐标可以通过pcd.get_center()得到。
- 根据欧拉角计算旋转矩阵
根据欧拉角计算旋转矩阵的函数为pcd.get_rotation_matrix_from_xyz(α, β, γ)。欧拉角旋转与旋转轴的先后顺序有关。除xyz之外还有xzy、yxz、yzx、zxy和zyx等。
- 根据旋转向量计算旋转矩阵
旋转向量用3行1列的列向量(x, y, z).T来表示。那么旋转轴为向量方向，旋转角度为向量模长。根据旋转向量计算旋转矩阵的函数为get_rotation_matrix_from_quaternion(α, β, γ)
- 根据四元数计算旋转矩阵
根据四元数计算旋转矩阵的函数为get_rotation_matrix_from_quaternion()。四元数用4行1列的列向量(w, x, y, z).T来表示。（这里暂时没有弄懂如何利用四元数向量来操作旋转方向，为此后续的可操作我只使用前两个方法）
参考代码：
def open3d_rotate(): # 读取点云文件 pcd_path = r"E:\Study\Machine Learning\Dataset3d\points_pcd\cat.pcd" pcd = open3d.io.read_point_cloud(pcd_path) print('pcd center point: ', pcd.get_center()) # 为了可以选择可以选择的代码 pcd = open3d.geometry.PointCloud(pcd) print('pcd center point: ', pcd.get_center()) # 1. 利用欧拉角来获取旋转矩阵 pcd_rotate1 = deepcopy(pcd) R = pcd_rotate1.get_rotation_matrix_from_xyz(rotation=[0, np.pi/2, 0]) # y轴顺时针转90° pcd_rotate1.paint_uniform_color(color=[1, 0, 0]) # 红色 pcd_rotate1.rotate(R=R, center=(0, 40, 0)) print('pcd_translate1 center point: ', pcd_rotate1.get_center()) # 2. 利用旋转向量获取旋转矩阵 pcd_rotate2 = deepcopy(pcd) R = pcd_rotate2.get_rotation_matrix_from_axis_angle(rotation=[0, -np.pi/2, 0]) # y轴逆时针转90° pcd_rotate2.paint_uniform_color(color=[0, 1, 0]) # 绿色 pcd_rotate2.rotate(R=R, center=(0, 40, 0)) print('pcd_translate2 center point: ', pcd_rotate2.get_center()) # 3. 利用4元数获取旋转矩阵 pcd_rotate3 = deepcopy(pcd) R = pcd_rotate3.get_rotation_matrix_from_quaternion(rotation=np.array([0, 0, 1, 0]).T) # y轴转180° pcd_rotate3.paint_uniform_color(color=[0, 0, 1]) # 蓝色 pcd_rotate3.rotate(R=R, center=(0, 40, 0)) print('pcd_translate3 center point: ', pcd_rotate3.get_center()) # 可视化点云列表 open3d.visualization.draw_geometries([pcd, pcd_rotate1, pcd_rotate2, pcd_rotate3], window_name="rotate", width=800, height=600)
结果展示：
4. Open3d点云仿射变换
open3d中的投影变换为函数为pcd.transform()，参数为投影变换矩阵T。其中变换矩阵[:3， :3]为旋转矩阵，[:3, -1]为平移向量，[-1, -1]为缩放系数，而最后[-1, :3]为透射变换。即：
矩阵T前3行对应仿射变换，最后一行对应透视变换。其中，s可以用来控制缩放系数，表示缩小的倍数。
参考代码：
def open3d_affine(): # 读取点云文件 pcd_path = r"E:\Study\Machine Learning\Dataset3d\points_pcd\cat.pcd" pcd = open3d.io.read_point_cloud(pcd_path) pcd = open3d.geometry.PointCloud(pcd) pcd.paint_uniform_color(color=[0.5, 0.5, 0.5]) # 灰色 # 仿射变换 pcd_affine = deepcopy(pcd) Rotate = pcd_affine.get_rotation_matrix_from_xyz(rotation=[0, 0, np.pi/2]) # y轴顺时针转90° Translate = np.array([100, 0, 50]).reshape(3, 1) # z轴平移50(向上), x轴平移100(向左) Scale = 2 # 缩小两倍 A1 = np.concatenate([Rotate, Translate], axis=1) A1 = np.concatenate([A1, np.zeros([1, 4])], axis=0) A1[-1, -1] = Scale print(A1) # [[ 6.123234e-17 -1.000000e+00 0.000000e+00 1.000000e+02] # [ 1.000000e+00 6.123234e-17 0.000000e+00 0.000000e+00] # [ 0.000000e+00 0.000000e+00 1.000000e+00 5.000000e+01] # [ 0.000000e+00 0.000000e+00 0.000000e+00 2.000000e+00]] pcd_affine.transform(A1) pcd_affine.paint_uniform_color(color=[0, 0, 1]) # 蓝色 # 可视化点云列表 open3d.visualization.draw_geometries([pcd, pcd_affine], window_name="affine", width=800, height=600)
结果展示：
5. Open3d点云缩放
点云缩放是指尺度按比例缩放一定的倍数，点云数量保持不变。点云缩放的方法主要有numpy数组法、open3d缩放函数、open3d投影变换函数。
- Numpy数组缩放
通过将点云数组乘以一个缩放因子来改变大小，同时通过加法运算实现质心平移
points = points/2.0 # 缩小到原来的一半 points[:, 0] = points[:, 0] + 20 # 质心平移到x=20处
- Open3d缩放函数
open3d的缩放函数为scale，包含两个参数。第一个参数是缩放的比例，即放大的倍数。第二个参数是坐标系原点移动到的位置，相当于缩放后的质心朝相反的方向平移相同的尺度。如果设置第二个参数为（40， 0， 0），那么缩放后的点云质心为（-40，0， 0）
pcd2.scale(2.0, (40, 0, 0)) # 点云放大两倍，质心平移至(-40, 0, 0)
- Open3d仿射变换
在上一节就知道可以构造一个仿射矩阵对点云进行仿射变换，也可以达到缩放的效果
T = np.array([[1, 0, 0, 0], [0, 1, 0, 80], [0, 0, 1, 0], [0, 0, 0, 3]]) # 点云缩小到1/3，质心平移到(0, 80, 0) pcd3.transform(T)
参考代码：
def open3d_scale(): # 读取点云文件 pcd_path = r"E:\Study\Machine Learning\Dataset3d\points_pcd\cat.pcd" pcd = open3d.io.read_point_cloud(pcd_path) pcd = open3d.geometry.PointCloud(pcd) pcd_scale = deepcopy(pcd) pcd_scale.scale(scale=2.0, center=(0, 0, 120)) # 可视化点云列表 open3d.visualization.draw_geometries([pcd, pcd_scale], window_name="scale", width=800, height=600)
结果展示：
6. Open3d点云质心
三维点云的质心实际上是所有点云坐标的平均值。其中，位置坐标是一个三维向量，由（x,y,z)表示。open3d提供了点云的计算方式，即get_center函数。
验证代码：
def open3d_center(): # 读取点云文件 pcd_path = r"E:\Study\Machine Learning\Dataset3d\points_pcd\cat.pcd" pcd = open3d.io.read_point_cloud(pcd_path) pcd = open3d.geometry.PointCloud(pcd) print('pcd center: ', pcd.get_center()) points = np.array(pcd.points) points_center = points.mean(axis=0) print('point center: ', points_center)
输出：
pcd center: [ 1.05603759e-02 -5.09065193e+00 2.88664306e+01] point center: [ 1.05603759e-02 -5.09065193e+00 2.88664306e+01]
7. Open3d点云三角化
点云三角化是将点云边界的点连接成一个个三角形，相当于是边界中的三个点组成一个平面，可以近似认为这个三角平面就是目标表面的一部分。因此，点云三角化实际上是三维目标表面的一种近似方法，这种表面近似也可以通过插值来实现。点云三角化实现了对物体边界平面的拟合，是三维重建的重要步骤。
这里使用TriangleMesh.create_from_point_cloud_ball_pivoting函数来实现三角化，其中第二个参数alpha控制三角化的程度，随着 alpha 值的降低，形状会缩小并产生空洞。
参考代码：
def open3d_triangulation(): # 读取点云文件 pcd_path = r"E:\Study\Machine Learning\Dataset3d\points_pcd\cat.pcd" pcd = open3d.io.read_point_cloud(pcd_path) pcd = open3d.geometry.PointCloud(pcd) pcd.paint_uniform_color(color=[0.5, 0.5, 0.5]) # 计算法向量 pcd.estimate_normals( search_param=open3d.geometry.KDTreeSearchParamHybrid(radius=0.01, max_nn=30) ) # 点云三角化 mesh = open3d.geometry.TriangleMesh.create_from_point_cloud_alpha_shape(pcd, alpha=5) # 可视化点云列表 pcd.translate((50, 50, 50), relative=False) open3d.visualization.draw_geometries([pcd, mesh], window_name="triangulation", width=800, height=600, point_show_normal=False, # 不显示法向量 mesh_show_wireframe=True, # 显示三角网格线 mesh_show_back_face=True) # 显示法向量垂直被部的平面
结果展示：
ps：这里为了能产生一个接近猫的形状，设置的参数alpha=5，随即在背上产生了部分空洞，但是脚部位置还是比较可观。
8. Open3d点云法向量旋转
在点云处理过程中，我们有时需要根据法向量把点云旋转到指定方向。例如，我们需要把激光雷达点云中地面旋转到与xoy平面平行。可以推广到任意点云的法向量旋转到指定方向。
- 推导过程
手写版的推导过程如下所示：
此时，该平面方程的法向量既是这两个向量的旋转轴。同时，由于具体知道这两个向量，那么可以根据两个法向量求得两个向量间的角度，既是旋转角。其计算公式为：
所以，旋转角为：
那么，旋转向量由单位旋转轴和选择角组成，计算公式为：r o t o t e _ v e c t o r = α ∗ a x i s / ∣ a x i s ∣ rotote\_vector = α * axis / |axis|rotote_vector=α∗axis/∣axis∣。得到了计算公式，随即可编写代码计算。
参考代码：
# 参数说明：pcd是点云数据，n0是原始法向量，n1是目标法向量 def normal_ratote(pcd, n0, n1): pcd_rotate = deepcopy(pcd) # 获取旋转向量 n0 = np.array(n0) n1 = np.array(n1) x0, y0, z0 = n0 x1, y1, z1 = n1 rotation_axis = np.array([y1*z0-y0*z1, x0*z1-x1*z0, x0*y1-x1*y0]) rotation_angle = np.arccos((n0*n1).sum() / (np.sqrt((n0**2).sum() + (n1**2).sum()))) rotation_vector = rotation_angle * rotation_axis / np.sqrt((rotation_axis**2).sum()) R = pcd_rotate.get_rotation_matrix_from_axis_angle(rotation=rotation_vector) print(R) # 点云旋转 pcd_rotate = open3d.geometry.PointCloud(pcd_rotate) pcd_rotate.rotate(R) pcd_rotate.paint_uniform_color(color=[0, 0, 1]) return pcd_rotate def open3d_normal_ratote(): # 读取点云文件 pcd_path = r"E:\Study\Machine Learning\Dataset3d\points_pcd\cat.pcd" pcd = open3d.io.read_point_cloud(pcd_path) pcd = open3d.geometry.PointCloud(pcd) pcd.paint_uniform_color(color=[0.5, 0.5, 0.5]) # 平面：Ax + By + Cz = 0 # 其中：n0=(0,0,1)相当于是xy平面，法向量是z轴; n1=(1,0,0)相当于是yz平面，法向量是x轴 pcd_rotate_x = normal_ratote(pcd=pcd, n0=(0, 0, 1), n1=(1, 0, 0)) # z轴向x轴旋转(坐标轴旋转默认是90°) pcd_rotate_y = normal_ratote(pcd=pcd, n0=(0, 0, 1), n1=(0, 1, 0)) # z轴向y轴旋转(坐标轴旋转默认是90°) # 可视化点云列表 open3d.visualization.draw_geometries([pcd, pcd_rotate_x, pcd_rotate_y], window_name="normal rotate", width=800, height=600)
结果显示：
分析：原始的点云假设其法向量是z轴，那么由向量（0,0,1）到（1,0,0）的转换，其实就是让点云从z轴旋转到x轴，就是一个旋转的过程。所以，在代码中也是通过计算旋转矩阵然后进行旋转所获得的。
参考资料：法向量点云旋转
