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

Send corners of min area rectangles (#382)

Browse Source 
Adds a new corners entry to targets. Breaks byte-packed backwards compatibility. 

Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Matt
2022-01-10 20:31:36 -08:00
committed by GitHub
parent e6d8e05b91
commit 3ad476bc28
12 changed files with 215 additions and 99 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -24,10 +24,12 @@ import java.util.List;
import java.util.function.BooleanSupplier;
import java.util.function.Consumer;
import java.util.function.Supplier;
import org.opencv.core.Point;
import org.photonvision.common.dataflow.CVPipelineResultConsumer;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.TargetCorner;
import org.photonvision.vision.pipeline.result.CVPipelineResult;
import org.photonvision.vision.target.TrackedTarget;
@@ -209,9 +211,20 @@ 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));
ret.add(
new PhotonTrackedTarget(
t.getYaw(), t.getPitch(), t.getArea(), t.getSkew(), t.getCameraToTarget()));
t.getYaw(),
t.getPitch(),
t.getArea(),
t.getSkew(),
t.getCameraToTarget(),
cornerList));
}
return ret;
}

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

@@ -108,7 +108,7 @@ public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectiveP
var sortContoursParams =
new SortContoursPipe.SortContoursParams(
settings.contourSortMode,
settings.outputShowMultipleTargets ? 5 : 1, // TODO don't hardcode?
settings.outputShowMultipleTargets ? 8 : 1, // TODO don't hardcode?
frameStaticProperties);
sortContoursPipe.setParams(sortContoursParams);

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

@@ -28,7 +28,7 @@ import org.photonvision.vision.opencv.*;
public class TrackedTarget implements Releasable {
public final Contour m_mainContour;
List<Contour> m_subContours; // can be empty
public List<Contour> m_subContours; // can be empty
private MatOfPoint2f m_approximateBoundingPolygon;

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

@@ -20,7 +20,9 @@ import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.util.Units;
import java.util.ArrayList;
import java.util.List;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.TargetCorner;
public class SimVisionSystem {
SimPhotonCamera cam;
@@ -151,8 +153,17 @@ public class SimVisionSystem {
- this.camPitchDegrees;
if (camCanSeeTarget(distMeters, yawDegrees, pitchDegrees, area)) {
// TODO simulate target corners
visibleTgtList.add(
new PhotonTrackedTarget(yawDegrees, pitchDegrees, area, 0.0, camToTargetTrans));
new PhotonTrackedTarget(
yawDegrees,
pitchDegrees,
area,
0.0,
camToTargetTrans,
List.of(
new TargetCorner(0, 0), new TargetCorner(0, 0),
new TargetCorner(0, 0), new TargetCorner(0, 0))));
}
});

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

@@ -18,19 +18,28 @@
#include "photonlib/PhotonTrackedTarget.h"
#include <iostream>
#include <utility>
#include <frc/geometry/Translation2d.h>
#include <wpi/SmallVector.h>
namespace photonlib {
PhotonTrackedTarget::PhotonTrackedTarget(double yaw, double pitch, double area,
double skew,
const frc::Transform2d& pose)
: yaw(yaw), pitch(pitch), area(area), skew(skew), cameraToTarget(pose) {}
PhotonTrackedTarget::PhotonTrackedTarget(
double yaw, double pitch, double area, double skew,
const frc::Transform2d& pose,
const wpi::SmallVector<std::pair<double, double>, 4> corners)
: yaw(yaw),
pitch(pitch),
area(area),
skew(skew),
cameraToTarget(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.cameraToTarget == cameraToTarget &&
other.corners == corners;
}
bool PhotonTrackedTarget::operator!=(const PhotonTrackedTarget& other) const {
@@ -38,10 +47,16 @@ bool PhotonTrackedTarget::operator!=(const PhotonTrackedTarget& other) const {
}
Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target) {
return packet << target.yaw << target.pitch << target.area << target.skew
<< target.cameraToTarget.Translation().X().value()
<< target.cameraToTarget.Translation().Y().value()
<< target.cameraToTarget.Rotation().Degrees().value();
packet << target.yaw << target.pitch << target.area << target.skew
<< target.cameraToTarget.Translation().X().value()
<< target.cameraToTarget.Translation().Y().value()
<< target.cameraToTarget.Rotation().Degrees().value();
for (int i = 0; i < 4; i++) {
packet << target.corners[i].first << target.corners[i].second;
}
return packet;
}
Packet& operator>>(Packet& packet, PhotonTrackedTarget& target) {
@@ -54,6 +69,15 @@ Packet& operator>>(Packet& packet, PhotonTrackedTarget& target) {
target.cameraToTarget =
frc::Transform2d(frc::Translation2d(units::meter_t(x), units::meter_t(y)),
units::degree_t(rot));
target.corners.clear();
for (int i = 0; i < 4; i++) {
double first = 0;
double second = 0;
packet >> first >> second;
target.corners.emplace_back(first, second);
}
return packet;
}

3
photon-lib/src/main/native/cpp/photonlib/SimVisionSystem.cpp
View File

@@ -86,7 +86,8 @@ void SimVisionSystem::ProcessFrame(frc::Pose2d robotPose) {
if (CamCanSeeTarget(distHypot, yawAngle, pitchAngle, area)) {
PhotonTrackedTarget newTgt = PhotonTrackedTarget(
yawAngle.value(), pitchAngle.value(), area, 0.0, camToTargetTrans);
yawAngle.value(), pitchAngle.value(), area, 0.0, camToTargetTrans,
{std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}});
visibleTgtList.push_back(newTgt);
}
}

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

@@ -19,9 +19,11 @@
#include <cstddef>
#include <string>
#include <utility>
#include <vector>
#include <frc/geometry/Transform2d.h>
#include <wpi/SmallVector.h>
#include "photonlib/Packet.h"
@@ -43,9 +45,12 @@ class PhotonTrackedTarget {
* @param area The area of the target.
* @param skew The skew of the target.
* @param pose The camera-relative pose of the target.
* @Param corners The corners of the bounding rectangle.
*/
PhotonTrackedTarget(double yaw, double pitch, double area, double skew,
const frc::Transform2d& pose);
PhotonTrackedTarget(
double yaw, double pitch, double area, double skew,
const frc::Transform2d& pose,
const wpi::SmallVector<std::pair<double, double>, 4> corners);
/**
* Returns the target yaw (positive-left).
@@ -71,6 +76,13 @@ class PhotonTrackedTarget {
*/
double GetSkew() const { return skew; }
/**
* Returns the corners of the minimum area rectangle bounding this target.
*/
wpi::SmallVector<std::pair<double, double>, 4> GetCorners() const {
return corners;
}
/**
* Returns the pose of the target relative to the robot.
* @return The pose of the target relative to the robot.
@@ -89,5 +101,6 @@ class PhotonTrackedTarget {
double area = 0;
double skew = 0;
frc::Transform2d cameraToTarget;
wpi::SmallVector<std::pair<double, double>, 4> corners;
};
} // namespace photonlib

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

@@ -26,13 +26,23 @@ import org.junit.jupiter.api.Test;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.TargetCorner;
class PacketTest {
@Test
void testSimpleTrackedTarget() {
var target =
new PhotonTrackedTarget(
3.0, 4.0, 9.0, -5.0, new Transform2d(new Translation2d(1, 2), new Rotation2d(1.5)));
3.0,
4.0,
9.0,
-5.0,
new Transform2d(new Translation2d(1, 2), new Rotation2d(1.5)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)));
var p = new Packet(PhotonTrackedTarget.PACK_SIZE_BYTES);
target.populatePacket(p);
@@ -62,13 +72,23 @@ class PacketTest {
-4.0,
9.0,
4.0,
new Transform2d(new Translation2d(1, 2), new Rotation2d(1.5))),
new Transform2d(new Translation2d(1, 2), new Rotation2d(1.5)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))),
new PhotonTrackedTarget(
3.0,
-4.0,
9.1,
6.7,
new Transform2d(new Translation2d(1, 5), new Rotation2d(1.5)))));
new Transform2d(new Translation2d(1, 5), new Rotation2d(1.5)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
var p2 = new Packet(result2.getPacketSize());
result2.populatePacket(p2);
@@ -77,46 +97,4 @@ class PacketTest {
Assertions.assertEquals(result2, b2);
}
@Test
void testBytePackFromCpp() {
byte[] bytePack = {
64, 8, 0, 0, 0, 0, 0, 0, 64, 16, 0, 0, 0, 0, 0, 0, 64, 34, 0, 0, 0, 0, 0, 0, -64, 20, 0, 0, 0,
0, 0, 0, 63, -16, 0, 0, 0, 0, 0, 0, 64, 0, 0, 0, 0, 0, 0, 0, 64, 85, 124, 101, 19, -54, -47,
122
};
var t = new PhotonTrackedTarget();
t.createFromPacket(new Packet(bytePack));
var target =
new PhotonTrackedTarget(
3.0, 4.0, 9.0, -5.0, new Transform2d(new Translation2d(1, 2), new Rotation2d(1.5)));
Assertions.assertEquals(t, target);
}
@Test
void testPacketv2021_1_6() {
// From v2021.1.6
var simplified =
new PhotonPipelineResult(
12.34,
List.of(
new PhotonTrackedTarget(
-23, -10, 6, 1, new Transform2d(new Translation2d(1, 2), new Rotation2d(3)))));
byte[] bytes = {
64, 40, -82, 20, 122, -31, 71, -82, 1, -64, 55, 0, 0, 0, 0, 0, 0, -64, 36, 0, 0, 0, 0, 0, 0,
64, 24, 0, 0, 0, 0, 0, 0, 63, -16, 0, 0, 0, 0, 0, 0, 63, -16, 0, 0, 0, 0, 0, 0, 64, 0, 0, 0,
0, 0, 0, 0, 64, 101, 124, 101, 19, -54, -47, 122, 0
};
// Let's check that those bytes still mean the same thing
Packet packet = new Packet(1);
packet.clear();
packet.setData(bytes);
var ret = new PhotonPipelineResult();
ret.createFromPacket(packet);
System.out.println(ret);
Assertions.assertEquals(simplified, ret);
}
}

48
photon-lib/src/test/native/cpp/PacketTest.cpp
View File

@@ -25,8 +25,13 @@
TEST(PacketTest, PhotonTrackedTarget) {
photonlib::PhotonTrackedTarget target{
3.0, 4.0, 9.0, -5.0,
frc::Transform2d(frc::Translation2d(1_m, 2_m), 1.5_rad)};
3.0,
4.0,
9.0,
-5.0,
frc::Transform2d(frc::Translation2d(1_m, 2_m), 1.5_rad),
{std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}};
photonlib::Packet p;
p << target;
@@ -52,11 +57,20 @@ TEST(PacketTest, PhotonPipelineResult) {
wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targets{
photonlib::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0,
frc::Transform2d(frc::Translation2d(1_m, 2_m), 1.5_rad)},
3.0,
-4.0,
9.0,
4.0,
frc::Transform2d(frc::Translation2d(1_m, 2_m), 1.5_rad),
{std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}},
photonlib::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7,
frc::Transform2d(frc::Translation2d(1_m, 5_m), 1.5_rad)}};
3.0,
-4.0,
9.1,
6.7,
frc::Transform2d(frc::Translation2d(1_m, 5_m), 1.5_rad),
{std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
std::pair{7, 8}}}};
photonlib::PhotonPipelineResult result2{2_s, targets};
photonlib::Packet p2;
@@ -67,25 +81,3 @@ TEST(PacketTest, PhotonPipelineResult) {
EXPECT_EQ(result2, b2);
}
TEST(PacketTest, BytePackFromJava) {
std::vector<signed char> bytePack{
64, 8, 0, 0, 0, 0, 0, 0, 64, 16, 0, 0, 0, 0,
0, 0, 64, 34, 0, 0, 0, 0, 0, 0, -64, 20, 0, 0,
0, 0, 0, 0, 63, -16, 0, 0, 0, 0, 0, 0, 64, 0,
0, 0, 0, 0, 0, 0, 64, 85, 124, 101, 19, -54, -47, 122};
std::vector<char> bytes;
for (auto a : bytePack) bytes.emplace_back(static_cast<char>(a));
photonlib::Packet packet{bytes};
photonlib::PhotonTrackedTarget res;
packet >> res;
photonlib::PhotonTrackedTarget target{
3.0, 4.0, 9.0, -5.0,
frc::Transform2d(frc::Translation2d(1_m, 2_m), 1.5_rad)};
EXPECT_EQ(res, target);
}

2
photon-server/src/main/resources/web/index.html
View File

@@ -1 +1 @@
<p>UI has not been copied!</p>
<p>UI has not been copied!</p>

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

@@ -19,26 +19,38 @@ package org.photonvision.targeting;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
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 * 7;
public static final int PACK_SIZE_BYTES = Double.BYTES * (7 + 2 * 4);
private double yaw;
private double pitch;
private double area;
private double skew;
private Transform2d cameraToTarget = new Transform2d();
private List<TargetCorner> targetCorners;
public PhotonTrackedTarget() {}
public PhotonTrackedTarget(double yaw, double pitch, double area, double skew, Transform2d pose) {
/** Construct a tracked target, given exactly 4 corners */
public PhotonTrackedTarget(
double yaw,
double pitch,
double area,
double skew,
Transform2d pose,
List<TargetCorner> corners) {
assert corners.size() == 4;
this.yaw = yaw;
this.pitch = pitch;
this.area = area;
this.skew = skew;
cameraToTarget = pose;
this.cameraToTarget = pose;
this.targetCorners = corners;
}
public double getYaw() {
@@ -57,6 +69,14 @@ public class PhotonTrackedTarget {
return skew;
}
/**
* Return a list of the 4 corners in image space (origin top left, x left, y down), in no
* particular order, of the minimum area bounding rectangle of this target
*/
public List<TargetCorner> getCorners() {
return targetCorners;
}
public Transform2d getCameraToTarget() {
return cameraToTarget;
}
@@ -69,7 +89,8 @@ 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(cameraToTarget, that.cameraToTarget)
&& Objects.equals(targetCorners, that.targetCorners);
}
@Override
@@ -84,16 +105,23 @@ public class PhotonTrackedTarget {
* @return The incoming packet.
*/
public Packet createFromPacket(Packet packet) {
yaw = packet.decodeDouble();
pitch = packet.decodeDouble();
area = packet.decodeDouble();
skew = packet.decodeDouble();
this.yaw = packet.decodeDouble();
this.pitch = packet.decodeDouble();
this.area = packet.decodeDouble();
this.skew = packet.decodeDouble();
double x = packet.decodeDouble();
double y = packet.decodeDouble();
double r = packet.decodeDouble();
cameraToTarget = new Transform2d(new Translation2d(x, y), Rotation2d.fromDegrees(r));
this.targetCorners = new ArrayList<>(4);
for (int i = 0; i < 4; i++) {
double cx = packet.decodeDouble();
double cy = packet.decodeDouble();
targetCorners.add(new TargetCorner(cx, cy));
}
this.cameraToTarget = new Transform2d(new Translation2d(x, y), Rotation2d.fromDegrees(r));
return packet;
}
@@ -113,6 +141,11 @@ public class PhotonTrackedTarget {
packet.encode(cameraToTarget.getTranslation().getY());
packet.encode(cameraToTarget.getRotation().getDegrees());
for (int i = 0; i < 4; i++) {
packet.encode(targetCorners.get(i).x);
packet.encode(targetCorners.get(i).y);
}
return packet;
}
}

51
photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java Normal file
View File

@@ -0,0 +1,51 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package org.photonvision.targeting;
import java.util.Objects;
/**
* Represents a point in an image at the corner of the minimum-area bounding rectangle, in pixels.
* Origin at the top left, plus-x to the right, plus-y down.
*/
public class TargetCorner {
public final double x;
public final double y;
public TargetCorner(double cx, double cy) {
this.x = cx;
this.y = cy;
}
@Override
public boolean equals(Object o) {
if (this == o) return true;
if (o == null || getClass() != o.getClass()) return false;
TargetCorner that = (TargetCorner) o;
return Double.compare(that.x, x) == 0 && Double.compare(that.y, y) == 0;
}
@Override
public int hashCode() {
return Objects.hash(x, y);
}
@Override
public String toString() {
return "(" + x + "," + y + ')';
}
}