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

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()));
}
}