PhotonVision
PhotonLib - Add observation fusion strategies
After https://github.com/PhotonVision/photonvision/pull/521 is merged....
Expand PhotonLib to have utility classes to help users merge the two reported poses for each target to avoid ambiguity issues.
Strategies could support:
- Multiple cameras looking at the same target, and needing to pick the pose which is most common between them
- One camera looking at multiple targets, again needing to pick the most common pose answer
- A camera seeing one target, with an assumed height off the ground, picking the pose which places it to the assumed height
- A camera seeing one target, and picking a pose most similar to the most recently observed pose
- A camera seeing one target, and picking a pose most similar to one provided externally (ie, from previous loop's odometry)
- A camera seeing one target, and picking the pose with the lowest ambiguity.
I'm not entirely certain exactly how to architect this, just stating some requirements to get the ball rolling.
Core design thoughts:
New class could be called PoseDisambiguator or RobotPoseEstimator
It should allow for configuration by which users pass in instances of PhotonCam's, along with a Transform3d representing how the cam is mounted on the robot.
It will also require the user populates a tag dictionary of poses of each tag on the field.
It will need a periodic update() method. This method will have to acquire all results from both cameras and produce a best estimate of where the robot is at.
It should have additional config or input methods where the user can supply the expected robot centroid height, a previous expected pose, allowable range of realistic poses (field peimiter?), interpolation strategies (?), etc.
It should report what method(s) from the above list it used to decide what the most likely pose of the robot is.
I'll work on this. Planned architecture: class RobotPoseEstimator enum PoseStrategy
