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

Expose both pose solutions (#521)

Browse Source 
* Half-add second pose

* add c++

* run wpiformat

* Fix c++
This commit is contained in:
Matt
2022-10-22 07:42:45 -04:00
committed by GitHub
parent 27198a3e32
commit 2d7a88e231
14 changed files with 142 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -41,12 +41,12 @@ PhotonTrackedTarget::PhotonTrackedTarget(
area(area),
skew(skew),
fiducialId(id),
cameraToTarget(pose),
bestCameraToTarget(pose),
corners(corners) {}
bool PhotonTrackedTarget::operator==(const PhotonTrackedTarget& other) const {
return other.yaw == yaw && other.pitch == pitch && other.area == area &&
other.skew == skew && other.cameraToTarget == cameraToTarget &&
other.skew == skew && other.bestCameraToTarget == bestCameraToTarget &&
other.corners == corners;
}
@@ -56,13 +56,21 @@ bool PhotonTrackedTarget::operator!=(const PhotonTrackedTarget& other) const {
Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target) {
packet << target.yaw << target.pitch << target.area << target.skew
<< target.fiducialId << target.cameraToTarget.Translation().X().value()
<< target.cameraToTarget.Translation().Y().value()
<< target.cameraToTarget.Translation().Z().value()
<< target.cameraToTarget.Rotation().GetQuaternion().W()
<< target.cameraToTarget.Rotation().GetQuaternion().X()
<< target.cameraToTarget.Rotation().GetQuaternion().Y()
<< target.cameraToTarget.Rotation().GetQuaternion().Z()
<< target.fiducialId
<< target.bestCameraToTarget.Translation().X().value()
<< target.bestCameraToTarget.Translation().Y().value()
<< target.bestCameraToTarget.Translation().Z().value()
<< target.bestCameraToTarget.Rotation().GetQuaternion().W()
<< target.bestCameraToTarget.Rotation().GetQuaternion().X()
<< target.bestCameraToTarget.Rotation().GetQuaternion().Y()
<< target.bestCameraToTarget.Rotation().GetQuaternion().Z()
<< target.altCameraToTarget.Translation().X().value()
<< target.altCameraToTarget.Translation().Y().value()
<< target.altCameraToTarget.Translation().Z().value()
<< target.altCameraToTarget.Rotation().GetQuaternion().W()
<< target.altCameraToTarget.Rotation().GetQuaternion().X()
<< target.altCameraToTarget.Rotation().GetQuaternion().Y()
<< target.altCameraToTarget.Rotation().GetQuaternion().Z()
<< target.poseAmbiguity;
for (int i = 0; i < 4; i++) {
@@ -75,17 +83,28 @@ Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target) {
Packet& operator>>(Packet& packet, PhotonTrackedTarget& target) {
packet >> target.yaw >> target.pitch >> target.area >> target.skew >>
target.fiducialId;
// We use these for best and alt transforms below
double x = 0;
double y = 0;
double z = 0;
double w = 0;
// First transform is the "best" pose
packet >> x >> y >> z;
const auto translation = frc::Translation3d(
const auto bestTranslation = frc::Translation3d(
units::meter_t(x), units::meter_t(y), units::meter_t(z));
packet >> w >> x >> y >> z;
const auto rotation = frc::Rotation3d(frc::Quaternion(w, x, y, z));
const auto bestRotation = frc::Rotation3d(frc::Quaternion(w, x, y, z));
target.bestCameraToTarget = frc::Transform3d(bestTranslation, bestRotation);
target.cameraToTarget = frc::Transform3d(translation, rotation);
// Second transform is the "alternate" pose
packet >> x >> y >> z;
const auto altTranslation = frc::Translation3d(
units::meter_t(x), units::meter_t(y), units::meter_t(z));
packet >> w >> x >> y >> z;
const auto altRotation = frc::Rotation3d(frc::Quaternion(w, x, y, z));
target.altCameraToTarget = frc::Transform3d(altTranslation, altRotation);
packet >> target.poseAmbiguity;