├── .gitignore
├── License.md
├── README.md
├── apriltag_detector.service
├── camera_params
    ├── Logitech_Webcam.json
    ├── ProBook_11_G2_Webcam.json
    ├── Small_USB_Camera.json
    └── Thinkpad_T490.json
├── cameras.json
├── detector.json
├── environment.json
├── images
    └── calibration_chessboard.jpeg
├── requirements.txt
├── setup.sh
├── src
    ├── cameras.py
    ├── driver_station.py
    ├── gui.py
    ├── main.py
    ├── messenger.py
    ├── quaternions.py
    ├── shufflelog_api.py
    ├── solver.py
    └── tag_tracker.py
├── templates
    ├── fail.html
    ├── form.html
    ├── index.html
    └── pass.html
└── tools
    ├── calibrate_camera.py
    └── camera_port_scanner.py


/.gitignore:
--------------------------------------------------------------------------------
1 | __pycache__/
2 | *.py[cod]
3 | Tracker.Log
4 | .installer_config
5 | 


--------------------------------------------------------------------------------
/License.md:
--------------------------------------------------------------------------------
 1 | MIT License
 2 | 
 3 | Copyright (c) 2022 SouthwestRoboticsProgramming
 4 | 
 5 | Permission is hereby granted, free of charge, to any person obtaining a copy
 6 | of this software and associated documentation files (the "Software"), to deal
 7 | in the Software without restriction, including without limitation the rights
 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 | 
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 | 
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # Apriltag Tracker for FRC
 2 | 
 3 | ## This is no longer actively maintained
 4 | Use [TagTracker-v2](https://github.com/SouthwestRoboticsProgramming/TagTracker-v2) instead.
 5 | 
 6 | TagTracker is the first FRC implementation of the AprilTag standard capable of full robot localization. It achieves this by combining estimated poses of multiple tags, even across multiple cameras!
 7 | 
 8 | This project was made public by FRC Team 2129, Ultraviolet. <br>
 9 | This project was modified by FRC Team 3620, The Average Joes.
10 | 
11 | To put this on your own robot, check out the [wiki](https://github.com/SouthwestRoboticsProgramming/TagTracker/wiki)!
12 | 


--------------------------------------------------------------------------------
/apriltag_detector.service:
--------------------------------------------------------------------------------
 1 | [Unit]
 2 | Description=Apriltag Detector Service
 3 | 
 4 | [Service]
 5 | Type=simple
 6 | WorkingDirectory = %p
 7 | ExecStart=/usr/bin/python3 %s
 8 | 
 9 | [Install]
10 | WantedBy=multi-user.target
11 | 


--------------------------------------------------------------------------------
/camera_params/Logitech_Webcam.json:
--------------------------------------------------------------------------------
1 | {
2 |     "fx": 676.6192195641298,
3 |     "fy": 676.8359339562655,
4 |     "cx": 385.1137834870396,
5 |     "cy": 201.81402152233636,
6 |     "dist": [ 0.01632932, -0.36390723, -0.01638719,  0.02577886,  0.93133364]
7 |   }
8 |   


--------------------------------------------------------------------------------
/camera_params/ProBook_11_G2_Webcam.json:
--------------------------------------------------------------------------------
1 | {
2 |     "fx": 595.7487947787047,
3 |     "fy": 595.9156142635239,
4 |     "cx": 309.56098609995973,
5 |     "cy": 248.43377626597933
6 | }


--------------------------------------------------------------------------------
/camera_params/Small_USB_Camera.json:
--------------------------------------------------------------------------------
1 | {
2 |     "fx": 671.7353387615264,
3 |     "fy": 676.3792450156916,
4 |     "cx": 378.77904795628274,
5 |     "cy": 272.38729137395103,
6 |     "dist": [-0.10392609, -0.63257957, -0.01078108,  0.00174896,  0.90013624]
7 | }


--------------------------------------------------------------------------------
/camera_params/Thinkpad_T490.json:
--------------------------------------------------------------------------------
1 | {
2 |     "fx" : 559.7699205140387,
3 |     "fy" : 561.9898855409883,
4 |     "cx" : 368.2361920292215,
5 |     "cy" : 225.225099916301
6 | }


--------------------------------------------------------------------------------
/cameras.json:
--------------------------------------------------------------------------------
 1 | {
 2 |     "cameras" : [
 3 |         
 4 |         {
 5 |             "name": "USB Webcam",
 6 |             "type": "Small_USB_Camera",
 7 |             "port": 2,
 8 |             "robot_pose": [
 9 |                 [1, 0, 0, 0.5],
10 |                 [0, -1, 0, 0],
11 |                 [0, 0, -1, 0],
12 |                 [0, 0, 0, 1]
13 |             ]
14 |         }
15 |     ]
16 | }


--------------------------------------------------------------------------------
/detector.json:
--------------------------------------------------------------------------------
 1 | {
 2 |     "families" : "tag16h5",
 3 |     "border" : 1,
 4 |     "nthreads" : 4,
 5 |     "quad_decimate" : 1,
 6 |     "quad_blur" : 0,
 7 |     "quad_sigma" : 1,
 8 |     "refine_edges" : true,
 9 |     "refine_decode" : false,
10 |     "refine_pose" : false,
11 |     "debug" : false,
12 |     "quad_contours" : true
13 | }
14 | 


--------------------------------------------------------------------------------
/environment.json:
--------------------------------------------------------------------------------
  1 | {
  2 |     "tag_family" : "tag16h5",
  3 | 
  4 |     "tags" : [
  5 |         {
  6 |             "size": 0.2032,
  7 |             "id": 1,
  8 |             "transform": [
  9 |                 -1,
 10 |                 0,
 11 |                 0,
 12 |                 7.24310,
 13 |                 0,
 14 |                 -1,
 15 |                 0,
 16 |                 -2.93659,
 17 |                 0,
 18 |                 0,
 19 |                 1,
 20 |                 0.46272,
 21 |                 0,
 22 |                 0,
 23 |                 0,
 24 |                 1
 25 |             ]
 26 |         },
 27 | 
 28 |         {
 29 |             "size": 0.2032,
 30 |             "id": 2,
 31 |             "transform": [
 32 |                 -1,
 33 |                 0,
 34 |                 0,
 35 |                 7.24310,
 36 |                 0,
 37 |                 -1,
 38 |                 0,
 39 |                 -1.26019,
 40 |                 0,
 41 |                 0,
 42 |                 1,
 43 |                 0.46272,
 44 |                 0,
 45 |                 0,
 46 |                 0,
 47 |                 1
 48 |             ]
 49 |         },
 50 |         {
 51 |             "size": 0.2032,
 52 |             "id": 3,
 53 |             "transform": [
 54 |                 -1,
 55 |                 0,
 56 |                 0,
 57 |                 7.24310,
 58 |                 0,
 59 |                 -1,
 60 |                 0,
 61 |                 0.41621,
 62 |                 0,
 63 |                 0,
 64 |                 1,
 65 |                 0.46272,
 66 |                 0,
 67 |                 0,
 68 |                 0,
 69 |                 1
 70 |             ]
 71 |         },
 72 |         {
 73 |             "size": 0.2032,
 74 |             "id": 4,
 75 |             "transform": [
 76 |                 -1,
 77 |                 0,
 78 |                 0,
 79 |                 7.90832,
 80 |                 0,
 81 |                 -1,
 82 |                 0,
 83 |                 2.74161,
 84 |                 0,
 85 |                 0,
 86 |                 1,
 87 |                 0.695452,
 88 |                 0,
 89 |                 0,
 90 |                 0,
 91 |                 1
 92 |             ]
 93 |         },
 94 |         {
 95 |             "size": 0.2032,
 96 |             "id": 5,
 97 |             "transform": [
 98 |                 1,
 99 |                 0,
100 |                 0,
101 |                 -7.90832,
102 |                 0,
103 |                 1,
104 |                 0,
105 |                 2.74161,
106 |                 0,
107 |                 0,
108 |                 1,
109 |                 0.695452,
110 |                 0,
111 |                 0,
112 |                 0,
113 |                 1
114 |             ]
115 |         },
116 |         {
117 |             "size": 0.2032,
118 |             "id": 6,
119 |             "transform": [
120 |                 1,
121 |                 0,
122 |                 0,
123 |                 -7.24310,
124 |                 0,
125 |                 1,
126 |                 0,
127 |                 0.41621,
128 |                 0,
129 |                 0,
130 |                 1,
131 |                 0.46272,
132 |                 0,
133 |                 0,
134 |                 0,
135 |                 1
136 |             ]
137 |         },
138 |         {
139 |             "size": 0.2032,
140 |             "id": 7,
141 |             "transform": [
142 |                 1,
143 |                 0,
144 |                 0,
145 |                 -7.24310,
146 |                 0,
147 |                 1,
148 |                 0,
149 |                 -1.26019,
150 |                 0,
151 |                 0,
152 |                 1,
153 |                 0.46272,
154 |                 0,
155 |                 0,
156 |                 0,
157 |                 1
158 |             ]
159 |         },
160 |         {
161 |             "size": 0.2032,
162 |             "id": 8,
163 |             "transform": [
164 |                 1,
165 |                 0,
166 |                 0,
167 |                 -7.24310,
168 |                 0,
169 |                 1,
170 |                 0,
171 |                 -2.93659,
172 |                 0,
173 |                 0,
174 |                 1,
175 |                 0.46272,
176 |                 0,
177 |                 0,
178 |                 0,
179 |                 1
180 |             ]
181 |         }
182 |     ]
183 | }
184 | 


--------------------------------------------------------------------------------
/images/calibration_chessboard.jpeg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/SouthwestRoboticsProgramming/TagTracker/ca083e8371f788274abc2b86a0db52e9e9d3f3e5/images/calibration_chessboard.jpeg


--------------------------------------------------------------------------------
/requirements.txt:
--------------------------------------------------------------------------------
1 | apriltag==0.0.16
2 | numpy==1.22.3
3 | opencv-contrib-python==4.5.5.64
4 | pynetworktables==2021.0.0
5 | imutils==0.5.4
6 | 


--------------------------------------------------------------------------------
/setup.sh:
--------------------------------------------------------------------------------
 1 | #!/bin/bash
 2 | 
 3 | echo "[INFO] Starting setup"
 4 | 
 5 | yes | sudo apt-get install libgl1-mesa-glx libegl1-mesa libxrandr2 libxrandr2 libxss1 libxcursor1 libxcomposite1 libasound2 libxi6 libxtst6
 6 | 
 7 | echo "[INFO] Finished installing Anaconda for python environment"
 8 | 
 9 | yes n | conda create -n apriltags python=3.8
10 | 
11 | echo "[INFO] Created environment for python"
12 | 
13 | conda init bash
14 | conda activate apriltags
15 | 
16 | pip install -r requirements.txt
17 | 
18 | echo "[INFO] Installed dependencies"
19 | 


--------------------------------------------------------------------------------
/src/cameras.py:
--------------------------------------------------------------------------------
  1 | from threading import Thread
  2 | import cv2
  3 | import numpy as np
  4 | import json
  5 | from main import logger
  6 | import time
  7 | 
  8 | class Camera:
  9 |     def __init__(self, camera_options):
 10 |         # Extract indevidual values
 11 |         camera_port = camera_options['port']
 12 |         self.name = camera_options['name']
 13 |         self.robot_position = camera_options['robot_pose']
 14 |         self.is_driver = camera_options.get('driver') is not None
 15 | 
 16 |         # Extract params JSON
 17 |         try:
 18 |             location = f"camera_params/{camera_options['type']}.json"
 19 |             params_json = open(location, 'r')
 20 |             params = json.load(params_json)
 21 |             logger.info(f"Camera params JSON loaded for {self.name}")
 22 |         except (FileNotFoundError, json.JSONDecodeError) as e:
 23 |             logger.exception("Could not open camera parameters JSON, qutting")
 24 |             raise FileNotFoundError("Could not open cameara params JSON '{}' for {}, \
 25 |                 is the path relative to /camera_params?".format(camera_options['type'], self.name)) from e
 26 | 
 27 |         params_json.close()
 28 | 
 29 |         # Convert params to tuple
 30 |         self.camera_params = (params['fx'], params['fy'], params['cx'], params['cy'])
 31 |         self.distortion = params['dist']
 32 |         self.matrix = np.array([
 33 |             self.camera_params[0], 0, self.camera_params[2],
 34 |             0, self.camera_params[1], self.camera_params[3],
 35 |             0, 0, 1
 36 |         ]).reshape(3, 3)
 37 | 
 38 | 
 39 |         self.capture = cv2.VideoCapture(camera_port)
 40 | 
 41 |     # def read(self, return_list=None, return_index=None):
 42 |     #     read_value = self.capture.read()
 43 | 
 44 |     #     if not return_list:  # If used outside of multithreaded camera system
 45 |     #         return read_value
 46 | 
 47 |     #     return_list[return_index] = read_value
 48 | 
 49 |     # Takes a queue, when reading the queue, you are reading the most up-to-date image
 50 |     def start_reader(self, images_list, list_index):
 51 |         while True:
 52 |             ret, frame = self.capture.read() # Read the camera
 53 | 
 54 |             if not ret:
 55 |                 images_list[list_index] = ((ret, frame), self)
 56 |                 continue
 57 | 
 58 |             height, width = frame.shape[:2]
 59 |             dist_coeffs = np.array(self.distortion)
 60 |             new_mtx, roi = cv2.getOptimalNewCameraMatrix(self.matrix, dist_coeffs, (width, height), 1, (width, height))
 61 | 
 62 |             undistorted = cv2.undistort(frame, new_mtx, dist_coeffs, None, self.matrix)
 63 |             crop_x, crop_y, crop_w, crop_h = roi
 64 |             undistorted = undistorted[crop_y:crop_y + crop_h, crop_x:crop_x + crop_w]
 65 | 
 66 |             images_list[list_index] = ((ret, frame), self)
 67 | 
 68 |     def release(self):
 69 |         self.capture.release()
 70 | 
 71 | 
 72 | class CameraArray:  # Multithread frame captures
 73 |     def __init__(self, logger, camera_list):
 74 |         # Put together a list of cameras
 75 |         self.camera_list = camera_list
 76 | 
 77 |         if not self.camera_list:
 78 |             logger.error("No cameras defined! Quitting")
 79 |             raise ValueError("No cameras defined in camera array!")
 80 | 
 81 |         self.image_list = []
 82 |         self.threads = []
 83 | 
 84 |         # Create threads for each camera
 85 |         for i, camera in enumerate(self.camera_list):
 86 |             self.image_list.append(None) # Add something for the thread to edit
 87 |             self.threads.append(Thread(target=camera.start_reader, args=(self.image_list, i,)))
 88 |             self.threads[i].start()
 89 | 
 90 |     def read_cameras(self):  # Returns map of images to camera that they came from
 91 |         final_images = []
 92 | 
 93 |         # Filter out failed image captures and log them
 94 |         for read_image in self.image_list:
 95 | 
 96 |             if not read_image:
 97 |                 continue
 98 | 
 99 |             image, camera = read_image
100 | 
101 |             ret, frame = image  # Extract image into ret and the frame
102 | 
103 |             if not ret:  # If the image couldn't be captured
104 |                 logger.error(f"{camera.name} failed to capture an image")
105 |             else:  # Otherwise, remove the ret and leave just the image
106 |                 final_images.append({
107 |                     'image': frame,  # Remove  ret
108 |                     'camera': camera
109 |                 })
110 |         return final_images
111 | 
112 |     def getParams(self):
113 |         params = []
114 | 
115 |         for camera in self.camera_list:
116 |             # Convert dictionary to tuple
117 |             camera_params_tuple = tuple(camera.camera_params.values())
118 |             params.append(camera_params_tuple)
119 | 
120 |         return params
121 | 
122 |     def release_cameras(self):
123 |         for camera in self.camera_list:
124 |             camera.release()
125 | 


--------------------------------------------------------------------------------
/src/driver_station.py:
--------------------------------------------------------------------------------
 1 | from imutils import build_montages
 2 | import numpy as np
 3 | import cv2
 4 | 
 5 | # WIP Feature, not currently used.
 6 | 
 7 | # data_dict holds both images and the camera that took them
 8 | def get_driver_frame(data_dict):
 9 |     # Just get the cameras marked as driver
10 |     drivers = [data['image'] for data in data_dict if data['camera'].is_driver]
11 | 
12 |     # TODO: No drivers?
13 |     if drivers:
14 |         # Arrange the images into one collage
15 |         collage = create_collage(drivers)
16 |         collage = cv2.resize(collage, (50,50))
17 |     else:
18 |         # Just give back a blank image
19 |         # TODO: Descriptive image
20 |         collage = np.zeros(shape=[512,512,3], dtype=np.uint8)
21 |     return collage
22 | 
23 | 
24 | 
25 | 
26 | def create_collage(images):
27 |     # TODO: Magic rescaling
28 |     montages = build_montages(images, (256, 256), (len(images),1))
29 |     return montages[0]


--------------------------------------------------------------------------------
/src/gui.py:
--------------------------------------------------------------------------------
 1 | import cv2
 2 | import numpy as np
 3 | 
 4 | DRAW_GUI = True
 5 | 
 6 | # TODO-Ryan: Don't draw pose too and just make it one function
 7 | def draw_bounding_box(overlay, detector_result, camera_params, pose):
 8 |     # Copied from AprilTag source
 9 | 
10 |     # Extract the bounding box (x, y)-coordinates for the AprilTag
11 |     # and convert each of the (x, y)-coordinate pairs to integers
12 |     (ptA, ptB, ptC, ptD) = detector_result.corners
13 |     ptB = (int(ptB[0]), int(ptB[1]))
14 |     ptC = (int(ptC[0]), int(ptC[1]))
15 |     ptD = (int(ptD[0]), int(ptD[1]))
16 |     ptA = (int(ptA[0]), int(ptA[1]))
17 | 
18 |     # draw the bounding box of the AprilTag detection
19 |     cv2.line(overlay, ptA, ptB, (0, 255, 0), 2)
20 |     cv2.line(overlay, ptB, ptC, (0, 255, 0), 2)
21 |     cv2.line(overlay, ptC, ptD, (0, 255, 0), 2)
22 |     cv2.line(overlay, ptD, ptA, (0, 255, 0), 2)
23 | 
24 |     # draw the center (x, y)-coordinates of the AprilTag
25 |     (cX, cY) = (int(detector_result.center[0]), int(detector_result.center[1]))
26 |     cv2.circle(overlay, (cX, cY), 5, (0, 0, 255), -1)
27 | 
28 |     # draw the tag family on the overlay
29 |     tagFamily = detector_result.tag_family.decode("utf-8")
30 |     cv2.putText(overlay, tagFamily, (ptA[0], ptA[1] - 15),
31 |     cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
32 | 
33 |     # draw the tag id on the overlay
34 |     tagId = str(detector_result.tag_id)
35 |     cv2.putText(overlay, tagId, ((cX, cY)),
36 |     cv2.FONT_HERSHEY_SIMPLEX, 3, (0, 255, 0), 2)
37 | 
38 |     draw_cube(overlay, camera_params, 1, pose)
39 | 
40 | # Not done TODO: Finish
41 | def draw_cube(overlay, camera_params, tag_size, pose, z_sign=1):
42 | 
43 |     opoints =  np.array([
44 |         -1, -1, 0,
45 |          1, -1, 0,
46 |          1,  1, 0,
47 |         -1,  1, 0,
48 |         -1, -1, -2*z_sign,
49 |          1, -1, -2*z_sign,
50 |          1,  1, -2*z_sign,
51 |         -1,  1, -2*z_sign,
52 |     ]).reshape(-1, 1, 3) * 0.5*tag_size
53 | 
54 |     edges = np.array([
55 |         0, 1,
56 |         1, 2,
57 |         2, 3,
58 |         3, 0,
59 |         0, 4,
60 |         1, 5,
61 |         2, 6,
62 |         3, 7,
63 |         4, 5,
64 |         5, 6,
65 |         6, 7,
66 |         7, 4
67 |     ]).reshape(-1, 2)
68 | 
69 |     fx, fy, cx, cy = camera_params
70 | 
71 |     K = np.array([fx, 0, cx, 0, fy, cy, 0, 0, 1]).reshape(3, 3)
72 | 
73 |     rvec, _ = cv2.Rodrigues(pose[:3,:3])
74 |     tvec = pose[:3, 3]
75 | 
76 |     dcoeffs = np.zeros(5)
77 | 
78 |     ipoints, _ = cv2.projectPoints(opoints, rvec, tvec, K, dcoeffs)
79 | 
80 |     ipoints = np.round(ipoints).astype(int)
81 | 
82 |     ipoints = [tuple(pt) for pt in ipoints.reshape(-1, 2)]
83 | 
84 |     for i, j in edges:
85 |         cv2.line(overlay, ipoints[i], ipoints[j], (0, 255, 0), 1, 16)
86 | 


--------------------------------------------------------------------------------
/src/main.py:
--------------------------------------------------------------------------------
  1 | import logging
  2 | from networktables import NetworkTables
  3 | import time
  4 | 
  5 | # Get field data to improve logging
  6 | fms = NetworkTables.getTable("FMSInfo")
  7 | match_type = fms.getEntry('MatchType')
  8 | match_number = fms.getEntry('MatchNumber')
  9 | 
 10 | # Configure logging first so other files can use the same formatting
 11 | LOG_FILENAME = 'Tracker.Log'
 12 | TIME_FORMAT = f"%y %a %b {match_type.getString('Test Match')} {match_number.getString('0')}"
 13 | 
 14 | LOG_FORMAT = '%(levelname)s %(asctime)s - %(message)s'
 15 | logging.basicConfig(filename=LOG_FILENAME,
 16 |                     level=logging.DEBUG,
 17 |                     format=LOG_FORMAT,
 18 |                     datefmt=TIME_FORMAT)
 19 | logger = logging.getLogger()
 20 | 
 21 | 
 22 | from argparse import ArgumentParser
 23 | from tag_tracker import *
 24 | from solver import *
 25 | from shufflelog_api import ShuffleLogAPI
 26 | from driver_station import get_driver_frame
 27 | import json
 28 | 
 29 | def main():
 30 |     # Create a parser to allow variable arguments
 31 |     parser = ArgumentParser(prog='AprilTag tracker',
 32 |                             description='AprilTag tracker for FRC')
 33 |     parser.add_argument('-i', '--networktable_ip', type=str, default='localhost', metavar='', help='RoboRIO ip to access network tables')
 34 |     parser.add_argument('-e', '--environment', type=str, default='environment.json', metavar='', help='Path to environment definition JSON')
 35 |     parser.add_argument('-c', '--cameras', type=str, default='cameras.json', metavar='', help='Path to camera definition JSON')
 36 |     parser.add_argument('-d', '--detector', type=str, default='detector.json', metavar='', help='Path to detector definition JSON')
 37 |     parser.add_argument('-n', '--no_gui',  action='store_true', help='Hide OpenCV gui.')
 38 | 
 39 |     args = parser.parse_args()
 40 | 
 41 |     # Configure NetworkTables
 42 |     networktable_ip = args.networktable_ip
 43 |     if networktable_ip:
 44 |         NetworkTables.initialize(server=networktable_ip)
 45 |     else:
 46 |         NetworkTables.initialize()
 47 |     # Tables to send back to RoboRIO and driver station
 48 |     table = NetworkTables.getTable("apriltag")
 49 | 
 50 |     # Extract environment JSON
 51 |     try:
 52 |         environment_json = open(args.environment, 'r')
 53 |         environment = json.load(environment_json)
 54 |         logger.info("Environment JSON loaded")
 55 |     except (FileNotFoundError, json.JSONDecodeError) as e:
 56 |         logger.exception("Could not open envionment JSON, quitting")
 57 |         raise FileNotFoundError(f"Could not open environment JSON '{args.environment}', is the path relative to /TagTracker/?") from e
 58 | 
 59 | 
 60 |     environment_json.close()
 61 | 
 62 |     # Exctract cameras JSON
 63 |     try:
 64 |         cameras_json = open(args.cameras, 'r')
 65 |         cameras = json.load(cameras_json)
 66 |         logger.info("Cameras JSON loaded")
 67 |     except (FileNotFoundError, json.JSONDecodeError) as exc:
 68 |         logger.exception("Could not open cameras JSON, quitting")
 69 |         raise FileNotFoundError(f"Could not open cameras JSON '{args.cameras}', is the path relative to /TagTracker?") from exc
 70 | 
 71 | 
 72 |     cameras_json.close()
 73 | 
 74 |     # Extract detector JSON
 75 |     try:
 76 |         detector_json = open(args.detector, 'r')
 77 |         detector = json.load(detector_json)
 78 |         logger.info("Detector JSON loaded")
 79 |     except (FileNotFoundError, json.JSONDecodeError) as err:
 80 |         logger.exception("Could not open detector JSON, qutting")
 81 |         raise FileNotFoundError(f"Could not open detector JSON '{args.detector}', is the path relative to /TagTracker?") from err
 82 | 
 83 | 
 84 |     detector_json.close()
 85 | 
 86 |     # Setup a detector with the JSON settings
 87 |     detector = Detector(logger, detector)
 88 | 
 89 |     camera_list = [Camera(camera_info) for camera_info in cameras['cameras']]
 90 | 
 91 |     # Setup a camera array with the JSON settings
 92 |     camera_array = CameraArray(logger, camera_list)
 93 | 
 94 |     # Create a solver to filter estimated positions
 95 |     # and localize robot
 96 |     solver = RobotPoseSolver(environment)
 97 | 
 98 |     # Initialize ShuffleLog API
 99 |     messenger_params = {
100 |         'host': 'localhost',
101 |         'port': 5805,
102 |         'name': 'TagTracker',
103 |         'mute_errors': True
104 |     }
105 |     api = ShuffleLogAPI(messenger_params, environment['tags'], cameras['cameras'])
106 | 
107 | 
108 |     # Main loop, run all the time like limelight
109 |     while True:
110 |         tic = time.perf_counter();
111 |         data = camera_array.read_cameras()
112 |         detection_poses = detector.getPoses(data)
113 | 
114 |         position, matrices = solver.solve(detection_poses)
115 | 
116 |         if not args.no_gui:
117 |             for i, image in enumerate(data):
118 |                 cv2.imshow(str(i), image['image'])
119 | 
120 |         # Send the solved position back to robot
121 |         api.publish_test_matrices(matrices)
122 |         if position is None: position = [0, 0, 0]
123 |         table.putNumberArray('position', position)
124 | 
125 |         # Read incoming API messages
126 |         api.read()
127 | 
128 |         # Q to stop the program
129 |         if cv2.waitKey(1) & 0xFF == ord('q'):
130 |             break
131 | 
132 |         toc = time.perf_counter()
133 |         print(f"FPS: {1/(toc-tic)}")
134 | 
135 |     # Disconnect from Messenger
136 |     api.shutdown()
137 | 
138 |     # Safely close video operation
139 |     cv2.destroyAllWindows()
140 |     camera_array.release_cameras()
141 | 
142 | 
143 | if __name__ == '__main__':
144 |     main()
145 | 


--------------------------------------------------------------------------------
/src/messenger.py:
--------------------------------------------------------------------------------
  1 | """
  2 | Python port of the MessengerClient API.
  3 | 
  4 | Use MessengerClient to connect to a server. In order to properly
  5 | receive messages, you must call the read_messages() method on it
  6 | while your program is running. The disconnect() method should be
  7 | called when the program ends to shut down background threads. If
  8 | you do not call disconnect(), the program will appear to hang.
  9 | """
 10 | 
 11 | import socket
 12 | import time
 13 | import threading
 14 | import struct
 15 | import select
 16 | 
 17 | 
 18 | class MessageBuilder:
 19 |     """
 20 |     Allows easy storage of data into a message.
 21 |     """
 22 | 
 23 |     def __init__(self, client, type):
 24 |         self.client = client
 25 |         self.type = type
 26 |         self.buffer = b''
 27 |         pass
 28 | 
 29 |     def send(self):
 30 |         """
 31 |         Sends the message with the type and data.
 32 |         """
 33 |         self.client._send_message(self.type, self.buffer)
 34 | 
 35 |     def add_boolean(self, b):
 36 |         """
 37 |         Adds a boolean to this message
 38 | 
 39 |         :param b: boolean to add
 40 |         :return: self
 41 |         """
 42 | 
 43 |         self.buffer += struct.pack('>?', b)
 44 |         return self
 45 | 
 46 |     def add_string(self, s):
 47 |         """
 48 |         Adds a string to this message
 49 | 
 50 |         :param s: string to add
 51 |         :return: self
 52 |         """
 53 | 
 54 |         self.buffer += _encode_string(s)
 55 |         return self
 56 | 
 57 |     def add_char(self, c):
 58 |         """
 59 |         Adds a character to this message
 60 | 
 61 |         :param c: character to add
 62 |         :return: self
 63 |         """
 64 | 
 65 |         self.buffer += struct.pack('>c', c)
 66 |         return self
 67 | 
 68 |     def add_byte(self, b):
 69 |         """
 70 |         Adds a byte to this message
 71 | 
 72 |         :param b: byte to add
 73 |         :return: self
 74 |         """
 75 | 
 76 |         self.buffer += struct.pack('>b', b)
 77 |         return self
 78 | 
 79 |     def add_short(self, s):
 80 |         """
 81 |         Adds a short to this message
 82 | 
 83 |         :param s: short to add
 84 |         :return: self
 85 |         """
 86 | 
 87 |         self.buffer += struct.pack('>s', s)
 88 |         return self
 89 | 
 90 |     def add_int(self, i):
 91 |         """
 92 |         Adds an int to this message
 93 | 
 94 |         :param i: int to add
 95 |         :return: self
 96 |         """
 97 | 
 98 |         self.buffer += struct.pack('>i', i)
 99 |         return self
100 | 
101 |     def add_long(self, l):
102 |         """
103 |         Adds a long to this message
104 | 
105 |         :param l: long to add
106 |         :return: self
107 |         """
108 | 
109 |         self.buffer += struct.pack('>q', l)
110 |         return self
111 | 
112 |     def add_float(self, f):
113 |         """
114 |         Adds a float to this message
115 | 
116 |         :param f: float to add
117 |         :return: self
118 |         """
119 | 
120 |         self.buffer += struct.pack('>f', f)
121 |         return self
122 | 
123 |     def add_double(self, d):
124 |         """
125 |         Adds a double to this message
126 | 
127 |         :param d: double to add
128 |         :return: self
129 |         """
130 | 
131 |         self.buffer += struct.pack('>d', d)
132 |         return self
133 | 
134 |     def add_raw(self, b):
135 |         """
136 |         Adds raw data to this message
137 | 
138 |         :param b: data to add
139 |         :return: self
140 |         """
141 | 
142 |         self.buffer += b
143 |         return self
144 | 
145 | 
146 | class MessageReader:
147 |     """
148 |     Allows easy access to data stored within a message.
149 |     """
150 | 
151 |     def __init__(self, data):
152 |         """
153 |         Creates a new MessageReader that reads from a raw byte array.
154 | 
155 |         :param data: raw data
156 |         """
157 | 
158 |         self.data = data
159 |         self.cursor = 0
160 | 
161 |     def _next(self, count):
162 |         # Reads the next count bytes from the data array as a bytes object
163 | 
164 |         read = self.data[self.cursor:(self.cursor + count)]
165 |         self.cursor += count
166 |         return read
167 | 
168 |     def read_boolean(self):
169 |         """
170 |         Reads a boolean from the message.
171 | 
172 |         :return: boolean read
173 |         """
174 | 
175 |         return struct.unpack('?', self._next(1))[0]
176 | 
177 |     def read_string(self):
178 |         """
179 |         Reads a string from the message.
180 | 
181 |         :return: string read
182 |         """
183 | 
184 |         utf_len = struct.unpack('>H', self._next(2))[0]
185 |         return self._next(utf_len).decode('utf-8')
186 | 
187 |     def read_char(self):
188 |         """
189 |         Reads a character from the message.
190 | 
191 |         :return: character read
192 |         """
193 | 
194 |         return chr(struct.unpack('>H', self._next(2)))[0]
195 | 
196 |     def read_byte(self):
197 |         """
198 |         Reads a byte from the message.
199 | 
200 |         :return: byte read
201 |         """
202 | 
203 |         return struct.unpack('b', self._next(1))[0]
204 | 
205 |     def read_short(self):
206 |         """
207 |         Reads a short from the message.
208 | 
209 |         :return: short read
210 |         """
211 | 
212 |         return struct.unpack('>h', self._next(2))[0]
213 | 
214 |     def read_int(self):
215 |         """
216 |         Reads an int from the message.
217 | 
218 |         :return: int read
219 |         """
220 | 
221 |         return struct.unpack('>i', self._next(4))[0]
222 | 
223 |     def read_long(self):
224 |         """
225 |         Reads a long from the message.
226 | 
227 |         :return: long read
228 |         """
229 | 
230 |         return struct.unpack('>q', self._next(8))[0]
231 | 
232 |     def read_float(self):
233 |         """
234 |         Reads a float from the message.
235 | 
236 |         :return: float read
237 |         """
238 | 
239 |         return struct.unpack('>f', self._next(4))[0]
240 | 
241 |     def read_double(self):
242 |         """
243 |         Reads a double from the message.
244 | 
245 |         :return: double read
246 |         """
247 | 
248 |         return struct.unpack('>d', self._next(8))[0]
249 | 
250 |     def read_raw(self, length):
251 |         """
252 |         Reads raw data as a byte array from the message.
253 | 
254 |         :param length: number of bytes to read
255 |         :return: bytes read
256 |         """
257 |         return self._next(length)
258 | 
259 |     def read_all_data(self):
260 |         """
261 |         Reads all remaining data as a bytes object
262 | 
263 |         :return: data read
264 |         """
265 |         return self.data[self.cursor:]
266 | 
267 | 
268 | # Built-in messages
269 | _HEARTBEAT = '_Heartbeat'
270 | _LISTEN = '_Listen'
271 | _DISCONNECT = '_Disconnect'
272 | 
273 | 
274 | def _heartbeat_thread(msg):
275 |     # Sends a heartbeat message once per second to keep the
276 |     # connection alive
277 | 
278 |     while not msg.heartbeat_event.is_set():
279 |         msg._send_message(_HEARTBEAT, b'')
280 |         time.sleep(1)
281 | 
282 | 
283 | def _connect_thread(msg):
284 |     # Repeatedly tries to connect to the server until successful
285 | 
286 |     while not msg.connected and not msg.connect_event.is_set():
287 |         try:
288 |             msg.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
289 |             msg.socket.connect((msg.host, msg.port))
290 | 
291 |             msg._send(_encode_string(msg.name))
292 |             msg.connected = True
293 | 
294 |             for listen in msg.listening:
295 |                 msg._listen(listen)
296 |         except Exception:
297 |             if msg.log_errors:
298 |                 print('Messenger connection failed')
299 | 
300 |         time.sleep(1)
301 |     msg.connect_thread = None
302 | 
303 | 
304 | def _encode_string(str):
305 |     # Encodes a string into a length-prefixed UTF-8 bytes object
306 | 
307 |     encoded_len = struct.pack(">h", len(str))
308 |     return encoded_len + str.encode("utf-8")
309 | 
310 | 
311 | class WildcardHandler:
312 |     # Handles wildcard patterns (i.e. patterns that end in '*')
313 | 
314 |     def __init__(self, pattern, handler):
315 |         self.pattern = pattern
316 |         self.handler = handler
317 | 
318 |     def handle(self, type, data):
319 |         if type.startswith(self.pattern):
320 |             self.handler(type, MessageReader(data))
321 | 
322 | 
323 | class DirectHandler:
324 |     # Handles simple patterns (type must match exactly)
325 | 
326 |     def __init__(self, type, handler):
327 |         self.type = type
328 |         self.handler = handler
329 | 
330 |     def handle(self, type, data):
331 |         if self.type == type:
332 |             self.handler(type, MessageReader(data))
333 | 
334 | 
335 | class MessengerClient:
336 |     """
337 |     Represents a connection to the Messenger server.
338 |     This can be used to send messages between processes.
339 |     """
340 | 
341 |     def __init__(self, host, port, name, mute_errors=False):
342 |         """
343 |         Creates a new instance and attempts to connect to a
344 |         Messenger server at the given address.
345 | 
346 |         :param host: server host
347 |         :param port: server port
348 |         :param name: unique string used in logging
349 |         """
350 | 
351 |         self.host = host
352 |         self.port = port
353 |         self.name = name
354 |         self.log_errors = not mute_errors
355 | 
356 |         self.socket = None
357 |         self.connected = False
358 | 
359 |         self.heartbeat_event = threading.Event()
360 |         self.heartbeat_thread = threading.Thread(target=_heartbeat_thread, args=(self,))
361 |         self.heartbeat_thread.start()
362 |         self.listening = []
363 |         self.handlers = []
364 | 
365 |         self._start_connect_thread()
366 | 
367 |     def reconnect(self, host, port, name):
368 |         """
369 |         Attempts to reconnect to a different Messenger server at
370 |         a given address.
371 | 
372 |         :param host: server host
373 |         :param port: server port
374 |         :param name: unique string used in logging
375 |         :return:
376 |         """
377 | 
378 |         self.host = host
379 |         self.port = port
380 |         self.name = name
381 | 
382 |         self.send(_DISCONNECT)
383 |         self._disconnect_socket()
384 |         self.connected = False
385 | 
386 |         self._start_connect_thread()
387 | 
388 |     def read_messages(self):
389 |         """
390 |         Reads all incoming messages. If not connected, this will do
391 |         nothing. Message handlers will be invoked from this method.
392 |         """
393 | 
394 |         if not self.connected:
395 |             return
396 | 
397 |         try:
398 |             while self._available():
399 |                 self._read_message()
400 |         except Exception:
401 |             self._handle_error()
402 | 
403 |     def is_connected(self):
404 |         """
405 |         Gets whether this client is currently connected to a server.
406 | 
407 |         :return: connected
408 |         """
409 |         return self.connected
410 | 
411 |     def disconnect(self):
412 |         """
413 |         Disconnects from the current server. After this method is called,
414 |         this object should no longer be used. If you want to change servers,
415 |         use reconnect(...).
416 |         """
417 | 
418 |         self.send(_DISCONNECT)
419 | 
420 |         self.heartbeat_event.set()
421 |         self.heartbeat_thread.join()
422 | 
423 |         self._disconnect_socket()
424 |         self.connected = False
425 | 
426 |     def prepare(self, type):
427 |         """
428 |         Prepares to send a message. This returns a MessageBuilder,
429 |         which allows you to add data to the message.
430 | 
431 |         :param type: type of the message to send
432 |         :return: builder to add data
433 |         """
434 | 
435 |         return MessageBuilder(self, type)
436 | 
437 |     def send(self, type):
438 |         """
439 |         Immediately sends a message with no data.
440 | 
441 |         :param type: type of the message to send
442 |         """
443 |         self._send_message(type, b'')
444 | 
445 |     def add_handler(self, type, handler):
446 |         """
447 |         Registers a message handler to handle incoming messages.
448 |         If the type ends in '*', the handler will be invoked for all messages
449 |         that match the content before. For example, "Foo*" would match a
450 |         message of type "Foo2", while "Foo" would only match messages of
451 |         type "Foo".
452 | 
453 |         The handler parameter should be a function with two parameters:
454 |         the type of message as a string, and a MessageReader to read data
455 |         from the message.
456 | 
457 |         :param type: type of message to listen to
458 |         :param handler: handler to invoke
459 |         """
460 | 
461 |         if type.endswith('*'):
462 |             h = WildcardHandler(type[:-1], handler)
463 |         else:
464 |             h = DirectHandler(type, handler)
465 |         self.handlers.append(h)
466 | 
467 |         if type not in self.listening:
468 |             self.listening.append(type)
469 |             if self.connected:
470 |                 self._listen(type)
471 | 
472 |     def _available(self):
473 |         readable = select.select([self.socket], [], [], 0)[0]
474 | 
475 |         return any(sock == self.socket for sock in readable)
476 | 
477 |     def _listen(self, type):
478 |         self.prepare(_LISTEN).add_string(type).send()
479 | 
480 |     def _handle_error(self):
481 |         self._disconnect_socket()
482 | 
483 |         if self.log_errors:
484 |             print("Messenger connection lost")
485 | 
486 |         self.connected = False
487 | 
488 |         self._start_connect_thread()
489 | 
490 |     def _read(self, count):
491 |         data = b""
492 |         while len(data) < count:
493 |             data += self.socket.recv(count - len(data))
494 |         return data
495 | 
496 |     def _send(self, data):
497 |         total = 0
498 |         while total < len(data):
499 |             send = self.socket.send(data[total:])
500 |             if send == 0:
501 |                 self._handle_error()
502 |                 return
503 |             total += send
504 | 
505 |     def _send_message(self, type, data):
506 |         if not self.connected:
507 |             return
508 | 
509 |         encoded_type = _encode_string(type)
510 |         encoded_data_len = struct.pack(">i", len(data))
511 |         encoded = encoded_type + encoded_data_len + data
512 | 
513 |         # packet_len = struct.pack(">i", len(encoded))
514 |         # packet = packet_len + encoded
515 |         packet = encoded
516 | 
517 |         self._send(packet)
518 | 
519 |     def _start_connect_thread(self):
520 |         self.connect_event = threading.Event()
521 |         self.connect_thread = threading.Thread(target=_connect_thread, args=(self,))
522 |         self.connect_thread.start()
523 | 
524 |     def _read_message(self):
525 |         type_len = struct.unpack('>h', self._read(2))[0]
526 |         message_type = self._read(type_len).decode('utf-8')
527 |         data_len = struct.unpack('>i', self._read(4))[0]
528 |         message_data = self._read(data_len)
529 | 
530 |         for handler in self.handlers:
531 |             handler.handle(message_type, message_data)
532 | 
533 |     def _disconnect_socket(self):
534 |         if self.connect_thread is not None:
535 |             self.connect_event.set()
536 |             self.connect_thread.join()
537 |         self.connect_thread = None
538 | 
539 |         self.socket.close()
540 | 


--------------------------------------------------------------------------------
/src/quaternions.py:
--------------------------------------------------------------------------------
 1 | import math
 2 | 
 3 | def matrixToQuat(m):
 4 |     r11 = m[0][0]; r12 = m[0][1]; r13 = m[0][2]
 5 |     r21 = m[1][0]; r22 = m[1][1]; r23 = m[1][2]
 6 |     r31 = m[2][0]; r32 = m[2][1]; r33 = m[2][2]
 7 | 
 8 |     q0 = math.sqrt((1 + r11 + r22 + r33) / 4)
 9 |     q1 = math.sqrt((1 + r11 - r22 - r33) / 4)
10 |     q2 = math.sqrt((1 - r11 + r22 - r33) / 4)
11 |     q3 = math.sqrt((1 - r11 - r22 + r33) / 4)
12 | 
13 |     if q0 > q1 and q0 > q2 and q0 > q3:
14 |         q1 = (r32 - r23) / (4 * q0)
15 |         q2 = (r13 - r31) / (4 * q0)
16 |         q3 = (r21 - r12) / (4 * q0)
17 |     elif q1 > q0 and q1 > q2 and q1 > q3:
18 |         q0 = (r32 - r23) / (4 * q1)
19 |         q2 = (r12 + r21) / (4 * q1)
20 |         q3 = (r13 + r31) / (4 * q1)
21 |     elif q2 > q0 and q2 > q1 and q2 > q3:
22 |         q0 = (r13 - r31) / (4 * q2)
23 |         q1 = (r12 + r21) / (4 * q2)
24 |         q3 = (r23 + r32) / (4 * q2)
25 |     elif q3 > q0 and q3 > q1 and q3 > q2:
26 |         q0 = (r21 - r12) / (4 * q3)
27 |         q1 = (r13 + r31) / (4 * q3)
28 |         q2 = (r23 + r32) / (4 * q3)
29 | 
30 |     return (q0, q1, q2, q3)
31 | 
32 | def invertQuat(q):
33 |     return (q[0], -q[1], -q[2], -q[3])
34 | 
35 | def quatToAxisAngle(q):
36 |     if q[0] == 1:
37 |         return (0, (1, 0, 0))
38 | 
39 |     theta = 2 * math.acos(q[0])
40 | 
41 |     s = math.sin(theta / 2)
42 |     x = q[1] / s
43 |     y = q[2] / s
44 |     z = q[3] / s
45 | 
46 |     return (theta, (x, y, z))
47 | 
48 | def quatToFUL(q):
49 |     x, y, z, w = q
50 |     
51 |     forward = (
52 |         2 * (x * z + w * y),
53 |         2 * (y * z - w * x),
54 |         1 - 2 * (x * x + y * y)
55 |     )
56 | 
57 |     up = (
58 |         2 * (x * y - w * z),
59 |         1 - 2 * (x * x + z * z),
60 |         2 * (y * z + w * x)
61 |     )
62 | 
63 |     left = (
64 |         1 - 2 * (y * y + z * z),
65 |         2 * (x * y + w * z),
66 |         2 * (x * z - w * y)
67 |     )
68 | 
69 |     return (forward, up, left)
70 | 


--------------------------------------------------------------------------------
/src/shufflelog_api.py:
--------------------------------------------------------------------------------
 1 | from itertools import product
 2 | from messenger import *
 3 | 
 4 | def _write_matrix(builder, matrix):
 5 |     # Write as column major
 6 |     for col, row in product(range(4), range(4)):
 7 |         # Float here since ShuffleLog stores matrices as float
 8 |         builder.add_float(matrix[row][col])
 9 | 
10 | class ShuffleLogAPI:
11 |     _MSG_QUERY_ENVIRONMENT = "TagTracker:QueryEnvironment"
12 |     _MSG_ENVIRONMENT = "TagTracker:Environment"
13 | 
14 |     def __init__(self, conn_params, tag_infos, camera_infos):
15 |         host = conn_params['host']
16 |         port = conn_params['port']
17 |         name = conn_params['name']
18 |         mute_errors = conn_params['mute_errors']
19 | 
20 |         self.msg = MessengerClient(host, port, name, mute_errors=mute_errors)
21 |         self.msg.add_handler(ShuffleLogAPI._MSG_QUERY_ENVIRONMENT, lambda t, r: self._on_query_environment(t, r))
22 |         
23 |         self.tag_infos = tag_infos
24 |         self.camera_infos = camera_infos
25 | 
26 |     def read(self):
27 |         self.msg.read_messages()
28 | 
29 |     def shutdown(self):
30 |         self.msg.disconnect()
31 | 
32 |     # This is temporary
33 |     def publish_detection_data(self, detections):
34 |         builder = self.msg.prepare('TagTracker:TestData')
35 |         builder.add_int(len(detections))
36 |         for detect in detections:
37 |             _write_matrix(builder, detect['pose'])
38 |             _write_matrix(builder, detect['camera'].robot_position)
39 |             builder.add_int(detect['tag_id'])
40 |         builder.send()
41 | 
42 |     def publish_test_matrices(self, matrices):
43 |         builder = self.msg.prepare('TagTracker:TestMtx')
44 |         builder.add_int(len(matrices))
45 |         for matrix in matrices:
46 |             _write_matrix(builder, matrix)
47 |         builder.send()
48 | 
49 |     def _on_query_environment(self, type, reader):
50 |         print('[debug] sending environment data to ShuffleLog')
51 |         builder = self.msg.prepare(ShuffleLogAPI._MSG_ENVIRONMENT)
52 | 
53 |         builder.add_int(len(self.tag_infos))
54 |         for tag in self.tag_infos:
55 |             builder.add_double(tag['size'])
56 |             builder.add_int(tag['id'])
57 |             _write_matrix(builder, tag['transform'])
58 |         
59 |         builder.add_int(len(self.camera_infos))
60 |         for camera in self.camera_infos:
61 |             builder.add_string(camera['name'])
62 |             builder.add_int(camera['port'])
63 |             _write_matrix(builder, camera['robot_pose'])
64 | 
65 |         builder.send()
66 | 


--------------------------------------------------------------------------------
/src/solver.py:
--------------------------------------------------------------------------------
  1 | # Solves for robot position based on results of found tags
  2 | import numpy as np
  3 | from main import logger
  4 | 
  5 | # TODO-Ryan: Finish/Fix
  6 | 
  7 | # Manual implementation of matrix inversion because np.linalg.inv is slow
  8 | def invert(m):
  9 | 	m00, m01, m02, m03, m10, m11, m12, m13, m20, m21, m22, m23, m30, m31, m32, m33 = np.ravel(m)
 10 | 	a2323 = m22 * m33 - m23 * m32
 11 | 	a1323 = m21 * m33 - m23 * m31
 12 | 	a1223 = m21 * m32 - m22 * m31
 13 | 	a0323 = m20 * m33 - m23 * m30
 14 | 	a0223 = m20 * m32 - m22 * m30
 15 | 	a0123 = m20 * m31 - m21 * m30
 16 | 	a2313 = m12 * m33 - m13 * m32
 17 | 	a1313 = m11 * m33 - m13 * m31
 18 | 	a1213 = m11 * m32 - m12 * m31
 19 | 	a2312 = m12 * m23 - m13 * m22
 20 | 	a1312 = m11 * m23 - m13 * m21
 21 | 	a1212 = m11 * m22 - m12 * m21
 22 | 	a0313 = m10 * m33 - m13 * m30
 23 | 	a0213 = m10 * m32 - m12 * m30
 24 | 	a0312 = m10 * m23 - m13 * m20
 25 | 	a0212 = m10 * m22 - m12 * m20
 26 | 	a0113 = m10 * m31 - m11 * m30
 27 | 	a0112 = m10 * m21 - m11 * m20
 28 | 
 29 | 	det = m00 * (m11 * a2323 - m12 * a1323 + m13 * a1223) \
 30 | 		- m01 * (m10 * a2323 - m12 * a0323 + m13 * a0223) \
 31 | 		+ m02 * (m10 * a1323 - m11 * a0323 + m13 * a0123) \
 32 | 		- m03 * (m10 * a1223 - m11 * a0223 + m12 * a0123)
 33 | 	det = 1 / det
 34 | 
 35 | 	return np.array([ \
 36 | 		det *  (m11 * a2323 - m12 * a1323 + m13 * a1223), \
 37 | 		det * -(m01 * a2323 - m02 * a1323 + m03 * a1223), \
 38 | 		det *  (m01 * a2313 - m02 * a1313 + m03 * a1213), \
 39 | 		det * -(m01 * a2312 - m02 * a1312 + m03 * a1212), \
 40 | 		det * -(m10 * a2323 - m12 * a0323 + m13 * a0223), \
 41 | 		det *  (m00 * a2323 - m02 * a0323 + m03 * a0223), \
 42 | 		det * -(m00 * a2313 - m02 * a0313 + m03 * a0213), \
 43 | 		det *  (m00 * a2312 - m02 * a0312 + m03 * a0212), \
 44 | 		det *  (m10 * a1323 - m11 * a0323 + m13 * a0123), \
 45 | 		det * -(m00 * a1323 - m01 * a0323 + m03 * a0123), \
 46 | 		det *  (m00 * a1313 - m01 * a0313 + m03 * a0113), \
 47 | 		det * -(m00 * a1312 - m01 * a0312 + m03 * a0112), \
 48 | 		det * -(m10 * a1223 - m11 * a0223 + m12 * a0123), \
 49 | 		det *  (m00 * a1223 - m01 * a0223 + m02 * a0123), \
 50 | 		det * -(m00 * a1213 - m01 * a0213 + m02 * a0113), \
 51 | 		det *  (m00 * a1212 - m01 * a0212 + m02 * a0112)  \
 52 | 	]).reshape(4, 4)
 53 | 
 54 | class RobotPoseSolver:
 55 | 	def __init__(self, environment_dict):
 56 | 		# Unpack tag positions into lookup dictionary
 57 | 		if not environment_dict['tags']:
 58 | 			logger.error('No tags defined! Quitting')
 59 | 			raise AssertionError('No tags defined in environment JSON')
 60 | 		self.tags_dict = {tag['id']: tag for tag in environment_dict['tags']}
 61 | 		self.tag_family = environment_dict['tag_family']
 62 | 
 63 | 		if self.tag_family != "tag16h5":
 64 | 			'''logger.warning('Are you sure that you want to look for, tags in the \
 65 | 				family {}. FRC uses 16h5'.format(self.tag_family))
 66 | 			'''
 67 | 	def solve(self, detection_poses):
 68 | 		# Master list of estimated poses to be combined
 69 | 		estimated_poses_list = []
 70 | 
 71 | 		# Apply camera and tag pose to estimated pose
 72 | 		for pose_dict in detection_poses:
 73 | 			estimated_pose = pose_dict['pose']
 74 | 			camera_pose = pose_dict['camera'].robot_position
 75 | 			tag_id = pose_dict['tag_id']
 76 | 			tag_family = pose_dict['tag_family']
 77 | 
 78 | 			# Find the tag info that matches that tag
 79 | 			if self.tag_family not in str(tag_family):
 80 | 				logger.warning(f"Found a tag that doesn't belong to {self.tag_family}")
 81 | 				continue
 82 | 
 83 | 			# Get the info for the tag
 84 | 			tag_dict = self.tags_dict.get(tag_id)
 85 | 
 86 | 			if not tag_dict:
 87 | 				logger.warning(f"Found a tag that isn't defined in environment. ID: {tag_id}")
 88 | 				continue
 89 | 
 90 | 			tag_pose = tag_dict['transform']
 91 | 
 92 | 			# Convert to numpy arrarys for math
 93 | 			estimated_pose = np.array(estimated_pose)
 94 | 			camera_pose = np.array(camera_pose)
 95 | 			tag_pose = np.array(tag_pose)
 96 | 
 97 | 			# Scale estimated position by tag size
 98 | 			size = tag_dict['size']
 99 | 			estimated_pose[0][3] *= size
100 | 			estimated_pose[1][3] *= size
101 | 			estimated_pose[2][3] *= size
102 | 
103 | 			# Find where the camera is if the tag is at the origin
104 | 			tag_relative_camera_pose = invert(estimated_pose)
105 | 
106 | 			# Find the camera position relative to the tag position
107 | 			world_camera_pose = np.matmul(tag_pose, tag_relative_camera_pose)
108 | 
109 | 			# Find the position of the robot from the camera position
110 | 			inv_rel_camera_pose = invert(camera_pose)
111 | 			robot_pose = np.matmul(world_camera_pose, inv_rel_camera_pose)
112 | 
113 | 			estimated_poses_list.append(robot_pose)
114 | 
115 | 		# Combine poses with average (just for position, not rotation)
116 | 
117 | 		if not estimated_poses_list:
118 | 			# If we have no samples, report none
119 | 			return (None, estimated_poses_list)
120 | 			
121 | 		# TODO: Figure out rotation
122 | 		total = np.array([0.0, 0.0, 0.0])
123 | 		for pose in estimated_poses_list:
124 | 			total += np.array([pose[0][3], pose[1][3], pose[2][3]])
125 | 		average = total / len(estimated_poses_list)
126 | 		return (average, estimated_poses_list)
127 | 


--------------------------------------------------------------------------------
/src/tag_tracker.py:
--------------------------------------------------------------------------------
 1 | from cameras import *
 2 | from gui import *
 3 | import apriltag
 4 | import cv2
 5 | 
 6 | class _DetectorOptions: # Converts JSON into object for apriltag's dector to read
 7 |     def __init__(self, dict=None):
 8 |         if dict:
 9 |             for key, value in dict.items():
10 |                 setattr(self, key, value)
11 | 
12 | class Detector: # Rename?
13 |     def __init__(self, logger, options):
14 |         options = _DetectorOptions(options)
15 |         self.detector = apriltag.Detector(options)
16 | 
17 |     def getPoses(self, images):
18 | 
19 |         # Make a list of estimated poses to add to
20 |         estimated_poses = []
21 | 
22 |         # Find every target in the images
23 |         for image_dict in images:
24 |             image = image_dict['image']
25 |             camera = image_dict['camera']
26 | 
27 |             # Convert image to grayscale
28 |             gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
29 | 
30 |             # Find basic information about tag (center location, ID, family...)
31 |             results = self.detector.detect(gray)
32 | 
33 |             # Estimate the pose of the camera relative to each target
34 |             for result in results:
35 |                 # Undistort corners
36 | 
37 |                 # print(result.corners)
38 |                 # for j in range(len(result.corners)):
39 |                 #     result.corners[j] = cv2.undistortPoints(result.corners[j].reshape(-1, 2), camera.matrix, dist_coeffs, P=new_mtx).reshape(-1)
40 |                 # print(result.corners)
41 | 
42 |                 pose, e0, e1 = self.detector.detection_pose(result, camera.camera_params)
43 | 
44 |                 # Draw bounding box
45 |                 draw_bounding_box(image, result, camera.camera_params, pose)
46 | 
47 |                 # TODO Finish
48 |                 # TODO: What are the 'e's?
49 |                 # TODO: Scale pose by tag size
50 |                 estimated_poses.append({
51 |                     'pose': pose,
52 |                     'camera': camera,
53 |                     'tag_id': result.tag_id,
54 |                     'tag_family' : result.tag_family
55 |                 })
56 | 
57 |             # Log number of tags found
58 |             if estimated_poses:
59 |                 logger.debug(f"Estimated pose on {len(estimated_poses)} AprilTags")
60 | 
61 |         return estimated_poses
62 | 


--------------------------------------------------------------------------------
/templates/fail.html:
--------------------------------------------------------------------------------
 1 | <!doctype html>
 2 | <html>
 3 | 
 4 | <body>
 5 |     <div id="reason" style="display: none;">
 6 |         {{reason}}
 7 |     </div>
 8 | 
 9 |     <script>
10 |         alert(document.getElementById("reason").innerText);
11 |         location.href = "../change%20password";
12 |     </script>
13 | </body>
14 | 
15 | </html>


--------------------------------------------------------------------------------
/templates/form.html:
--------------------------------------------------------------------------------
 1 | <!doctype html>
 2 | <html>
 3 | 
 4 | <body>
 5 |     <div id="reason" style="display: none;">
 6 |         {{reason}}
 7 |     </div>
 8 | 
 9 |     <script>
10 |         alert(document.getElementById("reason").innerText);
11 |         location.href = "../change%20password";
12 |     </script>
13 | </body>
14 | 
15 | </html>


--------------------------------------------------------------------------------
/templates/index.html:
--------------------------------------------------------------------------------
 1 | <body>
 2 |     <div class="container">
 3 |         <div class="row">
 4 |             <div class="col-lg-8  offset-lg-2">
 5 |                 <h3 class="mt-5">Mason's Cool Personal Website</h3>
 6 |                 <div>Green spaghetti</div>
 7 |                 <img src="{{ url_for('video_feed') }}" width="100%">
 8 |             </div>
 9 |         </div>
10 |     </div>
11 |     </body>


--------------------------------------------------------------------------------
/templates/pass.html:
--------------------------------------------------------------------------------
 1 | <!doctype html>
 2 | <html>
 3 | 
 4 | <body>
 5 |     <script>
 6 |         alert("Sucessfully changed password!");
 7 |         location.href = "..";
 8 |     </script>
 9 | 
10 |     <p>
11 |         {{reason}}
12 |     </p>
13 | </body>
14 | 
15 | </html>


--------------------------------------------------------------------------------
/tools/calibrate_camera.py:
--------------------------------------------------------------------------------
  1 | import random
  2 | 
  3 | import numpy as np
  4 | import cv2
  5 | import argparse
  6 | 
  7 | # Copied from AprilTag library source code
  8 | # and edited to work with live video stream
  9 | def main():
 10 | 
 11 |     parser = argparse.ArgumentParser(
 12 |         description='calibrate camera intrinsics using OpenCV')
 13 | 
 14 |     parser.add_argument('-r', '--rows', metavar='N', type=int,
 15 |                         required=True,
 16 |                         help='# of chessboard corners in vertical direction')
 17 | 
 18 |     parser.add_argument('-c', '--cols', metavar='N', type=int,
 19 |                         required=True,
 20 |                         help='# of chessboard corners in horizontal direction')
 21 | 
 22 |     parser.add_argument('-i', '--input_camera', type=int,
 23 |                     default=0, help='change which camera to reac from by openCV id')
 24 | 
 25 |     parser.add_argument('-m', '--max-images', metavar='N', type=int,
 26 |                         default=0,
 27 |                         help='max # of images to keep to use in calibration')
 28 | 
 29 |     options = parser.parse_args()
 30 | 
 31 |     if options.rows < options.cols:
 32 |         patternsize = (options.cols, options.rows)
 33 |     else:
 34 |         patternsize = (options.rows, options.cols)
 35 | 
 36 |     cam = options.input_camera
 37 | 
 38 |     x = np.arange(patternsize[0])
 39 |     y = np.arange(patternsize[1])
 40 | 
 41 |     xgrid, ygrid = np.meshgrid(x, y)
 42 |     zgrid = np.zeros_like(xgrid)
 43 |     opoints = np.dstack((xgrid, ygrid, zgrid)).reshape((-1, 1, 3)).astype(np.float32)
 44 | 
 45 |     imagesize = None
 46 | 
 47 |     cap = cv2.VideoCapture(cam)
 48 | 
 49 |     ipoints = []
 50 | 
 51 |     criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
 52 | 
 53 |     while True:
 54 | 
 55 |         ret, rgb = cap.read()
 56 |         
 57 |         if rgb is None:
 58 |             print('warning: error getting data from webcam, ending')
 59 |             break
 60 | 
 61 |         # cursize = (rgb.shape[1], rgb.shape[0])
 62 |         # print(cursize, rgb.shape[::-1])
 63 |         
 64 | 
 65 |         if len(rgb.shape) == 3:
 66 |             gray = cv2.cvtColor(rgb, cv2.COLOR_RGB2GRAY)
 67 |         else:
 68 |             gray = rgb
 69 |         
 70 |         cursize = gray.shape[::-1]
 71 | 
 72 |         if imagesize is None:
 73 |             imagesize = cursize
 74 |             print('Received frame of size {}x{}'.format(*imagesize))
 75 |         else:
 76 |             assert imagesize == cursize
 77 | 
 78 |         retval, corners = cv2.findChessboardCorners(gray, patternsize, None)
 79 | 
 80 |         if retval:
 81 |             # print('checkerboard acquired!')
 82 |             # Refine corners (apriltags devs forgot this)
 83 |             refined = cv2.cornerSubPix(gray, corners, (11,11), (-1,-1), criteria)
 84 |             ipoints.append(refined)
 85 | 
 86 |             cv2.drawChessboardCorners(rgb, patternsize, refined, retval)
 87 |         else:
 88 |             # print('warning: no chessboard found, skipping')
 89 |             # print('nothing found')
 90 |             pass
 91 | 
 92 |         cv2.putText(rgb, str(len(ipoints)), (10, 30),
 93 |                     cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
 94 |         cv2.imshow("Image", rgb)
 95 | 
 96 |         if cv2.waitKey(1) & 0xFF == ord('q'):
 97 |             break
 98 | 
 99 |     print (f'got {len(ipoints)} points')
100 |     if options.max_images != 0 and len(ipoints) > options.max_images:
101 |         random.shuffle(ipoints)
102 |         ipoints = ipoints[:options.max_images]
103 |         print (f'using {len(ipoints)} points')
104 | 
105 |     if len(ipoints) == 0:
106 |         print("no data!")
107 |     else:
108 |         opoints = [opoints] * len(ipoints)
109 | 
110 |         retval, K, dcoeffs, rvecs, tvecs = cv2.calibrateCamera(opoints, ipoints, imagesize, None, None)
111 | 
112 |         #assert( np.all(dcoeffs == 0) )
113 | 
114 |         fx = K[0,0]
115 |         fy = K[1,1]
116 |         cx = K[0,2]
117 |         cy = K[1,2]
118 | 
119 |         params = (fx, fy, cx, cy)
120 | 
121 |         print()
122 |         print('all units below measured in pixels:')
123 |         print('  fx = {}'.format(K[0,0]))
124 |         print('  fy = {}'.format(K[1,1]))
125 |         print('  cx = {}'.format(K[0,2]))
126 |         print('  cy = {}'.format(K[1,2]))
127 |         print()
128 |         print('pastable into Python:')
129 |         print('  fx, fy, cx, cy = {}'.format(repr(params)))
130 |         print()
131 |         print('json:')
132 |         print('{')
133 |         print('  "fx": {},'.format(K[0,0]))
134 |         print('  "fy": {},'.format(K[1,1]))
135 |         print('  "cx": {},'.format(K[0,2]))
136 |         print('  "cy": {},'.format(K[1,2]))
137 |         print('  "dist": {}'.format(dcoeffs.tolist()))
138 |         print('}')
139 | 
140 |     cv2.destroyAllWindows()
141 | 
142 | if __name__ == '__main__':
143 |     main()
144 | 


--------------------------------------------------------------------------------
/tools/camera_port_scanner.py:
--------------------------------------------------------------------------------
 1 | import cv2
 2 | from argparse import ArgumentParser
 3 | 
 4 | def main():
 5 |     # Create a parser for optional parameters
 6 |     parser = ArgumentParser(prog='Camera Port Scanner',
 7 |                             description='Camera Port Scannera')
 8 |     parser.add_argument('--min', type=int, default=-10, metavar='', help='Min port to scan')
 9 |     parser.add_argument('--max', type=int, default=10, metavar='', help='Max port to scan')
10 | 
11 |     args = parser.parse_args()
12 |     min_port = args.min
13 |     max_port = args.max
14 | 
15 |     if (min_port >= max_port):
16 |         raise ValueError('Make sure the minimum port is less than the maximum!')
17 | 
18 |     working_ids = []
19 |     for i in range(min_port, max_port):
20 |         cap = cv2.VideoCapture(i)
21 | 
22 |         if cap.isOpened():
23 |             working_ids.append(i)
24 | 
25 |         cap.release()
26 | 
27 |     print('Working Camera Ports: \n \
28 |     {}'.format(working_ids))
29 | 
30 | if __name__ == '__main__':
31 |     main()


--------------------------------------------------------------------------------