当前位置:网站首页>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 .
边栏推荐
- lolcat
- Kotlin 集合操作汇总
- 26. Delete duplicates in the ordered array (fast and slow pointer de duplication)
- Global and Chinese markets of hemoglobin analyzers in care points 2022-2028: Research Report on technology, participants, trends, market size and share
- 【leetcode】540. A single element in an ordered array
- Flutter 小技巧之 ListView 和 PageView 的各種花式嵌套
- 2022-2028 global industrial gasket plate heat exchanger industry research and trend analysis report
- PHP student achievement management system, the database uses mysql, including source code and database SQL files, with the login management function of students and teachers
- 回复评论的sql
- Write a jison parser from scratch (2/10): learn the correct posture of the parser generator parser generator
猜你喜欢
How should PMP learning ideas be realized?
QTreeView+自定义Model实现示例
MySQL foundation 02 - installing MySQL in non docker version
Baidu R & D suffered Waterloo on three sides: I was stunned by the interviewer's set of combination punches on the spot
自动化的优点有哪些?
How to batch change file extensions in win10
Hands on deep learning (32) -- fully connected convolutional neural network FCN
直方图均衡化
HMS core helps baby bus show high-quality children's digital content to global developers
Sort out the power node, Mr. Wang he's SSM integration steps
随机推荐
Hands on deep learning (36) -- language model and data set
Daughter love in lunch box
Launpad | Basics
Kubernetes CNI 插件之Fabric
Write a jison parser from scratch (3/10): a good beginning is half the success -- "politics" (Aristotle)
What is permission? What is a role? What are users?
JDBC and MySQL database
26. Delete duplicates in the ordered array (fast and slow pointer de duplication)
Get the source code in the mask with the help of shims
Kotlin:集合使用
The child container margin top acts on the parent container
C # use smtpclient The sendasync method fails to send mail, and always returns canceled
Web端自动化测试失败原因汇总
Write a jison parser from scratch (6/10): parse, not define syntax
Analysis report on the production and marketing demand and investment forecast of tellurium dioxide in the world and China Ⓣ 2022 ~ 2027
MySQL transaction mvcc principle
2022-2028 global protein confectionery industry research and trend analysis report
Write a jison parser from scratch (4/10): detailed explanation of the syntax format of the jison parser generator
品牌连锁店5G/4G无线组网方案
QTreeView+自定义Model实现示例