Skip to content

Free Motion Capture for Everyone πŸ’€βœ¨

TypeScript

break apart the algo into multiple files

Endurance Idehen7e4974663028
diff --git a/src/core_processor/board_detection/base_pose_estimation.py b/src/core_processor/board_detection/base_pose_estimation.py
new file mode 100644
index 00000000..15b6d9a3
--- /dev/null
+++ b/src/core_processor/board_detection/base_pose_estimation.py
@@ -0,0 +1,36 @@
+import cv2
+
+from src.core_processor.board_detection.charuco_constants import aruco_dict, board
+
+
+def detect_charuco_board(image):
+ """
+ Charuco base pose estimation.
+ more-or-less copied from - https://mecaruco2.readthedocs.io/en/latest/notebooks_rst/Aruco
+ /sandbox/ludovic/aruco_calibration_rotation.html
+ """
+ charuco_corners = []
+ charuco_ids = []
+
+ # SUB PIXEL CORNER DETECTION CRITERION
+ criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.00001)
+
+ gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
+ aruco_square_corners, aruco_square_ids, rejectedImgPoints = cv2.aruco.detectMarkers(gray,
+ aruco_dict)
+
+ if len(aruco_square_corners) > 0:
+ # SUB PIXEL DETECTION
+ for this_corner in aruco_square_corners:
+ cv2.cornerSubPix(gray, this_corner,
+ winSize=(3, 3),
+ zeroZone=(-1, -1),
+ criteria=criteria)
+ res2 = cv2.aruco.interpolateCornersCharuco(aruco_square_corners, aruco_square_ids, gray,
+ board)
+
+ if res2[1] is not None and res2[2] is not None and len(res2[1]) > 3:
+ charuco_corners = res2[1]
+ charuco_ids = res2[2]
+
+ return charuco_corners, charuco_ids, aruco_square_corners, aruco_square_ids
diff --git a/src/core_processor/board_detection/charuco_constants.py b/src/core_processor/board_detection/charuco_constants.py
new file mode 100644
index 00000000..a3e71562
--- /dev/null
+++ b/src/core_processor/board_detection/charuco_constants.py
@@ -0,0 +1,9 @@
+import cv2
+
+aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_4X4_250)
+charuco_length = 7
+charuco_width = 5
+
+board = cv2.aruco.CharucoBoard_create(charuco_length, charuco_width, 1, .8, aruco_dict)
+num_charuco_corners = (charuco_length - 1) * (charuco_width - 1)
+
diff --git a/src/core_processor/board_detection/charuco_image_annotator.py b/src/core_processor/board_detection/charuco_image_annotator.py
new file mode 100644
index 00000000..ce461ec5
--- /dev/null
+++ b/src/core_processor/board_detection/charuco_image_annotator.py
@@ -0,0 +1,62 @@
+import cv2
+import numpy as np
+
+from src.core_processor.board_detection.charuco_constants import num_charuco_corners
+
+
+def annotate_image_with_charuco_data(image, port_number, charuco_corners, charuco_ids) -> bool:
+ full_charuco_detected_on_this_frame = False
+
+ if len(charuco_ids) == num_charuco_corners:
+ full_charuco_detected_on_this_frame = True
+
+ image_w_markers = cv2.aruco.drawDetectedCornersCharuco(image,
+ np.array(charuco_corners),
+ np.array(charuco_ids),
+ (0, 255, 125, 255)) # yellow? I
+ # think cv2 uses BGR instead of RGB?
+
+ this_cam_name = "Camera " + str(port_number)
+
+ text_to_write_on_this_camera = ''
+ current_cam_corner_count_str = this_cam_name + ": " + str(
+ len(charuco_ids)) + " of " + str(
+ num_charuco_corners) + " ChAruco Corner Points detected | Full Board Detected: " + str(
+ full_charuco_detected_on_this_frame)
+ # TODO - Determine 'shared views' (i.e. frames in which a full board is detected by 2
+ # cameras)
+ # TODO - self.determine_shared_charuco_board_views()
+ # this_cam_shared_views_str = " | Shared Views: " + str(
+ # each_cameras_shared_board_view_count_total)
+ text_to_write_on_this_camera = current_cam_corner_count_str
+
+ position = (10, 50)
+ cv2.putText(
+ image_w_markers, # numpy array on which text is written
+ text_to_write_on_this_camera, # text
+ position, # position at which writing has to start
+ cv2.FONT_HERSHEY_SIMPLEX, # font family
+ .5, # font size
+ (30, 10, 0, 255), # font color
+ 2) # font stroke (draw a darker heavier font beneath a lighter/thinner copy for
+ # readability)
+
+ cv2.putText(
+ image_w_markers, # numpy array on which text is written
+ text_to_write_on_this_camera, # text
+ position, # position at which writing has to start
+ cv2.FONT_HERSHEY_SIMPLEX,
+ # font family (very limited selection, i think there's some interesting CV history
+ # here...)
+ .5, # font size
+ (209, 180, 0, 255), # font color
+ 1) # font stroke
+
+ if full_charuco_detected_on_this_frame:
+ cv2.polylines(image_w_markers, np.int32([charuco_corners]), False, (0, 100, 255), 2)
+ # for these_corners in charuco_points_from_previous_frames:
+ # if len(these_corners)>0:
+ # cv2.polylines(image_w_markers, np.int32([these_corners]), True, (0,100,255,
+ # 255/2), 2)
+
+ return True
diff --git a/src/core_processor/board_detection/detect.py b/src/core_processor/board_detection/detect.py
index 20608ae1..c1116c20 100644
--- a/src/core_processor/board_detection/detect.py
+++ b/src/core_processor/board_detection/detect.py
@@ -4,15 +4,11 @@ import logging
import cv2
import numpy as np
+from src.core_processor.board_detection.base_pose_estimation import detect_charuco_board
+from src.core_processor.board_detection.charuco_image_annotator import \
+ annotate_image_with_charuco_data
from src.core_processor.processor import create_opencv_cams
-aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_4X4_250)
-charuco_length = 7
-charuco_width = 5
-
-board = cv2.aruco.CharucoBoard_create(charuco_length, charuco_width, 1, .8, aruco_dict)
-num_charuco_corners = (charuco_length - 1) * (charuco_width - 1)
-
class BoardDetection:
@@ -24,6 +20,9 @@ class BoardDetection:
cv_cam.start_frame_capture()
while True:
+ exit_key = cv2.waitKey(1)
+ if exit_key == 27:
+ break
for cv_cam in cv_cams:
success, frame, timestamp = cv_cam.latest_frame()
if not success:
@@ -33,12 +32,11 @@ class BoardDetection:
continue
port_number = str(cv_cam.port_number)
- print(
- f'got image of shape {frame.shape} from camera at port {port_number}')
+ print(f'got image of shape {frame.shape} from camera at port {port_number}')
charuco_corners, charuco_ids, aruco_square_corners, aruco_square_ids = \
- self.detect_charuco_board(frame)
+ detect_charuco_board(frame)
# TODO - Pull out timestamps per frame and calculate fps to display on image
- success_bool = self.annotate_image_with_charuco_data(frame, port_number,
+ success_bool = annotate_image_with_charuco_data(frame, port_number,
charuco_corners, charuco_ids)
cv2.polylines(frame, np.int32([charuco_corners]), True, (0, 100, 255), 2)
@@ -48,119 +46,6 @@ class BoardDetection:
if exit_key == 27:
break
- def detect_charuco_board(self, image):
- """
- Charuco base pose estimation.
- more-or-less copied from - https://mecaruco2.readthedocs.io/en/latest/notebooks_rst/Aruco
- /sandbox/ludovic/aruco_calibration_rotation.html
- """
- charuco_corners = []
- charuco_ids = []
-
- # SUB PIXEL CORNER DETECTION CRITERION
- criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.00001)
-
- gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
- aruco_square_corners, aruco_square_ids, rejectedImgPoints = cv2.aruco.detectMarkers(gray,
- aruco_dict)
-
- if len(aruco_square_corners) > 0:
- # SUB PIXEL DETECTION
- for this_corner in aruco_square_corners:
- cv2.cornerSubPix(gray, this_corner,
- winSize=(3, 3),
- zeroZone=(-1, -1),
- criteria=criteria)
- res2 = cv2.aruco.interpolateCornersCharuco(aruco_square_corners, aruco_square_ids, gray,
- board)
-
- if res2[1] is not None and res2[2] is not None and len(res2[1]) > 3:
- charuco_corners = res2[1]
- charuco_ids = res2[2]
-
- return charuco_corners, charuco_ids, aruco_square_corners, aruco_square_ids
-
- def annotate_image_with_charuco_data(self, image, port_number, charuco_corners,
- charuco_ids) -> bool:
-
- full_charuco_detected_on_this_frame = False
- if len(charuco_ids) == num_charuco_corners:
- full_charuco_detected_on_this_frame = True
-
- image_w_markers = cv2.aruco.drawDetectedCornersCharuco(image,
- np.array(charuco_corners),
- np.array(charuco_ids),
- (0, 255, 125, 255)) # yellow? I
- # think cv2 uses BGR instead of RGB?
-
- this_cam_name = "Camera " + str(port_number)
-
- text_to_write_on_this_camera = ''
- current_cam_corner_count_str = this_cam_name + ": " + str(
- len(charuco_ids)) + " of " + str(
- num_charuco_corners) + " ChAruco Corner Points detected | Full Board Detected: " + str(
- full_charuco_detected_on_this_frame)
- # TODO - Determine 'shared views' (i.e. frames in which a full board is detected by 2
- # cameras)
- # TODO - self.determine_shared_charuco_board_views()
- # this_cam_shared_views_str = " | Shared Views: " + str(
- # each_cameras_shared_board_view_count_total)
- text_to_write_on_this_camera = current_cam_corner_count_str
-
- position = (10, 50)
- cv2.putText(
- image_w_markers, # numpy array on which text is written
- text_to_write_on_this_camera, # text
- position, # position at which writing has to start
- cv2.FONT_HERSHEY_SIMPLEX, # font family
- .5, # font size
- (30, 10, 0, 255), # font color
- 2) # font stroke (draw a darker heavier font beneath a lighter/thinner copy for
- # readability)
-
- cv2.putText(
- image_w_markers, # numpy array on which text is written
- text_to_write_on_this_camera, # text
- position, # position at which writing has to start
- cv2.FONT_HERSHEY_SIMPLEX,
- # font family (very limited selection, i think there's some interesting CV history
- # here...)
- .5, # font size
- (209, 180, 0, 255), # font color
- 1) # font stroke
-
- if full_charuco_detected_on_this_frame:
- cv2.polylines(image_w_markers, np.int32([charuco_corners]), False, (0, 100, 255), 2)
- # for these_corners in charuco_points_from_previous_frames:
- # if len(these_corners)>0:
- # cv2.polylines(image_w_markers, np.int32([these_corners]), True, (0,100,255,
- # 255/2), 2)
-
- return True
-
- def determine_shared_charuco_board_views(self):
- pass
- # # determine paired board views
- # for this_cam_num in range(multi_cam.num_cams):
- # if full_charuco_detected_on_this_frame[this_cam_num]:
- # for this_other_camera_num in range(multi_cam.num_cams):
- # if this_other_camera_num != this_cam_num:
- # if full_charuco_detected_on_this_frame[this_other_camera_num]:
- # each_cameras_shared_board_view_count_array[this_cam_num,
- # this_other_camera_num] += 1
- # each_cameras_shared_board_view_count_total = np.sum(
- # each_cameras_shared_board_view_count_array, axis=1)
- # # print(each_cameras_shared_board_view_count)
- #
- # for this_pair_num, this_cam_pair in enumerate(camera_pair_list):
- # camera_one_id = this_cam_pair[0]
- # camera_two_id = this_cam_pair[1]
- #
- # if full_charuco_detected_on_this_frame[camera_one_id] and
- # full_charuco_detected_on_this_frame[
- # camera_two_id]:
- # camera_pair_joint_view_count[this_pair_num] += 1
-
-
if __name__ == "__main__":
+ ## Jon, Run me to easily start a charuco board detect run
asyncio.run(BoardDetection().process())
diff --git a/src/core_processor/board_detection/shared_board_view_detection.py b/src/core_processor/board_detection/shared_board_view_detection.py
new file mode 100644
index 00000000..173af06f
--- /dev/null
+++ b/src/core_processor/board_detection/shared_board_view_detection.py
@@ -0,0 +1,23 @@
+
+def determine_shared_charuco_board_views(self):
+ pass
+ # # determine paired board views
+ # for this_cam_num in range(multi_cam.num_cams):
+ # if full_charuco_detected_on_this_frame[this_cam_num]:
+ # for this_other_camera_num in range(multi_cam.num_cams):
+ # if this_other_camera_num != this_cam_num:
+ # if full_charuco_detected_on_this_frame[this_other_camera_num]:
+ # each_cameras_shared_board_view_count_array[this_cam_num,
+ # this_other_camera_num] += 1
+ # each_cameras_shared_board_view_count_total = np.sum(
+ # each_cameras_shared_board_view_count_array, axis=1)
+ # # print(each_cameras_shared_board_view_count)
+ #
+ # for this_pair_num, this_cam_pair in enumerate(camera_pair_list):
+ # camera_one_id = this_cam_pair[0]
+ # camera_two_id = this_cam_pair[1]
+ #
+ # if full_charuco_detected_on_this_frame[camera_one_id] and
+ # full_charuco_detected_on_this_frame[
+ # camera_two_id]:
+ # camera_pair_joint_view_count[this_pair_num] += 1
\ No newline at end of file