mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-21 01:01:41 +00:00
Update C++ Simulation to match Java (#1026)
This commit is contained in:
Drew Williams
@@ -39,11 +39,7 @@
|
||||
#include <units/time.h>
|
||||
#include <wpi/deprecated.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"
|
||||
#include "photon/targeting//PhotonPipelineResult.h"
|
||||
|
||||
namespace cv {
|
||||
class Mat;
|
||||
@@ -172,6 +168,8 @@ class PhotonCamera {
|
||||
PhotonCamera::VERSION_CHECK_ENABLED = enabled;
|
||||
}
|
||||
|
||||
std::shared_ptr<nt::NetworkTable> GetCameraTable() const { return rootTable; }
|
||||
|
||||
// For use in tests
|
||||
bool test = false;
|
||||
PhotonPipelineResult testResult;
|
||||
|
||||
@@ -0,0 +1,449 @@
|
||||
/*
|
||||
* 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 <cameraserver/CameraServer.h>
|
||||
#include <photon/PhotonCamera.h>
|
||||
#include <photon/PhotonTargetSortMode.h>
|
||||
#include <photon/estimation/CameraTargetRelation.h>
|
||||
#include <photon/estimation/VisionEstimation.h>
|
||||
#include <photon/networktables/NTTopicSet.h>
|
||||
#include <photon/simulation/SimCameraProperties.h>
|
||||
#include <photon/simulation/VideoSimUtil.h>
|
||||
#include <photon/simulation/VisionTargetSim.h>
|
||||
|
||||
#include <algorithm>
|
||||
#include <limits>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include <frc/apriltag/AprilTagFieldLayout.h>
|
||||
#include <frc/apriltag/AprilTagFields.h>
|
||||
#include <units/math.h>
|
||||
#include <wpi/timestamp.h>
|
||||
|
||||
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();
|
||||
}
|
||||
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() {
|
||||
return minTargetAreaPercent / 100.0 * prop.GetResArea();
|
||||
}
|
||||
units::meter_t GetMaxSightRange() { return maxSightRange; }
|
||||
const cs::CvSource& GetVideoSimRaw() { return videoSimRaw; }
|
||||
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;
|
||||
|
||||
if (iter++ > 50) {
|
||||
timestamp = now;
|
||||
nextNTEntryTime = now + frameTime;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (timestamp != 0) {
|
||||
return timestamp;
|
||||
} else {
|
||||
return std::nullopt;
|
||||
}
|
||||
}
|
||||
void SetMinTargetAreaPercent(double areaPercent) {
|
||||
minTargetAreaPercent = areaPercent;
|
||||
}
|
||||
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) {
|
||||
videoSimWireframeResolution = resolution;
|
||||
}
|
||||
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<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,
|
||||
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,
|
||||
kAprilTag16h5);
|
||||
multiTagResults = MultiTargetPNPResult{pnpResult, usedIds};
|
||||
}
|
||||
|
||||
return PhotonPipelineResult{latency, detectableTgts, multiTagResults};
|
||||
}
|
||||
void SubmitProcessedFrame(const PhotonPipelineResult& result) {
|
||||
SubmitProcessedFrame(result, wpi::Now());
|
||||
}
|
||||
void SubmitProcessedFrame(const PhotonPipelineResult& result,
|
||||
uint64_t recieveTimestamp) {
|
||||
ts.latencyMillisEntry.Set(
|
||||
result.GetLatency().convert<units::milliseconds>().to<double>(),
|
||||
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:
|
||||
PhotonCamera* cam;
|
||||
|
||||
NTTopicSet ts{};
|
||||
uint64_t heartbeatCounter{0};
|
||||
|
||||
uint64_t nextNTEntryTime{wpi::Now()};
|
||||
|
||||
units::meter_t maxSightRange{std::numeric_limits<double>::max()};
|
||||
static constexpr double kDefaultMinAreaPx{100};
|
||||
double minTargetAreaPercent;
|
||||
|
||||
frc::AprilTagFieldLayout tagLayout{
|
||||
frc::LoadAprilTagLayoutField(frc::AprilTagField::k2023ChargedUp)};
|
||||
|
||||
cs::CvSource videoSimRaw;
|
||||
cv::Mat videoSimFrameRaw{};
|
||||
bool videoSimRawEnabled{true};
|
||||
bool videoSimWireframeEnabled{false};
|
||||
double videoSimWireframeResolution{0.1};
|
||||
cs::CvSource videoSimProcessed;
|
||||
cv::Mat videoSimFrameProcessed{};
|
||||
bool videoSimProcEnabled{true};
|
||||
};
|
||||
} // namespace photon
|
||||
@@ -0,0 +1,475 @@
|
||||
/*
|
||||
* 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 <photon/estimation/OpenCVHelp.h>
|
||||
|
||||
#include <algorithm>
|
||||
#include <random>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include <frc/Errors.h>
|
||||
#include <frc/MathUtil.h>
|
||||
#include <frc/geometry/Rotation2d.h>
|
||||
#include <frc/geometry/Translation3d.h>
|
||||
#include <units/frequency.h>
|
||||
#include <units/time.h>
|
||||
|
||||
namespace photon {
|
||||
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, 5, 1> newDistCoeffs;
|
||||
newDistCoeffs << 0, 0, 0, 0, 0;
|
||||
|
||||
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,
|
||||
const Eigen::Matrix<double, 3, 3>& newCamIntrinsics,
|
||||
const Eigen::Matrix<double, 5, 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>()});
|
||||
}
|
||||
}
|
||||
|
||||
void SetCalibError(double newAvgErrorPx, double newErrorStdDevPx) {
|
||||
avgErrorPx = newAvgErrorPx;
|
||||
errorStdDevPx = newErrorStdDevPx;
|
||||
}
|
||||
|
||||
void SetFPS(units::hertz_t fps) {
|
||||
frameSpeed = units::math::max(1 / fps, exposureTime);
|
||||
}
|
||||
|
||||
void SetExposureTime(units::second_t newExposureTime) {
|
||||
exposureTime = newExposureTime;
|
||||
frameSpeed = units::math::max(frameSpeed, exposureTime);
|
||||
}
|
||||
|
||||
void SetAvgLatency(units::second_t newAvgLatency) {
|
||||
avgLatency = newAvgLatency;
|
||||
}
|
||||
|
||||
void SetLatencyStdDev(units::second_t newLatencyStdDev) {
|
||||
latencyStdDev = newLatencyStdDev;
|
||||
}
|
||||
|
||||
int GetResWidth() const { return resWidth; }
|
||||
|
||||
int GetResHeight() const { return resHeight; }
|
||||
|
||||
int GetResArea() const { return resWidth * resHeight; }
|
||||
|
||||
double GetAspectRatio() const {
|
||||
return static_cast<double>(resWidth) / static_cast<double>(resHeight);
|
||||
}
|
||||
|
||||
Eigen::Matrix<double, 3, 3> GetIntrinsics() const { return camIntrinsics; }
|
||||
|
||||
Eigen::Matrix<double, 5, 1> GetDistCoeffs() const { return distCoeffs; }
|
||||
|
||||
units::hertz_t GetFPS() const { return 1 / frameSpeed; }
|
||||
|
||||
units::second_t GetFrameSpeed() const { return frameSpeed; }
|
||||
|
||||
units::second_t GetExposureTime() const { return exposureTime; }
|
||||
|
||||
units::second_t GetAverageLatency() const { return avgLatency; }
|
||||
|
||||
units::second_t GetLatencyStdDev() const { return latencyStdDev; }
|
||||
|
||||
double GetContourAreaPercent(const std::vector<cv::Point2f>& points) {
|
||||
return cv::contourArea(photon::OpenCVHelp::GetConvexHull(points)) /
|
||||
GetResArea() * 100;
|
||||
}
|
||||
|
||||
frc::Rotation2d GetPixelYaw(double pixelX) const {
|
||||
double fx = camIntrinsics(0, 0);
|
||||
double cx = camIntrinsics(0, 2);
|
||||
double xOffset = cx - pixelX;
|
||||
return frc::Rotation2d{fx, xOffset};
|
||||
}
|
||||
|
||||
frc::Rotation2d GetPixelPitch(double pixelY) const {
|
||||
double fy = camIntrinsics(1, 1);
|
||||
double cy = camIntrinsics(1, 2);
|
||||
double yOffset = cy - pixelY;
|
||||
return frc::Rotation2d{fy, -yOffset};
|
||||
}
|
||||
|
||||
frc::Rotation3d GetPixelRot(const cv::Point2d& point) const {
|
||||
return frc::Rotation3d{0_rad, GetPixelPitch(point.y).Radians(),
|
||||
GetPixelYaw(point.x).Radians()};
|
||||
}
|
||||
|
||||
frc::Rotation3d GetCorrectedPixelRot(const cv::Point2d& point) const {
|
||||
double fx = camIntrinsics(0, 0);
|
||||
double cx = camIntrinsics(0, 2);
|
||||
double xOffset = cx - point.x;
|
||||
|
||||
double fy = camIntrinsics(1, 1);
|
||||
double cy = camIntrinsics(1, 2);
|
||||
double yOffset = cy - point.y;
|
||||
|
||||
frc::Rotation2d yaw{fx, xOffset};
|
||||
frc::Rotation2d pitch{fy / std::cos(std::atan(xOffset / fx)), -yOffset};
|
||||
return frc::Rotation3d{0_rad, pitch.Radians(), yaw.Radians()};
|
||||
}
|
||||
|
||||
frc::Rotation2d GetHorizFOV() const {
|
||||
frc::Rotation2d left = GetPixelYaw(0);
|
||||
frc::Rotation2d right = GetPixelYaw(resWidth);
|
||||
return left - right;
|
||||
}
|
||||
|
||||
frc::Rotation2d GetVertFOV() const {
|
||||
frc::Rotation2d above = GetPixelPitch(0);
|
||||
frc::Rotation2d below = GetPixelPitch(resHeight);
|
||||
return below - above;
|
||||
}
|
||||
|
||||
frc::Rotation2d GetDiagFOV() const {
|
||||
return frc::Rotation2d{
|
||||
units::math::hypot(GetHorizFOV().Radians(), GetVertFOV().Radians())};
|
||||
}
|
||||
|
||||
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};
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<cv::Point2f> EstPixelNoise(
|
||||
const std::vector<cv::Point2f>& points) {
|
||||
if (avgErrorPx == 0 && errorStdDevPx == 0) {
|
||||
return points;
|
||||
}
|
||||
|
||||
std::vector<cv::Point2f> noisyPts;
|
||||
noisyPts.reserve(points.size());
|
||||
for (size_t i = 0; i < points.size(); i++) {
|
||||
cv::Point2f p = points[i];
|
||||
float error = avgErrorPx + gaussian(generator) * errorStdDevPx;
|
||||
float errorAngle =
|
||||
uniform(generator) * 2 * std::numbers::pi - std::numbers::pi;
|
||||
noisyPts.emplace_back(cv::Point2f{p.x + error * std::cos(errorAngle),
|
||||
p.y + error * std::sin(errorAngle)});
|
||||
}
|
||||
return noisyPts;
|
||||
}
|
||||
|
||||
units::second_t EstLatency() {
|
||||
return units::math::max(avgLatency + gaussian(generator) * latencyStdDev,
|
||||
0_s);
|
||||
}
|
||||
|
||||
units::second_t EstSecUntilNextFrame() {
|
||||
return frameSpeed + units::math::max(0_s, EstLatency() - frameSpeed);
|
||||
}
|
||||
|
||||
static SimCameraProperties PERFECT_90DEG() { return SimCameraProperties{}; }
|
||||
|
||||
static SimCameraProperties PI4_LIFECAM_320_240() {
|
||||
SimCameraProperties prop{};
|
||||
prop.SetCalibration(
|
||||
320, 240,
|
||||
(Eigen::MatrixXd(3, 3) << 328.2733242048587, 0.0, 164.8190261141906,
|
||||
0.0, 318.0609794305216, 123.8633838438093, 0.0, 0.0, 1.0)
|
||||
.finished(),
|
||||
Eigen::Matrix<double, 5, 1>{
|
||||
0.09957946553445934, -0.9166265114485799, 0.0019519890627236526,
|
||||
-0.0036071725380870333, 1.5627234622420942});
|
||||
prop.SetCalibError(0.21, 0.0124);
|
||||
prop.SetFPS(30_Hz);
|
||||
prop.SetAvgLatency(30_ms);
|
||||
prop.SetLatencyStdDev(10_ms);
|
||||
return prop;
|
||||
}
|
||||
|
||||
static SimCameraProperties PI4_LIFECAM_640_480() {
|
||||
SimCameraProperties prop{};
|
||||
prop.SetCalibration(
|
||||
640, 480,
|
||||
(Eigen::MatrixXd(3, 3) << 669.1428078983059, 0.0, 322.53377249329213,
|
||||
0.0, 646.9843137061716, 241.26567383784163, 0.0, 0.0, 1.0)
|
||||
.finished(),
|
||||
Eigen::Matrix<double, 5, 1>{
|
||||
0.12788470750464645, -1.2350335805796528, 0.0024990767286192732,
|
||||
-0.0026958287600230705, 2.2951386729115537});
|
||||
prop.SetCalibError(0.26, 0.046);
|
||||
prop.SetFPS(15_Hz);
|
||||
prop.SetAvgLatency(65_ms);
|
||||
prop.SetLatencyStdDev(15_ms);
|
||||
return prop;
|
||||
}
|
||||
|
||||
static SimCameraProperties LL2_640_480() {
|
||||
SimCameraProperties prop{};
|
||||
prop.SetCalibration(
|
||||
640, 480,
|
||||
(Eigen::MatrixXd(3, 3) << 511.22843367007755, 0.0, 323.62049380211096,
|
||||
0.0, 514.5452336723849, 261.8827920543568, 0.0, 0.0, 1.0)
|
||||
.finished(),
|
||||
Eigen::Matrix<double, 5, 1>{0.1917469998873756, -0.5142936883324216,
|
||||
0.012461562046896614, 0.0014084973492408186,
|
||||
0.35160648971214437});
|
||||
prop.SetCalibError(0.25, 0.05);
|
||||
prop.SetFPS(15_Hz);
|
||||
prop.SetAvgLatency(35_ms);
|
||||
prop.SetLatencyStdDev(8_ms);
|
||||
return prop;
|
||||
}
|
||||
|
||||
static SimCameraProperties LL2_960_720() {
|
||||
SimCameraProperties prop{};
|
||||
prop.SetCalibration(
|
||||
960, 720,
|
||||
(Eigen::MatrixXd(3, 3) << 769.6873145148892, 0.0, 486.1096609458122,
|
||||
0.0, 773.8164483705323, 384.66071662358354, 0.0, 0.0, 1.0)
|
||||
.finished(),
|
||||
Eigen::Matrix<double, 5, 1>{0.189462064814501, -0.49903003669627627,
|
||||
0.007468423590519429, 0.002496885298683693,
|
||||
0.3443122090208624});
|
||||
prop.SetCalibError(0.35, 0.10);
|
||||
prop.SetFPS(10_Hz);
|
||||
prop.SetAvgLatency(50_ms);
|
||||
prop.SetLatencyStdDev(15_ms);
|
||||
return prop;
|
||||
}
|
||||
|
||||
static SimCameraProperties LL2_1280_720() {
|
||||
SimCameraProperties prop{};
|
||||
prop.SetCalibration(
|
||||
1280, 720,
|
||||
(Eigen::MatrixXd(3, 3) << 1011.3749416937393, 0.0, 645.4955139388737,
|
||||
0.0, 1008.5391755084075, 508.32877656020196, 0.0, 0.0, 1.0)
|
||||
.finished(),
|
||||
Eigen::Matrix<double, 5, 1>{0.13730101577061535, -0.2904345656989261,
|
||||
8.32475714507539E-4, -3.694397782014239E-4,
|
||||
0.09487962227027584});
|
||||
prop.SetCalibError(0.37, 0.06);
|
||||
prop.SetFPS(7_Hz);
|
||||
prop.SetAvgLatency(60_ms);
|
||||
prop.SetLatencyStdDev(20_ms);
|
||||
return prop;
|
||||
}
|
||||
|
||||
private:
|
||||
std::mt19937 generator{std::random_device{}()};
|
||||
std::normal_distribution<float> gaussian{0.0, 1.0};
|
||||
std::uniform_real_distribution<float> uniform{0.0, 1.0};
|
||||
|
||||
int resWidth;
|
||||
int resHeight;
|
||||
Eigen::Matrix<double, 3, 3> camIntrinsics;
|
||||
Eigen::Matrix<double, 5, 1> distCoeffs;
|
||||
double avgErrorPx;
|
||||
double errorStdDevPx;
|
||||
units::second_t frameSpeed{0};
|
||||
units::second_t exposureTime{0};
|
||||
units::second_t avgLatency{0};
|
||||
units::second_t latencyStdDev{0};
|
||||
std::vector<Eigen::Matrix<double, 3, 1>> viewplanes{};
|
||||
};
|
||||
} // namespace photon
|
||||
@@ -0,0 +1,432 @@
|
||||
/*
|
||||
* 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 <cscore_cv.h>
|
||||
|
||||
#include <algorithm>
|
||||
#include <numeric>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include <frc/apriltag/AprilTag.h>
|
||||
#include <opencv2/core.hpp>
|
||||
#include <opencv2/imgcodecs.hpp>
|
||||
#include <opencv2/objdetect.hpp>
|
||||
#include <units/length.h>
|
||||
|
||||
#include "SimCameraProperties.h"
|
||||
#include "photon/estimation/RotTrlTransform3d.h"
|
||||
|
||||
namespace mathutil {
|
||||
template <typename T>
|
||||
int sgn(T val) {
|
||||
return (T(0) < val) - (val < T(0));
|
||||
}
|
||||
} // namespace mathutil
|
||||
|
||||
namespace photon {
|
||||
namespace VideoSimUtil {
|
||||
static constexpr int kNumTags16h5 = 30;
|
||||
|
||||
static constexpr units::meter_t fieldLength{16.54175_m};
|
||||
static constexpr units::meter_t fieldWidth{8.0137_m};
|
||||
|
||||
static cv::Mat Get16h5TagImage(int id) {
|
||||
wpi::RawFrame frame = frc::AprilTag::Generate16h5AprilTagImage(id);
|
||||
cv::Mat markerImage{frame.height, frame.width, CV_8UC1, frame.data};
|
||||
cv::Mat markerClone = markerImage.colRange(0, frame.dataLength).clone();
|
||||
return markerClone;
|
||||
}
|
||||
|
||||
static std::unordered_map<int, cv::Mat> LoadAprilTagImages() {
|
||||
std::unordered_map<int, cv::Mat> retVal{};
|
||||
for (int i = 0; i < kNumTags16h5; i++) {
|
||||
cv::Mat tagImage = Get16h5TagImage(i);
|
||||
retVal[i] = tagImage;
|
||||
}
|
||||
return retVal;
|
||||
}
|
||||
|
||||
static std::vector<cv::Point2f> GetImageCorners(const cv::Size& size) {
|
||||
std::vector<cv::Point2f> retVal{};
|
||||
retVal.emplace_back(cv::Point2f{-0.5f, -0.5f});
|
||||
retVal.emplace_back(cv::Point2f{size.width - 0.5f, -0.5f});
|
||||
retVal.emplace_back(cv::Point2f{size.width - 0.5f, size.height - 0.5f});
|
||||
retVal.emplace_back(cv::Point2f{-0.5f, size.height - 0.5f});
|
||||
return retVal;
|
||||
}
|
||||
|
||||
static std::vector<cv::Point2f> Get16h5MarkerPts(int scale) {
|
||||
cv::Rect2f roi16h5{cv::Point2f{1, 1}, cv::Point2f{6, 6}};
|
||||
roi16h5.x *= scale;
|
||||
roi16h5.y *= scale;
|
||||
roi16h5.width *= scale;
|
||||
roi16h5.height *= scale;
|
||||
std::vector<cv::Point2f> pts = GetImageCorners(roi16h5.size());
|
||||
for (size_t i = 0; i < pts.size(); i++) {
|
||||
cv::Point2f pt = pts[i];
|
||||
pts[i] = cv::Point2f{roi16h5.tl().x + pt.x, roi16h5.tl().y + pt.y};
|
||||
}
|
||||
return pts;
|
||||
}
|
||||
|
||||
static std::vector<cv::Point2f> Get16h5MarkerPts() {
|
||||
return Get16h5MarkerPts(1);
|
||||
}
|
||||
|
||||
static const std::unordered_map<int, cv::Mat> kTag16h5Images =
|
||||
LoadAprilTagImages();
|
||||
static const std::vector<cv::Point2f> kTag16h5MarkPts = Get16h5MarkerPts();
|
||||
|
||||
[[maybe_unused]] static void UpdateVideoProp(cs::CvSource& video,
|
||||
const SimCameraProperties& prop) {
|
||||
video.SetResolution(prop.GetResWidth(), prop.GetResHeight());
|
||||
video.SetFPS(prop.GetFPS().to<int>());
|
||||
}
|
||||
|
||||
[[maybe_unused]] static void Warp165h5TagImage(
|
||||
int tagId, const std::vector<cv::Point2f>& dstPoints, bool antialiasing,
|
||||
cv::Mat& destination) {
|
||||
if (!kTag16h5Images.contains(tagId)) {
|
||||
return;
|
||||
}
|
||||
cv::Mat tagImage = kTag16h5Images.at(tagId);
|
||||
std::vector<cv::Point2f> tagPoints{kTag16h5MarkPts};
|
||||
std::vector<cv::Point2f> tagImageCorners{GetImageCorners(tagImage.size())};
|
||||
std::vector<cv::Point2f> dstPointMat = dstPoints;
|
||||
cv::Rect boundingRect = cv::boundingRect(dstPointMat);
|
||||
cv::Mat perspecTrf = cv::getPerspectiveTransform(tagPoints, dstPointMat);
|
||||
std::vector<cv::Point2f> extremeCorners{};
|
||||
cv::perspectiveTransform(tagImageCorners, extremeCorners, perspecTrf);
|
||||
boundingRect = cv::boundingRect(extremeCorners);
|
||||
|
||||
double warpedContourArea = cv::contourArea(extremeCorners);
|
||||
double warpedTagUpscale =
|
||||
std::sqrt(warpedContourArea) / std::sqrt(tagImage.size().area());
|
||||
int warpStrat = cv::INTER_NEAREST;
|
||||
|
||||
int supersampling = 6;
|
||||
supersampling = static_cast<int>(std::ceil(supersampling / warpedTagUpscale));
|
||||
supersampling = std::max(std::min(supersampling, 8), 1);
|
||||
|
||||
cv::Mat scaledTagImage{};
|
||||
if (warpedTagUpscale > 2.0) {
|
||||
warpStrat = cv::INTER_LINEAR;
|
||||
int scaleFactor = static_cast<int>(warpedTagUpscale / 3.0) + 2;
|
||||
scaleFactor = std::max(std::min(scaleFactor, 40), 1);
|
||||
scaleFactor *= supersampling;
|
||||
cv::resize(tagImage, scaledTagImage, cv::Size{}, scaleFactor, scaleFactor,
|
||||
cv::INTER_NEAREST);
|
||||
tagPoints = Get16h5MarkerPts(scaleFactor);
|
||||
} else {
|
||||
scaledTagImage = tagImage;
|
||||
}
|
||||
|
||||
boundingRect.x -= 1;
|
||||
boundingRect.y -= 1;
|
||||
boundingRect.width += 2;
|
||||
boundingRect.height += 2;
|
||||
if (boundingRect.x < 0) {
|
||||
boundingRect.width += boundingRect.x;
|
||||
boundingRect.x = 0;
|
||||
}
|
||||
if (boundingRect.y < 0) {
|
||||
boundingRect.height += boundingRect.y;
|
||||
boundingRect.y = 0;
|
||||
}
|
||||
boundingRect.width =
|
||||
std::min(destination.size().width - boundingRect.x, boundingRect.width);
|
||||
boundingRect.height =
|
||||
std::min(destination.size().height - boundingRect.y, boundingRect.height);
|
||||
if (boundingRect.width <= 0 || boundingRect.height <= 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
std::vector<cv::Point2f> scaledDstPts{};
|
||||
if (supersampling > 1) {
|
||||
cv::multiply(dstPointMat,
|
||||
cv::Scalar{static_cast<double>(supersampling),
|
||||
static_cast<double>(supersampling)},
|
||||
scaledDstPts);
|
||||
boundingRect.x *= supersampling;
|
||||
boundingRect.y *= supersampling;
|
||||
boundingRect.width *= supersampling;
|
||||
boundingRect.height *= supersampling;
|
||||
} else {
|
||||
scaledDstPts = dstPointMat;
|
||||
}
|
||||
|
||||
cv::subtract(scaledDstPts,
|
||||
cv::Scalar{static_cast<double>(boundingRect.tl().x),
|
||||
static_cast<double>(boundingRect.tl().y)},
|
||||
scaledDstPts);
|
||||
perspecTrf = cv::getPerspectiveTransform(tagPoints, scaledDstPts);
|
||||
|
||||
cv::Mat tempRoi{};
|
||||
cv::warpPerspective(scaledTagImage, tempRoi, perspecTrf, boundingRect.size(),
|
||||
warpStrat);
|
||||
|
||||
if (supersampling > 1) {
|
||||
boundingRect.x /= supersampling;
|
||||
boundingRect.y /= supersampling;
|
||||
boundingRect.width /= supersampling;
|
||||
boundingRect.height /= supersampling;
|
||||
cv::resize(tempRoi, tempRoi, boundingRect.size(), 0, 0, cv::INTER_AREA);
|
||||
}
|
||||
|
||||
cv::Mat tempMask{cv::Mat::zeros(tempRoi.size(), CV_8UC1)};
|
||||
cv::subtract(extremeCorners,
|
||||
cv::Scalar{static_cast<float>(boundingRect.tl().x),
|
||||
static_cast<float>(boundingRect.tl().y)},
|
||||
extremeCorners);
|
||||
cv::Point2f tempCenter{};
|
||||
tempCenter.x =
|
||||
std::accumulate(extremeCorners.begin(), extremeCorners.end(), 0.0,
|
||||
[extremeCorners](float acc, const cv::Point2f& p2) {
|
||||
return acc + p2.x / extremeCorners.size();
|
||||
});
|
||||
tempCenter.y =
|
||||
std::accumulate(extremeCorners.begin(), extremeCorners.end(), 0.0,
|
||||
[extremeCorners](float acc, const cv::Point2f& p2) {
|
||||
return acc + p2.y / extremeCorners.size();
|
||||
});
|
||||
|
||||
for (auto& corner : extremeCorners) {
|
||||
float xDiff = corner.x - tempCenter.x;
|
||||
float yDiff = corner.y - tempCenter.y;
|
||||
xDiff += 1 * mathutil::sgn(xDiff);
|
||||
yDiff += 1 * mathutil::sgn(yDiff);
|
||||
corner = cv::Point2f{tempCenter.x + xDiff, tempCenter.y + yDiff};
|
||||
}
|
||||
|
||||
std::vector<cv::Point> extremeCornerInt{extremeCorners.begin(),
|
||||
extremeCorners.end()};
|
||||
cv::fillConvexPoly(tempMask, extremeCornerInt, cv::Scalar{255});
|
||||
|
||||
cv::copyTo(tempRoi, destination(boundingRect), tempMask);
|
||||
}
|
||||
|
||||
static double GetScaledThickness(double thickness480p,
|
||||
const cv::Mat& destinationMat) {
|
||||
double scaleX = destinationMat.size().width / 640.0;
|
||||
double scaleY = destinationMat.size().height / 480.0;
|
||||
double minScale = std::min(scaleX, scaleY);
|
||||
return std::max(thickness480p * minScale, 1.0);
|
||||
}
|
||||
|
||||
[[maybe_unused]] static void DrawInscribedEllipse(
|
||||
const std::vector<cv::Point2f>& dstPoints, const cv::Scalar& color,
|
||||
cv::Mat& destination) {
|
||||
cv::RotatedRect rect = OpenCVHelp::GetMinAreaRect(dstPoints);
|
||||
cv::ellipse(destination, rect, color, -1, cv::LINE_AA);
|
||||
}
|
||||
|
||||
static void DrawPoly(const std::vector<cv::Point2f>& dstPoints, int thickness,
|
||||
const cv::Scalar& color, bool isClosed,
|
||||
cv::Mat& destination) {
|
||||
std::vector<cv::Point> intDstPoints{dstPoints.begin(), dstPoints.end()};
|
||||
std::vector<std::vector<cv::Point>> listOfListOfPoints;
|
||||
listOfListOfPoints.emplace_back(intDstPoints);
|
||||
if (thickness > 0) {
|
||||
cv::polylines(destination, listOfListOfPoints, isClosed, color, thickness,
|
||||
cv::LINE_AA);
|
||||
} else {
|
||||
cv::fillPoly(destination, listOfListOfPoints, color, cv::LINE_AA);
|
||||
}
|
||||
}
|
||||
|
||||
[[maybe_unused]] static void DrawTagDetection(
|
||||
int id, const std::vector<cv::Point2f>& dstPoints, cv::Mat& destination) {
|
||||
double thickness = GetScaledThickness(1, destination);
|
||||
DrawPoly(dstPoints, static_cast<int>(thickness), cv::Scalar{0, 0, 255}, true,
|
||||
destination);
|
||||
cv::Rect2d rect{cv::boundingRect(dstPoints)};
|
||||
cv::Point2d textPt{rect.x + rect.width, rect.y};
|
||||
textPt.x += thickness;
|
||||
textPt.y += thickness;
|
||||
cv::putText(destination, std::to_string(id), textPt, cv::FONT_HERSHEY_PLAIN,
|
||||
1.5 * thickness, cv::Scalar{0, 200, 0},
|
||||
static_cast<int>(thickness), cv::LINE_AA);
|
||||
}
|
||||
|
||||
static std::vector<std::vector<frc::Translation3d>> GetFieldWallLines() {
|
||||
std::vector<std::vector<frc::Translation3d>> list;
|
||||
|
||||
const units::meter_t sideHt = 19.5_in;
|
||||
const units::meter_t driveHt = 35_in;
|
||||
const units::meter_t topHt = 78_in;
|
||||
|
||||
// field floor
|
||||
list.emplace_back(std::vector<frc::Translation3d>{
|
||||
frc::Translation3d{0_m, 0_m, 0_m},
|
||||
frc::Translation3d{fieldLength, 0_m, 0_m},
|
||||
frc::Translation3d{fieldLength, fieldWidth, 0_m},
|
||||
frc::Translation3d{0_m, fieldWidth, 0_m},
|
||||
frc::Translation3d{0_m, 0_m, 0_m}});
|
||||
|
||||
// right side wall
|
||||
list.emplace_back(std::vector<frc::Translation3d>{
|
||||
frc::Translation3d{0_m, 0_m, 0_m}, frc::Translation3d{0_m, 0_m, sideHt},
|
||||
frc::Translation3d{fieldLength, 0_m, sideHt},
|
||||
frc::Translation3d{fieldLength, 0_m, 0_m}});
|
||||
|
||||
// red driverstation
|
||||
list.emplace_back(std::vector<frc::Translation3d>{
|
||||
frc::Translation3d{fieldLength, 0_m, sideHt},
|
||||
frc::Translation3d{fieldLength, 0_m, topHt},
|
||||
frc::Translation3d{fieldLength, fieldWidth, topHt},
|
||||
frc::Translation3d{fieldLength, fieldWidth, sideHt},
|
||||
});
|
||||
list.emplace_back(std::vector<frc::Translation3d>{
|
||||
frc::Translation3d{fieldLength, 0_m, driveHt},
|
||||
frc::Translation3d{fieldLength, fieldWidth, driveHt}});
|
||||
|
||||
// left side wall
|
||||
list.emplace_back(std::vector<frc::Translation3d>{
|
||||
frc::Translation3d{0_m, fieldWidth, 0_m},
|
||||
frc::Translation3d{0_m, fieldWidth, sideHt},
|
||||
frc::Translation3d{fieldLength, fieldWidth, sideHt},
|
||||
frc::Translation3d{fieldLength, fieldWidth, 0_m}});
|
||||
|
||||
// blue driverstation
|
||||
list.emplace_back(std::vector<frc::Translation3d>{
|
||||
frc::Translation3d{0_m, 0_m, sideHt},
|
||||
frc::Translation3d{0_m, 0_m, topHt},
|
||||
frc::Translation3d{0_m, fieldWidth, topHt},
|
||||
frc::Translation3d{0_m, fieldWidth, sideHt},
|
||||
});
|
||||
list.emplace_back(std::vector<frc::Translation3d>{
|
||||
frc::Translation3d{0_m, 0_m, driveHt},
|
||||
frc::Translation3d{0_m, fieldWidth, driveHt}});
|
||||
|
||||
return list;
|
||||
}
|
||||
|
||||
static std::vector<std::vector<frc::Translation3d>> GetFieldFloorLines(
|
||||
int subdivisions) {
|
||||
std::vector<std::vector<frc::Translation3d>> list;
|
||||
const units::meter_t subLength = fieldLength / subdivisions;
|
||||
const units::meter_t subWidth = fieldWidth / subdivisions;
|
||||
|
||||
for (int i = 0; i < subdivisions; i++) {
|
||||
list.emplace_back(std::vector<frc::Translation3d>{
|
||||
frc::Translation3d{0_m, subWidth * (i + 1), 0_m},
|
||||
frc::Translation3d{fieldLength, subWidth * (i + 1), 0_m}});
|
||||
list.emplace_back(std::vector<frc::Translation3d>{
|
||||
frc::Translation3d{subLength * (i + 1), 0_m, 0_m},
|
||||
frc::Translation3d{subLength * (i + 1), fieldWidth, 0_m}});
|
||||
}
|
||||
return list;
|
||||
}
|
||||
|
||||
static std::vector<std::vector<cv::Point2f>> PolyFrom3dLines(
|
||||
const RotTrlTransform3d& camRt, const SimCameraProperties& prop,
|
||||
const std::vector<frc::Translation3d>& trls, double resolution,
|
||||
bool isClosed, cv::Mat& destination) {
|
||||
resolution = std::hypot(destination.size().height, destination.size().width) *
|
||||
resolution;
|
||||
std::vector<frc::Translation3d> pts{trls};
|
||||
if (isClosed) {
|
||||
pts.emplace_back(pts[0]);
|
||||
}
|
||||
std::vector<std::vector<cv::Point2f>> polyPointList{};
|
||||
|
||||
for (size_t i = 0; i < pts.size() - 1; i++) {
|
||||
frc::Translation3d pta = pts[i];
|
||||
frc::Translation3d ptb = pts[i + 1];
|
||||
|
||||
std::pair<std::optional<double>, std::optional<double>> inter =
|
||||
prop.GetVisibleLine(camRt, pta, ptb);
|
||||
if (!inter.second) {
|
||||
continue;
|
||||
}
|
||||
|
||||
double inter1 = inter.first.value();
|
||||
double inter2 = inter.second.value();
|
||||
frc::Translation3d baseDelta = ptb - pta;
|
||||
frc::Translation3d old_pta = pta;
|
||||
if (inter1 > 0) {
|
||||
pta = old_pta + baseDelta * inter1;
|
||||
}
|
||||
if (inter2 < 1) {
|
||||
ptb = old_pta + baseDelta * inter2;
|
||||
}
|
||||
baseDelta = ptb - pta;
|
||||
|
||||
std::vector<cv::Point2f> poly = OpenCVHelp::ProjectPoints(
|
||||
prop.GetIntrinsics(), prop.GetDistCoeffs(), camRt, {pta, ptb});
|
||||
cv::Point2d pxa = poly[0];
|
||||
cv::Point2d pxb = poly[1];
|
||||
|
||||
double pxDist = std::hypot(pxb.x - pxa.x, pxb.y - pxa.y);
|
||||
int subdivisions = static_cast<int>(pxDist / resolution);
|
||||
frc::Translation3d subDelta = baseDelta / (subdivisions + 1);
|
||||
std::vector<frc::Translation3d> subPts{};
|
||||
for (int j = 0; j < subdivisions; j++) {
|
||||
subPts.emplace_back(pta + (subDelta * (j + 1)));
|
||||
}
|
||||
if (subPts.size() > 0) {
|
||||
std::vector<cv::Point2f> toAdd = OpenCVHelp::ProjectPoints(
|
||||
prop.GetIntrinsics(), prop.GetDistCoeffs(), camRt, subPts);
|
||||
poly.insert(poly.begin() + 1, toAdd.begin(), toAdd.end());
|
||||
}
|
||||
|
||||
polyPointList.emplace_back(poly);
|
||||
}
|
||||
|
||||
return polyPointList;
|
||||
}
|
||||
|
||||
[[maybe_unused]] static void DrawFieldWireFrame(
|
||||
const RotTrlTransform3d& camRt, const SimCameraProperties& prop,
|
||||
double resolution, double wallThickness, const cv::Scalar& wallColor,
|
||||
int floorSubdivisions, double floorThickness, const cv::Scalar& floorColor,
|
||||
cv::Mat& destination) {
|
||||
for (const auto& trls : GetFieldFloorLines(floorSubdivisions)) {
|
||||
auto polys =
|
||||
PolyFrom3dLines(camRt, prop, trls, resolution, false, destination);
|
||||
for (const auto& poly : polys) {
|
||||
DrawPoly(poly,
|
||||
static_cast<int>(
|
||||
std::round(GetScaledThickness(floorThickness, destination))),
|
||||
floorColor, false, destination);
|
||||
}
|
||||
}
|
||||
for (const auto& trls : GetFieldWallLines()) {
|
||||
auto polys =
|
||||
PolyFrom3dLines(camRt, prop, trls, resolution, false, destination);
|
||||
for (const auto& poly : polys) {
|
||||
DrawPoly(poly,
|
||||
static_cast<int>(
|
||||
std::round(GetScaledThickness(wallThickness, destination))),
|
||||
wallColor, false, destination);
|
||||
}
|
||||
}
|
||||
}
|
||||
} // namespace VideoSimUtil
|
||||
} // namespace photon
|
||||
@@ -0,0 +1,282 @@
|
||||
/*
|
||||
* 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 <unordered_map>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include <frc/Timer.h>
|
||||
#include <frc/interpolation/TimeInterpolatableBuffer.h>
|
||||
#include <frc/smartdashboard/Field2d.h>
|
||||
#include <frc/smartdashboard/FieldObject2d.h>
|
||||
#include <frc/smartdashboard/SmartDashboard.h>
|
||||
|
||||
#include "photon/simulation/PhotonCameraSim.h"
|
||||
|
||||
namespace photon {
|
||||
class VisionSystemSim {
|
||||
public:
|
||||
explicit VisionSystemSim(std::string visionSystemName) {
|
||||
std::string tableName = "VisionSystemSim-" + visionSystemName;
|
||||
frc::SmartDashboard::PutData(tableName + "/Sim Field", &dbgField);
|
||||
}
|
||||
std::optional<PhotonCameraSim*> GetCameraSim(std::string name) {
|
||||
auto it = camSimMap.find(name);
|
||||
if (it != camSimMap.end()) {
|
||||
return std::make_optional(it->second);
|
||||
} else {
|
||||
return std::nullopt;
|
||||
}
|
||||
}
|
||||
std::vector<PhotonCameraSim*> GetCameraSims() {
|
||||
std::vector<PhotonCameraSim*> retVal;
|
||||
for (auto const& cam : camSimMap) {
|
||||
retVal.emplace_back(cam.second);
|
||||
}
|
||||
return retVal;
|
||||
}
|
||||
void AddCamera(PhotonCameraSim* cameraSim,
|
||||
const frc::Transform3d& robotToCamera) {
|
||||
auto found =
|
||||
camSimMap.find(std::string{cameraSim->GetCamera()->GetCameraName()});
|
||||
if (found == camSimMap.end()) {
|
||||
camSimMap[std::string{cameraSim->GetCamera()->GetCameraName()}] =
|
||||
cameraSim;
|
||||
camTrfMap.insert(std::make_pair(
|
||||
std::move(cameraSim),
|
||||
frc::TimeInterpolatableBuffer<frc::Pose3d>{bufferLength}));
|
||||
camTrfMap.at(cameraSim).AddSample(frc::Timer::GetFPGATimestamp(),
|
||||
frc::Pose3d{} + robotToCamera);
|
||||
}
|
||||
}
|
||||
void ClearCameras() {
|
||||
camSimMap.clear();
|
||||
camTrfMap.clear();
|
||||
}
|
||||
bool RemoveCamera(PhotonCameraSim* cameraSim) {
|
||||
int numOfElementsRemoved =
|
||||
camSimMap.erase(std::string{cameraSim->GetCamera()->GetCameraName()});
|
||||
if (numOfElementsRemoved == 1) {
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
std::optional<frc::Transform3d> GetRobotToCamera(PhotonCameraSim* cameraSim) {
|
||||
return GetRobotToCamera(cameraSim, frc::Timer::GetFPGATimestamp());
|
||||
}
|
||||
std::optional<frc::Transform3d> GetRobotToCamera(PhotonCameraSim* cameraSim,
|
||||
units::second_t time) {
|
||||
if (camTrfMap.find(cameraSim) != camTrfMap.end()) {
|
||||
frc::TimeInterpolatableBuffer<frc::Pose3d> trfBuffer =
|
||||
camTrfMap.at(cameraSim);
|
||||
std::optional<frc::Pose3d> sample = trfBuffer.Sample(time);
|
||||
if (!sample) {
|
||||
return std::nullopt;
|
||||
} else {
|
||||
return std::make_optional(
|
||||
frc::Transform3d{frc::Pose3d{}, sample.value_or(frc::Pose3d{})});
|
||||
}
|
||||
} else {
|
||||
return std::nullopt;
|
||||
}
|
||||
}
|
||||
std::optional<frc::Pose3d> GetCameraPose(PhotonCameraSim* cameraSim) {
|
||||
return GetCameraPose(cameraSim, frc::Timer::GetFPGATimestamp());
|
||||
}
|
||||
std::optional<frc::Pose3d> GetCameraPose(PhotonCameraSim* cameraSim,
|
||||
units::second_t time) {
|
||||
auto robotToCamera = GetRobotToCamera(cameraSim, time);
|
||||
if (!robotToCamera) {
|
||||
return std::nullopt;
|
||||
} else {
|
||||
return std::make_optional(GetRobotPose(time) + robotToCamera.value());
|
||||
}
|
||||
}
|
||||
bool AdjustCamera(PhotonCameraSim* cameraSim,
|
||||
const frc::Transform3d& robotToCamera) {
|
||||
if (camTrfMap.find(cameraSim) != camTrfMap.end()) {
|
||||
camTrfMap.at(cameraSim).AddSample(frc::Timer::GetFPGATimestamp(),
|
||||
frc::Pose3d{} + robotToCamera);
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
void ResetCameraTransforms() {
|
||||
for (const auto& pair : camTrfMap) {
|
||||
ResetCameraTransforms(pair.first);
|
||||
}
|
||||
}
|
||||
bool ResetCameraTransforms(PhotonCameraSim* cameraSim) {
|
||||
units::second_t now = frc::Timer::GetFPGATimestamp();
|
||||
if (camTrfMap.find(cameraSim) != camTrfMap.end()) {
|
||||
auto trfBuffer = camTrfMap.at(cameraSim);
|
||||
frc::Transform3d lastTrf{frc::Pose3d{},
|
||||
trfBuffer.Sample(now).value_or(frc::Pose3d{})};
|
||||
trfBuffer.Clear();
|
||||
AdjustCamera(cameraSim, lastTrf);
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
std::vector<VisionTargetSim> GetVisionTargets() {
|
||||
std::vector<VisionTargetSim> all{};
|
||||
for (const auto& entry : targetSets) {
|
||||
for (const auto& target : entry.second) {
|
||||
all.emplace_back(target);
|
||||
}
|
||||
}
|
||||
return all;
|
||||
}
|
||||
std::vector<VisionTargetSim> GetVisionTargets(std::string type) {
|
||||
return targetSets[type];
|
||||
}
|
||||
void AddVisionTargets(const std::vector<VisionTargetSim>& targets) {
|
||||
AddVisionTargets("targets", targets);
|
||||
}
|
||||
void AddVisionTargets(std::string type,
|
||||
const std::vector<VisionTargetSim>& targets) {
|
||||
if (!targetSets.contains(type)) {
|
||||
targetSets[type] = std::vector<VisionTargetSim>{};
|
||||
}
|
||||
for (const auto& tgt : targets) {
|
||||
targetSets[type].emplace_back(tgt);
|
||||
}
|
||||
}
|
||||
void AddAprilTags(const frc::AprilTagFieldLayout& layout) {
|
||||
std::vector<VisionTargetSim> targets;
|
||||
for (const frc::AprilTag& tag : layout.GetTags()) {
|
||||
targets.emplace_back(VisionTargetSim{layout.GetTagPose(tag.ID).value(),
|
||||
photon::kAprilTag16h5, tag.ID});
|
||||
}
|
||||
AddVisionTargets("apriltag", targets);
|
||||
}
|
||||
void ClearVisionTargets() { targetSets.clear(); }
|
||||
void ClearAprilTags() { RemoveVisionTargets("apriltag"); }
|
||||
void RemoveVisionTargets(std::string type) { targetSets.erase(type); }
|
||||
std::vector<VisionTargetSim> RemoveVisionTargets(
|
||||
const std::vector<VisionTargetSim>& targets) {
|
||||
std::vector<VisionTargetSim> removedList;
|
||||
for (auto& entry : targetSets) {
|
||||
for (auto target : entry.second) {
|
||||
auto it = std::find(targets.begin(), targets.end(), target);
|
||||
if (it != targets.end()) {
|
||||
removedList.emplace_back(target);
|
||||
entry.second.erase(it);
|
||||
}
|
||||
}
|
||||
}
|
||||
return removedList;
|
||||
}
|
||||
frc::Pose3d GetRobotPose() {
|
||||
return GetRobotPose(frc::Timer::GetFPGATimestamp());
|
||||
}
|
||||
frc::Pose3d GetRobotPose(units::second_t timestamp) {
|
||||
return robotPoseBuffer.Sample(timestamp).value_or(frc::Pose3d{});
|
||||
}
|
||||
void ResetRobotPose(const frc::Pose2d& robotPose) {
|
||||
ResetRobotPose(frc::Pose3d{robotPose});
|
||||
}
|
||||
void ResetRobotPose(const frc::Pose3d& robotPose) {
|
||||
robotPoseBuffer.Clear();
|
||||
robotPoseBuffer.AddSample(frc::Timer::GetFPGATimestamp(), robotPose);
|
||||
}
|
||||
frc::Field2d& GetDebugField() { return dbgField; }
|
||||
void Update(const frc::Pose2d& robotPose) { Update(frc::Pose3d{robotPose}); }
|
||||
void Update(const frc::Pose3d& robotPose) {
|
||||
for (auto& set : targetSets) {
|
||||
std::vector<frc::Pose2d> posesToAdd{};
|
||||
for (auto& target : set.second) {
|
||||
posesToAdd.emplace_back(target.GetPose().ToPose2d());
|
||||
}
|
||||
dbgField.GetObject(set.first)->SetPoses(posesToAdd);
|
||||
}
|
||||
|
||||
units::second_t now = frc::Timer::GetFPGATimestamp();
|
||||
robotPoseBuffer.AddSample(now, robotPose);
|
||||
dbgField.SetRobotPose(robotPose.ToPose2d());
|
||||
|
||||
std::vector<VisionTargetSim> allTargets{};
|
||||
for (const auto& set : targetSets) {
|
||||
for (const auto& target : set.second) {
|
||||
allTargets.emplace_back(target);
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<frc::Pose2d> visTgtPoses2d{};
|
||||
std::vector<frc::Pose2d> cameraPoses2d{};
|
||||
bool processed{false};
|
||||
for (const auto& entry : camSimMap) {
|
||||
auto camSim = entry.second;
|
||||
auto optTimestamp = camSim->ConsumeNextEntryTime();
|
||||
if (!optTimestamp) {
|
||||
continue;
|
||||
} else {
|
||||
processed = true;
|
||||
}
|
||||
uint64_t timestampNt = optTimestamp.value();
|
||||
units::second_t latency = camSim->prop.EstLatency();
|
||||
units::second_t timestampCapture =
|
||||
units::microsecond_t{static_cast<double>(timestampNt)} - latency;
|
||||
|
||||
frc::Pose3d lateRobotPose = GetRobotPose(timestampCapture);
|
||||
frc::Pose3d lateCameraPose =
|
||||
lateRobotPose + GetRobotToCamera(camSim, timestampCapture).value();
|
||||
cameraPoses2d.push_back(lateCameraPose.ToPose2d());
|
||||
|
||||
auto camResult = camSim->Process(latency, lateCameraPose, allTargets);
|
||||
camSim->SubmitProcessedFrame(camResult, timestampNt);
|
||||
for (const auto& target : camResult.GetTargets()) {
|
||||
auto trf = target.GetBestCameraToTarget();
|
||||
if (trf == kEmptyTrf) {
|
||||
continue;
|
||||
}
|
||||
visTgtPoses2d.push_back(lateCameraPose.TransformBy(trf).ToPose2d());
|
||||
}
|
||||
}
|
||||
if (processed) {
|
||||
dbgField.GetObject("visibleTargetPoses")->SetPoses(visTgtPoses2d);
|
||||
}
|
||||
if (cameraPoses2d.size() != 0) {
|
||||
dbgField.GetObject("cameras")->SetPoses(cameraPoses2d);
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
std::unordered_map<std::string, PhotonCameraSim*> camSimMap{};
|
||||
static constexpr units::second_t bufferLength{1.5_s};
|
||||
std::unordered_map<PhotonCameraSim*,
|
||||
frc::TimeInterpolatableBuffer<frc::Pose3d>>
|
||||
camTrfMap;
|
||||
frc::TimeInterpolatableBuffer<frc::Pose3d> robotPoseBuffer{bufferLength};
|
||||
std::unordered_map<std::string, std::vector<VisionTargetSim>> targetSets{};
|
||||
frc::Field2d dbgField{};
|
||||
const frc::Transform3d kEmptyTrf{};
|
||||
};
|
||||
} // namespace photon
|
||||
@@ -0,0 +1,73 @@
|
||||
/*
|
||||
* 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 <vector>
|
||||
|
||||
#include <frc/geometry/Pose3d.h>
|
||||
|
||||
#include "photon/estimation/TargetModel.h"
|
||||
|
||||
namespace photon {
|
||||
class VisionTargetSim {
|
||||
public:
|
||||
VisionTargetSim(const frc::Pose3d& pose, const TargetModel& model)
|
||||
: fiducialId(-1), pose(pose), model(model) {}
|
||||
VisionTargetSim(const frc::Pose3d& pose, const TargetModel& model, int id)
|
||||
: fiducialId(id), pose(pose), model(model) {}
|
||||
void SetPose(const frc::Pose3d& newPose) { pose = newPose; }
|
||||
void SetModel(const TargetModel& newModel) { model = newModel; }
|
||||
frc::Pose3d GetPose() const { return pose; }
|
||||
TargetModel GetModel() const { return model; }
|
||||
std::vector<frc::Translation3d> GetFieldVertices() const {
|
||||
return model.GetFieldVertices(pose);
|
||||
}
|
||||
int fiducialId;
|
||||
|
||||
bool operator<(const VisionTargetSim& right) const {
|
||||
return pose.Translation().Norm() < right.pose.Translation().Norm();
|
||||
}
|
||||
|
||||
bool operator==(const VisionTargetSim& other) const {
|
||||
return units::math::abs(pose.Translation().X() -
|
||||
other.GetPose().Translation().X()) < 1_in &&
|
||||
units::math::abs(pose.Translation().Y() -
|
||||
other.GetPose().Translation().Y()) < 1_in &&
|
||||
units::math::abs(pose.Translation().Z() -
|
||||
other.GetPose().Translation().Z()) < 1_in &&
|
||||
units::math::abs(pose.Rotation().X() -
|
||||
other.GetPose().Rotation().X()) < 1_deg &&
|
||||
units::math::abs(pose.Rotation().Y() -
|
||||
other.GetPose().Rotation().Y()) < 1_deg &&
|
||||
units::math::abs(pose.Rotation().Z() -
|
||||
other.GetPose().Rotation().Z()) < 1_deg &&
|
||||
model.GetIsPlanar() == other.GetModel().GetIsPlanar();
|
||||
}
|
||||
|
||||
private:
|
||||
frc::Pose3d pose;
|
||||
TargetModel model;
|
||||
};
|
||||
} // namespace photon
|
||||
Reference in New Issue
Block a user