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

Question

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

遗憾的是，我尝试的一切，triad_openvr 库的原生四元数，变换矩阵变换，甚至欧拉角，我根本无法让系统在所有可能的 movement/rotation 轴上运行。

# 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 space 数据中正确定向的 2D 切片，如果附加，最终将映射整个 3D space。目前，我只成功扫描了单个 Z 轴和俯仰旋转。我尝试了几乎无限数量的组合，有些是在其他帖子中找到的，有些是基于原始线性代数的，有些只是随机的。我做错了什么？

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)

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

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

Having trouble aligning 2D Lidar pointcloud to match the coordinate system of HTC Vive Controller

python-3.x

math

lidar

htc-vive

openvr