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 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

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),