Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-23 01:21:40 +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

5
photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java
View File

@@ -189,7 +189,7 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
targetAreaEntry.forceSetDouble(bestTarget.getArea());
targetSkewEntry.forceSetDouble(bestTarget.getSkew());
var pose = bestTarget.getCameraToTarget3d();
var pose = bestTarget.getBestCameraToTarget3d();
targetPoseEntry.forceSetDoubleArray(
new double[] {
pose.getTranslation().getX(),
@@ -232,7 +232,8 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
t.getArea(),
t.getSkew(),
t.getFiducialId(),
t.getCameraToTarget3d(),
t.getBestCameraToTarget3d(),
t.getAltCameraToTarget3d(),
t.getPoseAmbiguity(),
cornerList));
}

3
photon-core/src/main/java/org/photonvision/vision/pipe/impl/SolvePNPPipe.java
View File

@@ -100,8 +100,9 @@ public class SolvePNPPipe
Core.norm(rVec));
Pose3d targetPose = MathUtils.convertOpenCVtoPhotonPose(new Transform3d(translation, rotation));
target.setCameraToTarget3d(
target.setBestCameraToTarget3d(
new Transform3d(targetPose.getTranslation(), targetPose.getRotation()));
target.setAltCameraToTarget3d(new Transform3d());
}
Mat rotationMatrix = new Mat();

10
photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java
View File

@@ -143,9 +143,13 @@ public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipel
new TargetCalculationParameters(
false, null, null, null, null, frameStaticProperties));
var correctedPose = MathUtils.convertOpenCVtoPhotonPose(target.getCameraToTarget3d());
target.setCameraToTarget3d(
new Transform3d(correctedPose.getTranslation(), correctedPose.getRotation()));
var correctedBestPose = MathUtils.convertOpenCVtoPhotonPose(target.getBestCameraToTarget3d());
var correctedAltPose = MathUtils.convertOpenCVtoPhotonPose(target.getAltCameraToTarget3d());
target.setBestCameraToTarget3d(
new Transform3d(correctedBestPose.getTranslation(), correctedBestPose.getRotation()));
target.setAltCameraToTarget3d(
new Transform3d(correctedAltPose.getTranslation(), correctedAltPose.getRotation()));
targetList.add(target);
}

30
photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java
View File

@@ -46,7 +46,8 @@ public class TrackedTarget implements Releasable {
private double m_area;
private double m_skew;
private Transform3d m_cameraToTarget3d = new Transform3d();
private Transform3d m_bestCameraToTarget3d = new Transform3d();
private Transform3d m_altCameraToTarget3d = new Transform3d();
private CVShape m_shape;
@@ -74,15 +75,20 @@ public class TrackedTarget implements Releasable {
TargetCalculations.calculateYaw(
result.getCenterX(), params.cameraCenterPoint.x, params.horizontalFocalLength);
var bestPose = new Transform3d();
var altPose = new Transform3d();
if (result.getError1() <= result.getError2()) {
bestPose = result.getPoseResult1();
altPose = result.getPoseResult2();
} else {
bestPose = result.getPoseResult2();
altPose = result.getPoseResult1();
}
bestPose = MathUtils.convertApriltagtoOpenCV(bestPose);
altPose = MathUtils.convertApriltagtoOpenCV(altPose);
m_cameraToTarget3d = bestPose;
m_bestCameraToTarget3d = bestPose;
m_altCameraToTarget3d = altPose;
double[] corners = result.getCorners();
Point[] cornerPoints =
@@ -231,12 +237,20 @@ public class TrackedTarget implements Releasable {
return !m_subContours.isEmpty();
}
public Transform3d getCameraToTarget3d() {
return m_cameraToTarget3d;
public Transform3d getBestCameraToTarget3d() {
return m_bestCameraToTarget3d;
}
public void setCameraToTarget3d(Transform3d pose) {
this.m_cameraToTarget3d = pose;
public Transform3d getAltCameraToTarget3d() {
return m_altCameraToTarget3d;
}
public void setBestCameraToTarget3d(Transform3d pose) {
this.m_bestCameraToTarget3d = pose;
}
public void setAltCameraToTarget3d(Transform3d pose) {
this.m_altCameraToTarget3d = pose;
}
public Mat getCameraRelativeTvec() {
@@ -272,8 +286,8 @@ public class TrackedTarget implements Releasable {
ret.put("skew", getSkew());
ret.put("area", getArea());
ret.put("ambiguity", getPoseAmbiguity());
if (getCameraToTarget3d() != null) {
ret.put("pose", transformToMap(getCameraToTarget3d()));
if (getBestCameraToTarget3d() != null) {
ret.put("pose", transformToMap(getBestCameraToTarget3d()));
}
ret.put("fiducialId", getFiducialId());
return ret;

2
photon-core/src/test/java/org/photonvision/vision/pipeline/CirclePNPTest.java
View File

@@ -161,7 +161,7 @@ public class CirclePNPTest {
System.out.println(
"Found targets at "
+ pipelineResult.targets.stream()
.map(TrackedTarget::getCameraToTarget3d)
.map(TrackedTarget::getBestCameraToTarget3d)
.collect(Collectors.toList()));
}
}

6
photon-core/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java
View File

@@ -111,7 +111,7 @@ public class SolvePNPTest {
printTestResults(pipelineResult);
// these numbers are not *accurate*, but they are known and expected
var pose = pipelineResult.targets.get(0).getCameraToTarget3d();
var pose = pipelineResult.targets.get(0).getBestCameraToTarget3d();
Assertions.assertEquals(1.1, pose.getTranslation().getX(), 0.05);
Assertions.assertEquals(0.0, pose.getTranslation().getY(), 0.05);
@@ -159,7 +159,7 @@ public class SolvePNPTest {
pipelineResult.targets);
// these numbers are not *accurate*, but they are known and expected
var pose = pipelineResult.targets.get(0).getCameraToTarget3d();
var pose = pipelineResult.targets.get(0).getBestCameraToTarget3d();
Assertions.assertEquals(Units.inchesToMeters(240.26), pose.getTranslation().getX(), 0.05);
Assertions.assertEquals(Units.inchesToMeters(35), pose.getTranslation().getY(), 0.05);
// Z rotation should be mostly facing us
@@ -211,7 +211,7 @@ public class SolvePNPTest {
System.out.println(
"Found targets at "
+ pipelineResult.targets.stream()
.map(TrackedTarget::getCameraToTarget3d)
.map(TrackedTarget::getBestCameraToTarget3d)
.collect(Collectors.toList()));
}
}

2
photon-lib/src/main/java/org/photonvision/SimPhotonCamera.java
View File

@@ -144,7 +144,7 @@ public class SimPhotonCamera extends PhotonCamera {
targetAreaEntry.setDouble(bestTarget.getArea());
targetSkewEntry.setDouble(bestTarget.getSkew());
var transform = bestTarget.getCameraToTarget();
var transform = bestTarget.getBestCameraToTarget();
double[] poseData = {
transform.getX(), transform.getY(), transform.getRotation().toRotation2d().getDegrees()
};

1
photon-lib/src/main/java/org/photonvision/SimVisionSystem.java
View File

@@ -171,6 +171,7 @@ public class SimVisionSystem {
0.0,
-1, // TODO fiducial ID
new Transform3d(),
new Transform3d(),
0.25,
List.of(
new TargetCorner(0, 0), new TargetCorner(0, 0),

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;

19
photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h
View File

@@ -97,10 +97,22 @@ class PhotonTrackedTarget {
double GetPoseAmbiguity() const { return poseAmbiguity; }
/**
* Returns the pose of the target relative to the robot.
* Get the transform that maps camera space (X = forward, Y = left, Z = up) to
* object/fiducial tag space (X forward, Y left, Z up) with the lowest
* reprojection error. The ratio between this and the alternate target's
* reprojection error is the ambiguity, which is between 0 and 1.
* @return The pose of the target relative to the robot.
*/
frc::Transform3d GetCameraToTarget() const { return cameraToTarget; }
frc::Transform3d GetBestCameraToTarget() const { return bestCameraToTarget; }
/**
* Get the transform that maps camera space (X = forward, Y = left, Z = up) to
* object/fiducial tag space (X forward, Y left, Z up) with the highest
* reprojection error
*/
frc::Transform3d GetAlternateCameraToTarget() const {
return altCameraToTarget;
}
bool operator==(const PhotonTrackedTarget& other) const;
bool operator!=(const PhotonTrackedTarget& other) const;
@@ -114,7 +126,8 @@ class PhotonTrackedTarget {
double area = 0;
double skew = 0;
int fiducialId;
frc::Transform3d cameraToTarget;
frc::Transform3d bestCameraToTarget;
frc::Transform3d altCameraToTarget;
double poseAmbiguity;
wpi::SmallVector<std::pair<double, double>, 4> corners;
};

3
photon-lib/src/test/java/org/photonvision/PacketTest.java
View File

@@ -45,6 +45,7 @@ class PacketTest {
-5.0,
-1,
new Transform3d(new Translation3d(), new Rotation3d()),
new Transform3d(new Translation3d(), new Rotation3d()),
0.25,
List.of(
new TargetCorner(1, 2),
@@ -82,6 +83,7 @@ class PacketTest {
4.0,
2,
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
0.25,
List.of(
new TargetCorner(1, 2),
@@ -95,6 +97,7 @@ class PacketTest {
6.7,
3,
new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
0.25,
List.of(
new TargetCorner(1, 2),

4
photon-server/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java
View File

@@ -69,7 +69,7 @@ public class AprilTagTest {
TestUtils.showImage(pipelineResult.inputFrame.image.getMat(), "Pipeline output", 999999);
// these numbers are not *accurate*, but they are known and expected
var pose = pipelineResult.targets.get(0).getCameraToTarget3d();
var pose = pipelineResult.targets.get(0).getBestCameraToTarget3d();
Assertions.assertEquals(2, pose.getTranslation().getX(), 0.2);
Assertions.assertEquals(0.0, pose.getTranslation().getY(), 0.2);
Assertions.assertEquals(0.0, pose.getTranslation().getY(), 0.2);
@@ -97,7 +97,7 @@ public class AprilTagTest {
System.out.println(
"Found targets at "
+ pipelineResult.targets.stream()
.map(TrackedTarget::getCameraToTarget3d)
.map(TrackedTarget::getBestCameraToTarget3d)
.collect(Collectors.toList()));
}
}

75
photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java
View File

@@ -27,14 +27,15 @@ import java.util.Objects;
import org.photonvision.common.dataflow.structures.Packet;
public class PhotonTrackedTarget {
public static final int PACK_SIZE_BYTES = Double.BYTES * (5 + 7 + 2 * 4 + 1);
public static final int PACK_SIZE_BYTES = Double.BYTES * (5 + 7 + 2 * 4 + 1 + 7);
private double yaw;
private double pitch;
private double area;
private double skew;
private int fiducialId;
private Transform3d cameraToTarget = new Transform3d();
private Transform3d bestCameraToTarget = new Transform3d();
private Transform3d altCameraToTarget = new Transform3d();
private double poseAmbiguity;
private List<TargetCorner> targetCorners;
@@ -48,6 +49,7 @@ public class PhotonTrackedTarget {
double skew,
int id,
Transform3d pose,
Transform3d altPose,
double ambiguity,
List<TargetCorner> corners) {
assert corners.size() == 4;
@@ -56,7 +58,8 @@ public class PhotonTrackedTarget {
this.area = area;
this.skew = skew;
this.fiducialId = id;
this.cameraToTarget = pose;
this.bestCameraToTarget = pose;
this.altCameraToTarget = altPose;
this.targetCorners = corners;
this.poseAmbiguity = ambiguity;
}
@@ -100,10 +103,18 @@ public class PhotonTrackedTarget {
/**
* Get the transform that maps camera space (X = forward, Y = left, Z = up) to object/fiducial tag
* space (X right, Y up, Z towards the camera/out of the wall)
* space (X forward, Y left, Z up) with the lowest reprojection error
*/
public Transform3d getCameraToTarget() {
return cameraToTarget;
public Transform3d getBestCameraToTarget() {
return bestCameraToTarget;
}
/**
* Get the transform that maps camera space (X = forward, Y = left, Z = up) to object/fiducial tag
* space (X forward, Y left, Z up) with the highest reprojection error
*/
public Transform3d getAlternateCameraToTarget() {
return altCameraToTarget;
}
@Override
@@ -114,13 +125,37 @@ public class PhotonTrackedTarget {
return Double.compare(that.yaw, yaw) == 0
&& Double.compare(that.pitch, pitch) == 0
&& Double.compare(that.area, area) == 0
&& Objects.equals(cameraToTarget, that.cameraToTarget)
&& Objects.equals(bestCameraToTarget, that.bestCameraToTarget)
&& Objects.equals(altCameraToTarget, that.altCameraToTarget)
&& Objects.equals(targetCorners, that.targetCorners);
}
@Override
public int hashCode() {
return Objects.hash(yaw, pitch, area, cameraToTarget);
return Objects.hash(yaw, pitch, area, bestCameraToTarget, altCameraToTarget);
}
private static Transform3d decodeTransform(Packet packet) {
double x = packet.decodeDouble();
double y = packet.decodeDouble();
double z = packet.decodeDouble();
var translation = new Translation3d(x, y, z);
double w = packet.decodeDouble();
x = packet.decodeDouble();
y = packet.decodeDouble();
z = packet.decodeDouble();
var rotation = new Rotation3d(new Quaternion(w, x, y, z));
return new Transform3d(translation, rotation);
}
private static void encodeTransform(Packet packet, Transform3d transform) {
packet.encode(transform.getTranslation().getX());
packet.encode(transform.getTranslation().getY());
packet.encode(transform.getTranslation().getZ());
packet.encode(transform.getRotation().getQuaternion().getW());
packet.encode(transform.getRotation().getQuaternion().getX());
packet.encode(transform.getRotation().getQuaternion().getY());
packet.encode(transform.getRotation().getQuaternion().getZ());
}
/**
@@ -136,16 +171,9 @@ public class PhotonTrackedTarget {
this.skew = packet.decodeDouble();
this.fiducialId = packet.decodeInt();
double x = packet.decodeDouble();
double y = packet.decodeDouble();
double z = packet.decodeDouble();
var translation = new Translation3d(x, y, z);
double w = packet.decodeDouble();
x = packet.decodeDouble();
y = packet.decodeDouble();
z = packet.decodeDouble();
var rotation = new Rotation3d(new Quaternion(w, x, y, z));
this.cameraToTarget = new Transform3d(translation, rotation);
this.bestCameraToTarget = decodeTransform(packet);
this.altCameraToTarget = decodeTransform(packet);
this.poseAmbiguity = packet.decodeDouble();
this.targetCorners = new ArrayList<>(4);
@@ -170,13 +198,8 @@ public class PhotonTrackedTarget {
packet.encode(area);
packet.encode(skew);
packet.encode(fiducialId);
packet.encode(cameraToTarget.getTranslation().getX());
packet.encode(cameraToTarget.getTranslation().getY());
packet.encode(cameraToTarget.getTranslation().getZ());
packet.encode(cameraToTarget.getRotation().getQuaternion().getW());
packet.encode(cameraToTarget.getRotation().getQuaternion().getX());
packet.encode(cameraToTarget.getRotation().getQuaternion().getY());
packet.encode(cameraToTarget.getRotation().getQuaternion().getZ());
encodeTransform(packet, bestCameraToTarget);
encodeTransform(packet, altCameraToTarget);
packet.encode(poseAmbiguity);
for (int i = 0; i < 4; i++) {
@@ -201,7 +224,7 @@ public class PhotonTrackedTarget {
+ ", fiducialId="
+ fiducialId
+ ", cameraToTarget="
+ cameraToTarget
+ bestCameraToTarget
+ ", targetCorners="
+ targetCorners
+ '}';

2
photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/DrivetrainPoseEstimator.java
View File

@@ -87,7 +87,7 @@ public class DrivetrainPoseEstimator {
var res = cam.getLatestResult();
if (res.hasTargets()) {
double imageCaptureTime = Timer.getFPGATimestamp() - res.getLatencyMillis() / 1000.0;
Transform3d camToTargetTrans = res.getBestTarget().getCameraToTarget();
Transform3d camToTargetTrans = res.getBestTarget().getBestCameraToTarget();
var transform =
new Transform2d(
camToTargetTrans.getTranslation().toTranslation2d(),