Expose pose ambiguity (#483)

* Expose pose ambiguity

* Run spotless

* Add tooltips and expose number of iterations
This commit is contained in:
Matt
2022-10-08 09:27:00 -04:00
committed by GitHub
parent dafee954e0
commit 2c6b0ddac3
11 changed files with 97 additions and 9 deletions

View File

@@ -90,6 +90,12 @@ class PhotonTrackedTarget {
return corners;
}
/**
* Get the ratio of pose reprojection errors, called ambiguity. Numbers above
* 0.2 are likely to be ambiguous. -1 if invalid.
*/
double GetPoseAmbiguity() const { return poseAmbiguity; }
/**
* Returns the pose of the target relative to the robot.
* @return The pose of the target relative to the robot.
@@ -109,6 +115,7 @@ class PhotonTrackedTarget {
double skew = 0;
int fiducialId;
frc::Transform3d cameraToTarget;
double poseAmbiguity;
wpi::SmallVector<std::pair<double, double>, 4> corners;
};
} // namespace photonlib