将设备姿势转换为相机姿势

Convert device pose to camera pose

我正在使用相机内在（fx、fy、cx、cy、宽度、高度）来存储 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 = 深度相机帧 wrt IMU 帧
  2. device2IMU = 设备框架 wrt IMU 框架
  3. device2IMU^-1 = 反转设备框架 wrt IMU 框架
  4. camera2Device = device2IMU^-1 * depth2IMU

现在我可以将点云的每个点与 camera2Device 相乘。但那是对设备框架的转变。

有什么方法可以将设备姿势转换为相机姿势吗？

你算出的等式是正确的！但这还没有结束。

格式

为了形式化终端，让我们使用a_T_b作为转换矩阵，其中a代表基础框架，b代表目标框架。 a_T_b 是 a 帧相对于 b 帧。

计算矩阵

根据你的问题，我们已知的矩阵是：

start_service_T_device, imu_T_device, imu_T_depth

我们要得到的矩阵是：

start_service_T_depth

我们可以直接使用 "matrix chain" 得到结果：

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 是一个常量矩阵，您可以手动计算。 Here 是矩阵的 link，引用自 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 的示例代码

此外，在项目探戈示例中，点云示例对这些转换有很好的解释，这里是 links(c++, java, unity).