Return named type from PhotonPoseEstimator (#734)

Adds PhotonPoseEstimator class, and deprecates RobotPoseEstimator

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

@@ -48,7 +48,10 @@ enum PoseStrategy : int {
 };
 
 /**
- * A managing class to determine how an estimated pose should be chosen.
+ * The RobotPoseEstimator class filters or combines readings from all the
+ * fiducials visible at a given timestamp on the field to produce a single robot
+ * in field pose, using the strategy set below. Example usage can be found in
+ * our apriltagExample example project.
  */
 class RobotPoseEstimator {
  public:
@@ -59,44 +62,81 @@ class RobotPoseEstimator {
   /**
    * Create a new RobotPoseEstimator.
    *
-   * @param aprilTags A WPILib {@link AprilTagFieldLayout} linking AprilTag IDs
-   * to Pose3ds with respect to the FIRST field.
+   * <p>Example: {@code <code> <p> Map<Integer, Pose3d> map = new HashMap<>();
+   * <p> map.put(1, new Pose3d(1.0, 2.0, 3.0, new Rotation3d())); // Tag ID 1 is
+   * at (1.0,2.0,3.0) </code> }
+   *
+   * @param aprilTags A AprilTagFieldLayout linking AprilTag IDs to Pose3ds with
+   * respect to the FIRST field.
    * @param strategy The strategy it should use to determine the best pose.
    * @param cameras An ArrayList of Pairs of PhotonCameras and their respective
-   * Transform3ds from the center of the robot to the camera mount positions
-   * (ie, robot -> camera).
+   * Transform3ds from the center of the robot to the cameras.
    */
   explicit RobotPoseEstimator(
       std::shared_ptr<frc::AprilTagFieldLayout> aprilTags,
       PoseStrategy strategy, std::vector<map_value_type> cameras);
 
   /**
-   * Update the estimated pose using the selected strategy.
+   * Get the AprilTagFieldLayout being used by the PositionEstimator.
    *
-   * @return The updated estimated pose and the latency in milliseconds.
+   * @return the AprilTagFieldLayout
    */
-  std::pair<frc::Pose3d, units::millisecond_t> Update();
-
-  inline void SetPoseStrategy(PoseStrategy strat) { strategy = strat; }
-
-  inline void SetReferencePose(frc::Pose3d referencePose) {
-    this->referencePose = referencePose;
+  std::shared_ptr<frc::AprilTagFieldLayout> getFieldLayout() const {
+    return aprilTags;
   }
 
-  inline void SetLastPose(frc::Pose3d lastPose) { this->lastPose = lastPose; }
-
+  /**
+   * Set the cameras to be used by the PoseEstimator.
+   *
+   * @param cameras cameras to set.
+   */
   inline void SetCameras(
       const std::vector<std::pair<std::shared_ptr<PhotonCamera>,
                                   frc::Transform3d>>& cameras) {
     this->cameras = cameras;
   }
 
+  /**
+   * Get the Position Estimation Strategy being used by the Position Estimator.
+   *
+   * @return the strategy
+   */
   PoseStrategy GetPoseStrategy() const { return strategy; }
 
-  frc::Pose3d GetLastPose() const { return lastPose; }
+  /**
+   * Set the Position Estimation Strategy used by the Position Estimator.
+   *
+   * @param strategy the strategy to set
+   */
+  inline void SetPoseStrategy(PoseStrategy strat) { strategy = strat; }
 
+  /**
+   * Return the reference position that is being used by the estimator.
+   *
+   * @return the referencePose
+   */
   frc::Pose3d GetReferencePose() const { return referencePose; }
 
+  /**
+   * Update the stored reference pose for use when using the
+   * CLOSEST_TO_REFERENCE_POSE strategy.
+   *
+   * @param referencePose the referencePose to set
+   */
+  inline void SetReferencePose(frc::Pose3d referencePose) {
+    this->referencePose = referencePose;
+  }
+
+  /**
+   * Update the stored last pose. Useful for setting the initial estimate when
+   * using the CLOSEST_TO_LAST_POSE strategy.
+   *
+   * @param lastPose the lastPose to set
+   */
+  inline void SetLastPose(frc::Pose3d lastPose) { this->lastPose = lastPose; }
+
+  std::pair<frc::Pose3d, units::second_t> Update();
+
  private:
   std::shared_ptr<frc::AprilTagFieldLayout> aprilTags;
   PoseStrategy strategy;
@@ -104,13 +144,44 @@ class RobotPoseEstimator {
   frc::Pose3d lastPose;
   frc::Pose3d referencePose;
 
-  std::pair<frc::Pose3d, units::millisecond_t> LowestAmbiguityStrategy();
+  /**
+   * Return the estimated position of the robot with the lowest position
+   * ambiguity from a List of pipeline results.
+   *
+   * @return the estimated position of the robot in the FCS and the estimated
+   * timestamp of this estimation.
+   */
+  std::pair<frc::Pose3d, units::second_t> LowestAmbiguityStrategy();
 
-  std::pair<frc::Pose3d, units::millisecond_t> ClosestToCameraHeightStrategy();
+  /**
+   * Return the estimated position of the robot using the target with the lowest
+   * delta height difference between the estimated and actual height of the
+   * camera.
+   *
+   * @return the estimated position of the robot in the FCS and the estimated
+   * timestamp of this estimation.
+   */
+  std::pair<frc::Pose3d, units::second_t> ClosestToCameraHeightStrategy();
 
-  std::pair<frc::Pose3d, units::millisecond_t> ClosestToReferencePoseStrategy();
+  /**
+   * Return the estimated position of the robot using the target with the lowest
+   * delta in the vector magnitude between it and the reference pose.
+   *
+   * @param referencePose reference pose to check vector magnitude difference
+   * against.
+   * @return the estimated position of the robot in the FCS and the estimated
+   * timestamp of this estimation.
+   */
+  std::pair<frc::Pose3d, units::second_t> ClosestToReferencePoseStrategy();
 
-  std::pair<frc::Pose3d, units::millisecond_t> AverageBestTargetsStrategy();
+  /**
+   * Return the average of the best target poses using ambiguity as weight.
+
+   * @return the estimated position of the robot in the FCS and the estimated
+   timestamp of this
+   *     estimation.
+   */
+  std::pair<frc::Pose3d, units::second_t> AverageBestTargetsStrategy();
 };
 
 }  // namespace photonlib