Proper way to reset the Yaw?

[mfassler]

I'm trying to use VIO (visual odometry). Sometimes, I have external sensor data that I use to re-calibrate (reset) the pose. I'm using addAbsolutePose for this. It seems to work fine for position (x, y, z) but it only seems to work 1 time for orientation (w, x, y, z). After that, I can never reset orientation again.

(My goal here is to re-calibrate the Yaw using some other sensor. Roll and Pitch can be defined by gravity.)

import numpy as np
import depthai
import spectacularAI
import time
import transforms3d

pipeline = depthai.Pipeline()

vio_pipeline = spectacularAI.depthai.Pipeline(pipeline)

with depthai.Device(pipeline) as device, vio_pipeline.startSession(device) as vio_session:

    tStart = time.time()
    while True:
        out = vio_session.waitForOutput()
        pose = out.pose.asMatrix()

        #pkt = pose.tobytes()
        #sock.sendto(pkt, POSE_VISUALIZER)

        rot = pose[:3, :3]
        _pitch, roll, _yaw = transforms3d.euler.mat2euler(rot)

        yaw = -_yaw + np.pi/2
        pitch = _pitch + np.pi/2

        roll_deg = np.degrees(roll)
        pitch_deg = np.degrees(pitch)
        yaw_deg = np.degrees(yaw)

        print(f'{roll_deg:.2f} {pitch_deg:.2f} {yaw_deg:.2f}', out.pose.time)

        # Every once-in-a-while, we will re-calibrate odometry using
        # some external sensors:
        ts = time.time()
        if (ts - tStart) > 5:
            tStart = ts
            print('resetting pose:')

            newPose = spectacularAI.Pose.fromMatrix(out.pose.time, out.pose.asMatrix())

            # Calibrate the yaw to a known heading:
            newPose.orientation.w = -0.459
            newPose.orientation.x = 0.523
            newPose.orientation.y = -0.541
            newPose.orientation.z = 0.471

            newPose.time += 0.016

            covariance = np.zeros((3, 3))
            orientationVariance = 0.0

            vio_session.addAbsolutePose(newPose, covariance, orientationVariance)

With this code, the orientation is only reset once (the first time). It is never reset again. Am I doing this wrong? (Resetting the position always seems to work fine.)

[kaatrasa]

Hey,

We noticed an issue, where absolute pose orientation was only used for initialization by default. We will make this a default in next release, but for now, you can add configInternal = {"useExternalPoseOrientation" : "true"}. In addition, you will have to tune the external pose position covariance and orientation variance.

Note that the absolute pose feature is mainly meant to work with positioning systems (GPS, VPS, ...) where you get the poses or positions in consistent coordinate frame unlike your example where the pose is reset each time to some default value. At least in my example below, the VIO state explodes quite easily...

When implementing the yaw reset, you need to make sure that the gravity direction stays valid, or that will also affect tracking quality.

import spectacularAI
import depthai
import threading
import numpy as np
import time

from spectacularAI.cli.visualization.visualizer import Visualizer, VisualizerArgs

if __name__ == '__main__':
    configInternal = {"useExternalPoseOrientation" : "true"}
    visArgs = VisualizerArgs()
    visArgs.cameraFollow = False
    visArgs.targetFps = 30
    visualizer = Visualizer(visArgs)
    tStart = -1
    firstPose = None

    def onVioOutput(vioOutput, vioSession):
        global tStart
        global firstPose
        visualizer.onVioOutput(vioOutput.getCameraPose(0), status=vioOutput.status)

        if firstPose is None: firstPose = vioOutput.pose.asMatrix()
        if tStart < 0: tStart = time.time()

        # Every once-in-a-while, we will re-calibrate odometry using
        # some external sensors:
        ts = time.time()
        if (ts - tStart) > 5:
            print("Resetting...")
            tStart = ts

            # Calibrate the yaw to a known heading:
            # NOTE: this will not work well in practice because the poses aren't using consistent coordinate frame
            # (we always try to reset to first pose) and VIO state will easily explode... 
            # but you can use this approach if the external poses are accurate.
            newPose = spectacularAI.Pose.fromMatrix(vioOutput.pose.time, firstPose) # TODO: replace with a real pose

            covariance = 1e-5 * np.identity(3)
            orientationVariance = 1e-7
            vioSession.addAbsolutePose(newPose, covariance, orientationVariance)

    def captureLoop():
        print("Starting OAK-D device")
        pipeline = depthai.Pipeline()
        config = spectacularAI.depthai.Configuration()
        config.internalParameters = configInternal
        vioPipeline = spectacularAI.depthai.Pipeline(pipeline, config)

        with depthai.Device(pipeline) as device, \
            vioPipeline.startSession(device) as vioSession:
            while not visualizer.shouldQuit:
                onVioOutput(vioSession.waitForOutput(), vioSession)

    thread = threading.Thread(target=captureLoop)
    thread.start()
    visualizer.run()
    thread.join()

[kaatrasa]

More practical approach might be to implement the yaw reset outside the SDK. This could work like this:

1) Define your own coordinate frame 'output'. 2) Disable SLAM or (loop closures) to get smooth 'camera->world' poses from the SDK. 3) When you want to reset yaw direction, update your 'world->output' transformation.

Below is a simplified example without the yaw reset implemented:

'''
In this example the output pose is reset to identity every 5 seconds.
'''

import spectacularAI
import depthai
import numpy as np
import time

def resetYaw(cameraToWorld):
    # TODO: add actual implementation
    return np.linalg.inv(cameraToWorld)

if __name__ == '__main__':
    np.set_printoptions(precision=3, suppress=True)

    tStart = -1
    tPrint = -1
    worldToOutput = np.identity(4) # Your custom output coordinate used to implement the yaw resets

    def onVioOutput(vioOutput):
        global tStart, tPrint, worldToOutput
        cameraToWorld = vioOutput.pose.asMatrix()

        # Initialize first time
        if tStart < 0: 
            tStart = time.time()
            tPrint = tStart
            worldToOutput = resetYaw(cameraToWorld)

        # Every once-in-a-while, we will re-calibrate odometry using some external sensors:
        ts = time.time()
        if (ts - tStart) > 5:
            print("Resetting...")
            tStart = ts
            worldToOutput = resetYaw(cameraToWorld)

        # Print every 1 second...
        if (ts - tPrint) > 1:
            tPrint = time.time()

            # camera->output = camera->world->output = world->output * camera->world
            cameraToOutput = worldToOutput @ cameraToWorld
            print(cameraToOutput)
            print()

    print("Starting OAK-D device")
    pipeline = depthai.Pipeline()
    config = spectacularAI.depthai.Configuration()
    config.useSlam = False # If you need SLAM, delete this row and uncomment below
    # or
    # configInternal = {
    #     "applyLoopClosures" : "false"
    # }
    # config.internalParameters = configInternal
    vioPipeline = spectacularAI.depthai.Pipeline(pipeline, config)

    with depthai.Device(pipeline) as device, \
        vioPipeline.startSession(device) as vioSession:
        while True:
            onVioOutput(vioSession.waitForOutput())

[mfassler]

Hello. Thank you. Yes, in practice, my pose updates will have real, physical meaning. I expect the Yaw updates to be rather small. My code snippet here is just a toy example.

[kaatrasa]

Great, in that case, addAbsolutePose(...) should work.

[kaatrasa]

I forgot about this option: fixToExternalPose" : "true, that might be best for your use case.

import spectacularAI
import depthai
import threading
import numpy as np
import time

from spectacularAI.cli.visualization.visualizer import Visualizer, VisualizerArgs

if __name__ == '__main__':
    configInternal = {"fixToExternalPose" : "true"}
    visArgs = VisualizerArgs()
    visArgs.cameraFollow = False
    visArgs.targetFps = 30
    visualizer = Visualizer(visArgs)
    tStart = -1
    firstPose = None

    def onVioOutput(vioOutput, vioSession):
        global tStart
        global firstPose
        visualizer.onVioOutput(vioOutput.getCameraPose(0), status=vioOutput.status)

        if firstPose is None: firstPose = vioOutput.pose.asMatrix()
        if tStart < 0: tStart = time.time()

        # Every once-in-a-while, we will re-calibrate odometry using
        # some external sensors:
        ts = time.time()
        if (ts - tStart) > 5:
            print("Resetting...")
            tStart = ts
            newPose = spectacularAI.Pose.fromMatrix(vioOutput.pose.time, firstPose)
            covariance = 1e-5 * np.identity(3)
            vioSession.addAbsolutePose(newPose, covariance, -1)

    def captureLoop():
        print("Starting OAK-D device")
        pipeline = depthai.Pipeline()
        config = spectacularAI.depthai.Configuration()
        config.internalParameters = configInternal
        vioPipeline = spectacularAI.depthai.Pipeline(pipeline, config)

        with depthai.Device(pipeline) as device, \
            vioPipeline.startSession(device) as vioSession:
            while not visualizer.shouldQuit:
                onVioOutput(vioSession.waitForOutput(), vioSession)

    thread = threading.Thread(target=captureLoop)
    thread.start()
    visualizer.run()
    thread.join()

https://github.com/SpectacularAI/sdk-examples/assets/46484036/d4225e51-bcb7-4143-bf50-c33c25011f98

[mfassler]

Thank you. This is working for me on a real drone, with real sensors.