Add RobotPoseEstimator (#571)

RobotPoseEstimator can pick the most likely pose for the robot given a number of possible poses, using a number of different strategies. Examples are still WIP.

photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp

@@ -34,7 +34,8 @@ namespace photonlib {
 
 PhotonTrackedTarget::PhotonTrackedTarget(
     double yaw, double pitch, double area, double skew, int id,
-    const frc::Transform3d& pose,
+    const frc::Transform3d& pose, const frc::Transform3d& alternatePose,
+    double ambiguity,
     const wpi::SmallVector<std::pair<double, double>, 4> corners)
     : yaw(yaw),
       pitch(pitch),
@@ -42,6 +43,8 @@ PhotonTrackedTarget::PhotonTrackedTarget(
       skew(skew),
       fiducialId(id),
       bestCameraToTarget(pose),
+      altCameraToTarget(alternatePose),
+      poseAmbiguity(ambiguity),
       corners(corners) {}
 
 bool PhotonTrackedTarget::operator==(const PhotonTrackedTarget& other) const {