Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-20 00:51:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Make Vision pose estimation examples use all vision measurements (#1706)

Browse Source 
Resolves https://github.com/PhotonVision/photonvision/issues/1634

---------

Signed-off-by: Jade Turner <spacey-sooty@proton.me>
Co-authored-by: Sam Freund <techguy763@gmail.com>
This commit is contained in:
Jade
2025-04-15 11:33:42 +08:00
committed by GitHub
parent 63b1ff242c
commit 1c42755451
5 changed files with 46 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
photonlib-cpp-examples/poseest/src/main/cpp/Robot.cpp
View File

@@ -34,15 +34,7 @@ void Robot::RobotInit() {}
void Robot::RobotPeriodic() {
launcher.periodic();
drivetrain.Periodic();
auto visionEst = vision.GetEstimatedGlobalPose();
if (visionEst.has_value()) {
auto est = visionEst.value();
auto estPose = est.estimatedPose.ToPose2d();
auto estStdDevs = vision.GetEstimationStdDevs(estPose);
drivetrain.AddVisionMeasurement(est.estimatedPose.ToPose2d(), est.timestamp,
estStdDevs);
}
vision.Periodic();
drivetrain.Log();
}