From 1111d1ef47124f935f9cfd13d293870cb9a6693b Mon Sep 17 00:00:00 2001 From: Paul Hine Date: Sun, 6 Apr 2025 12:03:58 +0200 Subject: [PATCH 01/10] started relocalize april tags example --- python/oak/relocalize_apriltags.py | 51 ++++++++++++++++++++++++++++++ 1 file changed, 51 insertions(+) create mode 100644 python/oak/relocalize_apriltags.py diff --git a/python/oak/relocalize_apriltags.py b/python/oak/relocalize_apriltags.py new file mode 100644 index 0000000..b992d97 --- /dev/null +++ b/python/oak/relocalize_apriltags.py @@ -0,0 +1,51 @@ +import argparse +import spectacularAI +import cv2 + +frameTime = None +aprilTagFramePose = None +relocalizedFramePose = None + +def onOutput(output): + global frameTime + if output.frameSet is not None: + frameTime = output.frameSet.timestamp + +def onAprilTagMappingOutput(output, frameSet): + global frameTime, aprilTagFramePose + + for frame in frameSet: + if args.preview and frame.image: + cv2.imshow("Camera #{}".format(frame.index), cv2.cvtColor(frame.image.toArray(), cv2.COLOR_RGB2BGR)) + cv2.waitKey(1) + print(output.asJson()) + + if not output.map.keyFrames: return # empty map + + # Find latest keyframe + keyFrameId = max(output.map.keyFrames.keys()) + keyFrame = output.map.keyFrames[keyFrameId] + frameSet = keyFrame.frameSet + + frame = frameSet.primaryFrame + if frame is None or frame.image is None: return + if frame.visualMarkers is None or frame.visualMarkers == []: return + marker = frame.visualMarkers[0] + if not marker.hasPose: + print("Warning: Found April Tag but it's not in the tags.json file") + return + + frameTime = output.frameSet.timestamp + aprilTagFramePose = frame.cameraPose.asMatrix() + print("Found April Tag at timestamp [{}] with pose: {}".format(frame.timestamp, aprilTagFramePose)) + +if __name__ == '__main__': + p = argparse.ArgumentParser(__doc__) + p.add_argument("dataFolder", help="Folder containing the recorded session and tags.json", default="data") + p.add_argument("--preview", help="Show latest primary image as a preview", action="store_true") + args = p.parse_args() + + replay = spectacularAI.Replay(args.dataFolder) + + replay.setExtendedOutputCallback(onAprilTagMappingOutput) + replay.runReplay() From 4cc9b3e66ee8b2cc60c216c0de4ae7cabe57ed9f Mon Sep 17 00:00:00 2001 From: Paul Hine Date: Sun, 6 Apr 2025 13:25:44 +0200 Subject: [PATCH 02/10] frame pose and time for april tag --- python/oak/relocalize_apriltags.py | 56 +++++++++++++++++++----------- 1 file changed, 36 insertions(+), 20 deletions(-) diff --git a/python/oak/relocalize_apriltags.py b/python/oak/relocalize_apriltags.py index b992d97..c32eb4f 100644 --- a/python/oak/relocalize_apriltags.py +++ b/python/oak/relocalize_apriltags.py @@ -5,20 +5,27 @@ frameTime = None aprilTagFramePose = None relocalizedFramePose = None +args = None +replay = None -def onOutput(output): - global frameTime - if output.frameSet is not None: - frameTime = output.frameSet.timestamp +def onAprilTagExtendedOutput(output, frameSet): + global args, frameTime, aprilTagFramePose + if (aprilTagFramePose is not None): return # already found + # print(output.status) + + if not output.status == spectacularAI.TrackingStatus.TRACKING: return # not tracking yet -def onAprilTagMappingOutput(output, frameSet): - global frameTime, aprilTagFramePose - for frame in frameSet: - if args.preview and frame.image: - cv2.imshow("Camera #{}".format(frame.index), cv2.cvtColor(frame.image.toArray(), cv2.COLOR_RGB2BGR)) - cv2.waitKey(1) - print(output.asJson()) + if not frame.image: continue + aprilTagFramePose = frame.cameraPose.pose.asMatrix() + # print(dir(aprilTagFramePose)) + frameTime = output.pose.time + print("Found April Tag at: {}\n{}".format(frameTime, aprilTagFramePose)) + replay.close() # stop replay + break + +def onAprilTagOutput(output): + global args, frameTime, aprilTagFramePose if not output.map.keyFrames: return # empty map @@ -32,20 +39,29 @@ def onAprilTagMappingOutput(output, frameSet): if frame.visualMarkers is None or frame.visualMarkers == []: return marker = frame.visualMarkers[0] if not marker.hasPose: - print("Warning: Found April Tag but it's not in the tags.json file") - return + raise Exception("Found April Tag but it's not in the tags.json file") - frameTime = output.frameSet.timestamp - aprilTagFramePose = frame.cameraPose.asMatrix() - print("Found April Tag at timestamp [{}] with pose: {}".format(frame.timestamp, aprilTagFramePose)) +def replayAprilTags(): + global args, replay + + print("[Replay April Tags] {}".format(args)) + + configInternal = { + "aprilTagPath": args.dataFolder + "/tags.json", + "extendParameterSets" : ["april-tags"] + } + + replay = spectacularAI.Replay(args.dataFolder, onAprilTagOutput, configuration=configInternal) + replay.setExtendedOutputCallback(onAprilTagExtendedOutput) + replay.runReplay() if __name__ == '__main__': p = argparse.ArgumentParser(__doc__) - p.add_argument("dataFolder", help="Folder containing the recorded session and tags.json", default="data") + p.add_argument("dataFolder", nargs="?", help="Folder containing the recorded session and tags.json", default="data") p.add_argument("--preview", help="Show latest primary image as a preview", action="store_true") args = p.parse_args() - replay = spectacularAI.Replay(args.dataFolder) + replayAprilTags() - replay.setExtendedOutputCallback(onAprilTagMappingOutput) - replay.runReplay() + print("done") + \ No newline at end of file From 91e4219c0338b8d821a5dd5f5820cc2a9ab2d497 Mon Sep 17 00:00:00 2001 From: Paul Hine Date: Sun, 6 Apr 2025 15:14:22 +0200 Subject: [PATCH 03/10] add replayRelocalize --- python/oak/relocalize_apriltags.py | 46 +++++++++++++++++++++++++++--- 1 file changed, 42 insertions(+), 4 deletions(-) diff --git a/python/oak/relocalize_apriltags.py b/python/oak/relocalize_apriltags.py index c32eb4f..e1f4ef8 100644 --- a/python/oak/relocalize_apriltags.py +++ b/python/oak/relocalize_apriltags.py @@ -9,8 +9,8 @@ replay = None def onAprilTagExtendedOutput(output, frameSet): - global args, frameTime, aprilTagFramePose - if (aprilTagFramePose is not None): return # already found + global args, frameTime, relocalizedFramePose + if (relocalizedFramePose is not None): return # already found # print(output.status) if not output.status == spectacularAI.TrackingStatus.TRACKING: return # not tracking yet @@ -18,9 +18,24 @@ def onAprilTagExtendedOutput(output, frameSet): for frame in frameSet: if not frame.image: continue aprilTagFramePose = frame.cameraPose.pose.asMatrix() + frameTime = output.pose.time + print("Tracking started with April Tag at: {}\n{}".format(frameTime, aprilTagFramePose)) + replay.close() # stop replay + break + +def onRelocalizeExtendedOutput(output, frameSet): + global args, frameTime, aprilTagFramePose + if (aprilTagFramePose is not None): return # already found + print(output.status) + + if not output.status == spectacularAI.TrackingStatus.TRACKING: return # not tracking yet + + for frame in frameSet: + if not frame.image: continue + relocalizedFramePose = frame.cameraPose.pose.asMatrix() # print(dir(aprilTagFramePose)) frameTime = output.pose.time - print("Found April Tag at: {}\n{}".format(frameTime, aprilTagFramePose)) + print("Tracking started with Relocalization at: {}\n{}".format(frameTime, relocalizedFramePose)) replay.close() # stop replay break @@ -41,6 +56,9 @@ def onAprilTagOutput(output): if not marker.hasPose: raise Exception("Found April Tag but it's not in the tags.json file") +# def onRelocalizeOutput(output): + # nothing here + def replayAprilTags(): global args, replay @@ -55,13 +73,33 @@ def replayAprilTags(): replay.setExtendedOutputCallback(onAprilTagExtendedOutput) replay.runReplay() +def replayRelocalize(): + global args, replay + + print("[Replay Relocalize] {}".format(args)) + + configInternal = { + "mapLoadPath": args.dataFolder + "/tags.json", + "extendParameterSets" : ["relocalization"] + } + + replay = spectacularAI.Replay(args.dataFolder, configuration=configInternal) + replay.setExtendedOutputCallback(onRelocalizeExtendedOutput) + replay.runReplay() + if __name__ == '__main__': p = argparse.ArgumentParser(__doc__) p.add_argument("dataFolder", nargs="?", help="Folder containing the recorded session and tags.json", default="data") p.add_argument("--preview", help="Show latest primary image as a preview", action="store_true") args = p.parse_args() - replayAprilTags() + # replayAprilTags() + # if not frameTime: + # raise Exception("No valid frame found in April Tags replay") + + replayRelocalize() + if not frameTime: + raise Exception("No valid frame found in Relocalize replay") print("done") \ No newline at end of file From 22222ae854e33fea340bf63d4e7f96365200703b Mon Sep 17 00:00:00 2001 From: Paul Hine Date: Mon, 7 Apr 2025 11:25:47 +0200 Subject: [PATCH 04/10] finds poses for frame time in april tag and relocalize replays --- python/oak/relocalize_apriltags.py | 148 +++++++++++++++++++++-------- 1 file changed, 108 insertions(+), 40 deletions(-) diff --git a/python/oak/relocalize_apriltags.py b/python/oak/relocalize_apriltags.py index e1f4ef8..7caa865 100644 --- a/python/oak/relocalize_apriltags.py +++ b/python/oak/relocalize_apriltags.py @@ -1,47 +1,73 @@ import argparse import spectacularAI import cv2 - -frameTime = None +import time +import threading +import os +from enum import Enum + +class State(Enum): + FindAprilTagTime = 1 + FindRelocalizeTime = 2 + FindAprilTagPose = 3 + FindRelocalizePose = 4 + +pairedFrameTime = None +aprilTagFrameTime = None aprilTagFramePose = None +relocalizedFrameTime = None relocalizedFramePose = None args = None -replay = None +aprilTagReplay1 = None +aprilTagReplay2 = None +relocalizeReplay1 = None +relocalizeReplay2 = None +state = None def onAprilTagExtendedOutput(output, frameSet): - global args, frameTime, relocalizedFramePose - if (relocalizedFramePose is not None): return # already found + global args, aprilTagFrameTime, aprilTagFramePose, aprilTagReplay1, aprilTagReplay2, pairedFrameTime, state # print(output.status) - if not output.status == spectacularAI.TrackingStatus.TRACKING: return # not tracking yet for frame in frameSet: - if not frame.image: continue - aprilTagFramePose = frame.cameraPose.pose.asMatrix() - frameTime = output.pose.time - print("Tracking started with April Tag at: {}\n{}".format(frameTime, aprilTagFramePose)) - replay.close() # stop replay - break + if not pairedFrameTime: + if not aprilTagFrameTime: + aprilTagFrameTime = output.pose.time + print("Tracking started with April Tag at: {}".format(aprilTagFrameTime)) + # print("Close April Tag replay") + aprilTagReplay1.close() + break + elif state == State.FindAprilTagPose and output.pose.time >= pairedFrameTime: + if aprilTagFramePose is None: + aprilTagFramePose = frame.cameraPose.pose.asMatrix() + print("April Tag pose at: {}\n{}".format(output.pose.time, aprilTagFramePose)) + # print("Close April Tag replay") + aprilTagReplay2.close() + break def onRelocalizeExtendedOutput(output, frameSet): - global args, frameTime, aprilTagFramePose - if (aprilTagFramePose is not None): return # already found - print(output.status) - + global args, relocalizedFrameTime, relocalizedFramePose, relocalizeReplay1, relocalizeReplay2, pairedFrameTime, state + #print(output.status) if not output.status == spectacularAI.TrackingStatus.TRACKING: return # not tracking yet for frame in frameSet: if not frame.image: continue - relocalizedFramePose = frame.cameraPose.pose.asMatrix() - # print(dir(aprilTagFramePose)) - frameTime = output.pose.time - print("Tracking started with Relocalization at: {}\n{}".format(frameTime, relocalizedFramePose)) - replay.close() # stop replay - break + if not pairedFrameTime: + if not relocalizedFrameTime: + relocalizedFrameTime = output.pose.time + print("Tracking started with Relocalization at: {}".format(relocalizedFrameTime)) + # print("Close relocalize replay") + relocalizeReplay1.close() + break + elif state == State.FindRelocalizePose and output.pose.time >= pairedFrameTime: + if relocalizedFramePose is None: + relocalizedFramePose = frame.cameraPose.pose.asMatrix() + print("Relocalized pose at: {}\n{}".format(output.pose.time, relocalizedFramePose)) + # print("Close relocalize replay") + relocalizeReplay2.close() + break def onAprilTagOutput(output): - global args, frameTime, aprilTagFramePose - if not output.map.keyFrames: return # empty map # Find latest keyframe @@ -55,14 +81,10 @@ def onAprilTagOutput(output): marker = frame.visualMarkers[0] if not marker.hasPose: raise Exception("Found April Tag but it's not in the tags.json file") - -# def onRelocalizeOutput(output): - # nothing here def replayAprilTags(): - global args, replay - - print("[Replay April Tags] {}".format(args)) + global args, aprilTagReplay1, aprilTagReplay2, state + print("[Replay April Tags] {}".format(state)) configInternal = { "aprilTagPath": args.dataFolder + "/tags.json", @@ -71,35 +93,81 @@ def replayAprilTags(): replay = spectacularAI.Replay(args.dataFolder, onAprilTagOutput, configuration=configInternal) replay.setExtendedOutputCallback(onAprilTagExtendedOutput) + + if state == State.FindAprilTagTime: + aprilTagReplay1 = replay + else: + aprilTagReplay2 = replay + replay.runReplay() -def replayRelocalize(): - global args, replay - print("[Replay Relocalize] {}".format(args)) +def replayRelocalize(): + global args, relocalizeReplay1, relocalizeReplay2, state + # print("[Replay Relocalize] {}".format(args)) configInternal = { - "mapLoadPath": args.dataFolder + "/tags.json", + "mapLoadPath": args.dataFolder + "/map.bin", "extendParameterSets" : ["relocalization"] } replay = spectacularAI.Replay(args.dataFolder, configuration=configInternal) replay.setExtendedOutputCallback(onRelocalizeExtendedOutput) + + if state == State.FindRelocalizeTime: + relocalizeReplay1 = replay + else: + relocalizeReplay2 = replay + replay.runReplay() + if __name__ == '__main__': p = argparse.ArgumentParser(__doc__) p.add_argument("dataFolder", nargs="?", help="Folder containing the recorded session and tags.json", default="data") p.add_argument("--preview", help="Show latest primary image as a preview", action="store_true") args = p.parse_args() - # replayAprilTags() - # if not frameTime: - # raise Exception("No valid frame found in April Tags replay") - - replayRelocalize() - if not frameTime: + state = State.FindAprilTagTime + print("Finding first April Tag frame time...") + aprilTagThread = threading.Thread(target=replayAprilTags) + aprilTagThread.start() + while aprilTagFrameTime is None and aprilTagThread.is_alive(): + time.sleep(0.1) + if aprilTagFrameTime is None: + raise Exception("No valid frame found in April Tags replay") + + print("Finding first relocalized frame time...") + state = State.FindRelocalizeTime + relocalizeThread = threading.Thread(target=replayRelocalize) + relocalizeThread.start() + while relocalizedFrameTime is None and relocalizeThread.is_alive(): + time.sleep(0.1) + if relocalizedFrameTime is None: raise Exception("No valid frame found in Relocalize replay") + + pairedFrameTime = max(aprilTagFrameTime, relocalizedFrameTime) + print("Paired frame time: {}".format(pairedFrameTime)) + + print("Finding paired time in April Tags replay...") + state = State.FindAprilTagPose + aprilTagThread = threading.Thread(target=replayAprilTags) + aprilTagThread.start() + while aprilTagFramePose is None and aprilTagThread.is_alive(): + time.sleep(0.1) + if aprilTagFramePose is None: + raise Exception("Paired frame not found in April Tags replay") + + print("Finding paired time in Relocalize replay...") + state = State.FindRelocalizePose + relocalizeThread = threading.Thread(target=replayRelocalize) + relocalizeThread.start() + while relocalizedFramePose is None and relocalizeThread.is_alive(): + time.sleep(0.1) + if relocalizedFramePose is None: + raise Exception("Paired frame not found in Relocalize replay") print("done") + os._exit(0) + \ No newline at end of file From 9639737b5cc6910f7b68688d78c809bd4bb19f12 Mon Sep 17 00:00:00 2001 From: Paul Hine Date: Tue, 15 Apr 2025 01:19:35 +0200 Subject: [PATCH 05/10] find tag to camera instead of tag to world --- python/oak/relocalize_apriltags.py | 93 ++++++++++-------------------- 1 file changed, 32 insertions(+), 61 deletions(-) diff --git a/python/oak/relocalize_apriltags.py b/python/oak/relocalize_apriltags.py index 7caa865..25bdd4d 100644 --- a/python/oak/relocalize_apriltags.py +++ b/python/oak/relocalize_apriltags.py @@ -1,86 +1,76 @@ import argparse import spectacularAI -import cv2 import time import threading import os from enum import Enum class State(Enum): - FindAprilTagTime = 1 - FindRelocalizeTime = 2 - FindAprilTagPose = 3 - FindRelocalizePose = 4 + FindRelocalizeTime = 1 + FindAprilTagTime = 2 + FindRelocalizePose = 3 -pairedFrameTime = None aprilTagFrameTime = None aprilTagFramePose = None relocalizedFrameTime = None relocalizedFramePose = None args = None aprilTagReplay1 = None -aprilTagReplay2 = None relocalizeReplay1 = None relocalizeReplay2 = None -state = None - -def onAprilTagExtendedOutput(output, frameSet): - global args, aprilTagFrameTime, aprilTagFramePose, aprilTagReplay1, aprilTagReplay2, pairedFrameTime, state - # print(output.status) - if not output.status == spectacularAI.TrackingStatus.TRACKING: return # not tracking yet - - for frame in frameSet: - if not pairedFrameTime: - if not aprilTagFrameTime: - aprilTagFrameTime = output.pose.time - print("Tracking started with April Tag at: {}".format(aprilTagFrameTime)) - # print("Close April Tag replay") - aprilTagReplay1.close() - break - elif state == State.FindAprilTagPose and output.pose.time >= pairedFrameTime: - if aprilTagFramePose is None: - aprilTagFramePose = frame.cameraPose.pose.asMatrix() - print("April Tag pose at: {}\n{}".format(output.pose.time, aprilTagFramePose)) - # print("Close April Tag replay") - aprilTagReplay2.close() - break +state = None def onRelocalizeExtendedOutput(output, frameSet): global args, relocalizedFrameTime, relocalizedFramePose, relocalizeReplay1, relocalizeReplay2, pairedFrameTime, state - #print(output.status) if not output.status == spectacularAI.TrackingStatus.TRACKING: return # not tracking yet for frame in frameSet: if not frame.image: continue - if not pairedFrameTime: + if state == State.FindRelocalizeTime: if not relocalizedFrameTime: relocalizedFrameTime = output.pose.time print("Tracking started with Relocalization at: {}".format(relocalizedFrameTime)) # print("Close relocalize replay") relocalizeReplay1.close() break - elif state == State.FindRelocalizePose and output.pose.time >= pairedFrameTime: + elif state == State.FindRelocalizePose and output.pose.time >= aprilTagFrameTime: if relocalizedFramePose is None: relocalizedFramePose = frame.cameraPose.pose.asMatrix() - print("Relocalized pose at: {}\n{}".format(output.pose.time, relocalizedFramePose)) + print("Relocalized camera to world at: {}\n{}".format(output.pose.time, relocalizedFramePose)) # print("Close relocalize replay") relocalizeReplay2.close() break def onAprilTagOutput(output): + global args, aprilTagFrameTime, aprilTagFramePose, aprilTagReplay1, state + + if aprilTagFrameTime is not None: + # print("Close April Tag replay") + aprilTagReplay1.close() + return + + if not state == State.FindAprilTagTime: return # not looking for April Tag time + # if not output.status == spectacularAI.TrackingStatus.TRACKING: return # not tracking yet if not output.map.keyFrames: return # empty map # Find latest keyframe keyFrameId = max(output.map.keyFrames.keys()) keyFrame = output.map.keyFrames[keyFrameId] frameSet = keyFrame.frameSet - frame = frameSet.primaryFrame + frameTime = frame.cameraPose.pose.time + if not frameTime >= relocalizedFrameTime: return # not after relocalized time + + # find a frame with a marker if frame is None or frame.image is None: return if frame.visualMarkers is None or frame.visualMarkers == []: return marker = frame.visualMarkers[0] if not marker.hasPose: raise Exception("Found April Tag but it's not in the tags.json file") + + aprilTagFrameTime = frameTime + aprilTagFramePose = marker.pose.asMatrix() + print("April Tag to camera at: {}\n{}".format(frameTime, aprilTagFramePose)) def replayAprilTags(): global args, aprilTagReplay1, aprilTagReplay2, state @@ -91,15 +81,8 @@ def replayAprilTags(): "extendParameterSets" : ["april-tags"] } - replay = spectacularAI.Replay(args.dataFolder, onAprilTagOutput, configuration=configInternal) - replay.setExtendedOutputCallback(onAprilTagExtendedOutput) - - if state == State.FindAprilTagTime: - aprilTagReplay1 = replay - else: - aprilTagReplay2 = replay - - replay.runReplay() + aprilTagReplay1 = spectacularAI.Replay(args.dataFolder, onAprilTagOutput, configuration=configInternal) + aprilTagReplay1.runReplay() def replayRelocalize(): @@ -128,15 +111,6 @@ def replayRelocalize(): p.add_argument("--preview", help="Show latest primary image as a preview", action="store_true") args = p.parse_args() - state = State.FindAprilTagTime - print("Finding first April Tag frame time...") - aprilTagThread = threading.Thread(target=replayAprilTags) - aprilTagThread.start() - while aprilTagFrameTime is None and aprilTagThread.is_alive(): - time.sleep(0.1) - if aprilTagFrameTime is None: - raise Exception("No valid frame found in April Tags replay") - print("Finding first relocalized frame time...") state = State.FindRelocalizeTime relocalizeThread = threading.Thread(target=replayRelocalize) @@ -145,19 +119,16 @@ def replayRelocalize(): time.sleep(0.1) if relocalizedFrameTime is None: raise Exception("No valid frame found in Relocalize replay") - - pairedFrameTime = max(aprilTagFrameTime, relocalizedFrameTime) - print("Paired frame time: {}".format(pairedFrameTime)) - print("Finding paired time in April Tags replay...") - state = State.FindAprilTagPose + state = State.FindAprilTagTime + print("Finding April Tag frame time (after Relocalized time)...") aprilTagThread = threading.Thread(target=replayAprilTags) aprilTagThread.start() - while aprilTagFramePose is None and aprilTagThread.is_alive(): + while aprilTagFrameTime is None and aprilTagThread.is_alive(): time.sleep(0.1) - if aprilTagFramePose is None: - raise Exception("Paired frame not found in April Tags replay") - + if aprilTagFrameTime is None: + raise Exception("No valid frame found in April Tags replay") + print("Finding paired time in Relocalize replay...") state = State.FindRelocalizePose relocalizeThread = threading.Thread(target=replayRelocalize) From d92faf617d6fe98fe3ba59ff07278a5a67380125 Mon Sep 17 00:00:00 2001 From: Paul Hine Date: Tue, 15 Apr 2025 01:25:20 +0200 Subject: [PATCH 06/10] clarify output --- python/oak/relocalize_apriltags.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/oak/relocalize_apriltags.py b/python/oak/relocalize_apriltags.py index 25bdd4d..58d72cb 100644 --- a/python/oak/relocalize_apriltags.py +++ b/python/oak/relocalize_apriltags.py @@ -36,7 +36,7 @@ def onRelocalizeExtendedOutput(output, frameSet): elif state == State.FindRelocalizePose and output.pose.time >= aprilTagFrameTime: if relocalizedFramePose is None: relocalizedFramePose = frame.cameraPose.pose.asMatrix() - print("Relocalized camera to world at: {}\n{}".format(output.pose.time, relocalizedFramePose)) + print("Camera to relocalized world at: {}\n{}".format(output.pose.time, relocalizedFramePose)) # print("Close relocalize replay") relocalizeReplay2.close() break From c2853d0a77a8f00483c6f398004b5d8eab45bc54 Mon Sep 17 00:00:00 2001 From: Paul Hine Date: Tue, 15 Apr 2025 21:11:10 +0200 Subject: [PATCH 07/10] compute the SLAM->Unity transform --- python/oak/relocalize_apriltags.py | 47 ++++++++++++++++++++++++++++-- 1 file changed, 45 insertions(+), 2 deletions(-) diff --git a/python/oak/relocalize_apriltags.py b/python/oak/relocalize_apriltags.py index 58d72cb..0db6427 100644 --- a/python/oak/relocalize_apriltags.py +++ b/python/oak/relocalize_apriltags.py @@ -3,7 +3,9 @@ import time import threading import os +import numpy as np from enum import Enum +import json class State(Enum): FindRelocalizeTime = 1 @@ -70,7 +72,7 @@ def onAprilTagOutput(output): aprilTagFrameTime = frameTime aprilTagFramePose = marker.pose.asMatrix() - print("April Tag to camera at: {}\n{}".format(frameTime, aprilTagFramePose)) + print("Camera to April Tag at: {}\n{}".format(frameTime, aprilTagFramePose)) def replayAprilTags(): global args, aprilTagReplay1, aprilTagReplay2, state @@ -104,6 +106,29 @@ def replayRelocalize(): replay.runReplay() +def compute_slam_to_dt(camera_to_slam, camera_to_tag, tag_to_dt): + """ + Computes the transformation from the SLAM (relocalization) frame to the Digital Twin frame. + + Parameters: + camera_to_slam (np.ndarray): 4x4 transformation matrix (T^(S)_(C)) representing + the camera pose in the SLAM coordinate frame. + camera_to_tag (np.ndarray): 4x4 transformation matrix (T^(C)_(A)) representing + the AprilTag pose in the camera coordinate frame. + tag_to_dt (np.ndarray): 4x4 transformation matrix (T^(D)_(A)) representing + the AprilTag pose in the Digital Twin coordinate frame. + + Returns: + np.ndarray: 4x4 transformation matrix (T^(D)_(S)) that maps a point from the + SLAM coordinate frame to the Digital Twin coordinate frame. + """ + # Compute the inverse of camera_to_tag (T^(C)_(A)) + tag_to_camera = np.linalg.inv(camera_to_tag) + + # Compute the transformation from SLAM to Digital Twin (T^(D)_(S)) + slam_to_dt = tag_to_dt @ tag_to_camera @ camera_to_slam + + return slam_to_dt if __name__ == '__main__': p = argparse.ArgumentParser(__doc__) @@ -111,6 +136,12 @@ def replayRelocalize(): p.add_argument("--preview", help="Show latest primary image as a preview", action="store_true") args = p.parse_args() + print("Pose of first April Tag from tags.json:") + with open(args.dataFolder + "/tags.json", 'r') as tags_file: + tags_json = json.load(tags_file) + aprilTagToWorld = np.array(tags_json[0]['tagToWorld']) + print(aprilTagToWorld) + print("Finding first relocalized frame time...") state = State.FindRelocalizeTime relocalizeThread = threading.Thread(target=replayRelocalize) @@ -138,7 +169,19 @@ def replayRelocalize(): if relocalizedFramePose is None: raise Exception("Paired frame not found in Relocalize replay") - print("done") + print("SLAM to Digital Twin transformation:") + slam_to_dt = compute_slam_to_dt(relocalizedFramePose, + aprilTagFramePose, + aprilTagToWorld) + print(slam_to_dt) + + slam_config_json = { + "slamToUnity": slam_to_dt.tolist() + } + slam_config_path = args.dataFolder + "/slam_config.json" + with open(slam_config_path, 'w') as slam_config_file: + json.dump(slam_config_json, slam_config_file, indent=4) + print("Wrote transformation to: {}".format(slam_config_path)) os._exit(0) \ No newline at end of file From 74be99f19739c51302739184626266dc1a7dc5bc Mon Sep 17 00:00:00 2001 From: Paul Hine Date: Tue, 15 Apr 2025 22:05:58 +0200 Subject: [PATCH 08/10] fix transform logic --- python/oak/relocalize_apriltags.py | 22 +++++++++++++--------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/python/oak/relocalize_apriltags.py b/python/oak/relocalize_apriltags.py index 0db6427..ddfce6a 100644 --- a/python/oak/relocalize_apriltags.py +++ b/python/oak/relocalize_apriltags.py @@ -106,7 +106,9 @@ def replayRelocalize(): replay.runReplay() -def compute_slam_to_dt(camera_to_slam, camera_to_tag, tag_to_dt): +def compute_slam_to_dt(camera_to_slam: np.ndarray, + camera_to_tag: np.ndarray, + tag_to_dt: np.ndarray) -> np.ndarray: """ Computes the transformation from the SLAM (relocalization) frame to the Digital Twin frame. @@ -114,7 +116,7 @@ def compute_slam_to_dt(camera_to_slam, camera_to_tag, tag_to_dt): camera_to_slam (np.ndarray): 4x4 transformation matrix (T^(S)_(C)) representing the camera pose in the SLAM coordinate frame. camera_to_tag (np.ndarray): 4x4 transformation matrix (T^(C)_(A)) representing - the AprilTag pose in the camera coordinate frame. + the AprilTag pose in the camera coordinate frame. tag_to_dt (np.ndarray): 4x4 transformation matrix (T^(D)_(A)) representing the AprilTag pose in the Digital Twin coordinate frame. @@ -122,13 +124,15 @@ def compute_slam_to_dt(camera_to_slam, camera_to_tag, tag_to_dt): np.ndarray: 4x4 transformation matrix (T^(D)_(S)) that maps a point from the SLAM coordinate frame to the Digital Twin coordinate frame. """ - # Compute the inverse of camera_to_tag (T^(C)_(A)) - tag_to_camera = np.linalg.inv(camera_to_tag) - - # Compute the transformation from SLAM to Digital Twin (T^(D)_(S)) - slam_to_dt = tag_to_dt @ tag_to_camera @ camera_to_slam - - return slam_to_dt + # Derive the transformation from SLAM to AprilTag coordinates: + # T^(S)_(A) = T^(S)_(C) * inverse(T^(A)_(C)) + T_slam_apr = camera_to_slam @ np.linalg.inv(camera_to_tag) + + # The transformation from SLAM to Digital Twin coordinates is: + # T^(D)_(S) = T^(D)_(A) * inverse(T^(S)_(A)) + # Since inverse(T^(S)_(A)) = april_tag_pose @ inverse(relocalized_pose) + T_digitalTwin_slam = tag_to_dt @ camera_to_tag @ np.linalg.inv(camera_to_slam) + return T_digitalTwin_slam if __name__ == '__main__': p = argparse.ArgumentParser(__doc__) From 20dcd063981dbefcc2870880ea51f1f97f76130f Mon Sep 17 00:00:00 2001 From: Paul Hine Date: Thu, 17 Apr 2025 17:33:21 +0200 Subject: [PATCH 09/10] write matrix as flat array also add vio_config.yaml rewrite --- python/oak/relocalize_apriltags.py | 33 ++++++++++++++++++++++++++++-- 1 file changed, 31 insertions(+), 2 deletions(-) diff --git a/python/oak/relocalize_apriltags.py b/python/oak/relocalize_apriltags.py index ddfce6a..c197948 100644 --- a/python/oak/relocalize_apriltags.py +++ b/python/oak/relocalize_apriltags.py @@ -20,7 +20,32 @@ class State(Enum): aprilTagReplay1 = None relocalizeReplay1 = None relocalizeReplay2 = None -state = None +state = None + +vio_config_yaml = """ +depthErrorScale: 0.1 +useSlam: True +keyframeCandidateInterval: 0 +alreadyRectified: True +parameterSets: [wrapper-base,oak-d,live] +""" + +def writeVioConfig(): + global vio_config_yaml + vio_config_path = os.path.join(args.dataFolder, "vio_config.yaml") + with open(vio_config_path, 'w') as vio_config_file: + vio_config_file.write(vio_config_yaml) + print(f"Updated VIO configuration written to: {vio_config_path}") + +def renameMapBin(): + slam_map_path = os.path.join(args.dataFolder, "slam_map.bin") + map_path = os.path.join(args.dataFolder, "map.bin") + + if os.path.exists(slam_map_path): + if os.path.exists(map_path): + os.remove(map_path) + os.rename(slam_map_path, map_path) + print(f"Renamed {slam_map_path} to {map_path}") def onRelocalizeExtendedOutput(output, frameSet): global args, relocalizedFrameTime, relocalizedFramePose, relocalizeReplay1, relocalizeReplay2, pairedFrameTime, state @@ -75,7 +100,7 @@ def onAprilTagOutput(output): print("Camera to April Tag at: {}\n{}".format(frameTime, aprilTagFramePose)) def replayAprilTags(): - global args, aprilTagReplay1, aprilTagReplay2, state + global args, aprilTagReplay1, state print("[Replay April Tags] {}".format(state)) configInternal = { @@ -140,6 +165,9 @@ def compute_slam_to_dt(camera_to_slam: np.ndarray, p.add_argument("--preview", help="Show latest primary image as a preview", action="store_true") args = p.parse_args() + writeVioConfig() + renameMapBin() + print("Pose of first April Tag from tags.json:") with open(args.dataFolder + "/tags.json", 'r') as tags_file: tags_json = json.load(tags_file) @@ -183,6 +211,7 @@ def compute_slam_to_dt(camera_to_slam: np.ndarray, "slamToUnity": slam_to_dt.tolist() } slam_config_path = args.dataFolder + "/slam_config.json" + slam_config_json["slamToUnity"] = np.array(slam_config_json["slamToUnity"]).flatten().tolist() with open(slam_config_path, 'w') as slam_config_file: json.dump(slam_config_json, slam_config_file, indent=4) print("Wrote transformation to: {}".format(slam_config_path)) From f1f8fc8cedfc6e295181c894cf8e3ffb6d34a2bc Mon Sep 17 00:00:00 2001 From: Paul Hine Date: Thu, 17 Apr 2025 17:53:12 +0200 Subject: [PATCH 10/10] Update README.md --- python/oak/README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/python/oak/README.md b/python/oak/README.md index 27c0b3e..c27e60b 100644 --- a/python/oak/README.md +++ b/python/oak/README.md @@ -18,6 +18,7 @@ Also requires `PyOpenGL_accelerate` to be installed, see [`mixed_reality.py`](mixed_reality.py) for details. * **GNSS-VIO** example, reads external GNSS from standard input [`vio_gnss.py`](vio_gnss.py) (see also [these instructions](https://spectacularai.github.io/docs/pdf/GNSS-VIO_OAK-D_Python.pdf)) * **April Tag example**: Visualize detected April Tags [`python april_tag.py path/to/tags.json`](april_tag.py). See: https://spectacularai.github.io/docs/pdf/april_tag_instructions.pdf + * **Relocalize April Tag example**: Example of how to align a saved SLAM map with a recorded April Tag session [`python relocalize_apriltags.py path/to/recording/data`](relocalize_apriltags.py). For complete instructions on how to use this tool to align a digital twin in Unity with your SLAM map, see the https://github.com/SpectacularAI/unity-wrapper/tree/main/unity-examples/Assets/SpectacularAI/Examples/AprilTag/README.md. * **Remote visualization over SSH**. Can be achieved by combining the `vio_jsonl.py` and `vio_visu.py` scripts as follows: ssh user@example.org 'python -u /full/path/to/vio_jsonl.py' | python -u vio_visu.py --file=-