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

Auto-generate packet dataclasses with Jinja (#1374)

Browse Source
This commit is contained in:
Matt
2024-08-31 13:44:19 -04:00
committed by GitHub
parent c19d54c633
commit 169595e56e
140 changed files with 4445 additions and 2097 deletions
Download Patch File Download Diff File Expand all files Collapse all files

187
photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java
View File

@@ -17,22 +17,25 @@
package org.photonvision.common.dataflow.structures;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import java.util.Optional;
import org.photonvision.targeting.serde.PhotonStructSerializable;
/** A packet that holds byte-packed data to be sent over NetworkTables. */
public class Packet {
// Size of the packet.
int size;
// Data stored in the packet.
byte[] packetData;
// Read and write positions.
int readPos, writePos;
/**
* Constructs an empty packet.
* Constructs an empty packet. This buffer will dynamically expand if we need more data space.
*
* @param size The size of the packet buffer.
*/
public Packet(int size) {
this.size = size;
packetData = new byte[size];
}
@@ -43,27 +46,34 @@ public class Packet {
*/
public Packet(byte[] data) {
packetData = data;
size = packetData.length;
}
/** Clears the packet and resets the read and write positions. */
public void clear() {
packetData = new byte[size];
packetData = new byte[packetData.length];
readPos = 0;
writePos = 0;
}
public int getNumBytesWritten() {
return writePos + 1;
}
public int getNumBytesRead() {
return readPos + 1;
}
public int getSize() {
return size;
return packetData.length;
}
/**
* Returns the packet data.
* Returns a copy of only the packet data we've actually written to so far.
*
* @return The packet data.
*/
public byte[] getData() {
return packetData;
public byte[] getWrittenDataCopy() {
return Arrays.copyOfRange(packetData, 0, writePos);
}
/**
@@ -73,7 +83,64 @@ public class Packet {
*/
public void setData(byte[] data) {
packetData = data;
size = data.length;
}
// Logic taken from ArraysSupport, licensed under GPL V2
public static final int SOFT_MAX_ARRAY_LENGTH = Integer.MAX_VALUE - 8;
// Logic taken from ArraysSupport, licensed under GPL V2
private static int newLength(int oldLength, int minGrowth, int prefGrowth) {
// preconditions not checked because of inlining
// assert oldLength >= 0
// assert minGrowth > 0
int prefLength = oldLength + Math.max(minGrowth, prefGrowth); // might overflow
if (0 < prefLength && prefLength <= SOFT_MAX_ARRAY_LENGTH) {
return prefLength;
} else {
// put code cold in a separate method
return hugeLength(oldLength, minGrowth);
}
}
// Logic taken from ArraysSupport, licensed under GPL V2
private static int hugeLength(int oldLength, int minGrowth) {
int minLength = oldLength + minGrowth;
if (minLength < 0) { // overflow
throw new OutOfMemoryError(
"Required array length " + oldLength + " + " + minGrowth + " is too large");
} else if (minLength <= SOFT_MAX_ARRAY_LENGTH) {
return SOFT_MAX_ARRAY_LENGTH;
} else {
return minLength;
}
}
/**
* Increases the capacity to ensure that it can hold at least the number of elements specified by
* the minimum capacity argument.
*
* <p>This logic is copied from ArrayList, which is licensed GPL V2
*
* @param minCapacity the desired minimum capacity
* @return
*/
private void ensureCapacity(int bytesToAdd) {
int minCapacity = writePos + bytesToAdd;
int oldCapacity = packetData.length;
if (minCapacity <= oldCapacity) {
return;
}
if (oldCapacity > 0) {
int newCapacity =
Packet.newLength(
oldCapacity,
minCapacity - oldCapacity, /* minimum growth */
oldCapacity >> 1 /* preferred growth */);
packetData = Arrays.copyOf(packetData, newCapacity);
} else {
packetData = new byte[Math.max(256, minCapacity)];
}
}
/**
@@ -82,6 +149,7 @@ public class Packet {
* @param src The byte to encode.
*/
public void encode(byte src) {
ensureCapacity(1);
packetData[writePos++] = src;
}
@@ -91,6 +159,7 @@ public class Packet {
* @param src The short to encode.
*/
public void encode(short src) {
ensureCapacity(2);
packetData[writePos++] = (byte) (src >>> 8);
packetData[writePos++] = (byte) src;
}
@@ -101,6 +170,7 @@ public class Packet {
* @param src The integer to encode.
*/
public void encode(int src) {
ensureCapacity(4);
packetData[writePos++] = (byte) (src >>> 24);
packetData[writePos++] = (byte) (src >>> 16);
packetData[writePos++] = (byte) (src >>> 8);
@@ -113,6 +183,7 @@ public class Packet {
* @param src The float to encode.
*/
public void encode(float src) {
ensureCapacity(4);
int data = Float.floatToIntBits(src);
packetData[writePos++] = (byte) ((data >> 24) & 0xff);
packetData[writePos++] = (byte) ((data >> 16) & 0xff);
@@ -126,6 +197,7 @@ public class Packet {
* @param data The double to encode.
*/
public void encode(long data) {
ensureCapacity(8);
packetData[writePos++] = (byte) ((data >> 56) & 0xff);
packetData[writePos++] = (byte) ((data >> 48) & 0xff);
packetData[writePos++] = (byte) ((data >> 40) & 0xff);
@@ -142,6 +214,7 @@ public class Packet {
* @param src The double to encode.
*/
public void encode(double src) {
ensureCapacity(8);
long data = Double.doubleToRawLongBits(src);
packetData[writePos++] = (byte) ((data >> 56) & 0xff);
packetData[writePos++] = (byte) ((data >> 48) & 0xff);
@@ -159,9 +232,56 @@ public class Packet {
* @param src The boolean to encode.
*/
public void encode(boolean src) {
ensureCapacity(1);
packetData[writePos++] = src ? (byte) 1 : (byte) 0;
}
public void encode(List<Short> data) {
byte size = (byte) data.size();
if (data.size() > Byte.MAX_VALUE) {
throw new RuntimeException("Array too long! Got " + size);
}
// length byte
encode(size);
for (var f : data) {
encode(f);
}
}
public <T extends PhotonStructSerializable<T>> void encode(T data) {
data.getSerde().pack(this, data);
}
/**
* Encode a list of serializable structs. Lists are stored as [uint8 length, [length many] data
* structs]
*
* @param <T> the class this list will be packing
* @param data
*/
public <T extends PhotonStructSerializable<T>> void encodeList(List<T> data) {
byte size = (byte) data.size();
if (data.size() > Byte.MAX_VALUE) {
throw new RuntimeException("Array too long! Got " + size);
}
// length byte
encode(size);
for (var f : data) {
f.getSerde().pack(this, f);
}
}
public <T extends PhotonStructSerializable<T>> void encodeOptional(Optional<T> data) {
encode(data.isPresent());
if (data.isPresent()) {
data.get().getSerde().pack(this, data.get());
}
}
/**
* Returns a decoded byte from the packet.
*
@@ -275,4 +395,49 @@ public class Packet {
}
return (short) ((0xff & packetData[readPos++]) << 8 | (0xff & packetData[readPos++]));
}
/**
* Decode a list of serializable structs. Lists are stored as [uint8 length, [length many] data
* structs]. Because java sucks, we need to take the serde ref directly
*
* @param <T>
* @param serde
*/
public <T extends PhotonStructSerializable<T>> List<T> decodeList(PacketSerde<T> serde) {
byte length = decodeByte();
var ret = new ArrayList<T>();
ret.ensureCapacity(length);
for (int i = 0; i < length; i++) {
ret.add(serde.unpack(this));
}
return ret;
}
public <T extends PhotonStructSerializable<T>> Optional<T> decodeOptional(PacketSerde<T> serde) {
var present = decodeBoolean();
if (present) {
return Optional.of(serde.unpack(this));
}
return Optional.empty();
}
public List<Short> decodeShortList() {
byte length = decodeByte();
var ret = new ArrayList<Short>();
ret.ensureCapacity(length);
for (int i = 0; i < length; i++) {
ret.add(decodeShort());
}
return ret;
}
public <T extends PhotonStructSerializable<T>> T decode(PhotonStructSerializable<T> t) {
return t.getSerde().unpack(this);
}
}

4
photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/PacketSerde.java
View File

@@ -23,4 +23,8 @@ public interface PacketSerde<T> {
void pack(Packet packet, T value);
T unpack(Packet packet);
String getTypeString();
String getInterfaceUUID();
}

3
photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java
View File

@@ -77,7 +77,8 @@ public class NTTopicSet {
.getRawTopic("rawBytes")
.publish("rawBytes", PubSubOption.periodic(0.01), PubSubOption.sendAll(true));
resultPublisher = new PacketPublisher<>(rawBytesEntry, PhotonPipelineResult.serde);
resultPublisher =
new PacketPublisher<PhotonPipelineResult>(rawBytesEntry, PhotonPipelineResult.photonStruct);
protoResultPublisher =
subTable
.getProtobufTopic("result_proto", PhotonPipelineResult.proto)

29
photon-targeting/src/main/java/org/photonvision/common/networktables/PacketPublisher.java
View File

@@ -17,27 +17,44 @@
package org.photonvision.common.networktables;
import com.fasterxml.jackson.core.JsonProcessingException;
import com.fasterxml.jackson.databind.ObjectMapper;
import edu.wpi.first.networktables.RawPublisher;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.dataflow.structures.PacketSerde;
public class PacketPublisher<T> implements AutoCloseable {
public final RawPublisher publisher;
private final PacketSerde<T> serde;
private final PacketSerde<T> photonStruct;
public PacketPublisher(RawPublisher publisher, PacketSerde<T> serde) {
public PacketPublisher(RawPublisher publisher, PacketSerde<T> photonStruct) {
this.publisher = publisher;
this.serde = serde;
this.photonStruct = photonStruct;
var mapper = new ObjectMapper();
try {
this.publisher
.getTopic()
.setProperty("message_format", mapper.writeValueAsString(photonStruct.getTypeString()));
this.publisher
.getTopic()
.setProperty("message_uuid", mapper.writeValueAsString(photonStruct.getInterfaceUUID()));
} catch (JsonProcessingException e) {
// TODO Auto-generated catch block
e.printStackTrace();
throw new RuntimeException(e);
}
}
public void set(T value, int byteSize) {
var packet = new Packet(byteSize);
serde.pack(packet, value);
publisher.set(packet.getData());
photonStruct.pack(packet, value);
// todo: trim to only the bytes we need to send
publisher.set(packet.getWrittenDataCopy());
}
public void set(T value) {
set(value, serde.getMaxByteSize());
set(value, photonStruct.getMaxByteSize());
}
@Override

5
photon-targeting/src/main/java/org/photonvision/common/networktables/PacketSubscriber.java
View File

@@ -86,6 +86,11 @@ public class PacketSubscriber<T> implements AutoCloseable {
subscriber.close();
}
// TODO - i can see an argument for moving this logic all here instead of keeping in photoncamera
public String getInterfaceUUID() {
return subscriber.getTopic().getProperty("message_uuid");
}
public List<PacketResult<T>> getAllChanges() {
List<PacketResult<T>> ret = new ArrayList<>();

18
photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java
View File

@@ -31,6 +31,7 @@ import edu.wpi.first.math.numbers.*;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import java.util.Optional;
import org.ejml.simple.SimpleMatrix;
import org.opencv.calib3d.Calib3d;
import org.opencv.core.Core;
@@ -46,7 +47,7 @@ import org.opencv.core.Point3;
import org.opencv.core.Rect;
import org.opencv.core.RotatedRect;
import org.opencv.imgproc.Imgproc;
import org.photonvision.targeting.PNPResult;
import org.photonvision.targeting.PnpResult;
import org.photonvision.targeting.TargetCorner;
public final class OpenCVHelp {
@@ -402,7 +403,7 @@ public final class OpenCVHelp {
* @return The resulting transformation that maps the camera pose to the target pose and the
* ambiguity if an alternate solution is available.
*/
public static PNPResult solvePNP_SQUARE(
public static Optional<PnpResult> solvePNP_SQUARE(
Matrix<N3, N3> cameraMatrix,
Matrix<N8, N1> distCoeffs,
List<Translation3d> modelTrls,
@@ -467,14 +468,15 @@ public final class OpenCVHelp {
// check if solvePnP failed with NaN results and retrying failed
if (Double.isNaN(errors[0])) throw new Exception("SolvePNP_SQUARE NaN result");
if (alt != null) return new PNPResult(best, alt, errors[0] / errors[1], errors[0], errors[1]);
else return new PNPResult(best, errors[0]);
if (alt != null)
return Optional.of(new PnpResult(best, alt, errors[0] / errors[1], errors[0], errors[1]));
else return Optional.empty();
}
// solvePnP failed
catch (Exception e) {
System.err.println("SolvePNP_SQUARE failed!");
e.printStackTrace();
return new PNPResult();
return Optional.empty();
} finally {
// release our Mats from native memory
objectMat.release();
@@ -509,7 +511,7 @@ public final class OpenCVHelp {
* model points are supplied relative to the origin, this transformation brings the camera to
* the origin.
*/
public static PNPResult solvePNP_SQPNP(
public static Optional<PnpResult> solvePNP_SQPNP(
Matrix<N3, N3> cameraMatrix,
Matrix<N8, N1> distCoeffs,
List<Translation3d> objectTrls,
@@ -558,11 +560,11 @@ public final class OpenCVHelp {
// check if solvePnP failed with NaN results
if (Double.isNaN(error[0])) throw new Exception("SolvePNP_SQPNP NaN result");
return new PNPResult(best, error[0]);
return Optional.of(new PnpResult(best, error[0]));
} catch (Exception e) {
System.err.println("SolvePNP_SQPNP failed!");
e.printStackTrace();
return new PNPResult();
return Optional.empty();
}
}
}

47
photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java
View File

@@ -27,10 +27,11 @@ import edu.wpi.first.math.numbers.*;
import java.util.ArrayList;
import java.util.List;
import java.util.Objects;
import java.util.Optional;
import java.util.stream.Collectors;
import org.opencv.core.Point;
import org.photonvision.targeting.PNPResult;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.PnpResult;
import org.photonvision.targeting.TargetCorner;
public class VisionEstimation {
@@ -64,9 +65,9 @@ public class VisionEstimation {
* @param visTags The visible tags reported by PV. Non-tag targets are automatically excluded.
* @param tagLayout The known tag layout on the field
* @return The transformation that maps the field origin to the camera pose. Ensure the {@link
* PNPResult} are present before utilizing them.
* PnpResult} are present before utilizing them.
*/
public static PNPResult estimateCamPosePNP(
public static Optional<PnpResult> estimateCamPosePNP(
Matrix<N3, N3> cameraMatrix,
Matrix<N8, N1> distCoeffs,
List<PhotonTrackedTarget> visTags,
@@ -76,7 +77,7 @@ public class VisionEstimation {
|| visTags == null
|| tagLayout.getTags().isEmpty()
|| visTags.isEmpty()) {
return new PNPResult();
return Optional.empty();
}
var corners = new ArrayList<TargetCorner>();
@@ -93,7 +94,7 @@ public class VisionEstimation {
});
}
if (knownTags.isEmpty() || corners.isEmpty() || corners.size() % 4 != 0) {
return new PNPResult();
return Optional.empty();
}
Point[] points = OpenCVHelp.cornersToPoints(corners);
@@ -101,32 +102,34 @@ public class VisionEstimation {
if (knownTags.size() == 1) {
var camToTag =
OpenCVHelp.solvePNP_SQUARE(cameraMatrix, distCoeffs, tagModel.vertices, points);
if (!camToTag.isPresent) return new PNPResult();
var bestPose = knownTags.get(0).pose.transformBy(camToTag.best.inverse());
if (!camToTag.isPresent()) return Optional.empty();
var bestPose = knownTags.get(0).pose.transformBy(camToTag.get().best.inverse());
var altPose = new Pose3d();
if (camToTag.ambiguity != 0)
altPose = knownTags.get(0).pose.transformBy(camToTag.alt.inverse());
if (camToTag.get().ambiguity != 0)
altPose = knownTags.get(0).pose.transformBy(camToTag.get().alt.inverse());
var o = new Pose3d();
return new PNPResult(
new Transform3d(o, bestPose),
new Transform3d(o, altPose),
camToTag.ambiguity,
camToTag.bestReprojErr,
camToTag.altReprojErr);
return Optional.of(
new PnpResult(
new Transform3d(o, bestPose),
new Transform3d(o, altPose),
camToTag.get().ambiguity,
camToTag.get().bestReprojErr,
camToTag.get().altReprojErr));
}
// multi-tag pnp
else {
var objectTrls = new ArrayList<Translation3d>();
for (var tag : knownTags) objectTrls.addAll(tagModel.getFieldVertices(tag.pose));
var camToOrigin = OpenCVHelp.solvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, points);
if (!camToOrigin.isPresent) return new PNPResult();
return new PNPResult(
camToOrigin.best.inverse(),
camToOrigin.alt.inverse(),
camToOrigin.ambiguity,
camToOrigin.bestReprojErr,
camToOrigin.altReprojErr);
if (camToOrigin.isEmpty()) return Optional.empty();
return Optional.of(
new PnpResult(
camToOrigin.get().best.inverse(),
camToOrigin.get().alt.inverse(),
camToOrigin.get().ambiguity,
camToOrigin.get().bestReprojErr,
camToOrigin.get().altReprojErr));
}
}
}

55
photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResult.java
View File

@@ -18,22 +18,23 @@
package org.photonvision.targeting;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
import java.util.ArrayList;
import java.util.List;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.dataflow.structures.PacketSerde;
import org.photonvision.struct.MultiTargetPNPResultSerde;
import org.photonvision.targeting.proto.MultiTargetPNPResultProto;
import org.photonvision.targeting.serde.PhotonStructSerializable;
public class MultiTargetPNPResult implements ProtobufSerializable {
public class MultiTargetPNPResult
implements ProtobufSerializable, PhotonStructSerializable<MultiTargetPNPResult> {
// Seeing 32 apriltags at once seems like a sane limit
private static final int MAX_IDS = 32;
public PNPResult estimatedPose = new PNPResult();
public List<Integer> fiducialIDsUsed = List.of();
public PnpResult estimatedPose = new PnpResult();
public List<Short> fiducialIDsUsed = List.of();
public MultiTargetPNPResult() {}
public MultiTargetPNPResult(PNPResult results, List<Integer> ids) {
public MultiTargetPNPResult(PnpResult results, List<Short> ids) {
estimatedPose = results;
fiducialIDsUsed = ids;
}
@@ -71,39 +72,13 @@ public class MultiTargetPNPResult implements ProtobufSerializable {
+ "]";
}
public static final class APacketSerde implements PacketSerde<MultiTargetPNPResult> {
@Override
public int getMaxByteSize() {
// PNPResult + MAX_IDS possible targets (arbitrary upper limit that should never be hit,
// ideally)
return PNPResult.serde.getMaxByteSize() + (Short.BYTES * MAX_IDS);
}
@Override
public void pack(Packet packet, MultiTargetPNPResult result) {
PNPResult.serde.pack(packet, result.estimatedPose);
for (int i = 0; i < MAX_IDS; i++) {
if (i < result.fiducialIDsUsed.size()) {
packet.encode((short) result.fiducialIDsUsed.get(i).byteValue());
} else {
packet.encode((short) -1);
}
}
}
@Override
public MultiTargetPNPResult unpack(Packet packet) {
var results = PNPResult.serde.unpack(packet);
var ids = new ArrayList<Integer>(MAX_IDS);
for (int i = 0; i < MAX_IDS; i++) {
int targetId = packet.decodeShort();
if (targetId > -1) ids.add(targetId);
}
return new MultiTargetPNPResult(results, ids);
}
}
public static final APacketSerde serde = new APacketSerde();
public static final MultiTargetPNPResultProto proto = new MultiTargetPNPResultProto();
// tODO!
public static final MultiTargetPNPResultSerde photonStruct = new MultiTargetPNPResultSerde();
@Override
public PacketSerde<MultiTargetPNPResult> getSerde() {
return photonStruct;
}
}

107
photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineMetadata.java Normal file
View File

@@ -0,0 +1,107 @@
/*
* 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 org.photonvision.common.dataflow.structures.PacketSerde;
import org.photonvision.struct.PhotonPipelineMetadataSerde;
import org.photonvision.targeting.serde.PhotonStructSerializable;
public class PhotonPipelineMetadata implements PhotonStructSerializable<PhotonPipelineMetadata> {
// Mirror of the heartbeat entry -- monotonically increasing
public long sequenceID;
// Image capture and NT publish timestamp, in microseconds and in the
// coprocessor timebase. As
// reported by WPIUtilJNI::now.
public long captureTimestampMicros;
public long publishTimestampMicros;
public PhotonPipelineMetadata(
long captureTimestampMicros, long publishTimestampMicros, long sequenceID) {
this.captureTimestampMicros = captureTimestampMicros;
this.publishTimestampMicros = publishTimestampMicros;
this.sequenceID = sequenceID;
}
public PhotonPipelineMetadata() {
this(-1, -1, -1);
}
/** Returns the time between image capture and publish to NT */
public double getLatencyMillis() {
return (publishTimestampMicros - captureTimestampMicros) / 1e3;
}
/** The time that this image was captured, in the coprocessor's time base. */
public long getCaptureTimestampMicros() {
return captureTimestampMicros;
}
/** The time that this result was published to NT, in the coprocessor's time base. */
public long getPublishTimestampMicros() {
return publishTimestampMicros;
}
/**
* The number of non-empty frames processed by this camera since boot. Useful to checking if a
* camera is alive.
*/
public long getSequenceID() {
return sequenceID;
}
@Override
public String toString() {
return "PhotonPipelineMetadata [sequenceID="
+ sequenceID
+ ", captureTimestampMicros="
+ captureTimestampMicros
+ ", publishTimestampMicros="
+ publishTimestampMicros
+ "]";
}
@Override
public int hashCode() {
final int prime = 31;
int result = 1;
result = prime * result + (int) (sequenceID ^ (sequenceID >>> 32));
result = prime * result + (int) (captureTimestampMicros ^ (captureTimestampMicros >>> 32));
result = prime * result + (int) (publishTimestampMicros ^ (publishTimestampMicros >>> 32));
return result;
}
@Override
public boolean equals(Object obj) {
if (this == obj) return true;
if (obj == null) return false;
if (getClass() != obj.getClass()) return false;
PhotonPipelineMetadata other = (PhotonPipelineMetadata) obj;
if (sequenceID != other.sequenceID) return false;
if (captureTimestampMicros != other.captureTimestampMicros) return false;
if (publishTimestampMicros != other.publishTimestampMicros) return false;
return true;
}
public static final PhotonPipelineMetadataSerde photonStruct = new PhotonPipelineMetadataSerde();
@Override
public PacketSerde<PhotonPipelineMetadata> getSerde() {
return photonStruct;
}
}

229
photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java
View File

@@ -20,33 +20,33 @@ package org.photonvision.targeting;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
import java.util.ArrayList;
import java.util.List;
import org.photonvision.common.dataflow.structures.Packet;
import java.util.Optional;
import org.photonvision.common.dataflow.structures.PacketSerde;
import org.photonvision.struct.PhotonPipelineResultSerde;
import org.photonvision.targeting.proto.PhotonPipelineResultProto;
import org.photonvision.targeting.serde.PhotonStructSerializable;
/** Represents a pipeline result from a PhotonCamera. */
public class PhotonPipelineResult implements ProtobufSerializable {
public class PhotonPipelineResult
implements ProtobufSerializable, PhotonStructSerializable<PhotonPipelineResult> {
private static boolean HAS_WARNED = false;
// Image capture and NT publish timestamp, in microseconds and in the coprocessor timebase. As
// reported by WPIUtilJNI::now.
private long captureTimestampMicros = -1;
private long publishTimestampMicros = -1;
// Mirror of the heartbeat entry -- monotonically increasing
private long sequenceID = -1;
// Frame capture metadata
public PhotonPipelineMetadata metadata;
// Targets to store.
public final List<PhotonTrackedTarget> targets = new ArrayList<>();
public List<PhotonTrackedTarget> targets = new ArrayList<>();
// Multi-tag result
private MultiTargetPNPResult multiTagResult = new MultiTargetPNPResult();
public Optional<MultiTargetPNPResult> multitagResult;
// Since we don't trust NT time sync, keep track of when we got this packet into robot code
private long ntRecieveTimestampMicros;
// HACK: Since we don't trust NT time sync, keep track of when we got this packet into robot code
public long ntReceiveTimestampMicros = -1;
/** Constructs an empty pipeline result. */
public PhotonPipelineResult() {}
public PhotonPipelineResult() {
this(new PhotonPipelineMetadata(), List.of(), Optional.empty());
}
/**
* Constructs a pipeline result.
@@ -63,10 +63,10 @@ public class PhotonPipelineResult implements ProtobufSerializable {
long captureTimestamp,
long publishTimestamp,
List<PhotonTrackedTarget> targets) {
this.captureTimestampMicros = captureTimestamp;
this.publishTimestampMicros = publishTimestamp;
this.sequenceID = sequenceID;
this.targets.addAll(targets);
this(
new PhotonPipelineMetadata(captureTimestamp, publishTimestamp, sequenceID),
targets,
Optional.empty());
}
/**
@@ -85,12 +85,20 @@ public class PhotonPipelineResult implements ProtobufSerializable {
long captureTimestamp,
long publishTimestamp,
List<PhotonTrackedTarget> targets,
MultiTargetPNPResult result) {
this.captureTimestampMicros = captureTimestamp;
this.publishTimestampMicros = publishTimestamp;
this.sequenceID = sequenceID;
Optional<MultiTargetPNPResult> result) {
this(
new PhotonPipelineMetadata(captureTimestamp, publishTimestamp, sequenceID),
targets,
result);
}
public PhotonPipelineResult(
PhotonPipelineMetadata metadata,
List<PhotonTrackedTarget> targets,
Optional<MultiTargetPNPResult> result) {
this.metadata = metadata;
this.targets.addAll(targets);
this.multiTagResult = result;
this.multitagResult = result;
}
/**
@@ -99,10 +107,11 @@ public class PhotonPipelineResult implements ProtobufSerializable {
* @return The size of the packet needed to store this pipeline result.
*/
public int getPacketSize() {
return Double.BYTES // latency
+ 1 // target count
+ targets.size() * PhotonTrackedTarget.serde.getMaxByteSize()
+ MultiTargetPNPResult.serde.getMaxByteSize();
throw new RuntimeException("TODO");
// return Double.BYTES // latency
// + 1 // target count
// + targets.size() * PhotonTrackedTarget.serde.getMaxByteSize()
// + MultiTargetPNPResult.serde.getMaxByteSize();
}
/**
@@ -124,50 +133,6 @@ public class PhotonPipelineResult implements ProtobufSerializable {
return hasTargets() ? targets.get(0) : null;
}
/** Returns the time between image capture and publish to NT */
public double getLatencyMillis() {
return (publishTimestampMicros - captureTimestampMicros) / 1e3;
}
/**
* Returns the estimated time the frame was taken, in the recieved system's time base. This is
* calculated as (NT recieve time (robot base) - (publish timestamp, coproc timebase - capture
* timestamp, coproc timebase))
*
* @return The timestamp in seconds
*/
public double getTimestampSeconds() {
return (ntRecieveTimestampMicros - (publishTimestampMicros - captureTimestampMicros)) / 1e6;
}
/** The time that this image was captured, in the coprocessor's time base. */
public long getCaptureTimestampMicros() {
return captureTimestampMicros;
}
/** The time that this result was published to NT, in the coprocessor's time base. */
public long getPublishTimestampMicros() {
return publishTimestampMicros;
}
/**
* The number of non-empty frames processed by this camera since boot. Useful to checking if a
* camera is alive.
*/
public long getSequenceID() {
return sequenceID;
}
/** The time that the robot recieved this result, in the FPGA timebase. */
public long getNtRecieveTimestampMicros() {
return ntRecieveTimestampMicros;
}
/** Sets the FPGA timestamp this result was recieved by robot code */
public void setRecieveTimestampMicros(long timestampMicros) {
this.ntRecieveTimestampMicros = timestampMicros;
}
/**
* Returns whether the pipeline has targets.
*
@@ -192,22 +157,54 @@ public class PhotonPipelineResult implements ProtobufSerializable {
* Return the latest multi-target result. Be sure to check
* getMultiTagResult().estimatedPose.isPresent before using the pose estimate!
*/
public MultiTargetPNPResult getMultiTagResult() {
return multiTagResult;
public Optional<MultiTargetPNPResult> getMultiTagResult() {
return multitagResult;
}
/**
* Returns the estimated time the frame was taken, in the Received system's time base. This is
* calculated as (NT Receive time (robot base) - (publish timestamp, coproc timebase - capture
* timestamp, coproc timebase))
*
* @return The timestamp in seconds
*/
public double getTimestampSeconds() {
return (ntReceiveTimestampMicros
- (metadata.publishTimestampMicros - metadata.captureTimestampMicros))
/ 1e6;
}
/** The time that the robot Received this result, in the FPGA timebase. */
public long getNtReceiveTimestampMicros() {
return ntReceiveTimestampMicros;
}
/** Sets the FPGA timestamp this result was Received by robot code */
public void setReceiveTimestampMicros(long timestampMicros) {
this.ntReceiveTimestampMicros = timestampMicros;
}
@Override
public String toString() {
return "PhotonPipelineResult [metadata="
+ metadata
+ ", targets="
+ targets
+ ", multitagResult="
+ multitagResult
+ ", ntReceiveTimestampMicros="
+ ntReceiveTimestampMicros
+ "]";
}
@Override
public int hashCode() {
final int prime = 31;
int result = 1;
result = prime * result + (int) (captureTimestampMicros ^ (captureTimestampMicros >>> 32));
long temp;
temp = Double.doubleToLongBits(publishTimestampMicros);
result = prime * result + (int) (temp ^ (temp >>> 32));
result = prime * result + (int) (sequenceID ^ (sequenceID >>> 32));
result = prime * result + ((metadata == null) ? 0 : metadata.hashCode());
result = prime * result + ((targets == null) ? 0 : targets.hashCode());
result = prime * result + ((multiTagResult == null) ? 0 : multiTagResult.hashCode());
result = prime * result + (int) (ntRecieveTimestampMicros ^ (ntRecieveTimestampMicros >>> 32));
result = prime * result + ((multitagResult == null) ? 0 : multitagResult.hashCode());
result = prime * result + (int) (ntReceiveTimestampMicros ^ (ntReceiveTimestampMicros >>> 32));
return result;
}
@@ -217,70 +214,24 @@ public class PhotonPipelineResult implements ProtobufSerializable {
if (obj == null) return false;
if (getClass() != obj.getClass()) return false;
PhotonPipelineResult other = (PhotonPipelineResult) obj;
if (captureTimestampMicros != other.captureTimestampMicros) return false;
if (Double.doubleToLongBits(publishTimestampMicros)
!= Double.doubleToLongBits(other.publishTimestampMicros)) return false;
if (sequenceID != other.sequenceID) return false;
if (metadata == null) {
if (other.metadata != null) return false;
} else if (!metadata.equals(other.metadata)) return false;
if (targets == null) {
if (other.targets != null) return false;
} else if (!targets.equals(other.targets)) return false;
if (multiTagResult == null) {
if (other.multiTagResult != null) return false;
} else if (!multiTagResult.equals(other.multiTagResult)) return false;
if (ntRecieveTimestampMicros != other.ntRecieveTimestampMicros) return false;
if (multitagResult == null) {
if (other.multitagResult != null) return false;
} else if (!multitagResult.equals(other.multitagResult)) return false;
if (ntReceiveTimestampMicros != other.ntReceiveTimestampMicros) return false;
return true;
}
@Override
public String toString() {
return "PhotonPipelineResult [captureTimestamp="
+ captureTimestampMicros
+ ", publishTimestamp="
+ publishTimestampMicros
+ ", sequenceID="
+ sequenceID
+ ", targets="
+ targets
+ ", multiTagResult="
+ multiTagResult
+ ", ntRecieveTimestamp="
+ ntRecieveTimestampMicros
+ "]";
}
public static final class APacketSerde implements PacketSerde<PhotonPipelineResult> {
@Override
public int getMaxByteSize() {
// This uses dynamic packets so it doesn't matter
return -1;
}
@Override
public void pack(Packet packet, PhotonPipelineResult value) {
packet.encode(value.sequenceID);
packet.encode(value.captureTimestampMicros);
packet.encode(value.publishTimestampMicros);
packet.encode((byte) value.targets.size());
for (var target : value.targets) PhotonTrackedTarget.serde.pack(packet, target);
MultiTargetPNPResult.serde.pack(packet, value.multiTagResult);
}
@Override
public PhotonPipelineResult unpack(Packet packet) {
var seq = packet.decodeLong();
var cap = packet.decodeLong();
var pub = packet.decodeLong();
var len = packet.decodeByte();
var targets = new ArrayList<PhotonTrackedTarget>(len);
for (int i = 0; i < len; i++) {
targets.add(PhotonTrackedTarget.serde.unpack(packet));
}
var result = MultiTargetPNPResult.serde.unpack(packet);
return new PhotonPipelineResult(seq, cap, pub, targets, result);
}
}
public static final APacketSerde serde = new APacketSerde();
public static final PhotonPipelineResultSerde photonStruct = new PhotonPipelineResultSerde();
public static final PhotonPipelineResultProto proto = new PhotonPipelineResultProto();
@Override
public PacketSerde<PhotonPipelineResult> getSerde() {
return photonStruct;
}
}

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

@@ -19,32 +19,32 @@ package org.photonvision.targeting;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
import java.util.ArrayList;
import java.util.List;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.dataflow.structures.PacketSerde;
import org.photonvision.struct.PhotonTrackedTargetSerde;
import org.photonvision.targeting.proto.PhotonTrackedTargetProto;
import org.photonvision.utils.PacketUtils;
import org.photonvision.targeting.serde.PhotonStructSerializable;
public class PhotonTrackedTarget implements ProtobufSerializable {
public class PhotonTrackedTarget
implements ProtobufSerializable, PhotonStructSerializable<PhotonTrackedTarget> {
private static final int MAX_CORNERS = 8;
private final double yaw;
private final double pitch;
private final double area;
private final double skew;
private final int fiducialId;
private final int classId;
private final float objDetectConf;
private final Transform3d bestCameraToTarget;
private final Transform3d altCameraToTarget;
private final double poseAmbiguity;
public double yaw;
public double pitch;
public double area;
public double skew;
public int fiducialId;
public int objDetectId;
public float objDetectConf;
public Transform3d bestCameraToTarget;
public Transform3d altCameraToTarget;
public double poseAmbiguity;
// Corners from the min-area rectangle bounding the target
private final List<TargetCorner> minAreaRectCorners;
public List<TargetCorner> minAreaRectCorners;
// Corners from whatever corner detection method was used
private final List<TargetCorner> detectedCorners;
public List<TargetCorner> detectedCorners;
/** Construct a tracked target, given exactly 4 corners */
public PhotonTrackedTarget(
@@ -71,7 +71,7 @@ public class PhotonTrackedTarget implements ProtobufSerializable {
this.area = area;
this.skew = skew;
this.fiducialId = fiducialId;
this.classId = classId;
this.objDetectId = classId;
this.objDetectConf = objDetectConf;
this.bestCameraToTarget = pose;
this.altCameraToTarget = altPose;
@@ -80,6 +80,10 @@ public class PhotonTrackedTarget implements ProtobufSerializable {
this.poseAmbiguity = ambiguity;
}
public PhotonTrackedTarget() {
// TODO Auto-generated constructor stub
}
public double getYaw() {
return yaw;
}
@@ -103,7 +107,7 @@ public class PhotonTrackedTarget implements ProtobufSerializable {
/** Get the object detection class ID number, or -1 if not set. */
public int getDetectedObjectClassID() {
return classId;
return objDetectId;
}
/**
@@ -235,75 +239,11 @@ public class PhotonTrackedTarget implements ProtobufSerializable {
+ '}';
}
public static final class APacketSerde implements PacketSerde<PhotonTrackedTarget> {
@Override
public int getMaxByteSize() {
return Double.BYTES * (5 + 7 + 2 * 4 + 1 + 1 + 4 + 7 + 2 * MAX_CORNERS);
}
@Override
public void pack(Packet packet, PhotonTrackedTarget value) {
packet.encode(value.yaw);
packet.encode(value.pitch);
packet.encode(value.area);
packet.encode(value.skew);
packet.encode(value.fiducialId);
packet.encode(value.classId);
packet.encode(value.objDetectConf);
PacketUtils.packTransform3d(packet, value.bestCameraToTarget);
PacketUtils.packTransform3d(packet, value.altCameraToTarget);
packet.encode(value.poseAmbiguity);
for (int i = 0; i < 4; i++) {
TargetCorner.serde.pack(packet, value.minAreaRectCorners.get(i));
}
packet.encode((byte) Math.min(value.detectedCorners.size(), Byte.MAX_VALUE));
for (TargetCorner targetCorner : value.detectedCorners) {
TargetCorner.serde.pack(packet, targetCorner);
}
}
@Override
public PhotonTrackedTarget unpack(Packet packet) {
var yaw = packet.decodeDouble();
var pitch = packet.decodeDouble();
var area = packet.decodeDouble();
var skew = packet.decodeDouble();
var fiducialId = packet.decodeInt();
var classId = packet.decodeInt();
var objDetectConf = packet.decodeFloat();
Transform3d best = PacketUtils.unpackTransform3d(packet);
Transform3d alt = PacketUtils.unpackTransform3d(packet);
double ambiguity = packet.decodeDouble();
var minAreaRectCorners = new ArrayList<TargetCorner>(4);
for (int i = 0; i < 4; i++) {
minAreaRectCorners.add(TargetCorner.serde.unpack(packet));
}
var len = packet.decodeByte();
var detectedCorners = new ArrayList<TargetCorner>(len);
for (int i = 0; i < len; i++) {
detectedCorners.add(TargetCorner.serde.unpack(packet));
}
return new PhotonTrackedTarget(
yaw,
pitch,
area,
skew,
fiducialId,
classId,
objDetectConf,
best,
alt,
ambiguity,
minAreaRectCorners,
detectedCorners);
}
}
public static final APacketSerde serde = new APacketSerde();
public static final PhotonTrackedTargetProto proto = new PhotonTrackedTargetProto();
public static final PhotonTrackedTargetSerde photonStruct = new PhotonTrackedTargetSerde();
@Override
public PacketSerde<PhotonTrackedTarget> getSerde() {
return photonStruct;
}
}

81
photon-targeting/src/main/java/org/photonvision/targeting/PNPResult.java → photon-targeting/src/main/java/org/photonvision/targeting/PnpResult.java
View File

@@ -19,10 +19,10 @@ package org.photonvision.targeting;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.dataflow.structures.PacketSerde;
import org.photonvision.struct.PnpResultSerde;
import org.photonvision.targeting.proto.PNPResultProto;
import org.photonvision.utils.PacketUtils;
import org.photonvision.targeting.serde.PhotonStructSerializable;
/**
* The best estimated transformation from solvePnP, and possibly an alternate transformation
@@ -33,37 +33,30 @@ import org.photonvision.utils.PacketUtils;
* <p>Note that the coordinate frame of these transforms depends on the implementing solvePnP
* method.
*/
public class PNPResult implements ProtobufSerializable {
/**
* If this result is valid. A false value indicates there was an error in estimation, and this
* result should not be used.
*/
public final boolean isPresent;
public class PnpResult implements ProtobufSerializable, PhotonStructSerializable<PnpResult> {
/**
* The best-fit transform. The coordinate frame of this transform depends on the method which gave
* this result.
*/
public final Transform3d best;
public Transform3d best;
/** Reprojection error of the best solution, in pixels */
public final double bestReprojErr;
public double bestReprojErr;
/**
* Alternate, ambiguous solution from solvepnp. If no alternate solution is found, this is equal
* to the best solution.
*/
public final Transform3d alt;
public Transform3d alt;
/** If no alternate solution is found, this is bestReprojErr */
public final double altReprojErr;
public double altReprojErr;
/** If no alternate solution is found, this is 0 */
public final double ambiguity;
public double ambiguity;
/** An empty (invalid) result. */
public PNPResult() {
this.isPresent = false;
public PnpResult() {
this.best = new Transform3d();
this.alt = new Transform3d();
this.ambiguity = 0;
@@ -71,17 +64,16 @@ public class PNPResult implements ProtobufSerializable {
this.altReprojErr = 0;
}
public PNPResult(Transform3d best, double bestReprojErr) {
public PnpResult(Transform3d best, double bestReprojErr) {
this(best, best, 0, bestReprojErr, bestReprojErr);
}
public PNPResult(
public PnpResult(
Transform3d best,
Transform3d alt,
double ambiguity,
double bestReprojErr,
double altReprojErr) {
this.isPresent = true;
this.best = best;
this.alt = alt;
this.ambiguity = ambiguity;
@@ -93,7 +85,6 @@ public class PNPResult implements ProtobufSerializable {
public int hashCode() {
final int prime = 31;
int result = 1;
result = prime * result + (isPresent ? 1231 : 1237);
result = prime * result + ((best == null) ? 0 : best.hashCode());
long temp;
temp = Double.doubleToLongBits(bestReprojErr);
@@ -111,8 +102,7 @@ public class PNPResult implements ProtobufSerializable {
if (this == obj) return true;
if (obj == null) return false;
if (getClass() != obj.getClass()) return false;
PNPResult other = (PNPResult) obj;
if (isPresent != other.isPresent) return false;
PnpResult other = (PnpResult) obj;
if (best == null) {
if (other.best != null) return false;
} else if (!best.equals(other.best)) return false;
@@ -130,9 +120,7 @@ public class PNPResult implements ProtobufSerializable {
@Override
public String toString() {
return "PNPResult [isPresent="
+ isPresent
+ ", best="
return "PnpResult [best="
+ best
+ ", bestReprojErr="
+ bestReprojErr
@@ -145,42 +133,11 @@ public class PNPResult implements ProtobufSerializable {
+ "]";
}
public static final class APacketSerde implements PacketSerde<PNPResult> {
@Override
public int getMaxByteSize() {
return 1 + (Double.BYTES * 7 * 2) + (Double.BYTES * 3);
}
@Override
public void pack(Packet packet, PNPResult value) {
packet.encode(value.isPresent);
if (value.isPresent) {
PacketUtils.packTransform3d(packet, value.best);
PacketUtils.packTransform3d(packet, value.alt);
packet.encode(value.bestReprojErr);
packet.encode(value.altReprojErr);
packet.encode(value.ambiguity);
}
}
@Override
public PNPResult unpack(Packet packet) {
var present = packet.decodeBoolean();
if (!present) {
return new PNPResult();
}
var best = PacketUtils.unpackTransform3d(packet);
var alt = PacketUtils.unpackTransform3d(packet);
var bestEr = packet.decodeDouble();
var altEr = packet.decodeDouble();
var ambiguity = packet.decodeDouble();
return new PNPResult(best, alt, ambiguity, bestEr, altEr);
}
}
public static final APacketSerde serde = new APacketSerde();
public static final PNPResultProto proto = new PNPResultProto();
public static final PnpResultSerde photonStruct = new PnpResultSerde();
@Override
public PacketSerde<PnpResult> getSerde() {
return photonStruct;
}
}

38
photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java
View File

@@ -19,23 +19,28 @@ package org.photonvision.targeting;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
import java.util.Objects;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.dataflow.structures.PacketSerde;
import org.photonvision.struct.TargetCornerSerde;
import org.photonvision.targeting.proto.TargetCornerProto;
import org.photonvision.targeting.serde.PhotonStructSerializable;
/**
* 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 implements ProtobufSerializable {
public final double x;
public final double y;
public class TargetCorner implements ProtobufSerializable, PhotonStructSerializable<TargetCorner> {
public double x;
public double y;
public TargetCorner(double cx, double cy) {
this.x = cx;
this.y = cy;
}
public TargetCorner() {
this(0, 0);
}
@Override
public boolean equals(Object o) {
if (this == o) return true;
@@ -54,24 +59,11 @@ public class TargetCorner implements ProtobufSerializable {
return "(" + x + "," + y + ')';
}
public static final class APacketSerde implements PacketSerde<TargetCorner> {
@Override
public int getMaxByteSize() {
return Double.BYTES * 2;
}
@Override
public void pack(Packet packet, TargetCorner corner) {
packet.encode(corner.x);
packet.encode(corner.y);
}
@Override
public TargetCorner unpack(Packet packet) {
return new TargetCorner(packet.decodeDouble(), packet.decodeDouble());
}
}
public static final APacketSerde serde = new APacketSerde();
public static final TargetCornerProto proto = new TargetCornerProto();
public static final TargetCornerSerde photonStruct = new TargetCornerSerde();
@Override
public PacketSerde<TargetCorner> getSerde() {
return photonStruct;
}
}

12
photon-targeting/src/main/java/org/photonvision/targeting/proto/MultiTargetPNPResultProto.java
View File

@@ -21,7 +21,7 @@ import edu.wpi.first.util.protobuf.Protobuf;
import java.util.ArrayList;
import org.photonvision.proto.Photon.ProtobufMultiTargetPNPResult;
import org.photonvision.targeting.MultiTargetPNPResult;
import org.photonvision.targeting.PNPResult;
import org.photonvision.targeting.PnpResult;
import us.hebi.quickbuf.Descriptors.Descriptor;
import us.hebi.quickbuf.RepeatedInt;
@@ -39,7 +39,7 @@ public class MultiTargetPNPResultProto
@Override
public Protobuf<?, ?>[] getNested() {
return new Protobuf<?, ?>[] {PNPResult.proto};
return new Protobuf<?, ?>[] {PnpResult.proto};
}
@Override
@@ -49,17 +49,17 @@ public class MultiTargetPNPResultProto
@Override
public MultiTargetPNPResult unpack(ProtobufMultiTargetPNPResult msg) {
ArrayList<Integer> fidIdsUsed = new ArrayList<>(msg.getFiducialIdsUsed().length());
ArrayList<Short> fidIdsUsed = new ArrayList<>(msg.getFiducialIdsUsed().length());
for (var packedFidId : msg.getFiducialIdsUsed()) {
fidIdsUsed.add(packedFidId);
fidIdsUsed.add(packedFidId.shortValue());
}
return new MultiTargetPNPResult(PNPResult.proto.unpack(msg.getEstimatedPose()), fidIdsUsed);
return new MultiTargetPNPResult(PnpResult.proto.unpack(msg.getEstimatedPose()), fidIdsUsed);
}
@Override
public void pack(ProtobufMultiTargetPNPResult msg, MultiTargetPNPResult value) {
PNPResult.proto.pack(msg.getMutableEstimatedPose(), value.estimatedPose);
PnpResult.proto.pack(msg.getMutableEstimatedPose(), value.estimatedPose);
RepeatedInt idsUsed = msg.getMutableFiducialIdsUsed().reserve(value.fiducialIDsUsed.size());
for (int i = 0; i < value.fiducialIDsUsed.size(); i++) {

21
photon-targeting/src/main/java/org/photonvision/targeting/proto/PNPResultProto.java
View File

@@ -20,13 +20,13 @@ package org.photonvision.targeting.proto;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.util.protobuf.Protobuf;
import org.photonvision.proto.Photon.ProtobufPNPResult;
import org.photonvision.targeting.PNPResult;
import org.photonvision.targeting.PnpResult;
import us.hebi.quickbuf.Descriptors.Descriptor;
public class PNPResultProto implements Protobuf<PNPResult, ProtobufPNPResult> {
public class PNPResultProto implements Protobuf<PnpResult, ProtobufPNPResult> {
@Override
public Class<PNPResult> getTypeClass() {
return PNPResult.class;
public Class<PnpResult> getTypeClass() {
return PnpResult.class;
}
@Override
@@ -45,12 +45,8 @@ public class PNPResultProto implements Protobuf<PNPResult, ProtobufPNPResult> {
}
@Override
public PNPResult unpack(ProtobufPNPResult msg) {
if (!msg.getIsPresent()) {
return new PNPResult();
}
return new PNPResult(
public PnpResult unpack(ProtobufPNPResult msg) {
return new PnpResult(
Transform3d.proto.unpack(msg.getBest()),
Transform3d.proto.unpack(msg.getAlt()),
msg.getAmbiguity(),
@@ -59,12 +55,11 @@ public class PNPResultProto implements Protobuf<PNPResult, ProtobufPNPResult> {
}
@Override
public void pack(ProtobufPNPResult msg, PNPResult value) {
public void pack(ProtobufPNPResult msg, PnpResult value) {
Transform3d.proto.pack(msg.getMutableBest(), value.best);
Transform3d.proto.pack(msg.getMutableAlt(), value.alt);
msg.setAmbiguity(value.ambiguity)
.setBestReprojErr(value.bestReprojErr)
.setAltReprojErr(value.altReprojErr)
.setIsPresent(value.isPresent);
.setAltReprojErr(value.altReprojErr);
}
}

19
photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java
View File

@@ -18,6 +18,7 @@
package org.photonvision.targeting.proto;
import edu.wpi.first.util.protobuf.Protobuf;
import java.util.Optional;
import org.photonvision.proto.Photon.ProtobufPhotonPipelineResult;
import org.photonvision.targeting.MultiTargetPNPResult;
import org.photonvision.targeting.PhotonPipelineResult;
@@ -53,16 +54,24 @@ public class PhotonPipelineResultProto
msg.getCaptureTimestampMicros(),
msg.getNtPublishTimestampMicros(),
PhotonTrackedTarget.proto.unpack(msg.getTargets()),
MultiTargetPNPResult.proto.unpack(msg.getMultiTargetResult()));
msg.hasMultiTargetResult()
? Optional.of(MultiTargetPNPResult.proto.unpack(msg.getMultiTargetResult()))
: Optional.empty());
}
@Override
public void pack(ProtobufPhotonPipelineResult msg, PhotonPipelineResult value) {
PhotonTrackedTarget.proto.pack(msg.getMutableTargets(), value.getTargets());
MultiTargetPNPResult.proto.pack(msg.getMutableMultiTargetResult(), value.getMultiTagResult());
msg.setSequenceId(value.getSequenceID());
msg.setCaptureTimestampMicros(value.getCaptureTimestampMicros());
msg.setNtPublishTimestampMicros(value.getPublishTimestampMicros());
if (value.getMultiTagResult().isPresent()) {
MultiTargetPNPResult.proto.pack(
msg.getMutableMultiTargetResult(), value.getMultiTagResult().get());
} else {
msg.clearMultiTargetResult();
}
msg.setSequenceId(value.metadata.getSequenceID());
msg.setCaptureTimestampMicros(value.metadata.getCaptureTimestampMicros());
msg.setNtPublishTimestampMicros(value.metadata.getPublishTimestampMicros());
}
}

24
photon-targeting/src/main/java/org/photonvision/targeting/serde/PhotonStructSerializable.java Normal file
View File

@@ -0,0 +1,24 @@
/*
* 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.serde;
import org.photonvision.common.dataflow.structures.PacketSerde;
public interface PhotonStructSerializable<T> {
PacketSerde<T> getSerde();
}

38
photon-targeting/src/main/native/cpp/photon/targeting/MultiTargetPNPResult.cpp
View File

@@ -17,40 +17,4 @@
#include "photon/targeting/MultiTargetPNPResult.h"
namespace photon {
bool MultiTargetPNPResult::operator==(const MultiTargetPNPResult& other) const {
return other.result == result && other.fiducialIdsUsed == fiducialIdsUsed;
}
Packet& operator<<(Packet& packet, const MultiTargetPNPResult& result) {
packet << result.result;
size_t i;
for (i = 0; i < result.fiducialIdsUsed.capacity(); i++) {
if (i < result.fiducialIdsUsed.size()) {
packet << static_cast<int16_t>(result.fiducialIdsUsed[i]);
} else {
packet << static_cast<int16_t>(-1);
}
}
return packet;
}
Packet& operator>>(Packet& packet, MultiTargetPNPResult& result) {
packet >> result.result;
result.fiducialIdsUsed.clear();
for (size_t i = 0; i < result.fiducialIdsUsed.capacity(); i++) {
int16_t id = 0;
packet >> id;
if (id > -1) {
result.fiducialIdsUsed.push_back(id);
}
}
return packet;
}
} // namespace photon
namespace photon {} // namespace photon

77
photon-targeting/src/main/native/cpp/photon/targeting/PNPResult.cpp
View File

@@ -1,77 +0,0 @@
/*
* 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/>.
*/
#include "photon/targeting/PNPResult.h"
namespace photon {
bool PNPResult::operator==(const PNPResult& other) const {
return other.isPresent == isPresent && other.best == best &&
other.bestReprojErr == bestReprojErr && other.alt == alt &&
other.altReprojErr == altReprojErr && other.ambiguity == ambiguity;
}
// Encode a transform3d
Packet& operator<<(Packet& packet, const frc::Transform3d& transform) {
packet << transform.Translation().X().value()
<< transform.Translation().Y().value()
<< transform.Translation().Z().value()
<< transform.Rotation().GetQuaternion().W()
<< transform.Rotation().GetQuaternion().X()
<< transform.Rotation().GetQuaternion().Y()
<< transform.Rotation().GetQuaternion().Z();
return packet;
}
// Decode a transform3d
Packet& operator>>(Packet& packet, frc::Transform3d& transform) {
frc::Transform3d ret;
// We use these for best and alt transforms below
double x = 0;
double y = 0;
double z = 0;
double w = 0;
// decode and unitify translation
packet >> x >> y >> z;
const auto translation = frc::Translation3d(
units::meter_t(x), units::meter_t(y), units::meter_t(z));
// decode and add units to rotation
packet >> w >> x >> y >> z;
const auto rotation = frc::Rotation3d(frc::Quaternion(w, x, y, z));
transform = frc::Transform3d(translation, rotation);
return packet;
}
Packet& operator<<(Packet& packet, PNPResult const& result) {
packet << result.isPresent << result.best << result.alt
<< result.bestReprojErr << result.altReprojErr << result.ambiguity;
return packet;
}
Packet& operator>>(Packet& packet, PNPResult& result) {
packet >> result.isPresent >> result.best >> result.alt >>
result.bestReprojErr >> result.altReprojErr >> result.ambiguity;
return packet;
}
} // namespace photon

72
photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp
View File

@@ -17,74 +17,4 @@
#include "photon/targeting/PhotonPipelineResult.h"
#include <vector>
namespace photon {
PhotonPipelineResult::PhotonPipelineResult(
int64_t sequenceID, units::microsecond_t captureTimestamp,
units::microsecond_t publishTimestamp,
std::span<const PhotonTrackedTarget> targets,
MultiTargetPNPResult multitagResult)
: sequenceID(sequenceID),
captureTimestamp(captureTimestamp),
publishTimestamp(publishTimestamp),
targets(targets.data(), targets.data() + targets.size()),
multitagResult(multitagResult) {}
bool PhotonPipelineResult::operator==(const PhotonPipelineResult& other) const {
return sequenceID == other.sequenceID &&
captureTimestamp == other.captureTimestamp &&
publishTimestamp == other.publishTimestamp &&
ntRecieveTimestamp == other.ntRecieveTimestamp &&
targets == other.targets && multitagResult == other.multitagResult;
}
Packet& operator<<(Packet& packet, const PhotonPipelineResult& result) {
// Encode latency and number of targets.
packet << result.sequenceID
<< static_cast<int64_t>(result.captureTimestamp.value())
<< static_cast<int64_t>(result.publishTimestamp.value())
<< static_cast<int8_t>(result.targets.size());
// Encode the information of each target.
for (auto& target : result.targets) packet << target;
packet << result.multitagResult;
// Return the packet
return packet;
}
Packet& operator>>(Packet& packet, PhotonPipelineResult& result) {
// Decode latency, existence of targets, and number of targets.
int64_t sequenceID = 0;
int64_t capTS = 0;
int64_t pubTS = 0;
int8_t targetCount = 0;
std::vector<PhotonTrackedTarget> targets;
MultiTargetPNPResult multitagResult;
packet >> sequenceID >> capTS >> pubTS >> targetCount;
targets.clear();
targets.reserve(targetCount);
// Decode the information of each target.
for (int i = 0; i < targetCount; ++i) {
PhotonTrackedTarget target;
packet >> target;
targets.push_back(target);
}
packet >> multitagResult;
units::microsecond_t captureTS =
units::microsecond_t{static_cast<double>(capTS)};
units::microsecond_t publishTS =
units::microsecond_t{static_cast<double>(pubTS)};
result = PhotonPipelineResult{sequenceID, captureTS, publishTS, targets,
multitagResult};
return packet;
}
} // namespace photon
namespace photon {} // namespace photon

113
photon-targeting/src/main/native/cpp/photon/targeting/PhotonTrackedTarget.cpp
View File

@@ -27,115 +27,4 @@
static constexpr const uint8_t MAX_CORNERS = 8;
namespace photon {
PhotonTrackedTarget::PhotonTrackedTarget(
double yaw, double pitch, double area, double skew, int id, int objdetid,
float objdetconf, const frc::Transform3d& pose,
const frc::Transform3d& alternatePose, double ambiguity,
const wpi::SmallVector<std::pair<double, double>, 4> minAreaRectCorners,
const std::vector<std::pair<double, double>> detectedCorners)
: yaw(yaw),
pitch(pitch),
area(area),
skew(skew),
fiducialId(id),
objDetectId(objdetid),
objDetectConf(objdetconf),
bestCameraToTarget(pose),
altCameraToTarget(alternatePose),
poseAmbiguity(ambiguity),
minAreaRectCorners(minAreaRectCorners),
detectedCorners(detectedCorners) {}
bool PhotonTrackedTarget::operator==(const PhotonTrackedTarget& other) const {
return other.yaw == yaw && other.pitch == pitch && other.area == area &&
other.skew == skew && other.bestCameraToTarget == bestCameraToTarget &&
other.objDetectConf == objDetectConf &&
other.objDetectId == objDetectId &&
other.minAreaRectCorners == minAreaRectCorners;
}
Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target) {
packet << target.yaw << target.pitch << target.area << target.skew
<< target.fiducialId << target.objDetectId << target.objDetectConf
<< target.bestCameraToTarget.Translation().X().value()
<< target.bestCameraToTarget.Translation().Y().value()
<< target.bestCameraToTarget.Translation().Z().value()
<< target.bestCameraToTarget.Rotation().GetQuaternion().W()
<< target.bestCameraToTarget.Rotation().GetQuaternion().X()
<< target.bestCameraToTarget.Rotation().GetQuaternion().Y()
<< target.bestCameraToTarget.Rotation().GetQuaternion().Z()
<< target.altCameraToTarget.Translation().X().value()
<< target.altCameraToTarget.Translation().Y().value()
<< target.altCameraToTarget.Translation().Z().value()
<< target.altCameraToTarget.Rotation().GetQuaternion().W()
<< target.altCameraToTarget.Rotation().GetQuaternion().X()
<< target.altCameraToTarget.Rotation().GetQuaternion().Y()
<< target.altCameraToTarget.Rotation().GetQuaternion().Z()
<< target.poseAmbiguity;
for (int i = 0; i < 4; i++) {
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;
}
Packet& operator>>(Packet& packet, PhotonTrackedTarget& target) {
packet >> target.yaw >> target.pitch >> target.area >> target.skew >>
target.fiducialId >> target.objDetectId >> target.objDetectConf;
// We use these for best and alt transforms below
double x = 0;
double y = 0;
double z = 0;
double w = 0;
// First transform is the "best" pose
packet >> x >> y >> z;
const auto bestTranslation = frc::Translation3d(
units::meter_t(x), units::meter_t(y), units::meter_t(z));
packet >> w >> x >> y >> z;
const auto bestRotation = frc::Rotation3d(frc::Quaternion(w, x, y, z));
target.bestCameraToTarget = frc::Transform3d(bestTranslation, bestRotation);
// Second transform is the "alternate" pose
packet >> x >> y >> z;
const auto altTranslation = frc::Translation3d(
units::meter_t(x), units::meter_t(y), units::meter_t(z));
packet >> w >> x >> y >> z;
const auto altRotation = frc::Rotation3d(frc::Quaternion(w, x, y, z));
target.altCameraToTarget = frc::Transform3d(altTranslation, altRotation);
packet >> target.poseAmbiguity;
target.minAreaRectCorners.clear();
double first = 0;
double second = 0;
for (int i = 0; i < 4; i++) {
packet >> 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;
}
} // namespace photon
namespace photon {} // namespace photon

20
photon-targeting/src/main/native/cpp/photon/targeting/PnpResult.cpp Normal file
View File

@@ -0,0 +1,20 @@
/*
* 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/>.
*/
#include "photon/targeting/PnpResult.h"
namespace photon {} // namespace photon

14
photon-targeting/src/main/native/cpp/photon/targeting/proto/MultiTargetPNPResultProto.cpp
View File

@@ -33,24 +33,26 @@ wpi::Protobuf<photon::MultiTargetPNPResult>::Unpack(
static_cast<const photonvision::proto::ProtobufMultiTargetPNPResult*>(
&msg);
wpi::SmallVector<int16_t, 32> fiducialIdsUsed;
std::vector<int16_t> fiducialIdsUsed;
fiducialIdsUsed.reserve(32);
for (int i = 0; i < m->fiducial_ids_used_size(); i++) {
fiducialIdsUsed.push_back(m->fiducial_ids_used(i));
}
return photon::MultiTargetPNPResult{
wpi::UnpackProtobuf<photon::PNPResult>(m->estimated_pose()),
fiducialIdsUsed};
return photon::MultiTargetPNPResult{photon::MultiTargetPNPResult_PhotonStruct{
wpi::UnpackProtobuf<photon::PnpResult>(m->estimated_pose()),
fiducialIdsUsed}};
}
void wpi::Protobuf<photon::MultiTargetPNPResult>::Pack(
google::protobuf::Message* msg, const photon::MultiTargetPNPResult& value) {
auto m = static_cast<photonvision::proto::ProtobufMultiTargetPNPResult*>(msg);
wpi::PackProtobuf(m->mutable_estimated_pose(), value.result);
wpi::PackProtobuf(m->mutable_estimated_pose(), value.estimatedPose);
m->clear_fiducial_ids_used();
for (const auto& t : value.fiducialIdsUsed) {
for (const auto& t : value.fiducialIDsUsed) {
m->add_fiducial_ids_used(t);
}
}

24
photon-targeting/src/main/native/cpp/photon/targeting/proto/PNPResultProto.cpp
View File

@@ -19,33 +19,27 @@
#include "photon.pb.h"
google::protobuf::Message* wpi::Protobuf<photon::PNPResult>::New(
google::protobuf::Message* wpi::Protobuf<photon::PnpResult>::New(
google::protobuf::Arena* arena) {
return google::protobuf::Arena::CreateMessage<
photonvision::proto::ProtobufPNPResult>(arena);
}
photon::PNPResult wpi::Protobuf<photon::PNPResult>::Unpack(
photon::PnpResult wpi::Protobuf<photon::PnpResult>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const photonvision::proto::ProtobufPNPResult*>(&msg);
if (!m->is_present()) {
return photon::PNPResult();
}
return photon::PNPResult{true,
wpi::UnpackProtobuf<frc::Transform3d>(m->best()),
m->best_reproj_err(),
wpi::UnpackProtobuf<frc::Transform3d>(m->alt()),
m->alt_reproj_err(),
m->ambiguity()};
return photon::PnpResult{photon::PnpResult_PhotonStruct{
wpi::UnpackProtobuf<frc::Transform3d>(m->best()),
wpi::UnpackProtobuf<frc::Transform3d>(m->alt()), m->best_reproj_err(),
m->alt_reproj_err(), m->ambiguity()}};
}
void wpi::Protobuf<photon::PNPResult>::Pack(google::protobuf::Message* msg,
const photon::PNPResult& value) {
void wpi::Protobuf<photon::PnpResult>::Pack(google::protobuf::Message* msg,
const photon::PnpResult& value) {
auto m = static_cast<photonvision::proto::ProtobufPNPResult*>(msg);
m->set_is_present(value.isPresent);
// m->set_is_present(value.isPresent);
wpi::PackProtobuf(m->mutable_best(), value.best);
m->set_best_reproj_err(value.bestReprojErr);
wpi::PackProtobuf(m->mutable_alt(), value.alt);

33
photon-targeting/src/main/native/cpp/photon/targeting/proto/PhotonPipelineResultProto.cpp
View File

@@ -42,28 +42,39 @@ wpi::Protobuf<photon::PhotonPipelineResult>::Unpack(
targets.emplace_back(wpi::UnpackProtobuf<photon::PhotonTrackedTarget>(t));
}
return photon::PhotonPipelineResult{
m->sequence_id(),
units::microsecond_t{static_cast<double>(m->capture_timestamp_micros())},
units::microsecond_t{
static_cast<double>(m->nt_publish_timestamp_micros())},
return photon::PhotonPipelineResult{photon::PhotonPipelineResult_PhotonStruct{
photon::PhotonPipelineMetadata{
photon::PhotonPipelineMetadata_PhotonStruct{
m->sequence_id(),
m->capture_timestamp_micros(),
m->nt_publish_timestamp_micros(),
}},
targets,
wpi::UnpackProtobuf<photon::MultiTargetPNPResult>(
m->multi_target_result())};
// TODO need to pull this into an optional
m->has_multi_target_result()
? std::optional<photon::MultiTargetPNPResult>{wpi::UnpackProtobuf<
photon::MultiTargetPNPResult>(m->multi_target_result())}
: std::nullopt,
}};
}
void wpi::Protobuf<photon::PhotonPipelineResult>::Pack(
google::protobuf::Message* msg, const photon::PhotonPipelineResult& value) {
auto m = static_cast<photonvision::proto::ProtobufPhotonPipelineResult*>(msg);
m->set_sequence_id(value.sequenceID);
m->set_capture_timestamp_micros(value.captureTimestamp.value());
m->set_nt_publish_timestamp_micros(value.publishTimestamp.value());
m->set_sequence_id(value.metadata.sequenceID);
m->set_capture_timestamp_micros(value.metadata.captureTimestampMicros);
m->set_nt_publish_timestamp_micros(value.metadata.publishTimestampMicros);
m->clear_targets();
for (const auto& t : value.GetTargets()) {
wpi::PackProtobuf(m->add_targets(), t);
}
wpi::PackProtobuf(m->mutable_multi_target_result(), value.multitagResult);
// TODO this is dumb and bad
if (value.multitagResult) {
wpi::PackProtobuf(m->mutable_multi_target_result(), *value.multitagResult);
} else {
m->clear_multi_target_result();
}
}

37
photon-targeting/src/main/native/cpp/photon/targeting/proto/PhotonTrackedTargetProto.cpp
View File

@@ -22,6 +22,9 @@
#include "photon.pb.h"
using photon::TargetCorner;
using photon::TargetCorner_PhotonStruct;
google::protobuf::Message* wpi::Protobuf<photon::PhotonTrackedTarget>::New(
google::protobuf::Arena* arena) {
return google::protobuf::Arena::CreateMessage<
@@ -33,30 +36,26 @@ photon::PhotonTrackedTarget wpi::Protobuf<photon::PhotonTrackedTarget>::Unpack(
auto m = static_cast<const photonvision::proto::ProtobufPhotonTrackedTarget*>(
&msg);
wpi::SmallVector<std::pair<double, double>, 4> minAreaRectCorners;
std::vector<photon::TargetCorner> minAreaRectCorners;
minAreaRectCorners.reserve(4);
for (const auto& t : m->min_area_rect_corners()) {
minAreaRectCorners.emplace_back(t.x(), t.y());
minAreaRectCorners.emplace_back(
TargetCorner{TargetCorner_PhotonStruct{t.x(), t.y()}});
}
std::vector<std::pair<double, double>> detectedCorners;
std::vector<photon::TargetCorner> detectedCorners;
detectedCorners.reserve(m->detected_corners_size());
for (const auto& t : m->detected_corners()) {
detectedCorners.emplace_back(t.x(), t.y());
minAreaRectCorners.emplace_back(
TargetCorner{TargetCorner_PhotonStruct{t.x(), t.y()}});
}
return photon::PhotonTrackedTarget{
m->yaw(),
m->pitch(),
m->area(),
m->skew(),
m->fiducial_id(),
m->obj_detection_id(),
m->obj_detection_conf(),
return photon::PhotonTrackedTarget{photon::PhotonTrackedTarget_PhotonStruct{
m->yaw(), m->pitch(), m->area(), m->skew(), m->fiducial_id(),
m->obj_detection_id(), m->obj_detection_conf(),
wpi::UnpackProtobuf<frc::Transform3d>(m->best_camera_to_target()),
wpi::UnpackProtobuf<frc::Transform3d>(m->alt_camera_to_target()),
m->pose_ambiguity(),
minAreaRectCorners,
detectedCorners};
m->pose_ambiguity(), minAreaRectCorners, detectedCorners}};
}
void wpi::Protobuf<photon::PhotonTrackedTarget>::Pack(
@@ -78,14 +77,14 @@ void wpi::Protobuf<photon::PhotonTrackedTarget>::Pack(
m->clear_min_area_rect_corners();
for (const auto& t : value.GetMinAreaRectCorners()) {
auto* corner = m->add_min_area_rect_corners();
corner->set_x(t.first);
corner->set_y(t.second);
corner->set_x(t.x);
corner->set_y(t.y);
}
m->clear_detected_corners();
for (const auto& t : value.GetDetectedCorners()) {
auto* corner = m->add_detected_corners();
corner->set_x(t.first);
corner->set_y(t.second);
corner->set_x(t.x);
corner->set_y(t.y);
}
}

165
photon-targeting/src/main/native/include/photon/dataflow/structures/Packet.h
View File

@@ -20,11 +20,46 @@
#include <algorithm>
#include <bit>
#include <cstring>
#include <iostream>
#include <optional>
#include <span>
#include <string>
#include <vector>
#include <wpi/Demangle.h>
#include <wpi/ct_string.h>
#include <wpi/struct/Struct.h>
namespace photon {
class Packet;
// Struct is where all our actual ser/de methods are implemented
template <typename T>
struct SerdeType {};
template <typename T>
concept PhotonStructSerializable = requires(Packet& packet, const T& value) {
typename SerdeType<typename std::remove_cvref_t<T>>;
// MD6sum of the message definition
{
SerdeType<typename std::remove_cvref_t<T>>::GetSchemaHash()
} -> std::convertible_to<std::string_view>;
// JSON-encoded message chema
{
SerdeType<typename std::remove_cvref_t<T>>::GetSchema()
} -> std::convertible_to<std::string_view>;
// Unpack myself from a packet
{
SerdeType<typename std::remove_cvref_t<T>>::Unpack(packet)
} -> std::same_as<typename std::remove_cvref_t<T>>;
// Pack myself into a packet
{
SerdeType<typename std::remove_cvref_t<T>>::Pack(packet, value)
} -> std::same_as<void>;
};
/**
* A packet that holds byte-packed data to be sent over NetworkTables.
*/
@@ -33,7 +68,7 @@ class Packet {
/**
* Constructs an empty packet.
*/
Packet() = default;
explicit Packet(int initialCapacity = 0) : packetData(initialCapacity) {}
/**
* Constructs a packet with the given data.
@@ -58,47 +93,41 @@ class Packet {
*/
inline size_t GetDataSize() const { return packetData.size(); }
/**
* Adds a value to the data buffer. This should only be used with PODs.
* @tparam T The data type.
* @param src The data source.
* @return A reference to the current object.
*/
template <typename T>
Packet& operator<<(T src) {
packetData.resize(packetData.size() + sizeof(T));
std::memcpy(packetData.data() + writePos, &src, sizeof(T));
template <typename T, typename... I>
requires wpi::StructSerializable<T, I...>
inline void Pack(const T& value) {
// as WPI struct stuff assumes constant data length - reserve at least
// enough new space for our new member
size_t newWritePos = writePos + wpi::GetStructSize<T, I...>();
packetData.resize(newWritePos);
if constexpr (std::endian::native == std::endian::little) {
// Reverse to big endian for network conventions.
std::reverse(packetData.data() + writePos,
packetData.data() + writePos + sizeof(T));
}
wpi::PackStruct(
std::span<uint8_t>{packetData.begin() + writePos, packetData.end()},
value);
writePos += sizeof(T);
return *this;
writePos = newWritePos;
}
/**
* Extracts a value to the provided destination.
* @tparam T The type of value to extract.
* @param value The value to extract.
* @return A reference to the current object.
*/
template <typename T>
Packet& operator>>(T& value) {
if (!packetData.empty()) {
std::memcpy(&value, packetData.data() + readPos, sizeof(T));
requires(PhotonStructSerializable<T>)
inline void Pack(const T& value) {
SerdeType<typename std::remove_cvref_t<T>>::Pack(*this, value);
}
if constexpr (std::endian::native == std::endian::little) {
// Reverse to little endian for host.
uint8_t& raw = reinterpret_cast<uint8_t&>(value);
std::reverse(&raw, &raw + sizeof(T));
}
}
template <typename T, typename... I>
requires wpi::StructSerializable<T, I...>
inline T Unpack() {
// Unpack this member, starting at readPos
T ret = wpi::UnpackStruct<T, I...>(
std::span<uint8_t>{packetData.begin() + readPos, packetData.end()});
readPos += wpi::GetStructSize<T, I...>();
return ret;
}
readPos += sizeof(T);
return *this;
template <typename T>
requires(PhotonStructSerializable<T>)
inline T Unpack() {
return SerdeType<typename std::remove_cvref_t<T>>::Unpack(*this);
}
bool operator==(const Packet& right) const;
@@ -106,9 +135,73 @@ class Packet {
private:
// Data stored in the packet
std::vector<uint8_t> packetData;
std::vector<uint8_t> packetData{};
size_t readPos = 0;
size_t writePos = 0;
};
template <typename T>
concept arithmetic = std::integral<T> || std::floating_point<T>;
// support encoding vectors
template <typename T>
requires(PhotonStructSerializable<T> || arithmetic<T>)
struct SerdeType<std::vector<T>> {
static std::vector<T> Unpack(Packet& packet) {
uint8_t len = packet.Unpack<uint8_t>();
std::vector<T> ret;
ret.reserve(len);
for (size_t i = 0; i < len; i++) {
ret.push_back(packet.Unpack<T>());
}
return ret;
}
static void Pack(Packet& packet, const std::vector<T>& value) {
packet.Pack<uint8_t>(value.size());
for (const auto& thing : value) {
packet.Pack<T>(thing);
}
}
static constexpr std::string_view GetSchemaHash() {
// quick hack lol
return SerdeType<T>::GetSchemaHash();
}
static constexpr std::string_view GetSchema() {
// TODO: this gets us the plain type name of T, but this is not schema JSON
// compliant!
return "TODO[?]";
}
};
// support encoding optional types
template <typename T>
requires(PhotonStructSerializable<T> || arithmetic<T>)
struct SerdeType<std::optional<T>> {
static std::optional<T> Unpack(Packet& packet) {
if (packet.Unpack<uint8_t>() == 1u) {
return packet.Unpack<T>();
} else {
return std::nullopt;
}
}
static void Pack(Packet& packet, const std::optional<T>& value) {
packet.Pack<uint8_t>(value.has_value());
if (value) {
packet.Pack<T>(*value);
}
}
static constexpr std::string_view GetSchemaHash() {
// quick hack lol
return SerdeType<T>::GetSchemaHash();
}
static constexpr std::string_view GetSchema() {
// TODO: this gets us the plain type name of T, but this is not schema JSON
// compliant!
return "TODO?";
}
};
} // namespace photon

39
photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h
View File

@@ -29,9 +29,11 @@
#define OPENCV_DISABLE_EIGEN_TENSOR_SUPPORT
#include <opencv2/core/eigen.hpp>
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/PnpResult.h"
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/TargetCorner.h"
namespace photon {
namespace OpenCVHelp {
@@ -96,6 +98,16 @@ static std::vector<cv::Point3f> RotationToRVec(
return points[0];
}
[[maybe_unused]] static std::vector<photon::TargetCorner> PointsToTargetCorners(
const std::vector<cv::Point2f>& points) {
std::vector<photon::TargetCorner> retVal;
retVal.reserve(points.size());
for (size_t i = 0; i < points.size(); i++) {
retVal.emplace_back(photon::TargetCorner{points[i].x, points[i].y});
}
return retVal;
}
[[maybe_unused]] static std::vector<std::pair<float, float>> PointsToCorners(
const std::vector<cv::Point2f>& points) {
std::vector<std::pair<float, float>> retVal;
@@ -116,6 +128,17 @@ static std::vector<cv::Point3f> RotationToRVec(
return retVal;
}
[[maybe_unused]] static std::vector<cv::Point2f> CornersToPoints(
const std::vector<photon::TargetCorner>& corners) {
std::vector<cv::Point2f> retVal;
retVal.reserve(corners.size());
for (size_t i = 0; i < corners.size(); i++) {
retVal.emplace_back(cv::Point2f{static_cast<float>(corners[i].x),
static_cast<float>(corners[i].y)});
}
return retVal;
}
[[maybe_unused]] static cv::Rect GetBoundingRect(
const std::vector<cv::Point2f>& points) {
return cv::boundingRect(points);
@@ -184,7 +207,7 @@ static frc::Rotation3d RVecToRotation(const cv::Mat& rvecInput) {
units::radian_t{data[2]}});
}
[[maybe_unused]] static photon::PNPResult SolvePNP_Square(
[[maybe_unused]] static std::optional<photon::PnpResult> SolvePNP_Square(
const Eigen::Matrix<double, 3, 3>& cameraMatrix,
const Eigen::Matrix<double, 8, 1>& distCoeffs,
std::vector<frc::Translation3d> modelTrls,
@@ -233,26 +256,25 @@ static frc::Rotation3d RVecToRotation(const cv::Mat& rvecInput) {
if (std::isnan(errors[0])) {
fmt::print("SolvePNP_Square failed!\n");
return std::nullopt;
}
if (alt) {
photon::PNPResult result;
photon::PnpResult result;
result.best = best;
result.alt = alt.value();
result.ambiguity = errors[0] / errors[1];
result.bestReprojErr = errors[0];
result.altReprojErr = errors[1];
result.isPresent = true;
return result;
} else {
photon::PNPResult result;
photon::PnpResult result;
result.best = best;
result.bestReprojErr = errors[0];
result.isPresent = true;
return result;
}
}
[[maybe_unused]] static photon::PNPResult SolvePNP_SQPNP(
[[maybe_unused]] static std::optional<photon::PnpResult> SolvePNP_SQPNP(
const Eigen::Matrix<double, 3, 3>& cameraMatrix,
const Eigen::Matrix<double, 8, 1>& distCoeffs,
std::vector<frc::Translation3d> modelTrls,
@@ -283,10 +305,9 @@ static frc::Rotation3d RVecToRotation(const cv::Mat& rvecInput) {
if (std::isnan(error)) {
fmt::print("SolvePNP_Square failed!\n");
}
photon::PNPResult result;
photon::PnpResult result;
result.best = best;
result.bestReprojErr = error;
result.isPresent = true;
return result;
}
} // namespace OpenCVHelp

53
photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h
View File

@@ -47,16 +47,18 @@ static std::vector<frc::AprilTag> GetVisibleLayoutTags(
return retVal;
}
static PNPResult EstimateCamPosePNP(
#include <iostream>
static std::optional<PnpResult> EstimateCamPosePNP(
const Eigen::Matrix<double, 3, 3>& cameraMatrix,
const Eigen::Matrix<double, 8, 1>& distCoeffs,
const std::vector<PhotonTrackedTarget>& visTags,
const frc::AprilTagFieldLayout& layout, const TargetModel& tagModel) {
if (visTags.size() == 0) {
return PNPResult();
return PnpResult();
}
std::vector<std::pair<float, float>> corners{};
std::vector<photon::TargetCorner> corners{};
std::vector<frc::AprilTag> knownTags{};
for (const auto& tgt : visTags) {
@@ -70,30 +72,30 @@ static PNPResult EstimateCamPosePNP(
}
}
if (knownTags.size() == 0 || corners.size() == 0 || corners.size() % 4 != 0) {
return PNPResult{};
return PnpResult{};
}
std::vector<cv::Point2f> points = OpenCVHelp::CornersToPoints(corners);
if (knownTags.size() == 1) {
PNPResult camToTag = OpenCVHelp::SolvePNP_Square(
cameraMatrix, distCoeffs, tagModel.GetVertices(), points);
if (!camToTag.isPresent) {
return PNPResult{};
auto camToTag = OpenCVHelp::SolvePNP_Square(cameraMatrix, distCoeffs,
tagModel.GetVertices(), points);
if (!camToTag) {
return PnpResult{};
}
frc::Pose3d bestPose =
knownTags[0].pose.TransformBy(camToTag.best.Inverse());
knownTags[0].pose.TransformBy(camToTag->best.Inverse());
frc::Pose3d altPose{};
if (camToTag.ambiguity != 0) {
altPose = knownTags[0].pose.TransformBy(camToTag.alt.Inverse());
if (camToTag->ambiguity != 0) {
altPose = knownTags[0].pose.TransformBy(camToTag->alt.Inverse());
}
frc::Pose3d o{};
PNPResult result{};
PnpResult result{};
result.best = frc::Transform3d{o, bestPose};
result.alt = frc::Transform3d{o, altPose};
result.ambiguity = camToTag.ambiguity;
result.bestReprojErr = camToTag.bestReprojErr;
result.altReprojErr = camToTag.altReprojErr;
result.ambiguity = camToTag->ambiguity;
result.bestReprojErr = camToTag->bestReprojErr;
result.altReprojErr = camToTag->altReprojErr;
return result;
} else {
std::vector<frc::Translation3d> objectTrls{};
@@ -101,20 +103,15 @@ static PNPResult EstimateCamPosePNP(
auto verts = tagModel.GetFieldVertices(tag.pose);
objectTrls.insert(objectTrls.end(), verts.begin(), verts.end());
}
PNPResult camToOrigin = OpenCVHelp::SolvePNP_SQPNP(cameraMatrix, distCoeffs,
objectTrls, points);
if (!camToOrigin.isPresent) {
return PNPResult{};
} else {
PNPResult result{};
result.best = camToOrigin.best.Inverse(),
result.alt = camToOrigin.alt.Inverse(),
result.ambiguity = camToOrigin.ambiguity;
result.bestReprojErr = camToOrigin.bestReprojErr;
result.altReprojErr = camToOrigin.altReprojErr;
result.isPresent = true;
return result;
auto ret = OpenCVHelp::SolvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls,
points);
if (ret) {
// Invert best/alt transforms
ret->best = ret->best.Inverse();
ret->alt = ret->alt.Inverse();
}
return ret;
}
}

22
photon-targeting/src/main/native/include/photon/targeting/MultiTargetPNPResult.h
View File

@@ -17,21 +17,29 @@
#pragma once
#include <utility>
#include <frc/geometry/Transform3d.h>
#include <wpi/SmallVector.h>
#include "PNPResult.h"
#include "PnpResult.h"
#include "photon/dataflow/structures/Packet.h"
#include "photon/struct/MultiTargetPNPResultStruct.h"
namespace photon {
class MultiTargetPNPResult {
class MultiTargetPNPResult : public MultiTargetPNPResult_PhotonStruct {
using Base = MultiTargetPNPResult_PhotonStruct;
public:
PNPResult result;
wpi::SmallVector<int16_t, 32> fiducialIdsUsed;
explicit MultiTargetPNPResult(Base&& data) : Base(data) {}
bool operator==(const MultiTargetPNPResult& other) const;
template <typename... Args>
explicit MultiTargetPNPResult(Args&&... args)
: Base{std::forward<Args>(args)...} {}
friend Packet& operator<<(Packet& packet, const MultiTargetPNPResult& result);
friend Packet& operator>>(Packet& packet, MultiTargetPNPResult& result);
friend bool operator==(MultiTargetPNPResult const&,
MultiTargetPNPResult const&) = default;
};
} // namespace photon
#include "photon/serde/MultiTargetPNPResultSerde.h"

40
photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineMetadata.h Normal file
View File

@@ -0,0 +1,40 @@
/*
* 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/>.
*/
#pragma once
#include <utility>
#include "photon/struct/PhotonPipelineMetadataStruct.h"
namespace photon {
class PhotonPipelineMetadata : public PhotonPipelineMetadata_PhotonStruct {
using Base = PhotonPipelineMetadata_PhotonStruct;
public:
explicit PhotonPipelineMetadata(Base&& data) : Base(data) {}
template <typename... Args>
explicit PhotonPipelineMetadata(Args&&... args)
: Base{std::forward<Args>(args)...} {}
friend bool operator==(PhotonPipelineMetadata const&,
PhotonPipelineMetadata const&) = default;
};
} // namespace photon
#include "photon/serde/PhotonPipelineMetadataSerde.h"

90
photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h
View File

@@ -19,6 +19,7 @@
#include <span>
#include <string>
#include <utility>
#include <frc/Errors.h>
#include <units/time.h>
@@ -27,34 +28,41 @@
#include "MultiTargetPNPResult.h"
#include "PhotonTrackedTarget.h"
#include "photon/dataflow/structures/Packet.h"
#include "photon/struct/PhotonPipelineResultStruct.h"
namespace photon {
/**
* Represents a pipeline result from a PhotonCamera.
*/
class PhotonPipelineResult {
public:
/**
* Constructs an empty pipeline result
*/
PhotonPipelineResult() = default;
class PhotonPipelineResult : public PhotonPipelineResult_PhotonStruct {
using Base = PhotonPipelineResult_PhotonStruct;
/**
* Constructs a pipeline result.
* @param sequenceID The number of frames processed by this camera since boot
* @param captureTimestamp The time, in uS in the coprocessor's timebase, that
* the coprocessor captured the image this result contains the targeting info
* of
* @param publishTimestamp The time, in uS in the coprocessor's timebase, that
* the coprocessor published targeting info
* @param targets The list of targets identified by the pipeline.
* @param multitagResult The multitarget result. Default to empty
*/
PhotonPipelineResult(int64_t sequenceID,
units::microsecond_t captureTimestamp,
units::microsecond_t publishTimestamp,
std::span<const PhotonTrackedTarget> targets,
MultiTargetPNPResult multitagResult = {});
public:
PhotonPipelineResult() : Base() {}
explicit PhotonPipelineResult(Base&& data) : Base(data) {}
// Don't forget to deal with our ntRecieveTimestamp
PhotonPipelineResult(const PhotonPipelineResult& other)
: Base(other), ntReceiveTimestamp(other.ntReceiveTimestamp) {}
PhotonPipelineResult(PhotonPipelineResult& other)
: Base(other), ntReceiveTimestamp(other.ntReceiveTimestamp) {}
PhotonPipelineResult(PhotonPipelineResult&& other)
: Base(std::move(other)),
ntReceiveTimestamp(std::move(other.ntReceiveTimestamp)) {}
auto& operator=(const PhotonPipelineResult& other) {
Base::operator=(other);
ntReceiveTimestamp = other.ntReceiveTimestamp;
return *this;
}
auto& operator=(PhotonPipelineResult&& other) {
ntReceiveTimestamp = other.ntReceiveTimestamp;
Base::operator=(std::move(other));
return *this;
}
template <typename... Args>
explicit PhotonPipelineResult(Args&&... args)
: Base{std::forward<Args>(args)...} {}
/**
* Returns the best target in this pipeline result. If there are no targets,
@@ -73,7 +81,7 @@ class PhotonPipelineResult {
"http://docs.photonvision.org");
HAS_WARNED = true;
}
return HasTargets() ? targets[0] : PhotonTrackedTarget();
return HasTargets() ? targets[0] : PhotonTrackedTarget{};
}
/**
@@ -81,7 +89,8 @@ class PhotonPipelineResult {
* @return The latency in the pipeline.
*/
units::millisecond_t GetLatency() const {
return publishTimestamp - captureTimestamp;
return units::microsecond_t{static_cast<double>(
metadata.publishTimestampMicros - metadata.captureTimestampMicros)};
}
/**
@@ -91,7 +100,7 @@ class PhotonPipelineResult {
* with a timestamp.
*/
units::second_t GetTimestamp() const {
return ntRecieveTimestamp - (publishTimestamp - captureTimestamp);
return ntReceiveTimestamp - GetLatency();
}
/**
@@ -99,17 +108,19 @@ class PhotonPipelineResult {
* Be sure to check getMultiTagResult().estimatedPose.isPresent before using
* the pose estimate!
*/
const MultiTargetPNPResult& MultiTagResult() const { return multitagResult; }
const std::optional<MultiTargetPNPResult>& MultiTagResult() const {
return multitagResult;
}
/**
* The number of non-empty frames processed by this camera since boot. Useful
* to checking if a camera is alive.
*/
int64_t SequenceID() const { return sequenceID; }
int64_t SequenceID() const { return metadata.sequenceID; }
/** Sets the FPGA timestamp this result was recieved by robot code */
void SetRecieveTimestamp(const units::second_t timestamp) {
this->ntRecieveTimestamp = timestamp;
/** Sets the FPGA timestamp this result was Received by robot code */
void SetReceiveTimestamp(const units::second_t timestamp) {
this->ntReceiveTimestamp = timestamp;
}
/**
@@ -127,24 +138,15 @@ class PhotonPipelineResult {
return targets;
}
bool operator==(const PhotonPipelineResult& other) const;
friend bool operator==(PhotonPipelineResult const&,
PhotonPipelineResult const&) = default;
friend Packet& operator<<(Packet& packet, const PhotonPipelineResult& result);
friend Packet& operator>>(Packet& packet, PhotonPipelineResult& result);
// Mirror of the heartbeat entry -- monotonically increasing
int64_t sequenceID = -1;
// Image capture and NT publish timestamp, in microseconds and in the
// coprocessor timebase. As reported by WPIUtilJNI::now.
units::microsecond_t captureTimestamp;
units::microsecond_t publishTimestamp;
// Since we don't trust NT time sync, keep track of when we got this packet
// into robot code
units::microsecond_t ntRecieveTimestamp = -1_s;
units::microsecond_t ntReceiveTimestamp = -1_s;
wpi::SmallVector<PhotonTrackedTarget, 10> targets;
MultiTargetPNPResult multitagResult;
inline static bool HAS_WARNED = false;
};
} // namespace photon
#include "photon/serde/PhotonPipelineResultSerde.h"

57
photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h
View File

@@ -25,36 +25,23 @@
#include <frc/geometry/Transform3d.h>
#include <wpi/SmallVector.h>
#include "photon/dataflow/structures/Packet.h"
#include "photon/struct/PhotonTrackedTargetStruct.h"
namespace photon {
/**
* Represents a tracked target within a pipeline.
*/
class PhotonTrackedTarget {
class PhotonTrackedTarget : public PhotonTrackedTarget_PhotonStruct {
using Base = PhotonTrackedTarget_PhotonStruct;
public:
/**
* Constructs an empty target.
*/
PhotonTrackedTarget() = default;
/**
* Constructs a target.
* @param yaw The yaw of the target.
* @param pitch The pitch of the target.
* @param area The area of the target.
* @param skew The skew of the target.
* @param pose The camera-relative pose of the target.
* @param alternatePose The alternate camera-relative pose of the target.
* @param minAreaRectCorners The corners of the bounding rectangle.
* @param detectedCorners All detected corners
*/
PhotonTrackedTarget(
double yaw, double pitch, double area, double skew, int fiducialID,
int objDetectCassId, float objDetectConf, const frc::Transform3d& pose,
const frc::Transform3d& alternatePose, double ambiguity,
const wpi::SmallVector<std::pair<double, double>, 4> minAreaRectCorners,
const std::vector<std::pair<double, double>> detectedCorners);
explicit PhotonTrackedTarget(Base&& data) : Base(data) {}
template <typename... Args>
explicit PhotonTrackedTarget(Args&&... args)
: Base{std::forward<Args>(args)...} {}
/**
* Returns the target yaw (positive-left).
@@ -103,8 +90,7 @@ class PhotonTrackedTarget {
* down), in no particular order, of the minimum area bounding rectangle of
* this target
*/
const wpi::SmallVector<std::pair<double, double>, 4>& GetMinAreaRectCorners()
const {
const std::vector<photon::TargetCorner>& GetMinAreaRectCorners() const {
return minAreaRectCorners;
}
@@ -119,7 +105,7 @@ class PhotonTrackedTarget {
* V + Y | |
* 0 ----- 1
*/
const std::vector<std::pair<double, double>>& GetDetectedCorners() const {
const std::vector<photon::TargetCorner>& GetDetectedCorners() const {
return detectedCorners;
}
@@ -149,22 +135,9 @@ class PhotonTrackedTarget {
return altCameraToTarget;
}
bool operator==(const PhotonTrackedTarget& other) const;
friend Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target);
friend Packet& operator>>(Packet& packet, PhotonTrackedTarget& target);
double yaw = 0;
double pitch = 0;
double area = 0;
double skew = 0;
int fiducialId;
int objDetectId;
float objDetectConf;
frc::Transform3d bestCameraToTarget;
frc::Transform3d altCameraToTarget;
double poseAmbiguity;
wpi::SmallVector<std::pair<double, double>, 4> minAreaRectCorners;
std::vector<std::pair<double, double>> detectedCorners;
friend bool operator==(PhotonTrackedTarget const&,
PhotonTrackedTarget const&) = default;
};
} // namespace photon
#include "photon/serde/PhotonTrackedTargetSerde.h"

27
photon-targeting/src/main/native/include/photon/targeting/PNPResult.h → photon-targeting/src/main/native/include/photon/targeting/PnpResult.h
View File

@@ -17,29 +17,26 @@
#pragma once
#include <utility>
#include <frc/geometry/Transform3d.h>
#include "photon/dataflow/structures/Packet.h"
#include "photon/struct/PnpResultStruct.h"
namespace photon {
class PNPResult {
public:
// This could be wrapped in an std::optional, but chose to do it this way to
// mirror Java
bool isPresent{false};
struct PnpResult : public PnpResult_PhotonStruct {
using Base = PnpResult_PhotonStruct;
frc::Transform3d best{};
double bestReprojErr{0};
explicit PnpResult(Base&& data) : Base(data) {}
frc::Transform3d alt{};
double altReprojErr{0};
template <typename... Args>
explicit PnpResult(Args&&... args) : Base{std::forward<Args>(args)...} {}
double ambiguity{0};
bool operator==(const PNPResult& other) const;
friend Packet& operator<<(Packet& packet, const PNPResult& target);
friend Packet& operator>>(Packet& packet, PNPResult& target);
friend bool operator==(PnpResult const&, PnpResult const&) = default;
};
} // namespace photon
#include "photon/serde/PnpResultSerde.h"

38
photon-targeting/src/main/native/include/photon/targeting/TargetCorner.h Normal file
View File

@@ -0,0 +1,38 @@
/*
* 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/>.
*/
#include <utility>
#include "photon/struct/TargetCornerStruct.h"
#pragma once
namespace photon {
class TargetCorner : public TargetCorner_PhotonStruct {
using Base = TargetCorner_PhotonStruct;
public:
explicit TargetCorner(Base&& data) : Base(data) {}
template <typename... Args>
explicit TargetCorner(Args&&... args) : Base{std::forward<Args>(args)...} {}
friend bool operator==(TargetCorner const&, TargetCorner const&) = default;
};
} // namespace photon
#include "photon/serde/TargetCornerSerde.h"

8
photon-targeting/src/main/native/include/photon/targeting/proto/PNPResultProto.h
View File

@@ -19,12 +19,12 @@
#include <wpi/protobuf/Protobuf.h>
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/PnpResult.h"
template <>
struct wpi::Protobuf<photon::PNPResult> {
struct wpi::Protobuf<photon::PnpResult> {
static google::protobuf::Message* New(google::protobuf::Arena* arena);
static photon::PNPResult Unpack(const google::protobuf::Message& msg);
static photon::PnpResult Unpack(const google::protobuf::Message& msg);
static void Pack(google::protobuf::Message* msg,
const photon::PNPResult& value);
const photon::PnpResult& value);
};

3
photon-targeting/src/main/proto/photon.proto
View File

@@ -29,7 +29,6 @@ message ProtobufTargetCorner {
}
message ProtobufPNPResult {
bool is_present = 1;
wpi.proto.ProtobufTransform3d best = 2;
double best_reproj_err = 3;
optional wpi.proto.ProtobufTransform3d alt = 4;
@@ -62,7 +61,7 @@ message ProtobufPhotonPipelineResult {
double latency_ms = 1 [deprecated = true];
repeated ProtobufPhotonTrackedTarget targets = 2;
ProtobufMultiTargetPNPResult multi_target_result = 3;
optional ProtobufMultiTargetPNPResult multi_target_result = 3;
int64 sequence_id = 4;
int64 capture_timestamp_micros = 5;