python - 如何使用滚动、俯仰和偏航与我的 3-D 点融合以形成 3-D 表面？
问题描述
我有一个 3-D 点数组，每组 24 个 3-D 点都在同一个平面上，它们大致形成一个圆圈。我正在尝试将滚动、俯仰和偏航的数据添加到数据集中，以便在应用围绕轴的旋转角度后的每 24 组点以这样一种方式制定圆，使其看起来呈弯曲的管状. 我正在使用的代码使用 OpenGL 将其公式化为表面。收集的数据来自在管道内行进的 IMU。
我面临的问题是当我应用这些数据来创建这个管图时，直到沿 Z 轴旋转（即滚动）一切似乎都很好，但是当我沿 x 和 y 轴乘以旋转矩阵时，输出是倾斜的并且完全错误。谁能指出我在代码中做错了什么
这是我为旋转制作的代码
import numpy as np
roll = np.load("tube_roll.npy")
pitch = np.load("tube_pitch.npy")
yaw = np.load("tube_yaw.npy")
data = np.load("three_d.npy")
def matrix_angle(angle, a, b, c, d, e):
cos = np.cos(angle)
sin = np.sin(angle)
zer = np.zeros((angle.shape[0], 3,3))
zer[:, a[0], a[1]] = 1
zer[:, b[0], b[1]] = cos
zer[:, c[0], c[1]] = -sin
zer[:, d[0], d[1]] = sin
zer[:, e[0], e[1]] = cos
return zer
rot_along_x = matrix_angle(yaw, [0, 0], [1, 1], [1, 2], [2, 1], [2, 2])
rot_along_y = matrix_angle(pitch, [1, 1], [0, 0], [2, 0], [0, 2], [2, 2])
rot_along_z = matrix_angle(roll, [2, 2], [0, 0], [0, 1], [1, 0], [1, 1])
a = []
for j in range(0, len(rot_along_z)):
b = []
for i in range(0, 24):
dd = np.dot(rot_along_z[j], data[j, i, :])
dd = np.dot(rot_along_y[j], dd)
dd = np.dot(rot_along_x[j], dd)
b.append(dd)
a.append(b)
a = np.array(a)
np.save("three_data.npy", a)
这就是我用来制作 3-D 视图的方法。
import OpenGL.GL as gl
import OpenGL.arrays.vbo as glvbo
from PyQt5.Qt import *
import numpy as np
import math
import sys
VS = '''
attribute vec3 position;
attribute vec3 a_Color;
uniform mat4 u_proj;
uniform mat4 u_view;
uniform mat4 u_model;
out vec3 g_color;
void main() {
gl_Position = u_proj * u_view * u_model * vec4(position, 1.0);
g_color = a_Color;
}
'''
FS = '''
#version 450
in vec3 g_color;
out vec4 outColor;
void main()
{
float d = 1.0 - gl_FragCoord.z;
outColor = vec4(g_color * d, 1.0);
}
'''
class CreateTubeWindow(QMainWindow):
def __init__(self, *args):
super(CreateTubeWindow, self).__init__()
self.setWindowTitle('3-D Pipeline View')
self.plot = GLPlotWidget3D(self)
self.setCentralWidget(self.plot)
[_, w] = Sb.screen_size()
self.setGeometry(w[2]/2-50, w[3]/2-50, w[2]/2-50, w[3]/2-50)
self.plot.show()
self.show()
class GLPlotWidget3D(QGLWidget):
def __init__(self, *args):
# QGLWidget.__init__(self)
super(GLPlotWidget3D, self).__init__()
self.parent_s = args[0]
self.parent_s = self.parent_s
self.width, self.height = 100, 100
self.right, self.left, self.top, self.bottom = 21000, -21000, 10, -10
self.data = np.zeros((3, 10, 2))
self.vbo = glvbo.VBO(self.data)
self.x = 0
self.y = 0
self.vxb, self.vyb = 0, 0
self.count = 0
# self.showMaximized()
# self.show()
def initializeGL(self):
vs = Sb.compile_vertex_shader(VS)
fs = Sb.compile_fragment_shader(FS)
self.shaders_program_tube = Sb.link_shader_program(vs, fs)
self.attrib = {a: gl.glGetAttribLocation(self.shaders_program_tube, a) for a in ['position', 'a_Color']}
self.uniform = {u: gl.glGetUniformLocation(self.shaders_program_tube, u) for u in
['u_model', 'u_view', 'u_proj']}
self.roll = np.load('tube_roll.npy')
self.pitch = np.load('tube_pitch.npy')
self.yaw = np.load('tube_yaw.npy')
self.e = np.load("three_data.npy")
self.e = np.array(self.e, dtype=np.float32)
self.color = np.zeros((self.e.shape[0]*self.e.shape[1], 3), dtype=np.float32)
self.color.fill(1.0)
self.elems = self.elements_array(self.e.shape[0], self.e.shape[1])
self.elems = np.array(self.elems, dtype=np.int32)
self.vertexbuffer = gl.glGenBuffers(1)
gl.glBindBuffer(gl.GL_ARRAY_BUFFER, self.vertexbuffer)
gl.glBufferData(gl.GL_ARRAY_BUFFER, self.e, gl.GL_DYNAMIC_DRAW)
self.elementbuffer = gl.glGenBuffers(1)
gl.glBindBuffer(gl.GL_ELEMENT_ARRAY_BUFFER, self.elementbuffer)
gl.glBufferData(gl.GL_ELEMENT_ARRAY_BUFFER, self.elems, gl.GL_DYNAMIC_DRAW)
self.colorbuffer = gl.glGenBuffers(1)
gl.glBindBuffer(gl.GL_ARRAY_BUFFER, self.colorbuffer)
gl.glBufferData(gl.GL_ARRAY_BUFFER, self.color, gl.GL_DYNAMIC_DRAW)
self.scaleZ = 30/self.e[:, :, 2].max()
self.right, self.left, self.top, self.bottom, self.far, self.near = self.e[:, :, 1].min(), self.e[:, :,
1].max(), self.e[:,
:,
0].min(), self.e[
:,
:,
0].max(), self.e[
:,
:,
2].max(), self.e[
:,
:,
2].min()
self.far *= self.scaleZ
self.near *= self.scaleZ
self.angle = 0.962
self.angle_y = 133.544
self.zoom = True
self.zoom_c = 90.0
self.zoom_cv = 0.0
def perspective_view(self):
# projection matrix
aspect, ta, near, far = self.width / self.height, np.tan(np.radians(self.zoom_c) / 2), 10, 50
proj = np.matrix(((1 / ta / aspect, 0, 0, 0), (0, 1 / ta, 0, 0), (0, 0, -(far + near) / (far - near), -1),
(0, 0, -2 * far * near / (far - near), 0)), np.float32)
# view matrix
view = np.matrix(((1, 0, 0, 0), (0, 0, -1, 0), (0, 1, 0, 0), (0, 0, -30, 1)), np.float32)
# model matrix
c, s = math.cos(self.angle), math.sin(self.angle)
cy, sy = math.cos(self.angle_y), math.sin(self.angle_y)
scale = np.matrix(((1, 0, 0, 0), (0, 1, 0, 0), (0, 0, self.scaleZ, 0), (0, 0, 0, 1)), np.float32)
rotZ = np.array(((c, s, 0, 0), (-s, c, 0, 0), (0, 0, 1, 0), (0, 0, 0, 1)), np.float32)
rotY = np.matrix(((cy, 0, sy, 0), (0, 1, 0, 0), (-sy, 0, cy, 0), (0, 0, 0, 1)), np.float32)
trans = np.matrix(((1, 0, 0, 0), (0, 1, 0, 0), (0, 0, 1, 0), (0, 0, (self.near - self.far) / 2, 1)), np.float32)
model = scale * trans * rotY * rotZ
gl.glUniformMatrix4fv(self.uniform['u_proj'], 1, gl.GL_FALSE, proj)
gl.glUniformMatrix4fv(self.uniform['u_view'], 1, gl.GL_FALSE, view)
gl.glUniformMatrix4fv(self.uniform['u_model'], 1, gl.GL_FALSE, model)
def paintGL(self):
self.resizeGL(self.width, self.height)
gl.glClearColor(0.2, 0.2, 0.2, 0)
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glEnable(gl.GL_DEPTH_TEST)
gl.glUseProgram(self.shaders_program_tube)
gl.glBindBuffer(gl.GL_ARRAY_BUFFER, self.vertexbuffer)
stride = 0 # 3*self.e.itemsize
offset = None # ctypes.c_void_p(0)
loc = self.attrib['position']
gl.glEnableVertexAttribArray(loc)
gl.glVertexAttribPointer(loc, 3, gl.GL_FLOAT, False, stride, offset)
gl.glBindBuffer(gl.GL_ELEMENT_ARRAY_BUFFER, self.elementbuffer)
self.perspective_view()
loc = self.attrib['a_Color']
gl.glEnableVertexAttribArray(loc)
gl.glBindBuffer(gl.GL_ARRAY_BUFFER, self.colorbuffer)
gl.glVertexAttribPointer(loc, 3, gl.GL_FLOAT, False, stride, offset)
gl.glDrawElements(gl.GL_TRIANGLE_STRIP, self.elems.size, gl.GL_UNSIGNED_INT, None)
def resizeGL(self, width, height):
self.width, self.height = width, height
gl.glViewport(0, 0, width, height)
def mouseMoveEvent(self, e):
if self.zoom:
v = self.x-e.x()
if v > 2:
self.angle -= 0.01
elif v < -2:
self.angle += 0.01
self.x = e.x()
w = self.y-e.y()
if w > 2:
self.angle_y += 0.01
elif w < -2:
self.angle_y -= 0.01
self.y = e.y()
else:
if e.x() > self.zoom_cv:
self.zoom_c -= 1
else:
self.zoom_c += 1
self.zoom_cv = e.x()
self.update()
def keyPressEvent(self, k):
if k.key() == 16777249:
self.zoom = False
def keyReleaseEvent(self, k):
if k.key() == 16777249:
self.zoom = True
def wheelEvent(self, w):
e = w.angleDelta().y()
if e > 0:
self.zoom_c -= 1
else:
self.zoom_c += 1
self.update()
def elements_array(self, a, b):
ring_c = b
slice_c = a
my_si = np.arange(slice_c - 1)
my_elems = np.repeat(ring_c * my_si, 2)
temp = np.empty(2 * my_si.size, my_si.dtype)
temp[0::2] = my_si
temp[1::2] = my_si + 1
my_ri = np.arange(ring_c + 1)
my_ie = np.expand_dims(my_ri % ring_c, axis=1)
my_si = temp * ring_c + my_ie
my_elems = np.vstack((my_elems, my_si))
N = my_elems.shape[1] // (slice_c - 1)
return my_elems.reshape((my_elems.shape[0], -1, N)).swapaxes(0, 1).reshape(
(np.prod(my_elems.shape), -1)).squeeze()
if __name__ == '__main__':
app = QApplication(sys.argv)
editor = CreateTubeWindow()
sys.exit(app.exec_())
我的感觉是我的旋转矩阵做错了，因为沿 Z 轴的旋转输出似乎是错误的，因为它最初位于某种二维平面中，但随着我们进一步矩阵乘法增加 x 和 y 坐标的值也不应该发生。
下面还有使用的数据文件 https://drive.google.com/file/d/177knYTSmlWcC1RQjcXf-CKICWr09MU_i/view?usp=sharing
解决方案
用于numpy.matmul矩阵乘法：
rot_mat = np.matmul(rot_along_x, np.matmul(rot_along_y, rot_along_z))
或使用矩阵乘法 ( @) 运算符（请参阅array ）：
rot_mat = rot_along_z @ rot_along_y @ rot_along_x
用于numpy.dot变换顶点：
a = np.array([[np.dot(v, rot_mat) for v in row] for row in data])
推荐阅读
- c# - 让线程在开始下一组任务之前等待所有任务完成
- flutter - 使用 bloc 处理来自 Post 方法的回调响应
- java - 添加Jpa maven依赖项后无法运行spring boot应用程序
- java - 如何在 Xpath 中动态分配属性值
- r - 新版本 broom::tidy 和 speedglm 无法获取 p 值
- javascript - 从多个 csv 文件中绘制网站中的数据
- javascript - 如果复选框被选中隐藏复选框
- swift - SwiftUI 如果嵌入视图
- mysql - 将 Google 表格脚本连接到 MySQL
- navbar - 导航栏移动视图链接到页面不起作用