Skip to content

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

TypeScript

janky per-frame lens distortion correction

Jon46c146cb39df
diff --git a/src/cameras/capture/opencv_camera/opencv_camera.py b/src/cameras/capture/opencv_camera/opencv_camera.py
index b77abac2..51c4cf8f 100644
--- a/src/cameras/capture/opencv_camera/opencv_camera.py
+++ b/src/cameras/capture/opencv_camera/opencv_camera.py
@@ -23,8 +23,8 @@ class WebcamConfig(BaseModel):
resolution_width: int = 800
resolution_height: int = 600
save_video: bool = True
- fourcc: str = "MP4V"
- # fourcc: str = "MJPG"
+ # fourcc: str = "MP4V"
+ fourcc: str = "MJPG"
base_save_video_dir = _get_home_dir()
@@ -76,7 +76,7 @@ class OpenCVCamera:
self._opencv_video_capture_object = cv2.VideoCapture(
self._config.webcam_id, cap_backend
)
- # self._apply_configuration()
+ self._apply_configuration()
success, image = self._opencv_video_capture_object.read()
if not success:
@@ -129,6 +129,7 @@ class OpenCVCamera:
self._opencv_video_capture_object.set(
cv2.CAP_PROP_FRAME_HEIGHT, self._config.resolution_height
)
+
self._opencv_video_capture_object.set(
cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*self._config.fourcc)
)
diff --git a/src/core_processor/board_detection/base_pose_estimation.py b/src/core_processor/board_detection/base_pose_estimation.py
deleted file mode 100644
index d0a43174..00000000
--- a/src/core_processor/board_detection/base_pose_estimation.py
+++ /dev/null
@@ -1,51 +0,0 @@
-from dataclasses import dataclass
-from typing import List, Optional
-
-import cv2
-import numpy as np
-
-from src.core_processor.board_detection.charuco_constants import aruco_dict, board
-
-@dataclass
-class CharucoData:
- charuco_corners: Optional[np.ndarray]
- charuco_ids: Optional[np.ndarray]
- aruco_square_corners: Optional[np.ndarray]
- aruco_square_ids: Optional[np.ndarray]
-
-
-def detect_charuco_board(image) -> CharucoData:
- """
- 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 CharucoData(charuco_corners=charuco_corners,
- charuco_ids=charuco_ids,
- aruco_square_corners=aruco_square_corners,
- aruco_square_ids=aruco_square_ids)
diff --git a/src/core_processor/board_detection/board_detection.py b/src/core_processor/board_detection/board_detection.py
index 67c20ffd..e3d98e3e 100644
--- a/src/core_processor/board_detection/board_detection.py
+++ b/src/core_processor/board_detection/board_detection.py
@@ -12,7 +12,7 @@ import numpy as np
from src.cameras.capture.frame_payload import FramePayload
from src.cameras.multicam_manager.cv_camera_manager import CVCameraManager
from src.cameras.persistence.video_writer.video_writer import SaveOptions
-from src.core_processor.board_detection.base_pose_estimation import detect_charuco_board, CharucoData
+from src.core_processor.board_detection.detect_charuco_board import detect_charuco_board, CharucoData
from src.core_processor.board_detection.charuco_image_annotator import (
annotate_image_with_charuco_data,
)
@@ -25,7 +25,7 @@ logger = logging.getLogger(__name__)
@dataclass
class CharucoFramePayload:
raw_frame_payload: FramePayload
- annotated_frame_payload: FramePayload
+ annotated_frame_image: np.ndarray
charuco_data: CharucoData
class BoardDetection:
@@ -46,9 +46,9 @@ class BoardDetection:
if not cv_cam.is_capturing_frames:
return
charuco_frame_payload = self._detect_charuco_board_in_image(cv_cam)
- if cb and charuco_frame_payload.annotated_frame_payload.image is not None:
- writer.write(charuco_frame_payload.annotated_frame_payload)
- await cb(charuco_frame_payload.annotated_frame_payload.image)
+ if cb and charuco_frame_payload.annotated_frame_image is not None:
+ writer.write(charuco_frame_payload.raw_frame_payload)
+ await cb(charuco_frame_payload.annotated_frame_image)
except Exception as e:
logger.error("Printing traceback")
traceback.print_exc()
@@ -92,15 +92,16 @@ class BoardDetection:
continue
if save_video:
- writer.write(charuco_frame_payload.annotated_frame_payload)
+ writer.write(charuco_frame_payload.raw_frame_payload)
if post_processed_frame_cb:
- post_processed_frame_cb(current_webcam_id, charuco_frame_payload)
+ undistorted_annotated_image = post_processed_frame_cb(current_webcam_id, charuco_frame_payload)
+ charuco_frame_payload.annotated_frame_image = undistorted_annotated_image
fps_manager.increment_frame_processed_for(current_webcam_id)
if show_window:
should_continue = show_cam_window(
- current_webcam_id, charuco_frame_payload.annotated_frame_payload.image, fps_manager
+ current_webcam_id, charuco_frame_payload.annotated_frame_image, fps_manager
)
except:
logger.error("Printing traceback")
@@ -136,14 +137,17 @@ class BoardDetection:
charuco_data = detect_charuco_board(raw_frame_payload.image)
annotated_image = raw_frame_payload.image.copy()
+ if charuco_data.some_charuco_corners_found:
+ annotate_image_with_charuco_data(
+ annotated_image, #this image will have stuff drawn on top of it inside this function
+ cv_cam.webcam_id_as_str,
+ charuco_data
+ )
+ cv2.polylines(annotated_image, np.int32([charuco_data.charuco_corners]), True, (0, 100, 255), 2)
- success_bool = annotate_image_with_charuco_data(
- annotated_image, cv_cam.webcam_id_as_str, charuco_data
- )
- cv2.polylines(annotated_image, np.int32([charuco_data.charuco_corners]), True, (0, 100, 255), 2)
return CharucoFramePayload(
- annotated_frame_payload=FramePayload(image=annotated_image, timestamp=time.time_ns()),
+ annotated_frame_image=annotated_image,
raw_frame_payload=raw_frame_payload,
charuco_data=charuco_data
)
diff --git a/src/core_processor/board_detection/charuco_constants.py b/src/core_processor/board_detection/charuco_constants.py
index 3816642c..2eda9a8d 100644
--- a/src/core_processor/board_detection/charuco_constants.py
+++ b/src/core_processor/board_detection/charuco_constants.py
@@ -1,8 +1,8 @@
import cv2
-aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_4X4_250)
-charuco_length = 7
-charuco_width = 5
+aruco_marker_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_4X4_250)
+number_of_squares_width = 7
+number_of_squares_height = 5
-board = cv2.aruco.CharucoBoard_create(charuco_length, charuco_width, 1, 0.8, aruco_dict)
-num_charuco_corners = (charuco_length - 1) * (charuco_width - 1)
+charuco_board_object = cv2.aruco.CharucoBoard_create(number_of_squares_width, number_of_squares_height, 1, 0.8, aruco_marker_dict)
+number_of_charuco_corners = (number_of_squares_width - 1) * (number_of_squares_height - 1)
diff --git a/src/core_processor/board_detection/charuco_image_annotator.py b/src/core_processor/board_detection/charuco_image_annotator.py
index ed183c7b..62b2d703 100644
--- a/src/core_processor/board_detection/charuco_image_annotator.py
+++ b/src/core_processor/board_detection/charuco_image_annotator.py
@@ -3,20 +3,20 @@ from typing import List
import cv2
import numpy as np
-from src.core_processor.board_detection.charuco_constants import num_charuco_corners
+from src.core_processor.board_detection.charuco_constants import number_of_charuco_corners
def _board_text(
cam_name: str,
charuco_ids: List[int],
- charuco_corners: int,
- charuco_detect_bool: bool,
+ num_charuco_corners: int,
+ full_board_detected_bool: bool,
):
- ids_as_str = str(len(charuco_ids))
- corners_as_str = str(charuco_corners)
+ num_ids_as_str = str(len(charuco_ids))
+ num_corners_as_str = str(num_charuco_corners)
text = (
- f"{cam_name}: {ids_as_str} of {corners_as_str} Charuco Corner Points Detected "
- f"| Full Board Detected: {charuco_detect_bool}"
+ f"{cam_name}: {num_ids_as_str} of {num_corners_as_str} Charuco Corner Points Detected "
+ f"| Full Board Detected: {full_board_detected_bool}"
)
return text
@@ -24,10 +24,7 @@ def _board_text(
def annotate_image_with_charuco_data(
image, webcam_id: str, charuco_data
) -> bool:
- full_charuco_detected_on_this_frame = False
- if len(charuco_data.charuco_ids) == num_charuco_corners:
- full_charuco_detected_on_this_frame = True
image_w_markers = cv2.aruco.drawDetectedCornersCharuco(
image,
@@ -43,8 +40,8 @@ def annotate_image_with_charuco_data(
current_cam_corner_count_str = _board_text(
this_cam_name,
charuco_data.charuco_ids,
- num_charuco_corners,
- full_charuco_detected_on_this_frame,
+ number_of_charuco_corners,
+ charuco_data.full_board_found,
)
# TODO - Determine 'shared views' (i.e. frames in which a full board is detected by 2
# cameras)
@@ -77,13 +74,7 @@ def annotate_image_with_charuco_data(
1,
) # font stroke
- if full_charuco_detected_on_this_frame:
+ if charuco_data.full_board_found:
cv2.polylines(
image_w_markers, np.int32([charuco_data.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_charuco_board.py b/src/core_processor/board_detection/detect_charuco_board.py
new file mode 100644
index 00000000..502d0574
--- /dev/null
+++ b/src/core_processor/board_detection/detect_charuco_board.py
@@ -0,0 +1,75 @@
+from dataclasses import dataclass
+from typing import List, Optional
+
+import cv2
+import numpy as np
+
+from src.core_processor.board_detection.charuco_constants import aruco_marker_dict, charuco_board_object, \
+ number_of_charuco_corners
+
+
+@dataclass
+class CharucoData:
+ full_board_found: bool
+ some_charuco_corners_found: bool
+ any_markers_found: bool
+ charuco_corners: Optional[np.ndarray]
+ charuco_ids: Optional[np.ndarray]
+ aruco_square_corners: Optional[np.ndarray]
+ aruco_square_ids: Optional[np.ndarray]
+
+
+def detect_charuco_board(image) -> CharucoData:
+ """
+ detect charuco board in an image.
+ 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
+ termination_criteria_threshold = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.00001)
+
+ grayscale_image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
+
+ aruco_square_corners, aruco_square_ids, rejected_image_points = cv2.aruco.detectMarkers(
+ grayscale_image, aruco_marker_dict
+ )
+
+ full_board_found = False
+ any_markers_found = False
+ some_charuco_corners_found = False
+
+ if len(aruco_square_corners) > 0:
+ any_markers_found = True
+ # refine detected corner locations to provide sub-pixel precision
+ # https://docs.opencv.org/4.x/dd/d1a/group__imgproc__feature.html#ga354e0d7c86d0d9da75de9b9701a9a87e
+ for this_corner in aruco_square_corners:
+ cv2.cornerSubPix(
+ grayscale_image, this_corner, winSize=(3, 3), zeroZone=(-1, -1), criteria=termination_criteria_threshold
+ )
+ res2 = cv2.aruco.interpolateCornersCharuco(aruco_square_corners,
+ aruco_square_ids,
+ grayscale_image,
+ charuco_board_object
+ )
+
+ if res2[1] is not None and res2[2] is not None and len(res2[1]) > 3:
+ some_charuco_corners_found = True
+
+ if len(res2[2]) == number_of_charuco_corners:
+ full_board_found = True
+
+ charuco_corners = res2[1]
+ charuco_ids = res2[2]
+
+
+
+ return CharucoData(full_board_found=full_board_found,
+ some_charuco_corners_found=some_charuco_corners_found,
+ any_markers_found=any_markers_found,
+ charuco_corners=charuco_corners,
+ charuco_ids=charuco_ids,
+ aruco_square_corners=aruco_square_corners,
+ aruco_square_ids=aruco_square_ids)
diff --git a/src/core_processor/camera_calibration/camera_calibration.py b/src/core_processor/camera_calibration/camera_calibration.py
index 06952714..6a4b8c6c 100644
--- a/src/core_processor/camera_calibration/camera_calibration.py
+++ b/src/core_processor/camera_calibration/camera_calibration.py
@@ -1,20 +1,137 @@
-from src.cameras.capture.frame_payload import FramePayload
+from dataclasses import dataclass
+import time
+import cv2
+import numpy as np
+from rich import print
+
from src.core_processor.board_detection.board_detection import BoardDetection, CharucoFramePayload
+from src.core_processor.board_detection.charuco_constants import charuco_board_object
+
+number_of_distortion_coefficients = 5 # may be 4, 5, 8, or 12, not sure of the trade-off there
+
+@dataclass
+class SingleCameraCalibrationData:
+ reprojection_error: float = 1e9
+ camera_matrix: np.ndarray = None
+ lens_distortion_coefficients: np.ndarray = np.zeros((number_of_distortion_coefficients, 1))
+ rotation_vectors_of_the_board: np.ndarray = None
+ translation_vectors_of_the_board: np.ndarray = None
+ lens_distortion_std_dev: float = None
+ camera_location_std_dev: float = None
class CameraCalibration:
def __init__(self, board_detection: BoardDetection):
self._bd = board_detection
+ self._current_calibration = SingleCameraCalibrationData()
+ self._start_time = time.time()
+ self._all_charuco_corners_list = []
+ self._all_charuco_ids_list = []
+
+
def perform_calibration(self):
self._bd.process(
show_window=True,
- save_video=True,
+ save_video=False,
post_processed_frame_cb=self._calibration_work,
)
def _calibration_work(self, webcam_id: str, charuco_frame_payload: CharucoFramePayload):
- print(f"found frame from camera {webcam_id} at timestamp {charuco_frame_payload.annotated_frame_payload.timestamp/1e9}")
+ elapsed_time = (charuco_frame_payload.raw_frame_payload.timestamp / 1e9) - self._start_time
+ print(
+ f"found frame from camera {webcam_id} at timestamp {elapsed_time:.3f}")
+
+ if not charuco_frame_payload.charuco_data.full_board_found:
+ print(f'reprojection error - {self._current_calibration.reprojection_error:.3f}')
+ print(f'lens distortion coefficients - {np.array_str(np.squeeze(self._current_calibration.lens_distortion_coefficients), precision=2)}')
+ return charuco_frame_payload.annotated_frame_image
+
+ charuco_data = charuco_frame_payload.charuco_data
+ image_width, image_height, color_channels = charuco_frame_payload.raw_frame_payload.image.shape
+
+ self._current_calibration = self.calibrate_camera(charuco_data.charuco_corners,
+ charuco_data.charuco_ids,
+ image_width,
+ image_height,
+ )
+ print(f'camera matrix - \n{np.array_str(self._current_calibration.camera_matrix, precision=2)}')
+
+ # create new camera matrix that will show full undistored image (with black pixels in spots with no data (only for debugging)
+ new_camera_matrix, valid_ROI_lbwh = cv2.getOptimalNewCameraMatrix(self._current_calibration.camera_matrix,
+ self._current_calibration.lens_distortion_coefficients,
+ (image_width, image_height),
+ 1,
+ centerPrincipalPoint=True
+ )
+
+ # https://docs.opencv.org/4.5.5/d9/d0c/group__calib3d.html#ga69f2545a8b62a6b0fc2ee060dc30559d
+ undistorted_image = cv2.undistort(charuco_frame_payload.annotated_frame_image,
+ self._current_calibration.camera_matrix,
+ self._current_calibration.lens_distortion_coefficients,
+ None,
+ new_camera_matrix)
+
+ return undistorted_image
+
+
+ def calibrate_camera(self, charuco_corners_list, charuco_ids_list, image_width, image_height):
+ """
+ adapted from - https://mecaruco2.readthedocs.io/en/latest/notebooks_rst/Aruco/sandbox/ludovic/aruco_calibration_rotation.html
+
+ Calibrates the camera using the charuco data
+
+ helpful resources -
+ https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html#ga3207604e4b1a1758aa66acb6ed5aa65d <-opencv's 3d camera calibration docs
+ https://www.adamsmith.haus/python/docs/cv2.aruco.calibrateCameraCharucoExtended
+ https://www.mathworks.com/help/vision/ug/camera-calibration.html
+ https://docs.opencv.org/3.4/d9/db7/tutorial_py_table_of_contents_calib3d.html
+ https://calib.io/blogs/knowledge-base/camera-models <-a good one
+ """
+ print("CAMERA CALIBRATION")
+
+ if self._current_calibration.camera_matrix is None:
+ self._current_calibration.camera_matrix = np.array([[float(image_width), 0., image_width / 2.],
+ [0., float(image_width), image_height / 2.],
+ [0., 0., 1.]])
+
+
+
+ # flag definitions -> https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html#ga3207604e4b1a1758aa66acb6ed5aa65d
+ flags = (cv2.CALIB_USE_INTRINSIC_GUESS + cv2.CALIB_FIX_ASPECT_RATIO + cv2.CALIB_FIX_PRINCIPAL_POINT + cv2.CALIB_TILTED_MODEL)
+ # flags = (cv2.CALIB_RATIONAL_MODEL)
+
+ # https://docs.opencv.org/4.x/d9/d6a/group__aruco.html#gacf03e5afb0bc516b73028cf209984a06
+ (reprojection_error,
+ camera_matrix,
+ lens_distortion_coefficients,
+ rotation_vectors_of_the_board,
+ translation_vectors_of_the_board,
+ lens_distortion_std_dev,
+ camera_location_std_dev,
+ reprojection_error_per_view
+ ) = cv2.aruco.calibrateCameraCharucoExtended(
+ charucoCorners=charuco_corners_list,
+ charucoIds=charuco_ids_list,
+ board=charuco_board_object,
+ imageSize=(image_width, image_height),
+ cameraMatrix=self._current_calibration.camera_matrix.copy(),
+ distCoeffs=self._current_calibration.lens_distortion_coefficients.copy(),
+ flags=flags,
+ criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 10000, 1e-9))
+
+ if reprojection_error > self._current_calibration.reprojection_error:
+ return self._current_calibration
+
+ return SingleCameraCalibrationData(
+ reprojection_error=reprojection_error,
+ camera_matrix=camera_matrix,
+ lens_distortion_coefficients=lens_distortion_coefficients,
+ rotation_vectors_of_the_board=rotation_vectors_of_the_board,
+ translation_vectors_of_the_board=translation_vectors_of_the_board,
+ lens_distortion_std_dev=lens_distortion_std_dev,
+ camera_location_std_dev=camera_location_std_dev)
+
if __name__ == "__main__":