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

Expose detected tag corners (#702)

Browse Source 
Removes GetCorners, replaces with getMinAreaRectCorners and getDetectedCorners
This commit is contained in:
Matt
2023-01-06 19:20:27 -08:00
committed by GitHub
parent 16ca2671f0
commit 967be84b4b
10 changed files with 291 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -189,11 +189,21 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
public static List<PhotonTrackedTarget> simpleFromTrackedTargets(List<TrackedTarget> targets) {
var ret = new ArrayList<PhotonTrackedTarget>();
for (var t : targets) {
var points = new Point[4];
t.getMinAreaRect().points(points);
var cornerList = new ArrayList<TargetCorner>();
for (int i = 0; i < 4; i++) cornerList.add(new TargetCorner(points[i].x, points[i].y));
var minAreaRectCorners = new ArrayList<TargetCorner>();
var detectedCorners = new ArrayList<TargetCorner>();
{
var points = new Point[4];
t.getMinAreaRect().points(points);
for (int i = 0; i < 4; i++) {
minAreaRectCorners.add(new TargetCorner(points[i].x, points[i].y));
}
}
{
var points = t.getTargetCorners();
for (int i = 0; i < points.size(); i++) {
detectedCorners.add(new TargetCorner(points.get(i).x, points.get(i).y));
}
}
ret.add(
new PhotonTrackedTarget(
@@ -205,7 +215,8 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
t.getBestCameraToTarget3d(),
t.getAltCameraToTarget3d(),
t.getPoseAmbiguity(),
cornerList));
minAreaRectCorners,
detectedCorners));
}
return ret;
}

2
photon-core/src/main/java/org/photonvision/vision/pipe/impl/CornerDetectionPipe.java
View File

@@ -47,7 +47,7 @@ public class CornerDetectionPipe
{
var targetCorners =
detectExtremeCornersByApproxPolyDp(target, params.calculateConvexHulls);
target.setCorners(targetCorners);
target.setTargetCorners(targetCorners);
break;
}
default:

2
photon-core/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java
View File

@@ -223,7 +223,7 @@ public class ColoredShapePipeline
collect2dTargetsResult.output.forEach(
shape -> {
shape.getMinAreaRect().points(rectPoints);
shape.setCorners(Arrays.asList(rectPoints));
shape.setTargetCorners(Arrays.asList(rectPoints));
});
sumPipeNanosElapsed += cornerDetectionResult.nanosElapsed;

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

@@ -41,7 +41,7 @@ public class TrackedTarget implements Releasable {
private MatOfPoint2f m_approximateBoundingPolygon;
private List<Point> m_targetCorners;
private List<Point> m_targetCorners = List.of();
private Point m_targetOffsetPoint;
private Point m_robotOffsetPoint;
@@ -289,7 +289,7 @@ public class TrackedTarget implements Releasable {
if (m_cameraRelativeRvec != null) m_cameraRelativeRvec.release();
}
public void setCorners(List<Point> targetCorners) {
public void setTargetCorners(List<Point> targetCorners) {
this.m_targetCorners = targetCorners;
}

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

@@ -231,6 +231,9 @@ public class SimVisionSystem {
camToTargetTrans,
camToTargetTransAlt,
0.0, // TODO - simulate ambiguity when straight on?
List.of(
new TargetCorner(0, 0), new TargetCorner(0, 0),
new TargetCorner(0, 0), new TargetCorner(0, 0)),
List.of(
new TargetCorner(0, 0), new TargetCorner(0, 0),
new TargetCorner(0, 0), new TargetCorner(0, 0))));

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

@@ -30,13 +30,15 @@
#include <frc/geometry/Translation2d.h>
#include <wpi/SmallVector.h>
static constexpr const uint8_t MAX_CORNERS = 8;
namespace photonlib {
PhotonTrackedTarget::PhotonTrackedTarget(
double yaw, double pitch, double area, double skew, int id,
const frc::Transform3d& pose, const frc::Transform3d& alternatePose,
double ambiguity,
const wpi::SmallVector<std::pair<double, double>, 4> corners)
const wpi::SmallVector<std::pair<double, double>, 4> minAreaRectCorners)
: yaw(yaw),
pitch(pitch),
area(area),
@@ -45,12 +47,12 @@ PhotonTrackedTarget::PhotonTrackedTarget(
bestCameraToTarget(pose),
altCameraToTarget(alternatePose),
poseAmbiguity(ambiguity),
corners(corners) {}
minAreaRectCorners(minAreaRectCorners) {}
bool PhotonTrackedTarget::operator==(const PhotonTrackedTarget& other) const {
return other.yaw == yaw && other.pitch == pitch && other.area == area &&
other.skew == skew && other.bestCameraToTarget == bestCameraToTarget &&
other.corners == corners;
other.minAreaRectCorners == minAreaRectCorners;
}
bool PhotonTrackedTarget::operator!=(const PhotonTrackedTarget& other) const {
@@ -77,7 +79,16 @@ Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target) {
<< target.poseAmbiguity;
for (int i = 0; i < 4; i++) {
packet << target.corners[i].first << target.corners[i].second;
packet << target.minAreaRectCorners[i].first
<< target.minAreaRectCorners[i].second;
}
uint8_t num_corners =
std::min<uint8_t>(target.detectedCorners.size(), MAX_CORNERS);
packet << num_corners;
for (size_t i = 0; i < target.detectedCorners.size(); i++) {
packet << target.detectedCorners[i].first
<< target.detectedCorners[i].second;
}
return packet;
@@ -111,12 +122,21 @@ Packet& operator>>(Packet& packet, PhotonTrackedTarget& target) {
packet >> target.poseAmbiguity;
target.corners.clear();
target.minAreaRectCorners.clear();
double first = 0;
double second = 0;
for (int i = 0; i < 4; i++) {
double first = 0;
double second = 0;
packet >> first >> second;
target.corners.emplace_back(first, second);
target.minAreaRectCorners.emplace_back(first, second);
}
uint8_t numCorners = 0;
packet >> numCorners;
target.detectedCorners.clear();
target.detectedCorners.reserve(numCorners);
for (size_t i = 0; i < numCorners; i++) {
packet >> first >> second;
target.detectedCorners.emplace_back(first, second);
}
return packet;

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

@@ -92,10 +92,27 @@ class PhotonTrackedTarget {
int GetFiducialId() const { return fiducialId; }
/**
* Returns the corners of the minimum area rectangle bounding this target.
* Return a list of the 4 corners in image space (origin top left, x right, y
* down), in no particular order, of the minimum area bounding rectangle of
* this target
*/
wpi::SmallVector<std::pair<double, double>, 4> GetCorners() const {
return corners;
wpi::SmallVector<std::pair<double, double>, 4> GetMinAreaRectCorners() const {
return minAreaRectCorners;
}
/**
* Return a list of the n corners in image space (origin top left, x right, y
* down), in no particular order, detected for this target.
* For fiducials, the order is known and is always counter-clock wise around
* the tag, like so
*
* -> +X 3 ----- 2
* | | |
* V + Y | |
* 0 ----- 1
*/
std::vector<std::pair<double, double>> GetDetectedCorners() {
return detectedCorners;
}
/**
@@ -137,6 +154,7 @@ class PhotonTrackedTarget {
frc::Transform3d bestCameraToTarget;
frc::Transform3d altCameraToTarget;
double poseAmbiguity;
wpi::SmallVector<std::pair<double, double>, 4> corners;
wpi::SmallVector<std::pair<double, double>, 4> minAreaRectCorners;
std::vector<std::pair<double, double>> detectedCorners;
};
} // namespace photonlib

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

@@ -47,6 +47,11 @@ class PacketTest {
new Transform3d(new Translation3d(), new Rotation3d()),
new Transform3d(new Translation3d(), new Rotation3d()),
0.25,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
@@ -85,6 +90,11 @@ class PacketTest {
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),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
@@ -99,6 +109,11 @@ class PacketTest {
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),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),

90
photon-lib/src/test/java/org/photonvision/RobotPoseEstimatorTest.java
View File

@@ -94,6 +94,11 @@ class RobotPoseEstimatorTest {
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.7,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
@@ -108,6 +113,11 @@ class RobotPoseEstimatorTest {
new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(0, 0, 0)),
new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
0.3,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
@@ -127,6 +137,11 @@ class RobotPoseEstimatorTest {
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.4,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
@@ -166,6 +181,11 @@ class RobotPoseEstimatorTest {
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
0.7,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
@@ -180,6 +200,11 @@ class RobotPoseEstimatorTest {
new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
0.3,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
@@ -199,6 +224,11 @@ class RobotPoseEstimatorTest {
new Transform3d(new Translation3d(4, 4, 4), new Rotation3d()),
new Transform3d(new Translation3d(5, 5, 5), new Rotation3d()),
0.4,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
@@ -238,6 +268,11 @@ class RobotPoseEstimatorTest {
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
0.7,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
@@ -252,6 +287,11 @@ class RobotPoseEstimatorTest {
new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
0.3,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
@@ -271,6 +311,11 @@ class RobotPoseEstimatorTest {
new Transform3d(new Translation3d(2.2, 2.2, 2.2), new Rotation3d()),
new Transform3d(new Translation3d(2, 1.9, 2.1), new Rotation3d()),
0.4,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
@@ -311,6 +356,11 @@ class RobotPoseEstimatorTest {
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
0.7,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
@@ -325,6 +375,11 @@ class RobotPoseEstimatorTest {
new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
0.3,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
@@ -344,6 +399,11 @@ class RobotPoseEstimatorTest {
new Transform3d(new Translation3d(2.2, 2.2, 2.2), new Rotation3d()),
new Transform3d(new Translation3d(2, 1.9, 2.1), new Rotation3d()),
0.4,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
@@ -374,6 +434,11 @@ class RobotPoseEstimatorTest {
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
0.7,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
@@ -388,6 +453,11 @@ class RobotPoseEstimatorTest {
new Transform3d(new Translation3d(2.1, 1.9, 2), new Rotation3d()),
new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
0.3,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
@@ -406,6 +476,11 @@ class RobotPoseEstimatorTest {
new Transform3d(new Translation3d(2.4, 2.4, 2.2), new Rotation3d()),
new Transform3d(new Translation3d(2, 1, 2), new Rotation3d()),
0.4,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
@@ -439,6 +514,11 @@ class RobotPoseEstimatorTest {
new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
0.7,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
@@ -453,6 +533,11 @@ class RobotPoseEstimatorTest {
new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
0.3,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
@@ -472,6 +557,11 @@ class RobotPoseEstimatorTest {
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
new Transform3d(new Translation3d(2, 1.9, 2.1), new Rotation3d()),
0.4,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),

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

@@ -23,11 +23,12 @@ import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import java.util.ArrayList;
import java.util.List;
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 + 7);
private static final int MAX_CORNERS = 8;
public static final int PACK_SIZE_BYTES =
Double.BYTES * (5 + 7 + 2 * 4 + 1 + 7 + 2 * MAX_CORNERS);
private double yaw;
private double pitch;
@@ -37,7 +38,12 @@ public class PhotonTrackedTarget {
private Transform3d bestCameraToTarget = new Transform3d();
private Transform3d altCameraToTarget = new Transform3d();
private double poseAmbiguity;
private List<TargetCorner> targetCorners;
// Corners from the min-area rectangle bounding the target
private List<TargetCorner> minAreaRectCorners;
// Corners from whatever corner detection method was used
private List<TargetCorner> detectedCorners;
public PhotonTrackedTarget() {}
@@ -51,8 +57,14 @@ public class PhotonTrackedTarget {
Transform3d pose,
Transform3d altPose,
double ambiguity,
List<TargetCorner> corners) {
assert corners.size() == 4;
List<TargetCorner> minAreaRectCorners,
List<TargetCorner> detectedCorners) {
assert minAreaRectCorners.size() == 4;
if (detectedCorners.size() > MAX_CORNERS) {
detectedCorners = detectedCorners.subList(0, MAX_CORNERS);
}
this.yaw = yaw;
this.pitch = pitch;
this.area = area;
@@ -60,7 +72,8 @@ public class PhotonTrackedTarget {
this.fiducialId = id;
this.bestCameraToTarget = pose;
this.altCameraToTarget = altPose;
this.targetCorners = corners;
this.minAreaRectCorners = minAreaRectCorners;
this.detectedCorners = detectedCorners;
this.poseAmbiguity = ambiguity;
}
@@ -94,11 +107,28 @@ public class PhotonTrackedTarget {
}
/**
* Return a list of the 4 corners in image space (origin top left, x left, y down), in no
* Return a list of the 4 corners in image space (origin top left, x right, y down), in no
* particular order, of the minimum area bounding rectangle of this target
*/
public List<TargetCorner> getCorners() {
return targetCorners;
public List<TargetCorner> getMinAreaRectCorners() {
return minAreaRectCorners;
}
/**
* Return a list of the n corners in image space (origin top left, x right, y down), in no
* particular order, detected for this target.
*
* <p>For fiducials, the order is known and is always counter-clock wise around the tag, like so
*
* <p>spotless:off
* -> +X 3 ----- 2
* | | |
* V | |
* +Y 0 ----- 1
* spotless:on
*/
public List<TargetCorner> getDetectedCorners() {
return detectedCorners;
}
/**
@@ -118,21 +148,54 @@ public class PhotonTrackedTarget {
}
@Override
public boolean equals(Object o) {
if (this == o) return true;
if (o == null || getClass() != o.getClass()) return false;
PhotonTrackedTarget that = (PhotonTrackedTarget) o;
return Double.compare(that.yaw, yaw) == 0
&& Double.compare(that.pitch, pitch) == 0
&& Double.compare(that.area, area) == 0
&& Objects.equals(bestCameraToTarget, that.bestCameraToTarget)
&& Objects.equals(altCameraToTarget, that.altCameraToTarget)
&& Objects.equals(targetCorners, that.targetCorners);
public int hashCode() {
final int prime = 31;
int result = 1;
long temp;
temp = Double.doubleToLongBits(yaw);
result = prime * result + (int) (temp ^ (temp >>> 32));
temp = Double.doubleToLongBits(pitch);
result = prime * result + (int) (temp ^ (temp >>> 32));
temp = Double.doubleToLongBits(area);
result = prime * result + (int) (temp ^ (temp >>> 32));
temp = Double.doubleToLongBits(skew);
result = prime * result + (int) (temp ^ (temp >>> 32));
result = prime * result + fiducialId;
result = prime * result + ((bestCameraToTarget == null) ? 0 : bestCameraToTarget.hashCode());
result = prime * result + ((altCameraToTarget == null) ? 0 : altCameraToTarget.hashCode());
temp = Double.doubleToLongBits(poseAmbiguity);
result = prime * result + (int) (temp ^ (temp >>> 32));
result = prime * result + ((minAreaRectCorners == null) ? 0 : minAreaRectCorners.hashCode());
result = prime * result + ((detectedCorners == null) ? 0 : detectedCorners.hashCode());
return result;
}
@Override
public int hashCode() {
return Objects.hash(yaw, pitch, area, bestCameraToTarget, altCameraToTarget);
public boolean equals(Object obj) {
if (this == obj) return true;
if (obj == null) return false;
if (getClass() != obj.getClass()) return false;
PhotonTrackedTarget other = (PhotonTrackedTarget) obj;
if (Double.doubleToLongBits(yaw) != Double.doubleToLongBits(other.yaw)) return false;
if (Double.doubleToLongBits(pitch) != Double.doubleToLongBits(other.pitch)) return false;
if (Double.doubleToLongBits(area) != Double.doubleToLongBits(other.area)) return false;
if (Double.doubleToLongBits(skew) != Double.doubleToLongBits(other.skew)) return false;
if (fiducialId != other.fiducialId) return false;
if (bestCameraToTarget == null) {
if (other.bestCameraToTarget != null) return false;
} else if (!bestCameraToTarget.equals(other.bestCameraToTarget)) return false;
if (altCameraToTarget == null) {
if (other.altCameraToTarget != null) return false;
} else if (!altCameraToTarget.equals(other.altCameraToTarget)) return false;
if (Double.doubleToLongBits(poseAmbiguity) != Double.doubleToLongBits(other.poseAmbiguity))
return false;
if (minAreaRectCorners == null) {
if (other.minAreaRectCorners != null) return false;
} else if (!minAreaRectCorners.equals(other.minAreaRectCorners)) return false;
if (detectedCorners == null) {
if (other.detectedCorners != null) return false;
} else if (!detectedCorners.equals(other.detectedCorners)) return false;
return true;
}
private static Transform3d decodeTransform(Packet packet) {
@@ -158,6 +221,25 @@ public class PhotonTrackedTarget {
packet.encode(transform.getRotation().getQuaternion().getZ());
}
private static void encodeList(Packet packet, List<TargetCorner> list) {
packet.encode((byte) Math.min(list.size(), Byte.MAX_VALUE));
for (int i = 0; i < list.size(); i++) {
packet.encode(list.get(i).x);
packet.encode(list.get(i).y);
}
}
private static List<TargetCorner> decodeList(Packet p) {
byte len = p.decodeByte();
var ret = new ArrayList<TargetCorner>();
for (int i = 0; i < len; i++) {
double cx = p.decodeDouble();
double cy = p.decodeDouble();
ret.add(new TargetCorner(cx, cy));
}
return ret;
}
/**
* Populates the fields of this class with information from the incoming packet.
*
@@ -176,13 +258,15 @@ public class PhotonTrackedTarget {
this.poseAmbiguity = packet.decodeDouble();
this.targetCorners = new ArrayList<>(4);
this.minAreaRectCorners = new ArrayList<>(4);
for (int i = 0; i < 4; i++) {
double cx = packet.decodeDouble();
double cy = packet.decodeDouble();
targetCorners.add(new TargetCorner(cx, cy));
minAreaRectCorners.add(new TargetCorner(cx, cy));
}
detectedCorners = decodeList(packet);
return packet;
}
@@ -203,10 +287,12 @@ public class PhotonTrackedTarget {
packet.encode(poseAmbiguity);
for (int i = 0; i < 4; i++) {
packet.encode(targetCorners.get(i).x);
packet.encode(targetCorners.get(i).y);
packet.encode(minAreaRectCorners.get(i).x);
packet.encode(minAreaRectCorners.get(i).y);
}
encodeList(packet, detectedCorners);
return packet;
}
@@ -226,7 +312,7 @@ public class PhotonTrackedTarget {
+ ", cameraToTarget="
+ bestCameraToTarget
+ ", targetCorners="
+ targetCorners
+ minAreaRectCorners
+ '}';
}
}