From e22ffce6cc88c3c9637260a70efa014a787443c8 Mon Sep 17 00:00:00 2001 From: Felix Kloss Date: Mon, 5 Aug 2024 10:22:34 +0200 Subject: [PATCH 1/2] style: Use black's default line length --- pyproject.toml | 3 --- 1 file changed, 3 deletions(-) diff --git a/pyproject.toml b/pyproject.toml index d301a90..0ff478e 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,6 +1,3 @@ -[tool.black] -line-length = 79 - [tool.pylint.messages_control] disable = "C0330, C0326" From 5d84895999ab17c6c15e742d55c6abb56c97eccc Mon Sep 17 00:00:00 2001 From: Felix Kloss Date: Mon, 5 Aug 2024 10:23:20 +0200 Subject: [PATCH 2/2] Reformat Python --- demos/demo_camera.py | 4 +- demos/demo_tricamera.py | 8 +--- .../charuco_board_handler.py | 20 ++------ scripts/analyze_tricamera_log.py | 7 +-- scripts/calibrate_cameras.py | 47 +++++-------------- scripts/calibrate_trifingerpro_cameras.py | 22 +++------ scripts/check_camera_sharpness.py | 15 ++---- scripts/detect_aruco_marker.py | 4 +- scripts/overlay_real_and_rendered_images.py | 11 ++--- scripts/record_image_dataset.py | 8 +--- scripts/tricamera_log_viewer.py | 12 ++--- scripts/tricamera_monitor_rate.py | 8 +--- tests/test_camera_calibration_file.py | 4 +- tests/test_utils.py | 16 ++----- 14 files changed, 49 insertions(+), 137 deletions(-) diff --git a/demos/demo_camera.py b/demos/demo_camera.py index 884aef7..65e4427 100644 --- a/demos/demo_camera.py +++ b/demos/demo_camera.py @@ -43,9 +43,7 @@ def main() -> int: try: if args.pylon: - camera_driver = trifinger_cameras.camera.PylonDriver( - args.camera_id - ) + camera_driver = trifinger_cameras.camera.PylonDriver(args.camera_id) else: camera_id = int(args.camera_id) if args.camera_id else 0 camera_driver = trifinger_cameras.camera.OpenCVDriver(camera_id) diff --git a/demos/demo_tricamera.py b/demos/demo_tricamera.py index 148cf5c..c6e5c41 100644 --- a/demos/demo_tricamera.py +++ b/demos/demo_tricamera.py @@ -35,14 +35,10 @@ def main() -> None: # noqa: D103 camera_names = ["camera60", "camera180", "camera300"] if args.multi_process: - camera_data = trifinger_cameras.tricamera.MultiProcessData( - "tricamera", False - ) + camera_data = trifinger_cameras.tricamera.MultiProcessData("tricamera", False) else: camera_data = trifinger_cameras.tricamera.SingleProcessData() - camera_driver = trifinger_cameras.tricamera.TriCameraDriver( - *camera_names - ) + camera_driver = trifinger_cameras.tricamera.TriCameraDriver(*camera_names) camera_backend = trifinger_cameras.tricamera.Backend( # noqa: F841 camera_driver, camera_data ) diff --git a/python/trifinger_cameras/charuco_board_handler.py b/python/trifinger_cameras/charuco_board_handler.py index 4baf47f..df2043c 100644 --- a/python/trifinger_cameras/charuco_board_handler.py +++ b/python/trifinger_cameras/charuco_board_handler.py @@ -159,9 +159,7 @@ def visualize_board( debug_image = image if charuco_ids is not None: - debug_image = cv2.aruco.drawDetectedCornersCharuco( - image, charuco_corners - ) + debug_image = cv2.aruco.drawDetectedCornersCharuco(image, charuco_corners) if rvec is not None and tvec is not None: debug_image = cv2.aruco.drawAxis( @@ -194,9 +192,7 @@ def detect_board_in_camera_stream(self, device=0): ret, frame = cap.read() charuco_corners, charuco_ids, rvec, tvec = self.detect_board(frame) - if self.visualize_board( - frame, charuco_corners, charuco_ids, rvec, tvec, 1 - ): + if self.visualize_board(frame, charuco_corners, charuco_ids, rvec, tvec, 1): break # When everything done, release the capture @@ -235,9 +231,7 @@ def detect_board_in_image(self, filename, visualize=False): charuco_corners, charuco_ids, rvec, tvec = self.detect_board(img) if charuco_ids is not None: if visualize: - self.visualize_board( - img, charuco_corners, charuco_ids, rvec, tvec, 0 - ) + self.visualize_board(img, charuco_corners, charuco_ids, rvec, tvec, 0) if rvec is not None: print(json.dumps({"rvec": rvec.tolist(), "tvec": tvec.tolist()})) @@ -246,9 +240,7 @@ def detect_board_in_image(self, filename, visualize=False): return rvec, tvec - def detect_board_in_files( - self, files: typing.List[str], visualize: bool = False - ): + def detect_board_in_files(self, files: typing.List[str], visualize: bool = False): """Detect the board in multiple files. Tries to detect the Charuco board in the given list of image files. @@ -312,9 +304,7 @@ def calibrate( self.camera_matrix = None self.dist_coeffs = None - all_corners, all_ids, image_size = self.detect_board_in_files( - files, visualize - ) + all_corners, all_ids, image_size = self.detect_board_in_files(files, visualize) camera_matrix = np.zeros((3, 3)) dist_coeffs = np.zeros(4) diff --git a/scripts/analyze_tricamera_log.py b/scripts/analyze_tricamera_log.py index 277779c..f1bbba6 100644 --- a/scripts/analyze_tricamera_log.py +++ b/scripts/analyze_tricamera_log.py @@ -38,12 +38,7 @@ def main(): print("Total duration: {:.1f} seconds".format(duration)) stamps = [ - np.array( - [ - observation.cameras[c].timestamp - for observation in log_reader.data - ] - ) + np.array([observation.cameras[c].timestamp for observation in log_reader.data]) for c in range(3) ] diff --git a/scripts/calibrate_cameras.py b/scripts/calibrate_cameras.py index b53bda1..97ffa8c 100644 --- a/scripts/calibrate_cameras.py +++ b/scripts/calibrate_cameras.py @@ -36,9 +36,7 @@ def calibrate_intrinsic_parameters(calibration_data, calibration_results_file): pattern = os.path.join(calibration_data, "*.png") files = glob.glob(pattern) - camera_matrix, dist_coeffs, error = handler.calibrate( - files, visualize=True - ) + camera_matrix, dist_coeffs, error = handler.calibrate(files, visualize=True) camera_info = dict() camera_info["camera_matrix"] = dict() camera_info["camera_matrix"]["rows"] = 3 @@ -47,9 +45,7 @@ def calibrate_intrinsic_parameters(calibration_data, calibration_results_file): camera_info["distortion_coefficients"] = dict() camera_info["distortion_coefficients"]["rows"] = 1 camera_info["distortion_coefficients"]["cols"] = 5 - camera_info["distortion_coefficients"][ - "data" - ] = dist_coeffs.flatten().tolist() + camera_info["distortion_coefficients"]["data"] = dist_coeffs.flatten().tolist() with open(calibration_results_file, "w") as outfile: yaml.dump( @@ -111,9 +107,7 @@ def config_matrix(data): calibration_data["projection_matrix"] = dict() calibration_data["projection_matrix"]["rows"] = 4 calibration_data["projection_matrix"]["cols"] = 4 - calibration_data["projection_matrix"][ - "data" - ] = projection_matrix.flatten().tolist() + calibration_data["projection_matrix"]["data"] = projection_matrix.flatten().tolist() with open(extrinsic_calibration_filename, "w") as outfile: yaml.dump( @@ -258,9 +252,7 @@ def calibrate_mean_extrinsic_parameters( # x-distance of the pattern origin to the world origin tx = Dx - dx # y-distance of the pattern origin to the world origin - ty = (Dy - T2 * np.sin(alphr) - T1 * np.tan(alphr)) * np.cos( - alphr - ) - dy + ty = (Dy - T2 * np.sin(alphr) - T1 * np.tan(alphr)) * np.cos(alphr) - dy # z-distance of the pattern origin to the world origin tz = ( T2 @@ -283,9 +275,9 @@ def calibrate_mean_extrinsic_parameters( # absolute world vectors equivalent to cv2 tvec and rvec: tvecW = np.matmul(cv2.Rodrigues(rvec)[0], Tvec) + tvec.T - rvecW = cv2.Rodrigues( - np.matmul(cv2.Rodrigues(rvec)[0], np.matmul(zMat, xMat)) - )[0] + rvecW = cv2.Rodrigues(np.matmul(cv2.Rodrigues(rvec)[0], np.matmul(zMat, xMat)))[ + 0 + ] # embed() projection_matrix[ind, 0:4, 0:4] = utils.rodrigues_to_matrix(rvecW) @@ -406,22 +398,16 @@ def calibrate_mean_extrinsic_parameters( calibration_data["camera_matrix"] = dict() calibration_data["camera_matrix"]["rows"] = 3 calibration_data["camera_matrix"]["cols"] = 3 - calibration_data["camera_matrix"][ - "data" - ] = camera_matrix.flatten().tolist() + calibration_data["camera_matrix"]["data"] = camera_matrix.flatten().tolist() calibration_data["distortion_coefficients"] = dict() calibration_data["distortion_coefficients"]["rows"] = 1 calibration_data["distortion_coefficients"]["cols"] = 5 - calibration_data["distortion_coefficients"][ - "data" - ] = dist_coeffs.flatten().tolist() + calibration_data["distortion_coefficients"]["data"] = dist_coeffs.flatten().tolist() calibration_data["projection_matrix"] = dict() calibration_data["projection_matrix"]["rows"] = 4 calibration_data["projection_matrix"]["cols"] = 4 - calibration_data["projection_matrix"][ - "data" - ] = projection_matrix.flatten().tolist() + calibration_data["projection_matrix"]["data"] = projection_matrix.flatten().tolist() calibration_data["projection_matrix_std"] = dict() calibration_data["projection_matrix_std"]["rows"] = 4 @@ -502,10 +488,7 @@ def main(): ) elif args.action == "extrinsic_mean": - if ( - not args.calibration_data - and not args.intrinsic_calibration_filename - ): + if not args.calibration_data and not args.intrinsic_calibration_filename: raise RuntimeError( "neither calibration_data nor intrinsic_calibration_filename not specified." ) @@ -518,14 +501,10 @@ def main(): calibration_data = yaml.safe_load(file) def config_matrix(data): - return np.array(data["data"]).reshape( - data["rows"], data["cols"] - ) + return np.array(data["data"]).reshape(data["rows"], data["cols"]) camera_matrix = config_matrix(calibration_data["camera_matrix"]) - dist_coeffs = config_matrix( - calibration_data["distortion_coefficients"] - ) + dist_coeffs = config_matrix(calibration_data["distortion_coefficients"]) else: camera_matrix, dist_coeffs = calibrate_intrinsic_parameters( args.calibration_data, args.extrinsic_calibration_filename diff --git a/scripts/calibrate_trifingerpro_cameras.py b/scripts/calibrate_trifingerpro_cameras.py index 2e5e1ad..4de19eb 100644 --- a/scripts/calibrate_trifingerpro_cameras.py +++ b/scripts/calibrate_trifingerpro_cameras.py @@ -75,9 +75,7 @@ def calibrate_intrinsic_parameters( camera_info["distortion_coefficients"] = dict() camera_info["distortion_coefficients"]["rows"] = 1 camera_info["distortion_coefficients"]["cols"] = 5 - camera_info["distortion_coefficients"][ - "data" - ] = dist_coeffs.flatten().tolist() + camera_info["distortion_coefficients"]["data"] = dist_coeffs.flatten().tolist() with open(calibration_results_file, "w") as outfile: yaml.dump( @@ -164,9 +162,7 @@ def calibrate_mean_extrinsic_parameters( # x-distance of the pattern origin to the world origin tx = Dx - dx # y-distance of the pattern origin to the world origin - ty = (Dy - T2 * np.sin(alphr) - T1 * np.tan(alphr)) * np.cos( - alphr - ) - dy + ty = (Dy - T2 * np.sin(alphr) - T1 * np.tan(alphr)) * np.cos(alphr) - dy # z-distance of the pattern origin to the world origin tz = ( T2 @@ -189,9 +185,9 @@ def calibrate_mean_extrinsic_parameters( # absolute world vectors equivalent to cv2 tvec and rvec: tvecW = np.matmul(cv2.Rodrigues(rvec)[0], Tvec) + tvec.T - rvecW = cv2.Rodrigues( - np.matmul(cv2.Rodrigues(rvec)[0], np.matmul(zMat, xMat)) - )[0] + rvecW = cv2.Rodrigues(np.matmul(cv2.Rodrigues(rvec)[0], np.matmul(zMat, xMat)))[ + 0 + ] # embed() pose_matrix[ind, 0:4, 0:4] = utils.rodrigues_to_matrix(rvecW) @@ -307,9 +303,7 @@ def calibrate_mean_extrinsic_parameters( print("Std proj matrix:") print(camera_params.tf_world_to_camera_std) print("Rel std proj matrix:") - print( - camera_params.tf_world_to_camera_std / camera_params.tf_world_to_camera - ) + print(camera_params.tf_world_to_camera_std / camera_params.tf_world_to_camera) return camera_params @@ -417,9 +411,7 @@ def config_matrix(data): return np.array(data["data"]).reshape(data["rows"], data["cols"]) camera_matrix = config_matrix(calibration_data["camera_matrix"]) - dist_coeffs = config_matrix( - calibration_data["distortion_coefficients"] - ) + dist_coeffs = config_matrix(calibration_data["distortion_coefficients"]) else: camera_matrix, dist_coeffs = calibrate_intrinsic_parameters( image_files, output_file_full, args.visualize diff --git a/scripts/check_camera_sharpness.py b/scripts/check_camera_sharpness.py index f196479..98056e2 100644 --- a/scripts/check_camera_sharpness.py +++ b/scripts/check_camera_sharpness.py @@ -61,9 +61,7 @@ def add_label( return image -def auto_canny_params( - image: np.ndarray, sigma: float = 0.33 -) -> tuple[float, float]: +def auto_canny_params(image: np.ndarray, sigma: float = 0.33) -> tuple[float, float]: median = np.median(image) lower = max(0, (1.0 - sigma) * median) upper = min(255, (1.0 + sigma) * median) @@ -152,9 +150,7 @@ def main() -> None: max(200, params.canny_threshold1), params.set_canny_threshold1, ) - cv2.setTrackbarPos( - "Canny threshold 1", window_name, int(params.canny_threshold1) - ) + cv2.setTrackbarPos("Canny threshold 1", window_name, int(params.canny_threshold1)) cv2.createTrackbar( "Canny threshold 2", window_name, @@ -162,9 +158,7 @@ def main() -> None: max(400, params.canny_threshold2), params.set_canny_threshold2, ) - cv2.setTrackbarPos( - "Canny threshold 2", window_name, int(params.canny_threshold2) - ) + cv2.setTrackbarPos("Canny threshold 2", window_name, int(params.canny_threshold2)) cv2.createTrackbar( "Edge mean threshold", window_name, @@ -209,8 +203,7 @@ def main() -> None: add_label(image, args.camera_id, "top") add_label( image, - "Canny mean: %.1f | smoothed: %.1f" - % (edges_mean, mean_buffer_mean), + "Canny mean: %.1f | smoothed: %.1f" % (edges_mean, mean_buffer_mean), "bottom", ) diff --git a/scripts/detect_aruco_marker.py b/scripts/detect_aruco_marker.py index e1c3d7b..e4b1f97 100644 --- a/scripts/detect_aruco_marker.py +++ b/scripts/detect_aruco_marker.py @@ -4,9 +4,7 @@ def main(): # ArUco stuff - marker_dict = cv2.aruco.getPredefinedDictionary( - cv2.aruco.DICT_APRILTAG_16h5 - ) + marker_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_APRILTAG_16h5) cap = cv2.VideoCapture(0) while True: diff --git a/scripts/overlay_real_and_rendered_images.py b/scripts/overlay_real_and_rendered_images.py index 0699228..a4c9e77 100644 --- a/scripts/overlay_real_and_rendered_images.py +++ b/scripts/overlay_real_and_rendered_images.py @@ -68,9 +68,7 @@ def main(): finger.reset_finger_positions_and_velocities([-1.57, 1.57, -2.7] * 3) # load real images - image_dir = ( - args.image_dir if args.image_dir else args.config_dir[0] / "0001" - ) + image_dir = args.image_dir if args.image_dir else args.config_dir[0] / "0001" print("image_dir:", image_dir) real_images = [ cv2.imread(str(image_dir / "camera60.png")), @@ -94,8 +92,7 @@ def on_trackbar(val): sim_images = cameras.get_images() # images are rgb --> convert to bgr for opencv sim_images = [ - cv2.cvtColor(np.array(img), cv2.COLOR_RGB2BGR) - for img in sim_images + cv2.cvtColor(np.array(img), cv2.COLOR_RGB2BGR) for img in sim_images ] sim_images = np.hstack(sim_images) @@ -106,9 +103,7 @@ def on_trackbar(val): cv2.namedWindow(title_window) trackbar_name = "Alpha x %d" % SLIDER_MAX - cv2.createTrackbar( - trackbar_name, title_window, 0, SLIDER_MAX, on_trackbar - ) + cv2.createTrackbar(trackbar_name, title_window, 0, SLIDER_MAX, on_trackbar) # Show some stuff blend.on_trackbar(0) diff --git a/scripts/record_image_dataset.py b/scripts/record_image_dataset.py index a357b94..3bdc0a9 100644 --- a/scripts/record_image_dataset.py +++ b/scripts/record_image_dataset.py @@ -124,9 +124,7 @@ def main(): camera_module = trifinger_cameras.camera image_saver = SingleImageSaver(args.outdir, args.camera_id) elif args.driver == "opencv": - camera_driver = trifinger_cameras.camera.OpenCVDriver( - int(args.camera_id) - ) + camera_driver = trifinger_cameras.camera.OpenCVDriver(int(args.camera_id)) camera_module = trifinger_cameras.camera image_saver = SingleImageSaver(args.outdir, args.camera_id) @@ -147,9 +145,7 @@ def main(): observation = camera_frontend.get_latest_observation() if args.driver == "tri": for i, name in enumerate(camera_names): - image = utils.convert_image( - observation.cameras[i].image - ) + image = utils.convert_image(observation.cameras[i].image) cv2.imshow(name, image) else: image = utils.convert_image(observation.image) diff --git a/scripts/tricamera_log_viewer.py b/scripts/tricamera_log_viewer.py index 67d792a..ecc64b4 100644 --- a/scripts/tricamera_log_viewer.py +++ b/scripts/tricamera_log_viewer.py @@ -37,15 +37,9 @@ def main(): window_60 = "Image Stream camera60" window_180 = "Image Stream camera180" window_300 = "Image Stream camera300" - cv2.imshow( - window_60, utils.convert_image(observation.cameras[0].image) - ) - cv2.imshow( - window_180, utils.convert_image(observation.cameras[1].image) - ) - cv2.imshow( - window_300, utils.convert_image(observation.cameras[2].image) - ) + cv2.imshow(window_60, utils.convert_image(observation.cameras[0].image)) + cv2.imshow(window_180, utils.convert_image(observation.cameras[1].image)) + cv2.imshow(window_300, utils.convert_image(observation.cameras[2].image)) # stop if either "q" or ESC is pressed if cv2.waitKey(interval) in [ord("q"), 27]: # 27 = ESC diff --git a/scripts/tricamera_monitor_rate.py b/scripts/tricamera_monitor_rate.py index 1518d8c..fb858ca 100644 --- a/scripts/tricamera_monitor_rate.py +++ b/scripts/tricamera_monitor_rate.py @@ -45,16 +45,12 @@ def main() -> None: if args.multi_process: logging.debug("Start multi-process data") - camera_data = trifinger_cameras.tricamera.MultiProcessData( - "tricamera", False - ) + camera_data = trifinger_cameras.tricamera.MultiProcessData("tricamera", False) else: logging.debug("Start single-process data") camera_data = trifinger_cameras.tricamera.SingleProcessData() logging.debug("Start back end") - camera_driver = trifinger_cameras.tricamera.TriCameraDriver( - *camera_names - ) + camera_driver = trifinger_cameras.tricamera.TriCameraDriver(*camera_names) camera_backend = trifinger_cameras.tricamera.Backend( # noqa: F841 camera_driver, camera_data ) diff --git a/tests/test_camera_calibration_file.py b/tests/test_camera_calibration_file.py index 9cf6217..6f668a0 100644 --- a/tests/test_camera_calibration_file.py +++ b/tests/test_camera_calibration_file.py @@ -45,9 +45,7 @@ def test_file(self): np.testing.assert_array_equal( ccf["camera_matrix"], - np.array( - [[586.36, 0.0, 360.46], [0.0, 590.61, 286.55], [0.0, 0.0, 1.0]] - ), + np.array([[586.36, 0.0, 360.46], [0.0, 590.61, 286.55], [0.0, 0.0, 1.0]]), ) np.testing.assert_array_equal( diff --git a/tests/test_utils.py b/tests/test_utils.py index 7d851bc..05abf94 100644 --- a/tests/test_utils.py +++ b/tests/test_utils.py @@ -18,9 +18,7 @@ def test_rodrigues_to_matrix(self): ] ) - np.testing.assert_array_almost_equal( - utils.rodrigues_to_matrix(rotvec), mat - ) + np.testing.assert_array_almost_equal(utils.rodrigues_to_matrix(rotvec), mat) def test_convert_image(self): # NOTE: this is not testing if the result is correct but simply uses @@ -192,15 +190,9 @@ def test_convert_image(self): ) np.testing.assert_array_equal(utils.convert_image(raw_image), img_bgr) - np.testing.assert_array_equal( - utils.convert_image(raw_image, "bgr"), img_bgr - ) - np.testing.assert_array_equal( - utils.convert_image(raw_image, "rgb"), img_rgb - ) - np.testing.assert_array_equal( - utils.convert_image(raw_image, "gray"), img_gray - ) + np.testing.assert_array_equal(utils.convert_image(raw_image, "bgr"), img_bgr) + np.testing.assert_array_equal(utils.convert_image(raw_image, "rgb"), img_rgb) + np.testing.assert_array_equal(utils.convert_image(raw_image, "gray"), img_gray) # verify that invalid formats result in an error with self.assertRaises(ValueError):