T265: Regression with version 2.32.1 on macOS

[neilyoung]

Required Info
Camera Model	T265
Firmware Version	0.2.0.908
Operating System & Version	ubuntu 16.04.6 LTS, macOS Catalina, Raspbian Buster
Kernel Version (Linux Only)
Platform
SDK Version	2.32.1, master
Language
Segment	others

Issue Description

Unfortunately, a problem has crept in with 2.32.1, which I first noticed with the last development branch today: There is a significant delay in reading the poses when I compare the current system timestamp with the timestamp of the pose. The discrepancy is up to more than two seconds. I noticed it when my position shown on the floorplan was displayed with an extreme delay.

This problem can also be observed in the freshly installed current 2.32.1 release version (master branch). But not on all Linux based systems in my environment: There the time delay is limited. It's still quite high with about 0.3 seconds, but the script below is not that optimal.

The output of the script under Raspbian, USB 2.1:

Press Enter to STOP...
0.34208178520202637: confidence 2
0.33758115768432617: confidence 2
0.22131633758544922: confidence 2
0.2819828987121582: confidence 2
0.3376452922821045: confidence 2
0.3397374153137207: confidence 3
0.21799349784851074: confidence 3
0.27852582931518555: confidence 3
0.30374956130981445: confidence 3
0.34232306480407715: confidence 3
0.2203223705291748: confidence 3
0.1854846477508545: confidence 3
0.11700558662414551: confidence 3
0.12341928482055664: confidence 3
0.19746994972229004: confidence 3
0.2665226459503174: confidence 3
0.3371407985687256: confidence 3

The output under macOS Catalina, USB 3.1

Press Enter to START...

Press Enter to STOP...
0.6169295310974121: confidence 2
1.2879741191864014: confidence 2
1.9377093315124512: confidence 2
2.0766899585723877: confidence 2
2,162928819656372: confidence 2
2.1629104614257812: confidence 2
2.202058792114258: confidence 2
2.2452268600463867: confidence 2
2,1303274631500244: confidence 2
2.140265703201294: confidence 2

Please note the long delay. Under Ubuntu it looks similar to Raspbian.

I immediately checked out version 2.32.0 and compiled it under macOS. Here are the values:

0.5282917022705078: confidence 2
0.5844135284423828: confidence 2
0.5427703857421875: confidence 2
0.5603499412536621: confidence 2
0.5017528533935547: confidence 2
0.4952991008758545: confidence 2
0.553605318069458: confidence 2
0.56036376953125: confidence 2
0.5818815231323242: confidence 2
0.5251281261444092: confidence 2
0.5180854797363281: confidence 2
0.5468711853027344: confidence 2
0.530174970626831: confidence 2
0.5644876956939697: confidence 2
0.5016629695892334: confidence 2
0.5092761516571045: confidence 2
0.5395026206970215: confidence 2
0.5624520778656006: confidence 3
0.5254216194152832: confidence 3
0.5042383670806885: confidence 3
0.5275769233703613: confidence 3
0.4567422866821289: confidence 3

You see, not intoxicating either, but okay.

Here the script:

#!/usr/bin/python
# -*- coding: utf-8 -*-
import pyrealsense2 as rs
import threading
from threading import Lock
import logging
from datetime import datetime

class Pose:
    def __init__(self):
        self.__pose_sensor = None
        self.__pose_handler_thread = None
        self.__pipeline = None
        self.__frame_mutex = Lock()
        self.__frame_data = {"pose": None, "timestamp": None}
        self.__stopped = False
        self.__last_reported_raw_frame_number = 0
   


    # T265 frame callback
    def __frame_callback(self, frame):
        """This callback is called on a separate thread, so we must use a mutex to ensure that data is synchronized properly. We should also be
           careful not to do much work on this thread to avoid data backing up in the callback queue.
        """
        pose_frame = frame.as_pose_frame()
        self.__frame_mutex.acquire()
        self.__frame_data["pose"] = pose_frame
        self.__frame_data["timestamp"] = pose_frame.timestamp
        self.__frame_mutex.release()

    # Stop operation
    def stop(self):
        '''Stop operation'''
        self.__stopped = True
        self.__pose_handler_thread.join()
        self.__pipeline.stop()
        self.__pose_sensor = None
        self.__pipeline = None


    # Start operation
    def start(self):
        '''Start operation'''
        try:
            self.__pipeline = rs.pipeline()
            config = rs.config()
            config.enable_stream(rs.stream.pose)
            device = config.resolve(self.__pipeline).get_device()

            # This line causes an exception "No device connected" at second start
            self.__pose_sensor = device.first_pose_sensor()
 
            self.__pose_handler_thread = threading.Thread(target=self.__pose_handler_loop)
            self.__pose_handler_thread.daemon = True
            self.__pose_handler_thread.start()
            self.__pipeline.start(config, self.__frame_callback)

        except Exception:
            logging.exception("Exception in pose_handler")

    def __pose_handler_loop(self):
        while not self.__stopped:
            try:
                self.__frame_mutex.acquire()
                valid = self.__frame_data["timestamp"] is not None
                self.__frame_mutex.release()

                if valid:
                    self.__frame_mutex.acquire()
                    pose = self.__frame_data["pose"]
                    timestamp = self.__frame_data["timestamp"]
                    self.__frame_data["timestamp"] = None
                    self.__frame_mutex.release()
                    
                    data = pose.get_pose_data()

                    if pose.frame_number - self.__last_reported_raw_frame_number >= 200:
                        self.__last_reported_raw_frame_number = pose.frame_number
                        deltaT = datetime.now().timestamp() - (timestamp/1000)
                        print("{}: confidence {}".format(deltaT, data.tracker_confidence))

                
            except Exception:
                logging.exception("Exception in pose_handler")


while True:
    input("Press Enter to START...\n")
    pose = Pose()
    pose.start()
    input("Press Enter to STOP...\n")
    pose.stop()

[neilyoung]

Update: Seems, it is also not working well on Ubuntu. The deltaT goes up from 0.01 s to 1.3 secs after about 30 s. Regardless of whether it is using USB 2.1 or 3.1

Will test next days how it behaves in a real app (with floor plan display)

[neilyoung]

OK, it MUST be an issue with the SDK. According to realsense-viewer the firmware is 0.2.0.926

But - with the latest SDK there is this increasing delay and there are gaps in the frame numbers.
This is not the case on the same device with the same camera but SDK 2.32.0

Good luck finding it

[neilyoung]

Here is a different version of the test script, now derived from an official sample (t265_example.py)

#!/usr/bin/python
# -*- coding: utf-8 -*-
## License: Apache 2.0. See LICENSE file in root directory.
## Copyright(c) 2019 Intel Corporation. All Rights Reserved.

#####################################################
##           librealsense T265 example             ##
#####################################################

# First import the library
import pyrealsense2 as rs
from threading import Lock
from datetime import datetime

frame_mutex = Lock()
frame_data = {"pose": None, "timestamp": None}

def frame_callback(frame):
    pose_frame = frame.as_pose_frame()
    frame_mutex.acquire()
    frame_data["pose"] = pose_frame
    frame_data["timestamp"] = pose_frame.timestamp
    frame_mutex.release()

# Declare RealSense pipeline, encapsulating the actual device and sensors
pipe = rs.pipeline()

# Build config object and request pose data
cfg = rs.config()
cfg.enable_stream(rs.stream.pose)

# Start streaming with requested config
pipe.start(cfg, frame_callback)

try:
    while True:

        frame_mutex.acquire()
        valid = frame_data["timestamp"] is not None
        frame_mutex.release()

        if valid:
            frame_mutex.acquire()
            pose = frame_data["pose"]
            timestamp = frame_data["timestamp"]
            frame_data["timestamp"] = None
            frame_mutex.release()

            # Print some of the pose data to the terminal
            data = pose.get_pose_data()

            #print("Position: {}".format(data.translation))
            #print("Velocity: {}".format(data.velocity))
            #print("Acceleration: {}\n".format(data.acceleration))
            deltaT = datetime.now().timestamp() - (timestamp/1000)
            print("Frame #{}, deltaT {}, confidence {}".format(pose.frame_number, deltaT, data.tracker_confidence))

finally:
    pipe.stop()

On macOS you should see an increasing delay and gaps in the frame numbers with 2.23.1. Not the case with 2.23.0 (commit cb453b5 from Jan 5)

[neilyoung]

OK, I can give the all-clear. At least a little bit. The problem is reduced to the frame-callback mechanism. When I return to the old form, replacing the frame callback with the known loop, the delay is constant in the range of 0.005 seconds.

while True:
     frames = self.__pipeline.wait_for_frames(5000)

Perfectly okay.

But with the callback, there's nothing "in time" anymore.

I doubt that the mutex has become lame out of the sudden. It must be the way the callback is scheduled.

[neilyoung]

Anybody able to confirm?

[neilyoung]

Hi guys. I know you have a lot to do with all the open issues, but this lack of responses is frustrating. I think I'm not only asking stupid questions, but helping to fix issues as much as I can. But this requires at least a bit of attention. For which I'm asking hereby, with all respect. No offence contained.

[neilyoung]

2020-12-06 23:07:35,936 pose_pose.py INFO    : SDK: 2.39.0
2020-12-06 23:07:35,936 pose_pose.py INFO    : Firmware: 0.2.0.951
2020-12-06 23:07:35,936 pose_pose.py INFO    : USB: 3.1

Bug still present. But I have learned you won't fix it :)