Un-finalize robotToCamera, add getters and setters (#789)

Allows teams with a mechanism that moves the camera's position (eg, a pan and tilt mechanism) to update the location of their camera for their pose calculations.

---------

Co-authored-by: Matthew Morley <matthew.morley.ca@gmail.com>

photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java

@@ -67,7 +67,7 @@ public class PhotonPoseEstimator {
     private AprilTagFieldLayout fieldTags;
     private PoseStrategy strategy;
     private final PhotonCamera camera;
-    private final Transform3d robotToCamera;
+    private Transform3d robotToCamera;
 
     private Pose3d lastPose;
     private Pose3d referencePose;
@@ -183,6 +183,21 @@ public class PhotonPoseEstimator {
         setLastPose(new Pose3d(lastPose));
     }
 
+    /** @return The current transform from the center of the robot to the camera mount position */
+    public Transform3d getRobotToCameraTransform() {
+        return robotToCamera;
+    }
+
+    /**
+     * Useful for pan and tilt mechanisms and such.
+     *
+     * @param robotToCamera The current transform from the center of the robot to the camera mount
+     *     position
+     */
+    public void setRobotToCameraTransform(Transform3d robotToCamera) {
+        this.robotToCamera = robotToCamera;
+    }
+
     /**
      * Poll data from the configured cameras and update the estimated position of the robot. Returns
      * empty if there are no cameras set or no targets were found from the cameras.

photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h

@@ -123,6 +123,24 @@ class PhotonPoseEstimator {
     this->referencePose = referencePose;
   }
 
+  /**
+   * @return The current transform from the center of the robot to the camera
+   *         mount position.
+   */
+  inline frc::Transform3d GetRobotToCameraTransform() {
+    return m_robotToCamera;
+  }
+
+  /**
+   * Useful for pan and tilt mechanisms, or cameras on turrets
+   *
+   * @param robotToCamera The current transform from the center of the robot to
+   * the camera mount position.
+   */
+  inline void SetRobotToCameraTransform(frc::Transform3d robotToCamera) {
+    m_robotToCamera = robotToCamera;
+  }
+
   /**
    * Update the stored last pose. Useful for setting the initial estimate when
    * using the CLOSEST_TO_LAST_POSE strategy.