Normal vector point cloud rotation

         During point cloud processing , Sometimes we need to rotate the point cloud to the specified direction according to the normal vector . for example , We need to rotate the ground in the LIDAR point cloud to xoy The plane is parallel . This section will introduce the principle and python Code .

1 Plane equation

         The plane equation can be expressed by the following formula ：

be （A, B, C） Is a normal vector of the plane , Please refer to the blog for the derivation method ：python 3D point cloud projection （ One ）_Coding Leaves blog -CSDN Blog _ Point cloud projection .

2 Normal vector rotation

         As shown in the figure below , Suppose that the vector n0 Is the original normal vector ,n1 Is the direction of the target vector . Our goal is to n0 Rotate to n1 Direction .n0 The coordinates of the for （x0, y0, z0）,n1 The coordinates of the for （x1, y1, z1）, The origin coordinates are O（0, 0, 0）. These three coordinates form a plane , And the rotation axis is perpendicular to the plane , And pass through the coordinate origin , That is, the normal vector of the plane . Through these three points, the plane equation is calculated as follows ：

namely ：

         Then a normal vector of the plane is , That is, the vector of the rotation axis .

         vector n0 To vector n1 The angle of rotation of theta Is the angle between these two vectors , namely ：

3 open3d Point cloud rotation

         The method of point cloud rotation has been published in blog ： Point cloud rotation and Translation （ 3、 ... and ）—python open3d Point cloud rotation _Coding Leaves blog -CSDN Blog _python Point cloud rotation Detailed introduction , Including Euler angle rotation 、 Shaft angle rotation 、 Quaternion rotation, etc . Here, the shaft angle is used for rotation , The module length of the rotation axis vector is the size of the rotation angle . According to the first 2 The rotation axis vector and rotation angle in section can be obtained open3d Axis vector required in , The calculation formula is as follows ：

4 Reference code

         There are many ways to visualize point clouds , Such as open3d、mayavi、pcl、matplotib、cloudcompare etc. , It was introduced in previous blogs . Here the matplotlib Point cloud visualization . If you need to further translate the point cloud , You can refer to the blog before this column .

# -*- coding: utf-8 -*-
"""
 The official account of Lele perception school 
@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,  Enter the point cloud 
    n0 : array, 1x3,  Original normal vector 
    n1 : array, 1x3,  Target normal vector 
    Returns
    -------
    pcd : open3d PointCloud,  Rotate the point cloud 
    """
    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__':
    # Generate a plane point cloud 
    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.
    # Plane equation 
    z = (12 - 4*x -4*y) / 3
    p = np.concatenate((x, y, z), 1)
    p = p[np.where(p[:,2]>=0)]
    viz_matplot(p)
    # Original normal vector 
    n0 = np.array([4, 4, 3])
    # Target normal vector 
    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)
    # After rotation z The value of should be basically equal 
    print('min Z: ', np.min(p[:, -1]), 'max Z: ', np.max(p[:, -1]))
    viz_matplot(p)
    plt.show()

5 Rotation effect

         The pictures before and after rotation are shown in the following figure . The target normal vector is （0,0,1）, That is to say xoy The plane is vertical , The plane after rotation should be consistent with xoy The plane is parallel .

6 【python Three dimensional deep learning 】python 3D point cloud from basic to deep learning _Coding Leaves blog -CSDN Blog _python Three dimensional point cloud

More 3D 、 Please pay attention to two-dimensional perception algorithm and financial quantitative analysis algorithm “ Lele perception school ” WeChat official account , And will continue to update .