Project Tango onPoseAvailable()和getPoseAtTime()有差异

Question

我看到onPoseAvailable()回调和回调的姿势之间存在显着差异Tango.getPoseAtTime().我写了一个测试程序,在那里onPoseAvailable()我记录了传递的姿势并用于getPoseAtTime()使用之前2个回调的时间戳来请求姿势.KEY_BOOLEAN_SMOOTH_POSE已配置false.这是执行该操作的代码(timestamps_成员变量是a LinkedList<Double>):

@Override
public void onPoseAvailable(TangoPoseData poseData) {
   if (poseData != null && poseData.statusCode == TangoPoseData.POSE_VALID) {
      Log.v("bug",
         String.format("onPoseAvailable t: %f, base: %d,  target %d, p: (%f, %f, %f)",
            poseData.timestamp,
            poseData.baseFrame,
            poseData.targetFrame,
            poseData.translation[0], poseData.translation[1], poseData.translation[2]));
      timestamps_.add(poseData.timestamp);
      if (timestamps_.size() > 3)
         timestamps_.remove();
   }

   if (timestamps_.isEmpty())
      return;

   TangoCoordinateFramePair framePair = new TangoCoordinateFramePair(
      TangoPoseData.COORDINATE_FRAME_START_OF_SERVICE,
      TangoPoseData.COORDINATE_FRAME_DEVICE);
   poseData = tango_.getPoseAtTime(timestamps_.getFirst(), framePair);
   if (poseData != null && poseData.statusCode == TangoPoseData.POSE_VALID) {
      Log.v("bug",
         String.format("getPoseAtTime t: %f, base: %d,  target %d, p: (%f, %f, %f)",
            poseData.timestamp,
            poseData.baseFrame,
            poseData.targetFrame,
            poseData.translation[0], poseData.translation[1], poseData.translation[2]));
   }
}

以下是实际日志的摘录(为了清晰起见,我已对已记录的调用进行了解交织):

onPoseAvailable t: 2732.762486, base: 2,  target 4, p: (0.280245, 0.412468, 0.562201)
onPoseAvailable t: 2732.802553, base: 2,  target 4, p: (0.296951, 0.420919, 0.599938)
onPoseAvailable t: 2732.852638, base: 2,  target 4, p: (0.317444, 0.429809, 0.646445)
onPoseAvailable t: 2732.882689, base: 2,  target 4, p: (0.330845, 0.434106, 0.676810)
onPoseAvailable t: 2732.932774, base: 2,  target 4, p: (0.350995, 0.439777, 0.723639)
onPoseAvailable t: 2732.962825, base: 2,  target 4, p: (0.363319, 0.442731, 0.754508)
onPoseAvailable t: 2732.992875, base: 2,  target 4, p: (0.373911, 0.445289, 0.784786)
onPoseAvailable t: 2733.032943, base: 2,  target 4, p: (0.387709, 0.448182, 0.822682)
onPoseAvailable t: 2733.062994, base: 2,  target 4, p: (0.398502, 0.450481, 0.852662)
onPoseAvailable t: 2733.073011, base: 2,  target 4, p: (0.401869, 0.451084, 0.862530)
onPoseAvailable t: 2733.103062, base: 2,  target 4, p: (0.411136, 0.452486, 0.890441)

getPoseAtTime t: 2732.712401, base: 2,  target 4, p: (0.269301, 0.410911, 0.549182)
getPoseAtTime t: 2732.732435, base: 2,  target 4, p: (0.277217, 0.415130, 0.567040)
getPoseAtTime t: 2732.762486, base: 2,  target 4, p: (0.288928, 0.421914, 0.595162)
getPoseAtTime t: 2732.802553, base: 2,  target 4, p: (0.305241, 0.429648, 0.632158)
getPoseAtTime t: 2732.852638, base: 2,  target 4, p: (0.324359, 0.437655, 0.680300)
getPoseAtTime t: 2732.882689, base: 2,  target 4, p: (0.332997, 0.442538, 0.712727)
getPoseAtTime t: 2732.932774, base: 2,  target 4, p: (0.353665, 0.447269, 0.759725)
getPoseAtTime t: 2732.962825, base: 2,  target 4, p: (0.369174, 0.451645, 0.790263)
getPoseAtTime t: 2732.992875, base: 2,  target 4, p: (0.382584, 0.454754, 0.819555)
getPoseAtTime t: 2733.032943, base: 2,  target 4, p: (0.396857, 0.456922, 0.856626)
getPoseAtTime t: 2733.062994, base: 2,  target 4, p: (0.409672, 0.460060, 0.888748)

查看最后一个getPoseAtTime()条目,时间戳为2733.062994.请注意,其位置值onPoseAvailable与相同时间戳的姿势不匹配.有点不对劲.

我确实认为姿势的样条拟合不一定需要通过控制点,但我认为这不是一个可接受的解释.首先,拥有一个为同一测量提供不同值的API并没有多大意义.但另外,实际数字并没有支持这种猜想.

看看getPoseAtTime()Y值,0.460060.这超出了所有onPoseAvailable()Y值的Y范围,包括之前和之后(事实上在整个日志中).没有合理的插值模型可以产生这个值.

我想问题是这里发生了什么？姿势是不一致的,所以至少其中一个是错误的(如果不是两个).我的猜测onPoseAvailable()是更可能是正确的.

下面是两个姿势方法(Nash版本)的Y位置与时间的关系图,其中平板电脑固定在其底座中:

蓝线是onPoseAvailable()回调,红线是getPoseAtTime()轮询.这些结果有点奇怪.如果姿势完全不同,我希望轮询值更平滑,因为它可以使用轮询时间之前和之后的样本的贡献进行过滤,而回调值可以是未过滤的,也可以仅使用先前过滤样本.但这不是我们所看到的 - 调查后的价值看起来更嘈杂.

这是我在上下移动平板电脑时捕获的类似图表.轮询值仍然具有更高的频率,并且这两个信号不会特别紧密地跟踪.

Answer 1

感谢拉希莫托指出这一点！

\n\n

编辑

\n\n

我必须编辑我之前的帖子。我声称在使用 GetPoseAtTime 而不是 OnPoseAvailable 回调的姿势时我有更大的漂移。

\n\n

恰恰相反。我使用 GetPoseAtTime 获得了更好的结果。

\n\n

我通过在椅子上旋转 360\xc2\xb0 进行扫描。正如您在图片中看到的那样，我在办公桌前开始并停下来。

\n\n

扫描周期的开始和结束（单击以获得更高分辨率） \n

\n\n

上面的点云使用姿势 GetPoseAtTime，下面的点云通过 OnPoseAvailable 回调使用姿势。两人同时被抓获。GetPoseAtTime 的偏差很小，但 OnPoseAvailable 回调的偏差确实很大。

\n\n

到目前为止我发现 GetPoseAtTime 使用姿势图，并在检测到循环闭合时纠正姿势，请参阅本文。我测试了结果是否变得更好，如果我立即使用可用的点云访问姿势，或者在最后合并所有点云时访问姿势。

\n\n

事实上，结果要好得多。所以到目前为止我的经验是：

\n\n

OnPoseAvailabe 回调 < 立即获取可用点云的 GetPoseAtTime < 扫描结束时的 GetPoseAtTime

\n