[BUG] Significant drift in pose estimate when using VIO

[viggy96]

When using Basalt VIO in DepthAI v3, the pose estimate drifts significantly even when the device is still.

How can I improve the behavior of VIO?

I need accurate VIO for my application.

I have seen this behavior on both the OAK-D Pro W, and the OAK-D Lite (w/IMU).

[moratom]

Hi @viggy96, thanks for testing it out!

We've merged a critical change to develop after the rc.2 went out, so first thing to do would be to test on develop.

I assume you were using rc.2?

[viggy96]

Correct, I'm using rc.2, how would I go about testing develop?

[moratom]

Are you using Python or C++?

[viggy96]

I'm using Python. What critical change are you referring to, that you suspect will result in an improvement?

[moratom]

For Python you can do git checkout develop && git pull on this repository and then do python3 examples/python/install_requirements.py which will install the correct version for you.

Or you can just run the command below for the current develop:

python3 -m pip install -U --prefer-binary --user --extra-index-url https://artifacts.luxonis.com/artifactory/luxonis-python-snapshot-local depthai==3.0.0-rc.2.dev0+598c1139f42e3d78de4d12f8bf8d832f7a2c8105

I'm using Python. What critical change are you referring to, that you suspect will result in an improvement?

We unified coordinate systems for the IMUs and missed a change that needed to be done in BasaltVIO node. With it in place, the drfit should go away.

Out of curiousity, what do you plan to use the VIO for? We'd appreciate any furhter feedback.

[viggy96]

I plan on using it for FRC, FIRST Robotics Competition, which is a competition of high school robotics teams run by FIRST, a worldwide STEM non-profit, whose aim is to get young people excited about STEM fields, and give them real life skills to be better people.

In matches, it is often useful to know where the robot is on the field, such that the robot can automate certain actions when in a certain area, or to drive autonomously.

It would further help if AprilTags could be integrated, as anchor points, so that when the device sees AprilTags, it will correct its position. Preferably this AprilTag feature would automatically reject tags whose orientation appears to be incorrect, based on the camera's own orientation relative to the gravity direction, and the known orientation of the tag relative to gravity.

[viggy96]

The AprilTag integration as previously mentioned is the only part that is missing from DepthAI as far as I can see at the moment. Other useful features for this use case are the object tracker, for tracking game objects in order to intake and score them, as well as simple 2D AprilTag tracking, which can be useful in cases where a simplified approach to tracking a target is sufficient.

[viggy96]

Hmm, yes the drift is significantly lessened. What are some other strategies to further minimise it? It still drifts within a few centimeters.

Would the other VIO algorithms have better performance? Does including SLAM help?

The more accuracy the better for my use case.

[viggy96]

Hmm, actually I do still see significant drift, upon further testing.

The project is here: https://github.com/lasarobotics/PurpleRanger You can run ./main.py --mode test --pipeline vio --verbose

[viggy96]

I'd be happy to run any additional tests or provide more information @moratom

[viggy96]

RTAB-Map appears to be performing better than Basalt in terms of drift, but it appears to lose tracking after just a few movements.

[viggy96]

Yeah Basalt still drifts significantly

[viggy96]

I'm using an OAK-D Pro W device in my testing, and I'd like this to be just as accurate on an OAK-D Lite as well, as a lower cost option.

[viggy96]

What is the level of drift that you guys have seen in testing?

[moratom]

It's quite dependant on the environment, but the RTABMAP SLAM in a internal environemnt should not drift more than a few centimeters per minute.

I can reproduce the issue for a large drift with Basalt on some of the devices, we'll take a look.

Note that the SLAM nodes are still in "early access" mode, so we'll bring furhter improvements to them. In the mean team, can you work with RTABMAP SLAM?

[viggy96]

Possibly, though as mentioned earlier, it loses tracking a lot, with any sudden movement. So it's not really suitable for our use case. It only works when moving very slowly.

If this could be resolved then it would be fine

[Serafadam]

Hi @viggy96, could you run export DEPTHAI_DEBUG=1 before running the example and post the information logged?

[viggy96]

Hi @viggy96, could you run export DEPTHAI_DEBUG=1 before running the example and post the information logged?

Sure, with Basalt? Just want to clarify, since RTABMap was also suggested here.

[viggy96]

@Serafadam Here is a log file running the Basalt example code:

luxonis_basalt.log

[viggy96]

For reference, here is the exact code I ran to produce this log. You may uncomment the print line to see the pose data, and its drift. Also, the above log was produced with the device sitting motionless, face up on a table.

#!/usr/bin/env python

import signal
import time
import depthai as dai

from wpimath.geometry import Pose3d, Translation3d, Rotation3d, Quaternion

# Create pipeline
with dai.Pipeline() as p:
    fps = 60
    width = 640
    height = 400
    # Define sources and outputs
    left = p.create(dai.node.Camera).build(dai.CameraBoardSocket.CAM_B, sensorFps=fps)
    right = p.create(dai.node.Camera).build(dai.CameraBoardSocket.CAM_C, sensorFps=fps)
    imu = p.create(dai.node.IMU)
    odom = p.create(dai.node.BasaltVIO)

    imu.enableIMUSensor([dai.IMUSensor.ACCELEROMETER_RAW, dai.IMUSensor.GYROSCOPE_RAW], 200)
    imu.setBatchReportThreshold(1)
    imu.setMaxBatchReports(10)

    # Linking
    left.requestOutput((width, height)).link(odom.left)
    right.requestOutput((width, height)).link(odom.right)
    imu.out.link(odom.imu)
    transform_queue = odom.transform.createOutputQueue()

    p.start()
    while p.isRunning():
        transform_message = transform_queue.get()
        temp_point = transform_message.getTranslation()
        temp_quaternion = transform_message.getQuaternion()

        pose = Pose3d(
            Translation3d(temp_point.x, temp_point.y, temp_point.z),
            Rotation3d(Quaternion(temp_quaternion.qw, temp_quaternion.qx, temp_quaternion.qy, temp_quaternion.qz))
        )

        #print(str(pose))
        time.sleep(0.01)

[viggy96]

Is there any update on the drift issue?

[viggy96]

If there's any additional information I can provide, I would be happy to help. Anything to help improve the product experience.

[moratom]

Hi @viggy96 - at this time we're providing the SLAM examples as a starting point, but can't spend the resources for full support yet.

The good news is that the majority of the implementation is completly open source, so if you want you're free to poke around and try to improve the accuracy of the examples. The relevant implementation is here https://github.com/luxonis/depthai-core/blob/main/src/rtabmap/RTABMapSLAM.cpp

[viggy96]

I would appreciate any pointers as to what part of the Basalt algorithm is going wrong so I can look into fixing it. I believe you guys were able to replicate the drift issue?

[viggy96]

~~Further, how do I build depthai-core with Basalt? It appears that when I build depthai-core using the instructions provided in the README, the BasaltVIO node is missing.~~

Nevermind, I got it.

[viggy96]

@Serafadam Here are some additional logs with vio_debug = true

vio_debug.log vio_debug1.log vio_debug2.log

[Serafadam]

Hi, thanks for the additional logs, as we mentioned this issue is not deterministic so it might be hard to debug correctly, we will be looking into it more in depth, what could be also useful is if you could share some additional information, for example:

• What hardware are you running on
• What system
• Is this happening both on C++ and Python versions
• Could you provide some recordings so that we can see if the cause might be related to environment, for example if there are large shifts in exposure or some other external influences

[viggy96]

@Serafadam

I am currently using a OAK-D Pro W.

As far as my system specs, it is a Ryzen 9 5950X, 64GB RAM, 7900XTX GPU, running Manjaro kernel 6.15.

So far I've only tested Python, and I see the issue there, I can try to test on C++.

I will provide recordings of the passthrough as soon as I can.

[viggy96]

We also see similar behavior on an OAK-D Lite.

[viggy96]

@Serafadam Here is a video from the left camera:

https://github.com/user-attachments/assets/c70538e7-2f3c-4a3a-b1cd-3e2bc90f5de0