Project Tango onPoseAvailable() and getPoseAtTime() discrepancies

Question

I am seeing significant discrepancies between the poses from the onPoseAvailable() callback and Tango.getPoseAtTime(). I wrote a test program where in onPoseAvailable() I logged the delivered pose and used getPoseAtTime() to request the pose using the timestamp from 2 callbacks earlier. KEY_BOOLEAN_SMOOTH_POSE is configured false. Here is the code that does that (the timestamps_ member variable is 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]));
   }
}

Here is an excerpt from an actual log (I have deinterleaved the logged calls for clarity):

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)

Take a look at the last getPoseAtTime() entry, with timestamp 2733.062994. Note that its position values do not match the pose from onPoseAvailable with the identical timestamp. Something isn't right here.

I did consider that a spline fit of pose would not necessarily need to pass through the control points, but I don't think that is an acceptable explanation. First of all it doesn't make a lot of sense to have an API that delivers different values for the same measurement. But in addition the actual numbers don't back that conjecture.

Look at the getPoseAtTime() Y value, 0.460060. This is outside the Y range of all the onPoseAvailable() Y values, both before and after (over the entire log as a matter of fact). No reasonable interpolation model can produce this value.

I guess the question is what is going on here? The poses are inconsistent so at least one of them is wrong (if not both). My guess would be that the onPoseAvailable() is more likely to be correct.

Here is a graph of Y position versus time of the two pose methods (Nash release) with the tablet stationary in its dock:

The blue line is the onPoseAvailable() callback and the red line is the getPoseAtTime() polling. These results are kind of strange. If the poses are going to be different at all, I would expect that the polled value would be smoother because it could be filtered using contributions from samples before and after the poll time, while the callback value would either be unfiltered or filtered only using prior samples. But that isn't what we see - the polled value appears much noisier.

Here's a similar graph captured while I moved the tablet up and down. The polled value still has more high frequencies, and the two signals don't track particularly closely.

Answer 1

Thanks rhashimoto for pointing that out!

EDIT

I have to edit my previous post. I claimed that I had a greater drift while using GetPoseAtTime instead of the pose of the OnPoseAvailable callback.

It's just the other way round. I get much better results with GetPoseAtTime.

I made a scan by spinning 360° on my chair. I started and stopped at my desk as you can see in the picture.

start and end of the scanning cycle (click it for greater resolution)

The point clouds above use poses GetPoseAtTime and the point clouds below use pose by the OnPoseAvailable callback. Both captured at the same time. The drift with GetPoseAtTime is marginal, but with OnPoseAvailable callback really huge.

What I found out so far is that GetPoseAtTime uses a pose graph and corrects the poses if a loop closure is detected see this article. I tested if the results getting better, if I access the pose immediately with an available point cloud or just at the end while I'm merging all point clouds.

And indeed the result is way better. So my experience so far is:

OnPoseAvailabe callback < GetPoseAtTime immediately with an available point cloud < GetPoseAtTime at the end of scanning