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

Use ReadQueue for PhotonCamera timestamps (#1316)

Browse Source 
This removes the extra GetLastChange call to keep everything properly
atomic.

Closes #1303
This commit is contained in:
Matt
2024-08-04 14:23:46 -04:00
committed by GitHub
parent 37e9d40762
commit 67463a020a
29 changed files with 1057 additions and 1614 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
photon-lib/src/main/native/include/photon/PhotonCamera.h
View File

@@ -75,13 +75,20 @@ class PhotonCamera {
PhotonCamera(PhotonCamera&&) = default;
virtual ~PhotonCamera() = default;
~PhotonCamera() = default;
/**
* Returns the latest pipeline result.
* @return The latest pipeline result.
* The list of pipeline results sent by PhotonVision since the last call to
* GetAllUnreadResults(). Calling this function clears the internal FIFO
* queue, and multiple calls to GetAllUnreadResults() will return different
* (potentially empty) result arrays. Be careful to call this exactly ONCE per
* loop of your robot code! FIFO depth is limited to 20 changes, so make sure
* to call this frequently enough to avoid old results being discarded, too!
*/
virtual PhotonPipelineResult GetLatestResult();
std::vector<PhotonPipelineResult> GetAllUnreadResults();
[[deprecated("Replace with GetAllUnreadResults")]]
PhotonPipelineResult GetLatestResult();
/**
* Toggles driver mode.
@@ -173,7 +180,7 @@ class PhotonCamera {
// For use in tests
bool test = false;
PhotonPipelineResult testResult;
std::vector<PhotonPipelineResult> testResult;
protected:
std::shared_ptr<nt::NetworkTable> mainTable;
@@ -201,8 +208,6 @@ class PhotonCamera {
std::string path;
std::string cameraName;
mutable Packet packet;
private:
units::second_t lastVersionCheckTime = 0_s;
static bool VERSION_CHECK_ENABLED;

59
photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h
View File

@@ -29,6 +29,7 @@
#include <frc/apriltag/AprilTagFieldLayout.h>
#include <frc/geometry/Pose3d.h>
#include <frc/geometry/Transform3d.h>
#include <opencv2/core/mat.hpp>
#include "photon/PhotonCamera.h"
#include "photon/dataflow/structures/Packet.h"
@@ -37,10 +38,6 @@
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
namespace cv {
class Mat;
} // namespace cv
namespace photon {
enum PoseStrategy {
LOWEST_AMBIGUITY = 0,
@@ -85,28 +82,6 @@ class PhotonPoseEstimator {
/**
* Create a new PhotonPoseEstimator.
*
* <p>Example: {@code <code> <p> Map<Integer, Pose3d> map = new HashMap<>();
* <p> map.put(1, new Pose3d(1.0, 2.0, 3.0, new Rotation3d())); // Tag ID 1 is
* at (1.0,2.0,3.0) </code> }
*
* @param aprilTags A AprilTagFieldLayout linking AprilTag IDs to Pose3ds with
* respect to the FIRST field.
* @param strategy The strategy it should use to determine the best pose.
* @param camera PhotonCameras and
* @param robotToCamera Transform3d from the center of the robot to the camera
* mount positions (ie, robot ➔ camera).
*/
explicit PhotonPoseEstimator(frc::AprilTagFieldLayout aprilTags,
PoseStrategy strategy, PhotonCamera&& camera,
frc::Transform3d robotToCamera);
/**
* Create a new PhotonPoseEstimator.
*
* <p>Example: {@code <code> <p> Map<Integer, Pose3d> map = new HashMap<>();
* <p> map.put(1, new Pose3d(1.0, 2.0, 3.0, new Rotation3d())); // Tag ID 1 is
* at (1.0,2.0,3.0) </code> }
*
* @param aprilTags A AprilTagFieldLayout linking AprilTag IDs to Pose3ds with
* respect to the FIRST field.
* @param strategy The strategy it should use to determine the best pose.
@@ -198,32 +173,26 @@ class PhotonPoseEstimator {
inline void SetLastPose(frc::Pose3d lastPose) { this->lastPose = lastPose; }
/**
* Update the pose estimator. Internally grabs a new PhotonPipelineResult from
* the camera and process it.
*/
std::optional<EstimatedRobotPose> Update();
/**
* Update the pose estimator.
*/
std::optional<EstimatedRobotPose> Update(const PhotonPipelineResult& result);
/**
* Update the pose estimator.
* Update the pose estimator. If updating multiple times per loop, you should
* call this exactly once per new result, in order of increasing result
* timestamp.
*
* @param result The vision targeting result to process
* @param cameraIntrinsics The camera calibration pinhole coefficients matrix.
* Only required if doing multitag-on-rio, and may be nullopt otherwise.
* @param distCoeffsData The camera calibration distortion coefficients. Only
* required if doing multitag-on-rio, and may be nullopt otherwise.
*/
std::optional<EstimatedRobotPose> Update(
const PhotonPipelineResult& result,
std::optional<PhotonCamera::CameraMatrix> cameraMatrixData,
std::optional<PhotonCamera::DistortionMatrix> coeffsData);
inline std::shared_ptr<PhotonCamera> GetCamera() { return camera; }
std::optional<PhotonCamera::CameraMatrix> cameraMatrixData = std::nullopt,
std::optional<PhotonCamera::DistortionMatrix> coeffsData = std::nullopt);
private:
frc::AprilTagFieldLayout aprilTags;
PoseStrategy strategy;
PoseStrategy multiTagFallbackStrategy = LOWEST_AMBIGUITY;
std::shared_ptr<PhotonCamera> camera;
frc::Transform3d m_robotToCamera;
frc::Pose3d lastPose;
@@ -280,9 +249,7 @@ class PhotonPoseEstimator {
* @return the estimated position of the robot in the FCS
*/
std::optional<EstimatedRobotPose> MultiTagOnCoprocStrategy(
PhotonPipelineResult result,
std::optional<PhotonCamera::CameraMatrix> camMat,
std::optional<PhotonCamera::DistortionMatrix> distCoeffs);
PhotonPipelineResult result);
/**
* Return the pose calculation using all targets in view in the same PNP()

388
photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h
View File

@@ -49,382 +49,50 @@
namespace photon {
class PhotonCameraSim {
public:
explicit PhotonCameraSim(PhotonCamera* camera)
: PhotonCameraSim(camera, photon::SimCameraProperties::PERFECT_90DEG()) {}
PhotonCameraSim(PhotonCamera* camera, const SimCameraProperties& props)
: prop(props), cam(camera) {
SetMinTargetAreaPixels(kDefaultMinAreaPx);
videoSimRaw = frc::CameraServer::PutVideo(
std::string{camera->GetCameraName()} + "-raw", prop.GetResWidth(),
prop.GetResHeight());
videoSimRaw.SetPixelFormat(cs::VideoMode::PixelFormat::kGray);
videoSimProcessed = frc::CameraServer::PutVideo(
std::string{camera->GetCameraName()} + "-processed", prop.GetResWidth(),
prop.GetResHeight());
ts.subTable = cam->GetCameraTable();
ts.UpdateEntries();
}
explicit PhotonCameraSim(PhotonCamera* camera);
PhotonCameraSim(PhotonCamera* camera, const SimCameraProperties& props);
PhotonCameraSim(PhotonCamera* camera, const SimCameraProperties& props,
double minTargetAreaPercent, units::meter_t maxSightRange)
: PhotonCameraSim(camera, props) {
this->minTargetAreaPercent = minTargetAreaPercent;
this->maxSightRange = maxSightRange;
}
PhotonCamera* GetCamera() { return cam; }
double GetMinTargetAreaPercent() { return minTargetAreaPercent; }
double GetMinTargetAreaPixels() {
double minTargetAreaPercent, units::meter_t maxSightRange);
inline PhotonCamera* GetCamera() { return cam; }
inline double GetMinTargetAreaPercent() { return minTargetAreaPercent; }
inline double GetMinTargetAreaPixels() {
return minTargetAreaPercent / 100.0 * prop.GetResArea();
}
units::meter_t GetMaxSightRange() { return maxSightRange; }
const cs::CvSource& GetVideoSimRaw() { return videoSimRaw; }
const cv::Mat& GetVideoSimFrameRaw() { return videoSimFrameRaw; }
inline units::meter_t GetMaxSightRange() { return maxSightRange; }
inline const cs::CvSource& GetVideoSimRaw() { return videoSimRaw; }
inline const cv::Mat& GetVideoSimFrameRaw() { return videoSimFrameRaw; }
bool CanSeeTargetPose(const frc::Pose3d& camPose,
const VisionTargetSim& target) {
CameraTargetRelation rel{camPose, target.GetPose()};
return ((units::math::abs(rel.camToTargYaw.Degrees()) <
prop.GetHorizFOV().Degrees() / 2.0) &&
(units::math::abs(rel.camToTargPitch.Degrees()) <
prop.GetVertFOV().Degrees() / 2.0) &&
(!target.GetModel().GetIsPlanar() ||
units::math::abs(rel.targToCamAngle.Degrees()) < 90_deg) &&
(rel.camToTarg.Translation().Norm() <= maxSightRange));
}
bool CanSeeCorner(const std::vector<cv::Point2f>& points) {
for (const auto& pt : points) {
if (std::clamp<float>(pt.x, 0, prop.GetResWidth()) != pt.x ||
std::clamp<float>(pt.y, 0, prop.GetResHeight()) != pt.y) {
return false;
}
}
return true;
}
std::optional<uint64_t> ConsumeNextEntryTime() {
uint64_t now = wpi::Now();
uint64_t timestamp{};
int iter = 0;
while (now >= nextNTEntryTime) {
timestamp = nextNTEntryTime;
uint64_t frameTime = prop.EstSecUntilNextFrame()
.convert<units::microseconds>()
.to<uint64_t>();
nextNTEntryTime += frameTime;
const VisionTargetSim& target);
bool CanSeeCorner(const std::vector<cv::Point2f>& points);
std::optional<uint64_t> ConsumeNextEntryTime();
if (iter++ > 50) {
timestamp = now;
nextNTEntryTime = now + frameTime;
break;
}
}
if (timestamp != 0) {
return timestamp;
} else {
return std::nullopt;
}
}
void SetMinTargetAreaPercent(double areaPercent) {
inline void SetMinTargetAreaPercent(double areaPercent) {
minTargetAreaPercent = areaPercent;
}
void SetMinTargetAreaPixels(double areaPx) {
inline void SetMinTargetAreaPixels(double areaPx) {
minTargetAreaPercent = areaPx / prop.GetResArea() * 100;
}
void SetMaxSightRange(units::meter_t range) { maxSightRange = range; }
void EnableRawStream(bool enabled) { videoSimRawEnabled = enabled; }
void EnableDrawWireframe(bool enabled) { videoSimWireframeEnabled = enabled; }
void SetWireframeResolution(double resolution) {
inline void SetMaxSightRange(units::meter_t range) { maxSightRange = range; }
inline void EnableRawStream(bool enabled) { videoSimRawEnabled = enabled; }
inline void EnableDrawWireframe(bool enabled) {
videoSimWireframeEnabled = enabled;
}
inline void SetWireframeResolution(double resolution) {
videoSimWireframeResolution = resolution;
}
void EnabledProcessedStream(double enabled) { videoSimProcEnabled = enabled; }
inline void EnabledProcessedStream(double enabled) {
videoSimProcEnabled = enabled;
}
PhotonPipelineResult Process(units::second_t latency,
const frc::Pose3d& cameraPose,
std::vector<VisionTargetSim> targets) {
std::sort(
targets.begin(), targets.end(),
[cameraPose](const VisionTargetSim& t1, const VisionTargetSim& t2) {
units::meter_t dist1 =
t1.GetPose().Translation().Distance(cameraPose.Translation());
units::meter_t dist2 =
t2.GetPose().Translation().Distance(cameraPose.Translation());
return dist1 > dist2;
});
std::vector<VisionTargetSim> targets);
std::vector<std::pair<VisionTargetSim, std::vector<cv::Point2f>>>
visibleTgts{};
std::vector<PhotonTrackedTarget> detectableTgts{};
RotTrlTransform3d camRt = RotTrlTransform3d::MakeRelativeTo(cameraPose);
VideoSimUtil::UpdateVideoProp(videoSimRaw, prop);
VideoSimUtil::UpdateVideoProp(videoSimProcessed, prop);
cv::Size videoFrameSize{prop.GetResWidth(), prop.GetResHeight()};
cv::Mat blankFrame = cv::Mat::zeros(videoFrameSize, CV_8UC1);
blankFrame.assignTo(videoSimFrameRaw);
for (const auto& tgt : targets) {
if (!CanSeeTargetPose(cameraPose, tgt)) {
continue;
}
std::vector<frc::Translation3d> fieldCorners = tgt.GetFieldVertices();
if (tgt.GetModel().GetIsSpherical()) {
TargetModel model = tgt.GetModel();
fieldCorners = model.GetFieldVertices(TargetModel::GetOrientedPose(
tgt.GetPose().Translation(), cameraPose.Translation()));
}
std::vector<cv::Point2f> imagePoints = OpenCVHelp::ProjectPoints(
prop.GetIntrinsics(), prop.GetDistCoeffs(), camRt, fieldCorners);
if (tgt.GetModel().GetIsSpherical()) {
cv::Point2d center = OpenCVHelp::AvgPoint(imagePoints);
int l = 0;
int t = 0;
int b = 0;
int r = 0;
for (int i = 0; i < 4; i++) {
if (imagePoints[i].x < imagePoints[l].x) {
l = i;
}
}
cv::Point2d lc = imagePoints[l];
std::array<double, 4> angles{};
t = (l + 1) % 4;
b = (l + 1) % 4;
for (int i = 0; i < 4; i++) {
if (i == l) {
continue;
}
cv::Point2d ic = imagePoints[i];
angles[i] = std::atan2(lc.y - ic.y, ic.x - lc.x);
if (angles[i] >= angles[t]) {
t = i;
}
if (angles[i] <= angles[b]) {
b = i;
}
}
for (int i = 0; i < 4; i++) {
if (i != t && i != l && i != b) {
r = i;
}
}
cv::RotatedRect rect{
cv::Point2d{center.x, center.y},
cv::Size2d{imagePoints[r].x - lc.x,
imagePoints[b].y - imagePoints[t].y},
units::radian_t{-angles[r]}.convert<units::degrees>().to<float>()};
std::vector<cv::Point2f> points{};
rect.points(points);
// Can't find an easier way to convert from Point2f to Point2d
imagePoints.clear();
std::transform(points.begin(), points.end(),
std::back_inserter(imagePoints),
[](const cv::Point2f& p) { return (cv::Point2d)p; });
}
visibleTgts.emplace_back(std::make_pair(tgt, imagePoints));
std::vector<cv::Point2f> noisyTargetCorners =
prop.EstPixelNoise(imagePoints);
cv::RotatedRect minAreaRect =
OpenCVHelp::GetMinAreaRect(noisyTargetCorners);
std::vector<cv::Point2f> minAreaRectPts;
minAreaRectPts.reserve(4);
minAreaRect.points(minAreaRectPts);
cv::Point2d centerPt = minAreaRect.center;
frc::Rotation3d centerRot = prop.GetPixelRot(centerPt);
double areaPercent = prop.GetContourAreaPercent(noisyTargetCorners);
if (!(CanSeeCorner(noisyTargetCorners) &&
areaPercent >= minTargetAreaPercent)) {
continue;
}
PNPResult pnpSim{};
if (tgt.fiducialId >= 0 && tgt.GetFieldVertices().size() == 4) {
pnpSim = OpenCVHelp::SolvePNP_Square(
prop.GetIntrinsics(), prop.GetDistCoeffs(),
tgt.GetModel().GetVertices(), noisyTargetCorners);
}
std::vector<std::pair<float, float>> tempCorners =
OpenCVHelp::PointsToCorners(minAreaRectPts);
wpi::SmallVector<std::pair<double, double>, 4> smallVec;
for (const auto& corner : tempCorners) {
smallVec.emplace_back(
std::make_pair(static_cast<double>(corner.first),
static_cast<double>(corner.second)));
}
std::vector<std::pair<float, float>> cornersFloat =
OpenCVHelp::PointsToCorners(noisyTargetCorners);
std::vector<std::pair<double, double>> cornersDouble{cornersFloat.begin(),
cornersFloat.end()};
detectableTgts.emplace_back(PhotonTrackedTarget{
-centerRot.Z().convert<units::degrees>().to<double>(),
-centerRot.Y().convert<units::degrees>().to<double>(), areaPercent,
centerRot.X().convert<units::degrees>().to<double>(), tgt.fiducialId,
-1, -1, pnpSim.best, pnpSim.alt, pnpSim.ambiguity, smallVec,
cornersDouble});
}
if (videoSimRawEnabled) {
if (videoSimWireframeEnabled) {
VideoSimUtil::DrawFieldWireFrame(
camRt, prop, videoSimWireframeResolution, 1.5, cv::Scalar{80}, 6, 1,
cv::Scalar{30}, videoSimFrameRaw);
}
for (const auto& pair : visibleTgts) {
VisionTargetSim tgt = pair.first;
std::vector<cv::Point2f> corners = pair.second;
if (tgt.fiducialId > 0) {
VideoSimUtil::Warp165h5TagImage(tgt.fiducialId, corners, true,
videoSimFrameRaw);
} else if (!tgt.GetModel().GetIsSpherical()) {
std::vector<cv::Point2f> contour = corners;
if (!tgt.GetModel().GetIsPlanar()) {
contour = OpenCVHelp::GetConvexHull(contour);
}
VideoSimUtil::DrawPoly(contour, -1, cv::Scalar{255}, true,
videoSimFrameRaw);
} else {
VideoSimUtil::DrawInscribedEllipse(corners, cv::Scalar{255},
videoSimFrameRaw);
}
}
videoSimRaw.PutFrame(videoSimFrameRaw);
} else {
videoSimRaw.SetConnectionStrategy(
cs::VideoSource::ConnectionStrategy::kConnectionForceClose);
}
if (videoSimProcEnabled) {
cv::cvtColor(videoSimFrameRaw, videoSimFrameProcessed,
cv::COLOR_GRAY2BGR);
cv::drawMarker(
videoSimFrameProcessed,
cv::Point2d{prop.GetResWidth() / 2.0, prop.GetResHeight() / 2.0},
cv::Scalar{0, 255, 0}, cv::MARKER_CROSS,
static_cast<int>(
VideoSimUtil::GetScaledThickness(15, videoSimFrameProcessed)),
static_cast<int>(
VideoSimUtil::GetScaledThickness(1, videoSimFrameProcessed)),
cv::LINE_AA);
for (const auto& tgt : detectableTgts) {
auto detectedCornersDouble = tgt.GetDetectedCorners();
std::vector<std::pair<float, float>> detectedCornerFloat{
detectedCornersDouble.begin(), detectedCornersDouble.end()};
if (tgt.GetFiducialId() >= 0) {
VideoSimUtil::DrawTagDetection(
tgt.GetFiducialId(),
OpenCVHelp::CornersToPoints(detectedCornerFloat),
videoSimFrameProcessed);
} else {
cv::rectangle(videoSimFrameProcessed,
OpenCVHelp::GetBoundingRect(
OpenCVHelp::CornersToPoints(detectedCornerFloat)),
cv::Scalar{0, 0, 255},
static_cast<int>(VideoSimUtil::GetScaledThickness(
1, videoSimFrameProcessed)),
cv::LINE_AA);
wpi::SmallVector<std::pair<double, double>, 4> smallVec =
tgt.GetMinAreaRectCorners();
std::vector<std::pair<float, float>> cornersCopy{};
cornersCopy.reserve(4);
for (const auto& corner : smallVec) {
cornersCopy.emplace_back(
std::make_pair(static_cast<float>(corner.first),
static_cast<float>(corner.second)));
}
VideoSimUtil::DrawPoly(
OpenCVHelp::CornersToPoints(cornersCopy),
static_cast<int>(
VideoSimUtil::GetScaledThickness(1, videoSimFrameProcessed)),
cv::Scalar{255, 30, 30}, true, videoSimFrameProcessed);
}
}
videoSimProcessed.PutFrame(videoSimFrameProcessed);
} else {
videoSimProcessed.SetConnectionStrategy(
cs::VideoSource::ConnectionStrategy::kConnectionForceClose);
}
MultiTargetPNPResult multiTagResults{};
std::vector<frc::AprilTag> visibleLayoutTags =
VisionEstimation::GetVisibleLayoutTags(detectableTgts, tagLayout);
if (visibleLayoutTags.size() > 1) {
wpi::SmallVector<int16_t, 32> usedIds{};
std::transform(visibleLayoutTags.begin(), visibleLayoutTags.end(),
usedIds.begin(),
[](const frc::AprilTag& tag) { return tag.ID; });
std::sort(usedIds.begin(), usedIds.end());
PNPResult pnpResult = VisionEstimation::EstimateCamPosePNP(
prop.GetIntrinsics(), prop.GetDistCoeffs(), detectableTgts, tagLayout,
kAprilTag36h11);
multiTagResults = MultiTargetPNPResult{pnpResult, usedIds};
}
units::second_t now = frc::Timer::GetFPGATimestamp();
return PhotonPipelineResult{heartbeatCounter, now - latency, now,
detectableTgts, multiTagResults};
}
void SubmitProcessedFrame(const PhotonPipelineResult& result) {
SubmitProcessedFrame(result, wpi::Now());
}
void SubmitProcessedFrame(const PhotonPipelineResult& result);
void SubmitProcessedFrame(const PhotonPipelineResult& result,
uint64_t recieveTimestamp) {
ts.latencyMillisEntry.Set(
result.GetLatency().convert<units::milliseconds>().to<double>(),
recieveTimestamp);
uint64_t recieveTimestamp);
Packet newPacket{};
newPacket << result;
ts.rawBytesEntry.Set(newPacket.GetData(), recieveTimestamp);
bool hasTargets = result.HasTargets();
ts.hasTargetEntry.Set(hasTargets, recieveTimestamp);
if (!hasTargets) {
ts.targetPitchEntry.Set(0.0, recieveTimestamp);
ts.targetYawEntry.Set(0.0, recieveTimestamp);
ts.targetAreaEntry.Set(0.0, recieveTimestamp);
std::array<double, 3> poseData{0.0, 0.0, 0.0};
ts.targetPoseEntry.Set(poseData, recieveTimestamp);
ts.targetSkewEntry.Set(0.0, recieveTimestamp);
} else {
PhotonTrackedTarget bestTarget = result.GetBestTarget();
ts.targetPitchEntry.Set(bestTarget.GetPitch(), recieveTimestamp);
ts.targetYawEntry.Set(bestTarget.GetYaw(), recieveTimestamp);
ts.targetAreaEntry.Set(bestTarget.GetArea(), recieveTimestamp);
ts.targetSkewEntry.Set(bestTarget.GetSkew(), recieveTimestamp);
frc::Transform3d transform = bestTarget.GetBestCameraToTarget();
std::array<double, 4> poseData{
transform.X().to<double>(), transform.Y().to<double>(),
transform.Rotation().ToRotation2d().Degrees().to<double>()};
ts.targetPoseEntry.Set(poseData, recieveTimestamp);
}
auto intrinsics = prop.GetIntrinsics();
std::vector<double> intrinsicsView{intrinsics.data(),
intrinsics.data() + intrinsics.size()};
ts.cameraIntrinsicsPublisher.Set(intrinsicsView, recieveTimestamp);
auto distortion = prop.GetDistCoeffs();
std::vector<double> distortionView{distortion.data(),
distortion.data() + distortion.size()};
ts.cameraDistortionPublisher.Set(distortionView, recieveTimestamp);
ts.heartbeatPublisher.Set(heartbeatCounter++, recieveTimestamp);
}
SimCameraProperties prop;
private:

190
photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h
View File

@@ -45,77 +45,11 @@ class SimCameraProperties {
public:
SimCameraProperties() { SetCalibration(960, 720, frc::Rotation2d{90_deg}); }
SimCameraProperties(std::string path, int width, int height) {}
void SetCalibration(int width, int height, frc::Rotation2d fovDiag) {
if (fovDiag.Degrees() < 1_deg || fovDiag.Degrees() > 179_deg) {
fovDiag = frc::Rotation2d{
std::clamp<units::degree_t>(fovDiag.Degrees(), 1_deg, 179_deg)};
FRC_ReportError(
frc::err::Error,
"Requested invalid FOV! Clamping between (1, 179) degrees...");
}
double resDiag = std::sqrt(width * width + height * height);
double diagRatio = units::math::tan(fovDiag.Radians() / 2.0);
frc::Rotation2d fovWidth{
units::radian_t{std::atan(diagRatio * (width / resDiag)) * 2}};
frc::Rotation2d fovHeight{
units::radian_t{std::atan(diagRatio * (height / resDiag)) * 2}};
Eigen::Matrix<double, 8, 1> newDistCoeffs =
Eigen::Matrix<double, 8, 1>::Zero();
double cx = width / 2.0 - 0.5;
double cy = height / 2.0 - 0.5;
double fx = cx / units::math::tan(fovWidth.Radians() / 2.0);
double fy = cy / units::math::tan(fovHeight.Radians() / 2.0);
Eigen::Matrix<double, 3, 3> newCamIntrinsics;
newCamIntrinsics << fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0;
SetCalibration(width, height, newCamIntrinsics, newDistCoeffs);
}
void SetCalibration(int width, int height, frc::Rotation2d fovDiag);
void SetCalibration(int width, int height,
const Eigen::Matrix<double, 3, 3>& newCamIntrinsics,
const Eigen::Matrix<double, 8, 1>& newDistCoeffs) {
resWidth = width;
resHeight = height;
camIntrinsics = newCamIntrinsics;
distCoeffs = newDistCoeffs;
std::array<frc::Translation3d, 4> p{
frc::Translation3d{
1_m,
frc::Rotation3d{0_rad, 0_rad,
(GetPixelYaw(0) + frc::Rotation2d{units::radian_t{
-std::numbers::pi / 2.0}})
.Radians()}},
frc::Translation3d{
1_m, frc::Rotation3d{0_rad, 0_rad,
(GetPixelYaw(width) +
frc::Rotation2d{
units::radian_t{std::numbers::pi / 2.0}})
.Radians()}},
frc::Translation3d{
1_m,
frc::Rotation3d{0_rad,
(GetPixelPitch(0) + frc::Rotation2d{units::radian_t{
std::numbers::pi / 2.0}})
.Radians(),
0_rad}},
frc::Translation3d{
1_m, frc::Rotation3d{0_rad,
(GetPixelPitch(height) +
frc::Rotation2d{
units::radian_t{-std::numbers::pi / 2.0}})
.Radians(),
0_rad}},
};
viewplanes.clear();
for (size_t i = 0; i < p.size(); i++) {
viewplanes.emplace_back(Eigen::Matrix<double, 3, 1>{
p[i].X().to<double>(), p[i].Y().to<double>(), p[i].Z().to<double>()});
}
}
const Eigen::Matrix<double, 8, 1>& newDistCoeffs);
void SetCalibError(double newAvgErrorPx, double newErrorStdDevPx) {
avgErrorPx = newAvgErrorPx;
@@ -220,125 +154,7 @@ class SimCameraProperties {
std::pair<std::optional<double>, std::optional<double>> GetVisibleLine(
const RotTrlTransform3d& camRt, const frc::Translation3d& a,
const frc::Translation3d& b) const {
frc::Translation3d relA = camRt.Apply(a);
frc::Translation3d relB = camRt.Apply(b);
if (relA.X() <= 0_m && relB.X() <= 0_m) {
return {std::nullopt, std::nullopt};
}
Eigen::Matrix<double, 3, 1> av{relA.X().to<double>(), relA.Y().to<double>(),
relA.Z().to<double>()};
Eigen::Matrix<double, 3, 1> bv{relB.X().to<double>(), relB.Y().to<double>(),
relB.Z().to<double>()};
Eigen::Matrix<double, 3, 1> abv = bv - av;
bool aVisible = true;
bool bVisible = true;
for (size_t i = 0; i < viewplanes.size(); i++) {
Eigen::Matrix<double, 3, 1> normal = viewplanes[i];
double aVisibility = av.dot(normal);
if (aVisibility < 0) {
aVisible = false;
}
double bVisibility = bv.dot(normal);
if (bVisibility < 0) {
bVisible = false;
}
if (aVisibility <= 0 && bVisibility <= 0) {
return {std::nullopt, std::nullopt};
}
}
if (aVisible && bVisible) {
return {0, 1};
}
std::array<double, 4> intersections{std::nan(""), std::nan(""),
std::nan(""), std::nan("")};
std::vector<std::optional<Eigen::Matrix<double, 3, 1>>> ipts{
{std::nullopt, std::nullopt, std::nullopt, std::nullopt}};
for (size_t i = 0; i < viewplanes.size(); i++) {
Eigen::Matrix<double, 3, 1> normal = viewplanes[i];
Eigen::Matrix<double, 3, 1> a_projn{};
a_projn = (av.dot(normal) / normal.dot(normal)) * normal;
if (std::abs(abv.dot(normal)) < 1e-5) {
continue;
}
intersections[i] = a_projn.dot(a_projn) / -(abv.dot(a_projn));
Eigen::Matrix<double, 3, 1> apv{};
apv = intersections[i] * abv;
Eigen::Matrix<double, 3, 1> intersectpt{};
intersectpt = av + apv;
ipts[i] = intersectpt;
for (size_t j = 1; j < viewplanes.size(); j++) {
int oi = (i + j) % viewplanes.size();
Eigen::Matrix<double, 3, 1> onormal = viewplanes[oi];
if (intersectpt.dot(onormal) < 0) {
intersections[i] = std::nan("");
ipts[i] = std::nullopt;
break;
}
}
if (!ipts[i]) {
continue;
}
for (int j = i - 1; j >= 0; j--) {
std::optional<Eigen::Matrix<double, 3, 1>> oipt = ipts[j];
if (!oipt) {
continue;
}
Eigen::Matrix<double, 3, 1> diff{};
diff = oipt.value() - intersectpt;
if (diff.cwiseAbs().maxCoeff() < 1e-4) {
intersections[i] = std::nan("");
ipts[i] = std::nullopt;
break;
}
}
}
double inter1 = std::nan("");
double inter2 = std::nan("");
for (double inter : intersections) {
if (!std::isnan(inter)) {
if (std::isnan(inter1)) {
inter1 = inter;
} else {
inter2 = inter;
}
}
}
if (!std::isnan(inter2)) {
double max = std::max(inter1, inter2);
double min = std::min(inter1, inter2);
if (aVisible) {
min = 0;
}
if (bVisible) {
max = 1;
}
return {min, max};
} else if (!std::isnan(inter1)) {
if (aVisible) {
return {0, inter1};
}
if (bVisible) {
return {inter1, 1};
}
return {inter1, std::nullopt};
} else {
return {std::nullopt, std::nullopt};
}
}
const frc::Translation3d& b) const;
std::vector<cv::Point2f> EstPixelNoise(
const std::vector<cv::Point2f>& points) {

137
photon-lib/src/main/native/include/photon/simulation/SimPhotonCamera.h
View File

@@ -1,137 +0,0 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#pragma once
#include <algorithm>
#include <functional>
#include <memory>
#include <string>
#include <vector>
#include <networktables/NetworkTableInstance.h>
#include "photon/PhotonCamera.h"
#include "photon/PhotonTargetSortMode.h"
#include "photon/dataflow/structures/Packet.h"
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
namespace photon {
class SimPhotonCamera : public PhotonCamera {
public:
SimPhotonCamera(nt::NetworkTableInstance instance,
const std::string& cameraName)
: PhotonCamera(instance, cameraName) {
latencyMillisEntry = rootTable->GetEntry("latencyMillis");
hasTargetEntry = rootTable->GetEntry("hasTargetEntry");
targetPitchEntry = rootTable->GetEntry("targetPitchEntry");
targetYawEntry = rootTable->GetEntry("targetYawEntry");
targetAreaEntry = rootTable->GetEntry("targetAreaEntry");
targetSkewEntry = rootTable->GetEntry("targetSkewEntry");
targetPoseEntry = rootTable->GetEntry("targetPoseEntry");
rawBytesPublisher = rootTable->GetRawTopic("rawBytes").Publish("rawBytes");
versionEntry = instance.GetTable("photonvision")->GetEntry("version");
// versionEntry.SetString(PhotonVersion.versionString);
}
explicit SimPhotonCamera(const std::string& cameraName)
: SimPhotonCamera(nt::NetworkTableInstance::GetDefault(), cameraName) {}
virtual ~SimPhotonCamera() = default;
/**
* Simulate one processed frame of vision data, putting one result to NT.
*
* @param latency Latency of the provided frame
* @param targetList List of targets detected
*/
void SubmitProcessedFrame(units::millisecond_t latency,
std::vector<PhotonTrackedTarget> targetList) {
SubmitProcessedFrame(latency, PhotonTargetSortMode::LeftMost(), targetList);
}
/**
* Simulate one processed frame of vision data, putting one result to NT.
*
* @param latency Latency of the provided frame
* @param sortMode Order in which to sort targets
* @param targetList List of targets detected
*/
void SubmitProcessedFrame(
units::millisecond_t latency,
std::function<bool(const PhotonTrackedTarget& target1,
const PhotonTrackedTarget& target2)>
sortMode,
std::vector<PhotonTrackedTarget> targetList) {
latencyMillisEntry.SetDouble(latency.to<double>());
std::sort(targetList.begin(), targetList.end(),
[&](auto lhs, auto rhs) { return sortMode(lhs, rhs); });
PhotonPipelineResult newResult{0, 0_s, latency, targetList};
Packet packet{};
packet << newResult;
rawBytesPublisher.Set(
std::span{packet.GetData().data(), packet.GetDataSize()});
bool hasTargets = newResult.HasTargets();
hasTargetEntry.SetBoolean(hasTargets);
if (!hasTargets) {
targetPitchEntry.SetDouble(0.0);
targetYawEntry.SetDouble(0.0);
targetAreaEntry.SetDouble(0.0);
targetPoseEntry.SetDoubleArray(
std::vector<double>{0.0, 0.0, 0.0, 0, 0, 0, 0});
targetSkewEntry.SetDouble(0.0);
} else {
PhotonTrackedTarget bestTarget = newResult.GetBestTarget();
targetPitchEntry.SetDouble(bestTarget.GetPitch());
targetYawEntry.SetDouble(bestTarget.GetYaw());
targetAreaEntry.SetDouble(bestTarget.GetArea());
targetSkewEntry.SetDouble(bestTarget.GetSkew());
frc::Transform3d transform = bestTarget.GetBestCameraToTarget();
targetPoseEntry.SetDoubleArray(std::vector<double>{
transform.X().to<double>(), transform.Y().to<double>(),
transform.Z().to<double>(), transform.Rotation().GetQuaternion().W(),
transform.Rotation().GetQuaternion().X(),
transform.Rotation().GetQuaternion().Y(),
transform.Rotation().GetQuaternion().Z()});
}
}
private:
nt::NetworkTableEntry latencyMillisEntry;
nt::NetworkTableEntry hasTargetEntry;
nt::NetworkTableEntry targetPitchEntry;
nt::NetworkTableEntry targetYawEntry;
nt::NetworkTableEntry targetAreaEntry;
nt::NetworkTableEntry targetSkewEntry;
nt::NetworkTableEntry targetPoseEntry;
nt::NetworkTableEntry versionEntry;
nt::RawPublisher rawBytesPublisher;
};
} // namespace photon

228
photon-lib/src/main/native/include/photon/simulation/SimVisionSystem.h
View File

@@ -1,228 +0,0 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#pragma once
#include <string>
#include <vector>
#include <frc/smartdashboard/Field2d.h>
#include <frc/smartdashboard/SmartDashboard.h>
#include <units/angle.h>
#include <units/area.h>
#include "SimPhotonCamera.h"
#include "SimVisionTarget.h"
#include "photon/dataflow/structures/Packet.h"
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
namespace photon {
class SimVisionSystem {
public:
SimPhotonCamera cam;
units::radian_t camHorizFOV{0};
units::radian_t camVertFOV{0};
units::meter_t maxLEDRange{0};
int cameraResWidth{0};
int cameraResHeight{0};
double minTargetArea{0.0};
frc::Transform3d cameraToRobot;
frc::Field2d dbgField;
frc::FieldObject2d* dbgRobot;
frc::FieldObject2d* dbgCamera;
std::vector<SimVisionTarget> targetList;
/**
* Create a simulated vision system involving a camera and coprocessor mounted
* on a mobile robot running PhotonVision, detecting one or more targets
* scattered around the field. This assumes a fairly simple and
* distortion-less pinhole camera model.
*
* @param camName Name of the PhotonVision camera to create. Align it with the
* settings you use in the PhotonVision GUI.
* @param camDiagFOV Diagonal Field of View of the camera used. Align it with
* the manufacturer specifications, and/or whatever is configured in the
* PhotonVision Setting page.
* @param cameraToRobot Transform to move from the camera's mount position to
* the robot's position
* @param maxLEDRange Maximum distance at which your camera can illuminate the
* target and make it visible. Set to 9000 or more if your vision system does
* not rely on LED's.
* @param cameraResWidth Width of your camera's image sensor in pixels
* @param cameraResHeight Height of your camera's image sensor in pixels
* @param minTargetArea Minimum area that that the target should be before
* it's recognized as a target by the camera. Match this with your contour
* filtering settings in the PhotonVision GUI.
*/
SimVisionSystem(std::string camName, units::degree_t camDiagFOV,
frc::Transform3d cameraToRobot, units::meter_t maxLEDRange,
int cameraResWidth, int cameraResHeight, double minTargetArea)
: cam(camName),
camHorizFOV((camDiagFOV * cameraResWidth) /
std::hypot(cameraResWidth, cameraResHeight)),
camVertFOV((camDiagFOV * cameraResHeight) /
std::hypot(cameraResWidth, cameraResHeight)),
maxLEDRange(maxLEDRange),
cameraResWidth(cameraResWidth),
cameraResHeight(cameraResHeight),
minTargetArea(minTargetArea),
cameraToRobot(cameraToRobot),
dbgField(),
dbgRobot(dbgField.GetRobotObject()),
dbgCamera(dbgField.GetObject(camName + " Camera")) {
frc::SmartDashboard::PutData(camName + " Sim Field", &dbgField);
}
/**
* Add a target on the field which your vision system is designed to detect.
* The PhotonCamera from this system will report the location of the robot
* relative to the subset of these targets which are visible from the given
* robot position.
*
* @param target Target to add to the simulated field
*/
void AddSimVisionTarget(SimVisionTarget target) {
targetList.push_back(target);
dbgField.GetObject("Target " + std::to_string(target.targetId))
->SetPose(target.targetPose.ToPose2d());
}
/**
* Clears all sim vision targets.
* This is useful for switching alliances and needing to repopulate the sim
* targets. NOTE: Old targets will still show on the Field2d unless
* overwritten by new targets with the same ID
*/
void ClearVisionTargets() { targetList.clear(); }
/**
* Adjust the camera position relative to the robot. Use this if your camera
* is on a gimbal or turret or some other mobile platform.
*
* @param newCameraToRobot New Transform from the robot to the camera
*/
void MoveCamera(frc::Transform3d newCameraToRobot) {
cameraToRobot = newCameraToRobot;
}
/**
* Periodic update. Call this once per frame of image data you wish to process
* and send to NetworkTables
*
* @param robotPose current pose of the robot on the field. Will be used to
* calculate which targets are actually in view, where they are at relative to
* the robot, and relevant PhotonVision parameters.
*/
void ProcessFrame(frc::Pose2d robotPose) {
ProcessFrame(frc::Pose3d{
robotPose.X(), robotPose.Y(), 0.0_m,
frc::Rotation3d{0_rad, 0_rad, robotPose.Rotation().Radians()}});
}
/**
* Periodic update. Call this once per frame of image data you wish to process
* and send to NetworkTables
*
* @param robotPose current pose of the robot in space. Will be used to
* calculate which targets are actually in view, where they are at relative to
* the robot, and relevant PhotonVision parameters.
*/
void ProcessFrame(frc::Pose3d robotPose) {
frc::Pose3d cameraPose = robotPose.TransformBy(cameraToRobot.Inverse());
dbgRobot->SetPose(robotPose.ToPose2d());
dbgCamera->SetPose(cameraPose.ToPose2d());
std::vector<PhotonTrackedTarget> visibleTargetList{};
for (const auto& target : targetList) {
frc::Transform3d camToTargetTransform{cameraPose, target.targetPose};
frc::Translation3d camToTargetTranslation{
camToTargetTransform.Translation()};
frc::Translation3d altTranslation{camToTargetTranslation.X(),
-1.0 * camToTargetTranslation.Y(),
camToTargetTranslation.Z()};
frc::Rotation3d altRotation{camToTargetTransform.Rotation() * -1.0};
frc::Transform3d camToTargetAltTransform{altTranslation, altRotation};
units::meter_t dist{camToTargetTranslation.Norm()};
double areaPixels{target.targetArea / GetM2PerPx(dist)};
units::radian_t yaw{units::math::atan2(camToTargetTranslation.Y(),
camToTargetTranslation.X())};
units::meter_t cameraHeightOffGround{cameraPose.Z()};
units::meter_t targetHeightAboveGround(target.targetPose.Z());
units::radian_t camPitch{cameraPose.Rotation().Y()};
frc::Transform2d transformAlongGround{cameraPose.ToPose2d(),
target.targetPose.ToPose2d()};
units::meter_t distanceAlongGround{
transformAlongGround.Translation().Norm()};
units::radian_t pitch =
units::math::atan2(targetHeightAboveGround - cameraHeightOffGround,
distanceAlongGround) -
camPitch;
if (CamCamSeeTarget(dist, yaw, pitch, areaPixels)) {
visibleTargetList.push_back(
PhotonTrackedTarget{yaw.convert<units::degree>().to<double>(),
pitch.convert<units::degree>().to<double>(),
areaPixels,
0.0,
target.targetId,
-1,
-1,
camToTargetTransform,
// TODO sim alternate pose
camToTargetTransform,
// TODO ambiguity
0.0,
{{0, 0}, {0, 0}, {0, 0}, {0, 0}},
{{0, 0}, {0, 0}, {0, 0}, {0, 0}}});
}
cam.SubmitProcessedFrame(0_s, visibleTargetList);
}
}
units::square_meter_t GetM2PerPx(units::meter_t dist) {
units::meter_t widthMPerPx =
2 * dist * units::math::tan(camHorizFOV / 2) / cameraResWidth;
units::meter_t heightMPerPx =
2 * dist * units::math::tan(camVertFOV / 2) / cameraResHeight;
return widthMPerPx * heightMPerPx;
}
bool CamCamSeeTarget(units::meter_t dist, units::radian_t yaw,
units::radian_t pitch, double area) {
bool inRange = dist < maxLEDRange;
bool inHorizAngle = units::math::abs(yaw) < camHorizFOV / 2;
bool inVertAngle = units::math::abs(pitch) < camVertFOV / 2;
bool targetBigEnough = area > minTargetArea;
return (inRange && inHorizAngle && inVertAngle && targetBigEnough);
}
};
} // namespace photon

65
photon-lib/src/main/native/include/photon/simulation/SimVisionTarget.h
View File

@@ -1,65 +0,0 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#pragma once
#include <frc/geometry/Pose3d.h>
#include <units/area.h>
#include "photon/dataflow/structures/Packet.h"
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
namespace photon {
class SimVisionTarget {
public:
SimVisionTarget() = default;
/**
* Describes a vision target located somewhere on the field that your
* SimVisionSystem can detect.
*
* @param targetPose Pose3d of the target in field-relative coordinates
* @param targetWidth Width of the outer bounding box of the target.
* @param targetHeight Pair Height of the outer bounding box of the
* target.
* @param targetId Id of the target
*/
SimVisionTarget(frc::Pose3d targetPose, units::meter_t targetWidth,
units::meter_t targetHeight, int targetId)
: targetPose(targetPose),
targetWidth(targetWidth),
targetHeight(targetHeight),
targetArea(targetHeight * targetWidth),
targetId(targetId) {}
frc::Pose3d targetPose;
units::meter_t targetWidth;
units::meter_t targetHeight;
units::square_meter_t targetArea;
int targetId;
};
} // namespace photon

3
photon-lib/src/main/native/include/photon/simulation/VisionTargetSim.h
View File

@@ -46,6 +46,9 @@ class VisionTargetSim {
}
int fiducialId;
int objDetClassId = -1;
float objDetConf = -1;
bool operator<(const VisionTargetSim& right) const {
return pose.Translation().Norm() < right.pose.Translation().Norm();
}