Advanced strategies for using vision processing results involve working with the robot’s pose on the field. A pose is a combination an X/Y coordinate, and an angle describing where the robot’s front is pointed. It is always considered relative to some fixed point on the field.
WPILib provides a Pose2d class to describe poses in software.
Knowledge and Equipment Needed
A Coprocessor running PhotonVision - Accurate camera calibration to support “3D mode” required
A Drivetrain with wheels and sensors - Sufficient sensors to measure wheel rotation - Capable of closed-loop velocity control
A gyroscope or IMU measuring actual robot heading
Experience using some path-planning library (WPILib is our recommendation)
Path Planning in a Target-Centered Reference Frame
When using 3D mode in PhotonVision, the SolvePNP Algorithm is used to deduce the camera's position in a 3D coordinate system centered on the target itself.
A simple algorithm for using this measurement is:
Assume your robot needs to be at a fixed
Pose2Drelative to the target.
When triggered: #. Read the most recent vision measurement - this is your actual pose. #. Generate a simple trajectory to the goal position #. Execute the trajectory
There is not currently an example demonstrating this technique.
Global Pose Estimation
A more complex way to utilize a camera-supplied
Pose2D is to incorporate it into an estimation of the robot’s
Pose2D in a global field reference frame.
When using this strategy, the measurements made by the camera are fused with measurements from other sensors, a model of expected robot behavior, and a matrix of weights that describes how trustworthy each sensor is. The result is a best-guess at the current pose on the field.
In turn, this best-guess position is used to path plan to the known positions on the field, which may or may not have vision targets nearby.
See the Pose Estimation example for more information.