Skip to content

Commit 46eec49

Browse files
BerconBercon
authored
Add FrontRightDown calibration tool (#86)
* Add FrontRightDown calibration tool
1 parent a13e75c commit 46eec49

3 files changed

Lines changed: 228 additions & 0 deletions

File tree

‎python/cli/calibrate_frd/__init__.py‎

Whitespace-only changes.

‎python/cli/calibrate_frd/calibrate_frd.py‎

Lines changed: 226 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,226 @@
1+
"""
2+
Calibrate device Front-Right-Down orientation.
3+
"""
4+
5+
import spectacularAI
6+
import cv2
7+
import numpy as np
8+
import os
9+
import json
10+
11+
12+
def define_args(parser):
13+
parser.add_argument("sdk_recording_path", help="Path to the Spectacular AI SDK recording directory.")
14+
parser.add_argument(
15+
"--index",
16+
type=int,
17+
default=0,
18+
help="Camera index that is used for aligning the forward diretion. Default is 0."
19+
)
20+
parser.add_argument(
21+
"--skip_outputs",
22+
type=int,
23+
default=0,
24+
help="Optional: Number of VIO outputs to skip before displaying an image. Default is 0."
25+
)
26+
parser.add_argument(
27+
"--zoom",
28+
type=float,
29+
default=1.0,
30+
help="Optional: Zoom factor for the image selection window. E.g., 2.0 for 2x zoom. Default is 1.0."
31+
)
32+
parser.add_argument(
33+
'--no_confirm',
34+
action='store_true',
35+
help='Select double clicked target without confirmation')
36+
parser.add_argument(
37+
'--no_gravity',
38+
action='store_true',
39+
help='Do not use gravity to compute a Front-Right-Down IMU-to-output matrix and only compute IMU forward vector')
40+
return parser
41+
42+
43+
def define_subparser(subparsers):
44+
sub = subparsers.add_parser('calibrate-frd', help=__doc__.strip())
45+
sub.set_defaults(func=calibrate_frd)
46+
return define_args(sub)
47+
48+
49+
def draw_rectigle(img, pixel, color):
50+
x, y = pixel
51+
CROSSHAIR_SIZE = 15
52+
CENTER_GAP = 2
53+
cv2.line(img,
54+
(x, y - CROSSHAIR_SIZE),
55+
(x, y - CENTER_GAP),
56+
color, 1)
57+
cv2.line(img,
58+
(x, y + CENTER_GAP),
59+
(x, y + CROSSHAIR_SIZE),
60+
color, 1)
61+
cv2.line(img,
62+
(x - CROSSHAIR_SIZE, y),
63+
(x - CENTER_GAP, y),
64+
color, 1)
65+
cv2.line(img,
66+
(x + CENTER_GAP, y),
67+
(x + CROSSHAIR_SIZE, y),
68+
color, 1)
69+
70+
71+
class RayApp:
72+
def __init__(self, args):
73+
self.args = args
74+
self.vio_output_counter = 0
75+
self.should_quit = False
76+
self.index = args.index
77+
with open(os.path.join(args.sdk_recording_path, 'calibration.json')) as f:
78+
self.calibration_json = json.load(f)
79+
num_cams = len(self.calibration_json['cameras'])
80+
if (self.index >= num_cams): raise Exception(f"Too large camera index {self.index}, must be between 0 and {num_cams - 1} (inclusive).")
81+
self.replay = spectacularAI.Replay(
82+
args.sdk_recording_path,
83+
ignoreFolderConfiguration=True,
84+
configuration={'useStereo': num_cams != 1, 'useMagnetometer': False, 'parameterSets': ['no-threads']})
85+
86+
self.imuToCam = np.array(self.calibration_json['cameras'][args.index]['imuToCamera'])
87+
88+
self.replay.setExtendedOutputCallback(self.on_vio_output)
89+
self.replay.setPlaybackSpeed(-1)
90+
91+
def on_vio_output(self, _, frames):
92+
"""
93+
Callback function that gets called for each VIO output from the replay.
94+
"""
95+
if self.should_quit:
96+
self.replay.close()
97+
return
98+
99+
self.vio_output_counter += 1
100+
101+
# Skip outputs if requested
102+
if self.vio_output_counter <= self.args.skip_outputs:
103+
if self.vio_output_counter % 100 == 0:
104+
print(f"Skipped {self.vio_output_counter} outputs...")
105+
return
106+
107+
frame = frames[self.index]
108+
if frame is None:
109+
print("No frame")
110+
return
111+
112+
image = frame.image.toArray()
113+
114+
# --- Get Pixel from User Click (with zoom) ---
115+
zoom_factor = self.args.zoom
116+
if zoom_factor < 0.1:
117+
print("Warning: Zoom factor is very small, clamping to 0.1.")
118+
zoom_factor = 0.1
119+
120+
if zoom_factor != 1.0:
121+
display_image = cv2.resize(image, None, fx=zoom_factor, fy=zoom_factor, interpolation=cv2.INTER_LINEAR)
122+
else:
123+
display_image = image
124+
125+
display_image = cv2.cvtColor(display_image, cv2.COLOR_GRAY2BGR)
126+
127+
window_name = "Double-click target. Press SPACE to confirm selection. Use W, A, S, D keys to fine tune target."
128+
cv2.namedWindow(window_name)
129+
clicked_point = None
130+
hover_point = None
131+
def mouse_callback(event, x, y, *args, **kwargs):
132+
nonlocal clicked_point, hover_point
133+
if event == cv2.EVENT_LBUTTONDBLCLK:
134+
clicked_point = (x, y)
135+
elif event == cv2.EVENT_MOUSEMOVE:
136+
hover_point = (x, y)
137+
cv2.setMouseCallback(window_name, mouse_callback)
138+
139+
print("Please double-click on a point in the image to select it. Press SPACE to confirm selection. Use W, A, S, D keys to fine tune target.")
140+
self.original_point = None
141+
selected_point = None
142+
while True:
143+
temp_img = display_image.copy()
144+
if hover_point: draw_rectigle(temp_img, hover_point, (0, 20, 225))
145+
if selected_point: draw_rectigle(temp_img, selected_point, (20, 225, 0))
146+
147+
# 4. Update the image to be displayed
148+
cv2.imshow(window_name, temp_img)
149+
150+
key = cv2.waitKey(1) & 0xFF
151+
if key == 27 or key == ord("q"):
152+
self.should_quit = True
153+
return
154+
elif key == ord("w") and selected_point: clicked_point = (selected_point[0], selected_point[1] - 1)
155+
elif key == ord("a") and selected_point: clicked_point = (selected_point[0] - 1, selected_point[1])
156+
elif key == ord("s") and selected_point: clicked_point = (selected_point[0], selected_point[1] + 1)
157+
elif key == ord("d") and selected_point: clicked_point = (selected_point[0] + 1, selected_point[1])
158+
elif key != 0xFF:
159+
if not self.args.no_confirm and self.original_point is not None:
160+
break
161+
if clicked_point is not None:
162+
x, y = clicked_point
163+
selected_point = (x, y)
164+
self.original_point = (x / zoom_factor, y / zoom_factor)
165+
clicked_point = None
166+
167+
self.should_quit = True # Mark as done to prevent re-triggering
168+
169+
if self.original_point is None:
170+
print("No point was selected. Exiting.")
171+
return
172+
173+
main_camera = frame.cameraPose.camera
174+
ray = main_camera.pixelToRay(spectacularAI.PixelCoordinates(*self.original_point))
175+
if ray is None:
176+
print("pixelToRay failed (outside valid FoV?)")
177+
return
178+
179+
camToImu = self.imuToCam[:3,:3].transpose()
180+
self.rayImu = camToImu @ [ray.x, ray.y, ray.z]
181+
182+
if not self.args.no_gravity:
183+
camToWorld = frame.cameraPose.getCameraToWorldMatrix()
184+
imuToWorld = camToWorld[:3, :3] @ self.imuToCam[:3,:3]
185+
worldToImu = imuToWorld[:3, :3].transpose()
186+
downVectorWorld = [0,0,-1]
187+
downVectorImu = worldToImu @ downVectorWorld
188+
rightVectorImu = np.cross(downVectorImu, self.rayImu)
189+
forwardVectorImu = np.cross(rightVectorImu, downVectorImu)
190+
191+
self.frd = np.eye(4)
192+
self.frd[:3,:3] = np.hstack([v[:, np.newaxis] / np.linalg.norm(v) for v in [forwardVectorImu, rightVectorImu, downVectorImu]]).transpose()
193+
self.rayImu = self.frd[:3, 0]
194+
195+
self.displayResults()
196+
197+
def displayResults(self):
198+
print("\n" + "="*45)
199+
print("Camera ray in World coordinates")
200+
print("="*45)
201+
print(f"Original pixel coordinates: {self.original_point}")
202+
print(f"Ray direction (IMU): {np.array2string(self.rayImu, precision=4)}")
203+
print("="*45)
204+
print('Updated calibration.json:\n')
205+
self.calibration_json['imuForward'] = self.rayImu.tolist()
206+
if not self.args.no_gravity:
207+
self.calibration_json['imuToOutput'] = self.frd.tolist()
208+
print(json.dumps(self.calibration_json, indent=2))
209+
210+
def run(self):
211+
self.replay.runReplay()
212+
213+
214+
def calibrate_frd(args):
215+
app = RayApp(args)
216+
app.run()
217+
218+
219+
if __name__ == '__main__':
220+
def parse_args():
221+
import argparse
222+
parser = argparse.ArgumentParser(description=__doc__.strip())
223+
parser = define_args(parser)
224+
return parser.parse_args()
225+
226+
calibrate_frd(parse_args())

‎python/cli/sai_cli.py‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@
66
from .smooth import define_subparser as smooth_define_subparser
77
from .calibrate.calibrate import define_subparser as calibrate_define_subparser
88
from .diagnose.diagnose import define_subparser as diagnose_define_subparser
9+
from .calibrate_frd.calibrate_frd import define_subparser as frd_define_subparser
910

1011
def parse_args():
1112
def get_sdk_version():
@@ -29,6 +30,7 @@ def get_sdk_version():
2930
calibrate_define_subparser(subparsers)
3031
convert_define_subparser(subparsers)
3132
diagnose_define_subparser(subparsers)
33+
frd_define_subparser(subparsers)
3234
return parser.parse_args()
3335

3436
def main():

0 commit comments

Comments
 (0)