Matrix 将设备姿势转换为摄影机姿势

我使用相机内部（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)

问题是姿势是设备帧wrt服务帧的开始。深度图像从深度摄影机帧获取其点。我怎样才能匹配它们

有一种方法可以通过以下方式将深度摄影机点转换为设备帧：

depth2IMU=深度相机帧wrt IMU帧

device2IMU=设备帧wrt IMU帧

device2IMU^-1=反转设备帧wrt IMU帧

camera2Device=device2IMU^-1*深度2IMU

现在我可以用Camera2设备将点云的每个点相乘。但这就是对设备框架的转变

---

有没有办法将设备姿势转换为相机姿势？

您建立的方程式是正确的！但它还没有完成

---

格式 为了形式化术语，让我们使用

a\u T\u b

作为转换矩阵，其中

a

表示基本帧，

b

表示目标帧<相对于

b

帧，code>a\u\u b是

a

帧

---

计算矩阵 根据你的问题，我们知道的矩阵是：

启动服务\u-T\u设备

，

imu-T\u设备

，

imu-T\u深度

我们想要得到的矩阵是：

start\u service\u T\u depth

我们可以使用“矩阵链”来获得结果：

start_service_T_depth = start_service_T_device * 
                        inverse(imu_T_device) * 
                        imu_T_depth;

现在，假设我们在深度帧中有一个点

p_depth

。要应用此点的姿势并将其转换为

start\u service

frame，我们可以使用：

P_ss = start_service_T_depth * P_depth;

---

把它放在OpenGL框架中 在大多数情况下，您可能希望将其转换为便于图形库渲染的协调帧。以OpenGL为例，我们可以将该点转换为OpenGL世界坐标系，如下所示：

请注意，

start\u service\u T\u opengl\u world

是一个可以手动计算的常量矩阵。是一个链接到矩阵，引用项目探戈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;

---

Tango项目的示例代码

---

此外，在项目tango示例中，点云示例对这些转换有很好的解释，以下是链接（，）。

感谢您的回答！是的，如果我想将深度点转置到一个开放的gl帧，这就是它的工作方式。我只是建议如何将点转移到设备框架以演示链。但这不是核心问题。我的问题是，我真的想要相机帧中的点，这样我就可以计算出具有正确分辨率的深度图像等。但我还想存储一个姿势，它表示相机的平移和旋转，而不是设备帧的平移和旋转。我看到了你的点。我认为也可以用类似的方法来解决这个问题。首先，点云已经在深度摄影机帧中，如果要将其转换为彩色摄影机帧，应执行以下操作：P_color=color_T_depth*P_depth；。该矩阵由Tango Exterinsics API提供。关于在深度框中放置姿势，我认为您想要的结果矩阵是开始服务深度，因此矩阵链应该是这样的：ss_T_depth=ss_T_设备*设备_T_imu*imu T_深度；（ss是服务的开始）。顺便问一下，您看过Tango c API示例中的rgb深度同步示例吗？我感觉您正在尝试实现一些类似的功能。这个例子可能会有所帮助。这里是链接：谢谢，这真的很有帮助！矩阵链必须像（（ss\u T\u设备*设备\u T\u imu）*imu\u T\u深度）那样计算，对吗？最后一个问题。。如何从矩阵中导出四元数的姿势<代码>浮点qw=（浮点）（Math.sqrt（1+开始服务深度[0]+开始服务深度[5]+开始服务深度[10]）/2）；浮点qx=（启动服务深度[6]-启动服务深度[9]）/（4*qw）；浮点qy=（开始服务深度[8]-开始服务深度[2]）/（4*qw）；浮点qz=（开始服务深度[1]-开始服务深度[4]）/（4*qw）。。看起来很糟糕。你有什么建议吗？