Optical Flow Script (Pioneer Team Spring 2025)

    def _rotation_vector_to_euler_angles(self, rvec):
        R, _ = cv2.Rodrigues(rvec)
        sy = math.sqrt(R[0, 0]**2 + R[1, 0]**2)
        singular = sy < 1e-6

        if not singular:
            x = math.atan2(R[2, 1], R[2, 2])
            y = math.atan2(-R[2, 0], sy)
            z = math.atan2(R[1, 0], R[0, 0])
        else:
            x = math.atan2(-R[1, 2], R[1, 1])
            y = math.atan2(-R[2, 0], sy)
            z = 0

        return (math.degrees(x), math.degrees(y), math.degrees(z))


    def _calculate_reprojection_error(self, obj_points, img_points, rvecs, tvecs):
        mean_error = 0
        for i in range(len(obj_points)):
            img_points2, _ = cv2.projectPoints(
                obj_points[i], rvecs[i], tvecs[i], self.camera_matrix, self.dist_coeffs)
            error = cv2.norm(img_points[i], img_points2, cv2.NORM_L2) / len(img_points2)
            mean_error += error
        return mean_error / len(obj_points)

 def auto_tilt_servo_alignment(self, servo_pin=1, neutral_angle=0, max_correction=90):
        if not self.tilt_angles:
            raise RuntimeError("No tilt angles available. Run calibrate_camera() first.")
        px = Picarx()
        pitch_angles = [angle[1] for angle in self.tilt_angles]
        avg_pitch = np.mean(pitch_angles)
        
        correction = -avg_pitch
        correction = np.clip(correction, -max_correction, max_correction)
        
        target_angle = np.clip(neutral_angle + correction, 0, 180)
        print(f"Moving tilt servo to {avg_pitch:.2f}° to correct pitch...")
        
        from gpiozero import AngularServo
        servo = AngularServo(servo_pin, min_angle=0, max_angle=180,
                             min_pulse_width=0.5/1000, max_pulse_width=2.5/1000)
        servo.angle = target_angle 
        px.set_cam_tilt_angle(target_angle)   
        time.sleep(0.5)
        px.stop()
        print("Tilt servo adjustment complete.")
        print(f"Adjusting pan servo to: {target_angle}")

        
    def auto_pan_servo_alignment(self, servo_pin=0, neutral_angle=0, max_correction=90):
        if not self.tilt_angles:
            raise RuntimeError("No tilt angles available. Run calibrate_camera() first.")
        px = Picarx()
        yaw_angles = [angle[2] for angle in self.tilt_angles]
        avg_yaw = np.mean(yaw_angles)

        
        correction = -avg_yaw
        correction = np.clip(correction, -max_correction, max_correction)
        
        target_angle = np.clip(neutral_angle + correction, 0, 180)
        print(f"Moving tilt servo to {target_angle:.4f}° to correct pitch...")
        
        from gpiozero import AngularServo
        servo = AngularServo(servo_pin, min_angle=0, max_angle=180,
                             min_pulse_width=0.5/1000, max_pulse_width=2.5/1000)
        servo.angle = target_angle
        px.set_cam_pan_angle(target_angle)
        time.sleep(0.5)
        px.stop()                     
        print("Pan servo adjustment complete.")
        print(f"Adjusting pan servo to: {target_angle}")
        
    def correct_roll_only(self, image):
        if not self.calibrated:
            raise RuntimeError("Camera not calibrated")

        if hasattr(self, 'avg_roll') and abs(self.avg_roll) > 0.5:
            h, w = image.shape[:2]
            center = (w//2, h//2)
            M = cv2.getRotationMatrix2D(center, angle = 90-self.avg_roll, scale = 1.0)
            image = cv2.warpAffine(image, M, (w,h), flags=cv2.INTER_LINEAR, borderMode=cv2.BORDER_REPLICATE)
        return image