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

[photon-targeting][photon-lib] Add tests to the targeting classes and cleanup photon-lib & photon-targeting (#1007)

Browse Source 
* Make MultiTagPNPResult and PNPResult singular

* add java tests

* Formatting fixes

* bring in the rest of the little stuff

* final things

* Formatting fixes

* add multisubscriber back

* Formatting fixes

* make comments better about x and y relationship
This commit is contained in:
Sriman Achanta
2023-11-15 18:28:26 -05:00
committed by GitHub
parent 524b135142
commit 308fd801d4
31 changed files with 833 additions and 191 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java
View File

@@ -79,7 +79,7 @@ public class SimCameraProperties {
private double avgLatencyMs = 0;
private double latencyStdDevMs = 0;
// util
private List<DMatrix3> viewplanes = new ArrayList<>();
private final List<DMatrix3> viewplanes = new ArrayList<>();
/** Default constructor which is the same as {@link #PERFECT_90DEG} */
public SimCameraProperties() {
@@ -215,8 +215,9 @@ public class SimCameraProperties {
0, getPixelPitch(resHeight).plus(new Rotation2d(-Math.PI / 2)).getRadians(), 0))
};
viewplanes.clear();
for (int i = 0; i < p.length; i++) {
viewplanes.add(new DMatrix3(p[i].getX(), p[i].getY(), p[i].getZ()));
for (Translation3d translation3d : p) {
viewplanes.add(
new DMatrix3(translation3d.getX(), translation3d.getY(), translation3d.getZ()));
}
}
@@ -457,8 +458,7 @@ public class SimCameraProperties {
// check if the ends of the line segment are visible
boolean aVisible = true;
boolean bVisible = true;
for (int i = 0; i < viewplanes.size(); i++) {
var normal = viewplanes.get(i);
for (DMatrix3 normal : viewplanes) {
double aVisibility = CommonOps_DDF3.dot(av, normal);
if (aVisibility < 0) aVisible = false;
double bVisibility = CommonOps_DDF3.dot(bv, normal);
@@ -467,7 +467,7 @@ public class SimCameraProperties {
if (aVisibility <= 0 && bVisibility <= 0) return new Pair<>(null, null);
}
// both ends are inside frustum
if (aVisible && bVisible) return new Pair<>(Double.valueOf(0), Double.valueOf(1));
if (aVisible && bVisible) return new Pair<>((double) 0, 1.0);
// parametrized (t=0 at a, t=1 at b) intersections with viewplanes
double[] intersections = {Double.NaN, Double.NaN, Double.NaN, Double.NaN};
@@ -546,8 +546,8 @@ public class SimCameraProperties {
}
// one viewplane intersection
else if (!Double.isNaN(inter1)) {
if (aVisible) return new Pair<>(Double.valueOf(0), inter1);
if (bVisible) return new Pair<>(inter1, Double.valueOf(1));
if (aVisible) return new Pair<>((double) 0, inter1);
if (bVisible) return new Pair<>(inter1, 1.0);
return new Pair<>(inter1, null);
}
// no intersections