Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-22 01:11:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Expose pose ambiguity (#483)

Browse Source 
* 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
Download Patch File Download Diff File Expand all files Collapse all files

5
photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp
View File

@@ -62,7 +62,8 @@ Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target) {
<< target.cameraToTarget.Rotation().GetQuaternion().W()
<< target.cameraToTarget.Rotation().GetQuaternion().X()
<< target.cameraToTarget.Rotation().GetQuaternion().Y()
<< target.cameraToTarget.Rotation().GetQuaternion().Z();
<< target.cameraToTarget.Rotation().GetQuaternion().Z()
<< target.poseAmbiguity;
for (int i = 0; i < 4; i++) {
packet << target.corners[i].first << target.corners[i].second;
@@ -86,6 +87,8 @@ Packet& operator>>(Packet& packet, PhotonTrackedTarget& target) {
target.cameraToTarget = frc::Transform3d(translation, rotation);
packet >> target.poseAmbiguity;
target.corners.clear();
for (int i = 0; i < 4; i++) {
double first = 0;

7
photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h
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