diff --git a/comp/vision/manual_calibrator.py b/comp/vision/manual_calibrator.py new file mode 100644 index 000000000..c21c70bd3 --- /dev/null +++ b/comp/vision/manual_calibrator.py @@ -0,0 +1,224 @@ +""" +MANUAL CALIBRATOR + +This simply finds tags in the viewport and displays their centers in pixels and meters. + +It uses 20% scaled tags 33.02 mm +""" +# pylint: disable=missing-module-docstring +# pylint: disable=missing-function-docstring +# pylint: disable=missing-class-docstring +# pylint: disable=import-error + + +import cv2 +import sys +import libcamera +import numpy as np + +import robotpy_apriltag + +from cscore import CameraServer +from picamera2 import Picamera2 +from wpimath.geometry import Transform3d + + +class TagFinder: + def __init__(self, width, height, model): + self.width = width + self.height = height + self.model = model + + self.at_detector = robotpy_apriltag.AprilTagDetector() + self.at_detector.addFamily("tag36h11") + + # i think we should just focus on k1 and k2, and assume zero tangential, and ignore the higher order k factors. + + if self.model == "imx708_wide": + print("V3 WIDE CAMERA") + self.mtx = np.array([[497, 0, 578], [0, 498, 328], [0, 0, 1]]) + self.dist = np.array( + [ + [ + -1.18341279e00, + 7.13453990e-01, + 7.90204163e-04, + -7.38879856e-04, + -2.94529084e-03, + -1.14073111e00, + 6.16356154e-01, + 5.86094708e-02, + ] + ] + ) + self.estimator = robotpy_apriltag.AprilTagPoseEstimator( + robotpy_apriltag.AprilTagPoseEstimator.Config( + 0.1651, # tagsize 6.5 inches + 497, # fx + 498, # fy + width / 2, # cx + height / 2, # cy + ) + ) + elif self.model == "imx219": + print("V2 CAMERA (NOT WIDE ANGLE)") + self.mtx = np.array([[658, 0, 422], [0, 660, 318], [0, 0, 1]]) + self.dist = np.array( + [ + [ + 2.26767723e-02, + 3.92792657e01, + 5.34833047e-04, + -1.76949201e-03, + -6.59779907e01, + -5.75883422e-02, + 3.81831051e01, + -6.37029103e01, + ] + ] + ) + self.estimator = robotpy_apriltag.AprilTagPoseEstimator( + robotpy_apriltag.AprilTagPoseEstimator.Config( + 0.1651, # tagsize 6.5 inches + 658, # fx + 660, # fy + width / 2, # cx + height / 2, # cy + ) + ) + else: + print("UNKNOWN CAMERA MODEL") + sys.exit() + + self.output_stream = CameraServer.putVideo("Processed", width, height) + + def analyze(self, request): + buffer = request.make_buffer("lores") + + y_len = self.width * self.height + + # truncate, ignore chrominance. this makes a view, very fast (300 ns) + img = np.frombuffer(buffer, dtype=np.uint8, count=y_len) + + # this makes a view, very fast (150 ns) + img = img.reshape((self.height, self.width)) + + # for calibration we don't undistort + # turn this on to check the calibration! + # img = cv2.undistort(img, self.mtx, self.dist) + + result = self.at_detector.detect(img) + + for result_item in result: + if result_item.getHamming() > 0: + continue + pose = self.estimator.estimate(result_item) + self.draw_result(img, result_item, pose) + + self.output_stream.putFrame(img) + + def draw_result(self, image, result_item, pose: Transform3d): + color = (255, 255, 255) + + # Draw lines around the tag + for i in range(4): + j = (i + 1) % 4 + point1 = (int(result_item.getCorner(i).x), int(result_item.getCorner(i).y)) + point2 = (int(result_item.getCorner(j).x), int(result_item.getCorner(j).y)) + cv2.line(image, point1, point2, color, 2) + + (c_x, c_y) = (int(result_item.getCenter().x), int(result_item.getCenter().y)) + cv2.circle(image, (c_x, c_y), 10, (255, 255, 255), -1) + self.draw_text(image, f"cx {c_x} cy {c_y}", (c_x, c_y)) + if pose is not None: + t = pose.translation() + self.draw_text( + image, + f"t: {t.x:4.1f},{t.y:4.1f},{t.z:4.1f}", + (c_x - 50, c_y + 40), + ) + + # these are white with black outline + def draw_text(self, image, msg, loc): + cv2.putText(image, msg, loc, cv2.FONT_HERSHEY_SIMPLEX, 1.5, (0, 0, 0), 6) + cv2.putText(image, msg, loc, cv2.FONT_HERSHEY_SIMPLEX, 1.5, (255, 255, 255), 2) + + +def main(): + camera = Picamera2() + + model = camera.camera_properties["Model"] + print("\nMODEL " + model) + + if model == "imx708_wide": + print("V3 Wide Camera") + # full frame is 4608x2592; this is 2x2 + fullwidth = 2304 + fullheight = 1296 + # medium detection resolution, compromise speed vs range + width = 1152 + height = 648 + elif model == "imx219": + print("V2 Camera") + # full frame, 2x2, to set the detector mode to widest angle possible + fullwidth = 1664 # slightly larger than the detector, to match stride + fullheight = 1232 + # medium detection resolution, compromise speed vs range + width = 832 + height = 616 + else: + print("UNKNOWN CAMERA MODEL: " + model) + sys.exit() + + camera_config = camera.create_still_configuration( + # 2 buffers => low latency (32-48 ms), low fps (15-20) + # 5 buffers => mid latency (40-55 ms), high fps (22-28) + # 3 buffers => high latency (50-70 ms), mid fps (20-23) + # robot goes at 50 fps, so roughly a frame every other loop + # fps doesn't matter much, so minimize latency + buffer_count=2, + main={ + "format": "YUV420", + "size": (fullwidth, fullheight), + }, + lores={"format": "YUV420", "size": (width, height)}, + controls={ + # these manual controls are useful sometimes but turn them off for now + # because auto mode seems fine + # fast shutter means more gain + # "AnalogueGain": 8.0, + # try faster shutter to reduce blur. with 3ms, 3 rad/s seems ok. + "ExposureTime": 3000, + # limit auto: go as fast as possible but no slower than 30fps + # without a duration limit, we slow down in the dark, which is fine + # "FrameDurationLimits": (5000, 33333), # 41 fps + # noise reduction takes time, don't need it. + "NoiseReductionMode": libcamera.controls.draft.NoiseReductionModeEnum.Off, + }, + ) + + print("\nREQUESTED CONFIG") + print(camera_config) + camera.align_configuration(camera_config) + print("\nALIGNED CONFIG") + print(camera_config) + camera.configure(camera_config) + print("\nCONTROLS") + print(camera.camera_controls) + output = TagFinder(width, height, model) + + camera.start() + try: + while True: + # the most recent completed frame, from the recent past + request = camera.capture_request() + try: + output.analyze(request) + finally: + # the frame is owned by the camera so remember to release it + request.release() + finally: + camera.stop() + + +main()