mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-23 01:21:40 +00:00
Auto-generate packet dataclasses with Jinja (#1374)
This commit is contained in:
@@ -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
|
||||
|
||||
@@ -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
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -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"
|
||||
|
||||
@@ -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"
|
||||
@@ -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"
|
||||
|
||||
@@ -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"
|
||||
|
||||
@@ -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"
|
||||
@@ -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"
|
||||
@@ -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);
|
||||
};
|
||||
|
||||
Reference in New Issue
Block a user