当前位置:网站首页>Normal vector point cloud rotation
Normal vector point cloud rotation
2022-07-04 09:40:00 【Coding leaves】
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 .
边栏推荐
- 2022-2028 global protein confectionery industry research and trend analysis report
- Analysis report on the development status and investment planning of China's modular power supply industry Ⓠ 2022 ~ 2028
- pcl::fromROSMsg报警告Failed to find match for field ‘intensity‘.
- Four common methods of copying object attributes (summarize the highest efficiency)
- Upgrading Xcode 12 caused Carthage to build cartfile containing only rxswift to fail
- How to batch change file extensions in win10
- 技术管理进阶——如何设计并跟进不同层级同学的绩效
- Nuxt reports an error: render function or template not defined in component: anonymous
- Problems encountered by scan, scanf and scanln in golang
- ASP. Net to access directory files outside the project website
猜你喜欢
C语言指针面试题——第二弹
直方图均衡化
2022-2028 global protein confectionery industry research and trend analysis report
Ultimate bug finding method - two points
libmysqlclient. so. 20: cannot open shared object file: No such file or directory
SSM online examination system source code, database using mysql, online examination system, fully functional, randomly generated question bank, supporting a variety of question types, students, teache
2022-2028 global tensile strain sensor industry research and trend analysis report
Four common methods of copying object attributes (summarize the highest efficiency)
Hands on deep learning (36) -- language model and data set
Latex download installation record
随机推荐
Global and Chinese market of wheel hubs 2022-2028: Research Report on technology, participants, trends, market size and share
2022-2028 global special starch industry research and trend analysis report
Explanation of for loop in golang
You can see the employment prospects of PMP project management
Golang Modules
Ultimate bug finding method - two points
Golang Modules
查看CSDN个人资源下载明细
Investment analysis and prospect prediction report of global and Chinese high purity tin oxide Market Ⓞ 2022 ~ 2027
2022-2028 global industrial gasket plate heat exchanger industry research and trend analysis report
Write a jison parser from scratch (2/10): learn the correct posture of the parser generator parser generator
Daughter love in lunch box
Investment analysis and future production and marketing demand forecast report of China's paper industry Ⓥ 2022 ~ 2028
How web pages interact with applets
2022-2028 global tensile strain sensor industry research and trend analysis report
DR6018-CP01-wifi6-Qualcomm-IPQ6010-IPQ6018-FAMILY-2T2R-2.5G-ETH-port-CP01-802-11AX-MU-MIMO-OFDMA
自动化的优点有哪些?
Upgrading Xcode 12 caused Carthage to build cartfile containing only rxswift to fail
Solution to null JSON after serialization in golang
Kotlin:集合使用