Closed mfassler closed 6 months ago
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()
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())
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.
Great, in that case, addAbsolutePose(...)
should work.
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
Thank you. This is working for me on a real drone, with real sensors.
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.)
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.)