Skip to content

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

TypeScript

leaving lens calibration there for now

Jon2a9e32fc86fe
diff --git a/src/core_processor/camera_calibration/calibration_diagnostics_visualizer.py b/src/core_processor/camera_calibration/calibration_diagnostics_visualizer.py
index 04eefd1b..0ef6e8ee 100644
--- a/src/core_processor/camera_calibration/calibration_diagnostics_visualizer.py
+++ b/src/core_processor/camera_calibration/calibration_diagnostics_visualizer.py
@@ -2,13 +2,17 @@ import numpy as np
import pyqtgraph as pg
import cv2
+
class CalibrationDiagnosticsVisualizer:
def __init__(self):
self.graphics_layout_window = pg.GraphicsLayoutWidget(show=True)
self.graphics_layout_window.setWindowTitle('Reprojection Error')
+ # dummy data to initialize subplots
self.reprojection_error_all = np.empty(0)
self.reprojection_error_current_best = np.empty(0)
+ self.image_point_original = np.empty(0)
+ self.image_point_remapped = np.empty(0)
# create image subplot (based on `GraphicsItems - ImageItem - video` example `python -m pyqtgraph.examples`)
self.undistorted_image_subplot = self.graphics_layout_window.addViewBox()
@@ -16,28 +20,56 @@ class CalibrationDiagnosticsVisualizer:
self.image_item_holder = pg.ImageItem(border='w')
self.undistorted_image_subplot.addItem(self.image_item_holder)
- # create reprojection error sublot
+ # create reprojection error subplot
self.reprojection_error_subplot = self.graphics_layout_window.addPlot()
self.reprojection_error_subplot.setLabel('left', "Reprojection Error", units='pixels')
self.reprojection_error_subplot.setLabel('bottom', "Time", units='Frame#')
self.reprojection_error_subplot.setYRange(0, 2)
-
+ self.reprojection_error_subplot.addLegend()
self.reprojection_error_all_line = self.reprojection_error_subplot.plot(self.reprojection_error_all,
pen=(255, 0, 128),
name="reprojection error- all")
+ self.reprojection_error_current_best_line = self.reprojection_error_subplot.plot(
+ self.reprojection_error_current_best,
+ name="current best")
+
+ # make next row
+ self.graphics_layout_window.nextRow()
+
+ # create lens distortion remapping subplot
+ self.image_point_remapping_subplot = self.graphics_layout_window.addPlot()
+ self.image_point_remapping_subplot.setLabel('left', "Y-position", units='pixels')
+ self.image_point_remapping_subplot.setLabel('bottom', "X-position", units='pixels')
+ self.image_point_remapping_subplot.addLegend()
+
+ self.image_point_original_line = self.image_point_remapping_subplot.plot(self.image_point_original,
+ pen=(255, 200, 200),
+ name="image points - original")
+ self.image_point_remapped_line = self.image_point_remapping_subplot.plot(self.image_point_remapped,
+ pen=(0, 255, 128),
+ name="image points - remapped")
+
+
- self.reprojection_error_current_best_line = self.reprojection_error_subplot.plot(self.reprojection_error_current_best)
+
+ def update_image_subplot(self, new_image):
+ self.image_item_holder.setImage(cv2.cvtColor(cv2.rotate(new_image, cv2.ROTATE_90_CLOCKWISE), cv2.COLOR_BGR2RGB))
def update_reprojection_error_subplot(self, reprojection_error_new, reprojection_error_current_best):
- # update reprojection error subplot
self.reprojection_error_all = np.append(self.reprojection_error_all, reprojection_error_new)
- self.reprojection_error_current_best = np.append(self.reprojection_error_current_best, reprojection_error_current_best)
+ self.reprojection_error_current_best = np.append(self.reprojection_error_current_best,
+ reprojection_error_current_best)
self.reprojection_error_all_line.setData(self.reprojection_error_all)
self.reprojection_error_current_best_line.setData(self.reprojection_error_current_best)
- def update_image_subplot(self, new_image):
- self.image_item_holder.setImage(cv2.cvtColor(cv2.rotate(new_image, cv2.ROTATE_90_CLOCKWISE), cv2.COLOR_BGR2RGB))
+ def update_image_point_remapping_subplot(self, image_point_original_x, image_point_original_y,
+ image_point_remapped_x, image_point_remapped_y):
+ self.image_point_original_line.setData(x=image_point_original_x,
+ y=image_point_original_y)
+
+ self.image_point_original_line.setData(x=image_point_remapped_x,
+ y=image_point_remapped_y)
def close(self):
self.graphics_layout_window.close()
diff --git a/src/core_processor/camera_calibration/camera_calibration.py b/src/core_processor/camera_calibration/camera_calibration.py
index fc2866fd..08f907a2 100644
--- a/src/core_processor/camera_calibration/camera_calibration.py
+++ b/src/core_processor/camera_calibration/camera_calibration.py
@@ -33,6 +33,7 @@ class CameraCalibration:
self._all_charuco_views = []
self._num_charuco_views = 0
self._best_combo_of_charuco_views = []
+ self._previous_best_calibrations = []
if visualize_calibration_disagnostics:
self.calibration_diagnostics_visualizer = CalibrationDiagnosticsVisualizer()
@@ -60,7 +61,6 @@ class CameraCalibration:
self.estimate_lens_distortion(new_charuco_view)
-
# raw_image = charuco_frame_payload.raw_frame_payload.image
raw_image = charuco_frame_payload.annotated_frame_image
@@ -107,11 +107,8 @@ class CameraCalibration:
image_width = new_charuco_view.image_width
image_height = new_charuco_view.image_height
- initial_camera_matrix = np.array([[float(image_width), 0., image_width / 2.],
- [0., float(image_width), image_height / 2.],
- [0., 0., 1.]])
- number_of_distortion_coefficients = 4 # may be 4, 5, 8, or 12, not sure of the trade-off there but low numbers seem more stable
- initial_lens_distortion_coefficients = np.zeros((number_of_distortion_coefficients, 1))
+ default_calibration = self.get_default_calibration(new_charuco_view,
+ number_of_distortion_coefficients=5)
num_charuco_views_per_combo = 10
if self._num_charuco_views < num_charuco_views_per_combo:
@@ -145,8 +142,8 @@ class CameraCalibration:
charucoIds=[this_charuco_view.charuco_ids],
board=charuco_board_object,
imageSize=(image_width, image_height),
- cameraMatrix=initial_camera_matrix,
- distCoeffs=initial_lens_distortion_coefficients,
+ cameraMatrix=default_calibration.camera_matrix,
+ distCoeffs=default_calibration.lens_distortion_coefficients,
flags=flags,
criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 10000, 1e-9))
@@ -168,6 +165,7 @@ class CameraCalibration:
if self.is_this_calibration_valid(this_calibration):
self._current_calibration = this_calibration
+ self._previous_best_calibrations.append(self._current_calibration)
def generate_combos_of_charuco_views(self,
new_charuco_view,
@@ -182,10 +180,22 @@ class CameraCalibration:
list_of_combos_of_charuco_views = []
+ # add default calibration (i.e. option not to change anything)
+ # list_of_combos_of_charuco_views.append([self.get_default_calibration(new_charuco_view)])
+
+ # add previous best view
+ if self._best_combo_of_charuco_views:
+ list_of_combos_of_charuco_views.append(self._best_combo_of_charuco_views.copy())
+
+ # add previous best view + new view
best_combo_plus_new = self._best_combo_of_charuco_views.copy()
best_combo_plus_new.append(new_charuco_view)
list_of_combos_of_charuco_views.append(best_combo_plus_new)
+ # add all previous best calibrations
+ # list_of_combos_of_charuco_views.append([cc for cc in self._previous_best_calibrations])
+
+ # and a some random combos - half with the new view and half without it
for this_iter in range(num_combos_to_generate):
this_random_sample_of_charuco_views = np.random.choice(num_views_per_combo,
size=min(len(self._all_charuco_views),
@@ -207,12 +217,12 @@ class CameraCalibration:
if not self.lens_distortion_is_monotonic(this_calibration):
return False
- if self.too_many_invalid_pixels(this_calibration):
+ if self.check_if_too_many_invalid_pixels(this_calibration):
return False
return True
- def lens_distortion_is_monotonic(self, this_calibration) -> bool:
+ def lens_distortion_is_monotonic(self, this_calibration, assume_wide_angle_lens=True) -> bool:
"""
check if the lens distortion coefficients are valid by checking it the resulting undistorted image is monotonically increasing along each diagonal (i.e. the image doesn't wrap around itself)
@@ -224,10 +234,11 @@ class CameraCalibration:
non-monotonic behavior means the calibration is invalid, so we check for that by examining the `np.diff` of a vector of points extending from image center to the top left coners to make sure it never goes negative(NOTE - check other directions?)
- since most cameras are wide-angle (i.e. they give a wider field of view than a pinhole camera), we may constrain things further be requiring things be monotonically increasing (i.e. only return pincushion distortions)
+ since most cameras are wide-angle (i.e. they give a wider field of view than a pinhole camera), we may constrain things further be requiring things be monotonically increasing (i.e. only return pincushion distortions to correct for the barrel distortion of a wide-angle field of view)
see -
+ https://www.mathworks.com/help/vision/ug/camera-calibration.html <- matlab docs (best ones, maybe)
https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html <-mathematical defintion of lens distortion
https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html#ga3207604e4b1a1758aa66acb6ed5aa65d <- docs explaining lens_distortion_coefficients
https://github.com/opencv/opencv/issues/15992 <-conversation about the fact that opencv doesn't check for this kinda thing
@@ -235,42 +246,34 @@ class CameraCalibration:
"""
calibration_valid = False
- pixel_distance_to_image_corner = np.sqrt(np.power(this_calibration.image_width, 2)
- + np.power(this_calibration.image_height, 2))
-
- distances_from_image_center = np.linspace(-pixel_distance_to_image_corner,
- pixel_distance_to_image_corner) # aka, the `r` as defined in teh docs
-
- # following naming conventions
+ # following naming conventions of open cv docs
k1 = this_calibration.lens_distortion_coefficients[0]
k2 = this_calibration.lens_distortion_coefficients[1]
p1 = this_calibration.lens_distortion_coefficients[2] # I think p1 and p2 are for tangential distortion...
- p2 = this_calibration.lens_distortion_coefficients[3] # ...which I might want to check on later?
+ p2 = this_calibration.lens_distortion_coefficients[
+ 3] # ...which I might want to check on later? Currently fixed at Zero
k3 = this_calibration.lens_distortion_coefficients[4]
- distorted_points = 1 + \
- k1 * np.power(distances_from_image_center, 2) + \
- k2 * np.power(distances_from_image_center, 4) + \
- k3 * np.power(distances_from_image_center, 6)
+ number_of_points = 100
+ original_points_x = np.linspace(-this_calibration.image_width, this_calibration.image_width, number_of_points)
+ original_points_y = np.linspace(-this_calibration.image_height, this_calibration.image_height, number_of_points)
- diff_distorted_points = np.diff(distances_from_image_center)
+ original_points_xy = np.vstack((original_points_x, original_points_y))
- # if show_debug_plot:
- # fig = plt.Figure()
- # fig.suptitle('is_calibration_valid - debug plot')
- # plt.plot(distances_from_image_center, label='Distance from center of image')
- # plt.plot(distorted_points, label='Distorted points')
- # plt.legend(loc="upper left")
- # plt.show()
+ distorted_points_xy = cv2.undistortPoints(original_points_xy,
+ this_calibration.camera_matrix,
+ this_calibration.lens_distortion_coefficients)
+ distorted_points_x = np.squeeze(distorted_points_xy[0])
+ distorted_points_y = np.squeeze(distorted_points_xy[1])
- if np.sum(diff_distorted_points < 0) == 0:
- print(f'Yay! Monotonic! k1={k1}, k2={k2}, k3={k3}')
- return True
+ self.calibration_diagnostics_visualizer.update_image_point_remapping_subplot(original_points_x,
+ original_points_y,
+ distorted_points_x,
+ distorted_points_y)
+ return True
- print(f'Boo! Non-monotonic! k1={k1}, k2={k2}, k3={k3}')
- return False
- def too_many_invalid_pixels(self, this_calibration, how_many_is_too_many=.75) -> bool:
+ def check_if_too_many_invalid_pixels(self, this_calibration, how_many_is_too_many=.5) -> bool:
"""
create new camera matrix that will show full undistored image with black pixels in spots with no data.
return FALSE if black pixels are greater than `how_many_is_too_many` proportion of the full image
@@ -306,6 +309,23 @@ class CameraCalibration:
return undistorted_image
+ @staticmethod
+ def get_default_calibration(charuco_view: CharucoViewData, number_of_distortion_coefficients=4):
+ image_width = charuco_view.image_width
+ image_height = charuco_view.image_height
+
+ initial_camera_matrix = np.array([[float(image_width), 0., image_width / 2.],
+ [0., float(image_width), image_height / 2.],
+ [0., 0., 1.]])
+ initial_lens_distortion_coefficients = np.zeros((number_of_distortion_coefficients, 1))
+
+ return SingleCameraCalibrationData(
+ camera_matrix=initial_camera_matrix,
+ lens_distortion_coefficients=initial_lens_distortion_coefficients,
+ image_width=image_width,
+ image_height=image_height,
+ )
+
if __name__ == "__main__":
print('start main')