Преобразование позы устройства в позу камеры

Я использую внутреннюю камеру (fx, fy, cx, cy, width, hight) для хранения изображения глубины буфера TangoXyzIjData.xyz. Поэтому я вычисляю для каждой точки xyz соответствующую точку изображения и сохраняю ее значение z.

x' = (fx * x) / z + cx
y' = (fy * y) / z + cy
depthImage[x'][y'] = z

Теперь я хотел бы также сохранить соответствующие данные позы. Я использую метку времени TangoXyzIjData.timestamp и следующую функцию

getPoseAtTime(double timestamp, TangoCoordinateFramePair framePair)

с парой кадров

new TangoCoordinateFramePair(TangoPoseData.COORDINATE_FRAME_START_OF_SERVICE, TangoPoseData.COORDINATE_FRAME_DEVICE)

Проблема в том, что поза - это кадр устройства по отношению к началу кадра службы. И изображение глубины получает свои точки от кадра камеры глубины. Как я могу соответствовать им?

Существует способ преобразовать точки камеры глубины в кадр устройства:

  1. depth2IMU = кадр камеры глубины относительно кадра IMU
  2. device2IMU = кадр устройства относительно кадра IMU
  3. device2IMU^-1 = инвертировать кадр устройства относительно кадра IMU
  4. camera2Device = устройство2IMU^-1 * depth2IMU

Теперь я мог умножить каждую точку облака точек на camera2Device. Но это преобразование в рамку устройства.

Есть ли способ преобразовать позу устройства в позу камеры?


matrix depth google-project-tango
person bashbug    schedule 06.10.2015    source источник

Ответы (1)


arrow_upward
2
arrow_downward

Уравнение, которое вы составили, верно! Но это не закончено.

Формат

Чтобы формализовать терминологию, давайте использовать a_T_b в качестве матрицы преобразования, где a представляет базовый кадр, b представляет целевой кадр. a_T_b - это кадр a по отношению к кадру b.

Вычислить матрицу

Исходя из вашего вопроса, известные нам матрицы:

start_service_T_device, imu_T_device , imu_T_depth

Матрица, которую мы хотим получить:

start_service_T_depth

Мы можем просто использовать «цепочку матриц», чтобы получить результат:

start_service_T_depth = start_service_T_device * 
                        inverse(imu_T_device) * 
                        imu_T_depth;

Теперь предположим, что у нас есть точка P_depth в кадре глубины. Чтобы применить позу для этой точки и преобразовать ее в кадр start_service, мы могли бы использовать:

P_ss = start_service_T_depth * P_depth;

Поместите его в фрейм OpenGL

В большинстве случаев вы можете захотеть преобразовать его в какой-нибудь координатный фрейм, который легко визуализировать в графической библиотеке. Возьмем, к примеру, OpenGL, мы можем преобразовать эту точку в мировую систему координат OpenGL следующим образом:

Обратите внимание, что start_service_T_opengl_world — это постоянная матрица, которую вы можете вычислить вручную. Здесь находится ссылка на матрица, взятая из примера Project Tango c++.

P_gl = opengl_world_T_start_service * P_ss;

Мы можем расширить все, что мы только что написали, и поместить это в одно уравнение:

P_gl = opengl_world_T_start_service * 
       start_service_T_device * 
       inverse(imu_T_device) * 
       imu_T_depth * 
       P_depth;

Пример кода из Project Tango

Кроме того, в примерах танго проекта пример облака точек имеет довольно хорошее объяснение этих преобразований, вот ссылки (c++, java, единство).

person xuguo    schedule 07.10.2015
comment
Спасибо за Ваш ответ! Да, конечно, если я захочу перенести точки глубины в открытый кадр gl, это так и работает. Я лишь предложил, как переставить точки на рамку устройства, чтобы продемонстрировать цепочку. Но не это было основной проблемой. У меня проблема в том, что мне действительно нужны точки в кадре камеры, чтобы я мог вычислить изображение глубины с правильным разрешением и т. д. Но я также хочу сохранить позу, которая представляет перемещение и вращение камеры, а не рамка устройства. - person bashbug; 08.10.2015
comment
Я вижу вашу точку зрения. Я думаю, что это может быть решено аналогичным методом. Во-первых, облако точек уже находится в кадре камеры глубины, если вы хотите преобразовать его в кадр камеры цвета, вы должны сделать: P_color = color_T_depth * P_depth;. Эта матрица предоставляется внешним API Tango. Что касается размещения позы в кадре глубины, я думаю, что результирующая матрица, которую вы хотите, это start_service_T_depth, поэтому цепочка матриц должна быть такой: ss_T_depth = ss_T_device * device_T_imu * imu_T_depth; (сс — начало службы). - person xuguo; 10.10.2015
comment
кстати, вы смотрели rgb-depth-sync-example из примеров Tango c API? У меня такое ощущение, что вы пытаетесь реализовать некоторые аналогичные функции. Этот пример может быть полезен. Вот ссылка: github.com/googlesamples/ танго-примеры-c/дерево/мастер/ - person xuguo; 10.10.2015
comment
Спасибо, это действительно полезно! Цепочка матриц должна вычисляться как ((ss_T_device * device_T_imu) *imu_T_depth), верно? Последний вопрос... как я могу получить позу из этой матрицы в виде кватерниона? float qw = (float) (Math.sqrt(1 + start_service_T_depth[0] + start_service_T_depth[5] + start_service_T_depth[10])/2); float qx = (start_service_T_depth[6] - start_service_T_depth[9])/(4*qw); float qy = (start_service_T_depth[8] - start_service_T_depth[2])/(4*qw); float qz = (start_service_T_depth[1] - start_service_T_depth[4])/(4*qw); .. плохо выглядит. Есть ли у вас какие-либо предложения? - person bashbug; 10.10.2015