-
Notifications
You must be signed in to change notification settings - Fork 0
/
tag_localizer.py
121 lines (88 loc) · 3.33 KB
/
tag_localizer.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
"""
Use multiple AprilTags to estimate the pose of the camera
Utility for detect_apriltag.py
"""
import json
import re
# from detect_apriltag import pose_to_transform
import numpy as np
# from detect_apriltag import pose_to_transform
from utils import print_matrix
def pose_to_transform(poseR, pose_t):
rotationAndTranslation = np.concatenate((poseR, pose_t), 1)
return np.concatenate((rotationAndTranslation, np.array([[0, 0, 0 , 1]])), 0)
def load_tag_config(filename: str) -> dict:
"""
Load the tag configuration from a SpectacularAI formatted JSON file.
"""
with open(filename, 'r') as f:
data = json.load(f)
tags = {}
for value in data:
id = value['id']
tagToWorld = value['tagToWorld'] # 4x4 matrix
tags[id] = tagToWorld
return tags
def np_to_list(np_array: np.ndarray) -> list[list[float]]:
"""
Convert a numpy array to a list of lists
"""
return [list(row) for row in np_array]
def get_global_pose(detections: list, tags: dict, use_closest=True) -> np.ndarray:
"""
Estimate the global field pose of the camera from the detected AprilTags
Simple averaging of the poses of the detected tags
Args:
- detections (list): A list of AprilTag detections, each (id, pose) tuple
- tags (dict): A dictionary of tag id to tagToWorld transform
- use_closest (bool): If True, use the closest tag to estimate the camera pose, otherwise use all tags
Returns:
- np.ndarray: A 4x4 homogenous matrix representing the pose of the camera in the world frame
"""
B = np.array([[0, 0, 1, 0],
[1, 0, 0, 0],
[0, -1, 0, 0],
[0, 0, 0, 1]])
# get the pose of each tag
tag_poses = []
for detection in detections:
# id, pose = detection
id = detection.tag_id
poseR = detection.pose_R
poseT = detection.pose_t
pose = pose_to_transform(poseR, poseT)
try:
tagToWorld = tags[id] @ B # apply correction matrix for coordinate system
tag_poses.append(tagToWorld @ pose) # transform the pose to the world frame
except KeyError:
print(f"Tag {id} not found in tag config")
continue
if len(tag_poses) == 0:
return None
if use_closest:
# Use the pose of the closest tag
tag_poses = sorted(tag_poses, key=lambda pose: np.linalg.norm(pose[:3, 3]))
camera_pose = tag_poses[0]
return camera_pose
# Separate rotation and translation components
rotations = [pose[:3, :3] for pose in tag_poses]
translations = [pose[:3, 3] for pose in tag_poses]
# Average rotations and translations separately
avg_rotation = np.mean(rotations, axis=0)
avg_translation = np.mean(translations, axis=0)
# Construct the averaged camera pose matrix
camera_pose = np.eye(4)
camera_pose[:3, :3] = avg_rotation
camera_pose[:3, 3] = avg_translation
return camera_pose
def main():
tags = load_tag_config("apriltag_configs/crescendo/crescendo_apriltags.json")
for id, tagToWorld in tags.items():
print()
print(f"Tag {id}:")
print_matrix(tagToWorld)
fake_camera_pose = np.eye(4)
fake_camera_pose[:3, 3] = np.array([0, 0.5, 3])
# print(tags[3] @ fake_camera_pose)
if __name__ == "__main__":
main()