Как преобразовать матрицу Kinect Trajectory в матрицу 4x4 гомогена (пример)

Question

Недавно я использовал RGBD-Slam для получения траектории движения камеры kinect. Каждый кадр будет иметь это:

метка времени tx ty tz qx qy qz qw

tx ty tz задает положение оптического центра цветной камеры относительно мирового происхождения, как определено системой захвата движения.

qx qy qz qw задает ориентацию оптического центра цветной камеры в форме единичного кватерниона относительно начала мира, как это определено системой захвата движения.

Мне нужно преобразовать его в однородную матрицу преобразования 4х4. Любые советы, чтобы решить эту проблему? Давайте скажем пример следующим образом:

1421307756.191874 0,004530 0,050319 0,003332 0,003734 0,023578 -0,054199 0,998245

так,

метка времени: 1421307756.191874; tx: 0.004530

ты: 0,050319; тз: 0,003332

qx: 0,003734; qy: 0,023578

qz: -0,054199; qw: 0.998245

и fx = 525,0; фу = 525,0; // фокусное расстояние по умолчанию

cx = 319,5; cy = 239,5; // оптический центр по умолчанию

Добрая помощь будет принята с благодарностью.

Благодарю.

Answer 1

Извлечение угла и оси кватерниона не должно быть жестким, не так ли? тогда вы можете найти общее выражение матрицы вращения с заданной осью

тогда просто объединяя полученную матрицу с матрицей перевода в правильном порядке, нужно добиться цели

это мой первый пост, пожалуйста, будьте снисходительны, если я говорю дерьмо:-P