Возникли проблемы с выравниванием 2D Lidar pointcloud в соответствии с системой координат HTC Vive Controller - PullRequest
Новая
       90

Возникли проблемы с выравниванием 2D Lidar pointcloud в соответствии с системой координат HTC Vive Controller

0 голосов
/ 06 ноября 2019

Я привязал RPLidar A1 к контроллеру HTC Vive и написал скрипт на python, который преобразует облако точек лидара в координаты XY, а затем преобразует эти точки в соответствии с вращением и перемещением контроллера Vive. Конечная цель - иметь возможность сканировать трехмерное пространство, используя контроллер в качестве отслеживания.

К сожалению, все, что я пытаюсь, собственный кватернион библиотеки 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

Ожидаемые результаты - это правильно ориентированный 2D-срез в 3D-пространстве данных, который в случае добавления в конечном итоге отобразит все 3D-пространство. В настоящее время мне удалось только успешно сканировать только по одной оси Z и повороту шага. Я пробовал почти бесконечное количество комбинаций, некоторые из которых были найдены в других статьях, некоторые основаны на простой линейной алгебре, а некоторые просто случайны. Что я делаю не так?

1 Ответ

0 голосов
/ 10 ноября 2019

Хорошо, мы поняли это, работая с вращениями Эйлера и преобразовывая их в кватернионы:

Нам пришлось изменить все определение того, как 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]

И затем пришлось дополнительно делать некоторые повороты эйлеровых координат, происходящих от контроллера здесь (крен соответствует X, рыскание соответствует Y, а шаг соответствует оси Z по неизвестной причине): 

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

Asа также предварительно поверните наши точки 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)

Это был, наконец, случай восстановления кватернионов из углов Эйлера (обходной путь, как мы знаем)и выполняйте вращение и перевод в указанном порядке.

...