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.pyindex 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 npimport pyqtgraph as pgimport 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 subplotsself.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 subplotself.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 subplotself.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.pyindex 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 = 0self._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.imageraw_image = charuco_frame_payload.annotated_frame_image@@ -107,11 +107,8 @@ class CameraCalibration:image_width = new_charuco_view.image_widthimage_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 = 10if 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 viewbest_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 itfor 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 Falsereturn 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 distortionhttps://docs.opencv.org/4.x/d9/d0c/group__calib3d.html#ga3207604e4b1a1758aa66acb6ed5aa65d <- docs explaining lens_distortion_coefficientshttps://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 docsk1 = 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 Zerok3 = 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')