python-3.x - 无法对齐 2D 激光雷达点云以匹配 HTC Vive 控制器的坐标系

Question

我将 RPLidar A1 绑在 HTC Vive 控制器上，并编写了一个 python 脚本，将激光雷达的点云转换为 XY 坐标，然后转换这些点以匹配 Vive 控制器的旋转和移动。最终目标是能够使用控制器作为跟踪来扫描 3D 空间。

可悲的是，我尝试的一切，triad_openvr 库的本机四元数，变换矩阵变换，甚至欧拉角，我根本无法让系统在所有可能的运动/旋转轴上运行。

# This converts the angle and distance measurement of lidar into x,y coordinates 
# (divided by 1000 to convert from mm to m)

coord_x = (float(polar_point[0])/1000)*math.sin(math.radians(float(polar_point[1])))

coord_y = (float(polar_point[0])/1000)*math.cos(math.radians(float(polar_point[1])))


# I then tried to use the transformation matrix of the 
# vive controller on these points to no avail

matrix = vr.devices["controller_1"].get_pose_matrix()
x = (matrix[0][0]*coord_x+matrix[0][1]*coord_y+matrix[0][2]*coord_z+(pos_x-float(position_x)))
y = (matrix[1][0]*coord_x+matrix[1][1]*coord_y+matrix[1][2]*coord_z+(pos_y-float(position_y)))
z = (matrix[2][0]*coord_x+matrix[2][1]*coord_y+matrix[2][2]*coord_z+(pos_z-float(position_z)))

# I tried making quaternions using the euler angles and world axes
# and noticed that the math for getting euler angles does not correspond
# to the math included in the triad_vr library so I tried both to no avail
>>>>my euler angles
>>>>angle_x = math.atan2(matrix[2][1],matrix[2][2])
>>>>angle_y = math.atan2(-matrix[2][0],math.sqrt(math.pow(matrix[2][1],2)+math.pow(matrix[2][2],2)))
>>>>angle_z = math.atan2(matrix[1][0],matrix[0][0])

euler = v.devices["controller_1"].get_pose_euler()
>>>>their euler angles (pose_mat = matrix)
>>>>yaw = math.pi * math.atan2(pose_mat[1][0], pose_mat[0][0])
>>>>pitch = math.pi * math.atan2(pose_mat[2][0], pose_mat[0][0])
>>>>roll = math.pi * math.atan2(pose_mat[2][1], pose_mat[2][2])

#quaternion is just a generic conversion from the transformation matrix
#etc

预期结果是 3D 数据空间中正确定向的 2D 切片，如果附加，最终将映射整个 3D 空间。目前，我只成功地仅在单轴 Z 和俯仰旋转上进行扫描。我尝试了几乎无限数量的组合，一些在其他帖子中找到，一些基于原始线性代数，还有一些只是随机的。我究竟做错了什么？

标签： python-3.xmathlidarhtc-viveopenvr

Answer 1

好吧，我们通过使用欧拉旋转并将其转换为四元数来解决这个问题：

我们不得不修改 triad_openvr 如何计算欧拉角的整个定义

def convert_to_euler(pose_mat):
pitch = 180 / math.pi * math.atan2(pose_mat[2][1], pose_mat[2][2])
yaw = 180 / math.pi * math.asin(pose_mat[2][0])
roll = 180 / math.pi * math.atan2(-pose_mat[1][0], pose_mat[0][0])
x = pose_mat[0][3]
y = pose_mat[1][3]
z = pose_mat[2][3]
return [x,y,z,yaw,pitch,roll]

然后不得不进一步对源自控制器的欧拉坐标进行一些旋转（roll 对应于 X，yaw 对应于 Y，pitch 对应于 Z 轴，原因不明）：

r0 = np.array([math.radians(-180-euler[5]), math.radians(euler[3]), -math.radians(euler[4]+180)])

以及预先旋转我们的 LiDAR 点以对应于我们现实世界结构的轴位移：

coord_x = (float(polar_point[0])/1000)*math.sin(math.radians(float(polar_point[1])))*(-1)
coord_y = (float(polar_point[0])/1000)*math.cos(math.radians(float(polar_point[1])))*(math.sqrt(3))*(0.5)-0.125
coord_z = (float(polar_point[0])/1000)*math.cos(math.radians(float(polar_point[1])))*(-0.5)

最后是从欧拉角重建四元数（我们知道的一种解决方法）并按此顺序进行旋转和平移的情况。