我已将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上扫描并俯仰旋转。我尝试了几乎无限数量的组合，其中一些是在其他职位上发现的，有些是基于原始线性代数的，而有些则是随机的。我在做什么错了？