From 72e0ef41aceb07b13c94049c26011587f508ff3a Mon Sep 17 00:00:00 2001 From: Matt Date: Sun, 4 Aug 2024 10:17:40 -0700 Subject: [PATCH] Yay --- .../java/org/photonvision/PhotonCamera.java | 7 ++- .../org/photonvision/PhotonPoseEstimator.java | 3 +- .../main/native/cpp/photon/PhotonCamera.cpp | 13 +++--- .../cpp/photon/simulation/PhotonCameraSim.cpp | 23 +++++----- .../include/photon/estimation/OpenCVHelp.h | 8 ++-- .../photon/estimation/VisionEstimation.h | 43 +++++++++---------- 6 files changed, 46 insertions(+), 51 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index 80588cc02f..62c90b326b 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -137,13 +137,12 @@ public PhotonCamera(NetworkTableInstance instance, String cameraName) { cameraTable .getRawTopic("rawBytes") .subscribe( - "rawBytes", new byte[] {}, + "rawBytes", + new byte[] {}, PubSubOption.periodic(0.01), PubSubOption.sendAll(true), PubSubOption.pollStorage(20)); - resultSubscriber = - new PacketSubscriber<>( - rawBytesEntry, PhotonPipelineResult.photonStruct); + resultSubscriber = new PacketSubscriber<>(rawBytesEntry, PhotonPipelineResult.photonStruct); driverModePublisher = cameraTable.getBooleanTopic("driverModeRequest").publish(); driverModeSubscriber = cameraTable.getBooleanTopic("driverMode").subscribe(false); inputSaveImgEntry = cameraTable.getIntegerTopic("inputSaveImgCmd").getEntry(0); diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index abaf29e956..e91a33f571 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -393,8 +393,7 @@ private Optional update( return estimatedPose; } - private Optional multiTagOnCoprocStrategy( - PhotonPipelineResult result) { + private Optional multiTagOnCoprocStrategy(PhotonPipelineResult result) { if (result.getMultiTagResult().isPresent()) { var best_tf = result.getMultiTagResult().get().estimatedPose.best; var best = diff --git a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp index a8a36794f3..281df4b743 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp @@ -122,9 +122,6 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() { // Prints warning if not connected VerifyVersion(); - // Clear the current packet. - packet.Clear(); - // Fill the packet with latest data and populate result. units::microsecond_t now = units::microsecond_t(frc::RobotController::GetFPGATime()); @@ -151,8 +148,9 @@ std::vector PhotonCamera::GetAllUnreadResults() { const auto changes = rawBytesEntry.ReadQueue(); - // Create the new result list -- these will be updated in-place - std::vector ret(changes.size()); + // Create the new result list + std::vector ret; + ret.reserve(changes.size()); for (size_t i = 0; i < changes.size(); i++) { const nt::Timestamped>& value = changes[i]; @@ -163,13 +161,14 @@ std::vector PhotonCamera::GetAllUnreadResults() { // Fill the packet with latest data and populate result. photon::Packet packet{value.value}; + auto result = packet.Unpack(); - PhotonPipelineResult& result = ret[i]; - packet >> result; // TODO: NT4 timestamps are still not to be trusted. But it's the best we // can do until we can make time sync more reliable. result.SetRecieveTimestamp(units::microsecond_t(value.time) - result.GetLatency()); + + ret.push_back(result); } return ret; diff --git a/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp b/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp index 381e99c9c1..0153f9d3a4 100644 --- a/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp +++ b/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp @@ -201,7 +201,7 @@ PhotonPipelineResult PhotonCameraSim::Process( continue; } - PNPResult pnpSim{}; + std::optional pnpSim{}; if (tgt.fiducialId >= 0 && tgt.GetFieldVertices().size() == 4) { pnpSim = OpenCVHelp::SolvePNP_Square( prop.GetIntrinsics(), prop.GetDistCoeffs(), @@ -226,8 +226,10 @@ PhotonPipelineResult PhotonCameraSim::Process( -centerRot.Z().convert().to(), -centerRot.Y().convert().to(), areaPercent, centerRot.X().convert().to(), tgt.fiducialId, - tgt.objDetClassId, tgt.objDetConf, pnpSim.best, pnpSim.alt, - pnpSim.ambiguity, smallVec, cornersDouble); + tgt.objDetClassId, tgt.objDetConf, + pnpSim ? pnpSim->best : frc::Transform3d{}, + pnpSim ? pnpSim->alt : frc::Transform3d{}, + pnpSim ? pnpSim->ambiguity : -1, smallVec, cornersDouble); } if (videoSimRawEnabled) { @@ -291,16 +293,14 @@ PhotonPipelineResult PhotonCameraSim::Process( 1, videoSimFrameProcessed)), cv::LINE_AA); - wpi::SmallVector, 4> smallVec = - tgt.GetMinAreaRectCorners(); + auto smallVec = tgt.GetMinAreaRectCorners(); std::vector> cornersCopy{}; cornersCopy.reserve(4); for (const auto& corner : smallVec) { - cornersCopy.emplace_back( - std::make_pair(static_cast(corner.first), - static_cast(corner.second))); + cornersCopy.emplace_back(std::make_pair( + static_cast(corner.x), static_cast(corner.y))); } VideoSimUtil::DrawPoly( @@ -321,12 +321,13 @@ PhotonPipelineResult PhotonCameraSim::Process( std::vector visibleLayoutTags = VisionEstimation::GetVisibleLayoutTags(detectableTgts, tagLayout); if (visibleLayoutTags.size() > 1) { - wpi::SmallVector usedIds{}; + std::vector usedIds{}; + usedIds.reserve(32); 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( + auto pnpResult = VisionEstimation::EstimateCamPosePNP( prop.GetIntrinsics(), prop.GetDistCoeffs(), detectableTgts, tagLayout, kAprilTag36h11); multiTagResults = MultiTargetPNPResult{pnpResult, usedIds}; @@ -346,7 +347,7 @@ void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result, recieveTimestamp); Packet newPacket{}; - newPacket << result; + newPacket.Pack(result); ts.rawBytesEntry.Set(newPacket.GetData(), recieveTimestamp); diff --git a/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h b/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h index 85ba543b12..794883e2c6 100644 --- a/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h +++ b/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h @@ -184,7 +184,7 @@ static frc::Rotation3d RVecToRotation(const cv::Mat& rvecInput) { units::radian_t{data[2]}}); } -[[maybe_unused]] static photon::PnpResult SolvePNP_Square( +[[maybe_unused]] static std::optional SolvePNP_Square( const Eigen::Matrix& cameraMatrix, const Eigen::Matrix& distCoeffs, std::vector modelTrls, @@ -233,6 +233,7 @@ static frc::Rotation3d RVecToRotation(const cv::Mat& rvecInput) { if (std::isnan(errors[0])) { fmt::print("SolvePNP_Square failed!\n"); + return std::nullopt; } if (alt) { photon::PnpResult result; @@ -241,18 +242,16 @@ static frc::Rotation3d RVecToRotation(const cv::Mat& rvecInput) { result.ambiguity = errors[0] / errors[1]; result.bestReprojErr = errors[0]; result.altReprojErr = errors[1]; - result.isPresent = true; return result; } else { photon::PnpResult result; result.best = best; result.bestReprojErr = errors[0]; - result.isPresent = true; return result; } } -[[maybe_unused]] static photon::PnpResult SolvePNP_SQPNP( +[[maybe_unused]] static std::optional SolvePNP_SQPNP( const Eigen::Matrix& cameraMatrix, const Eigen::Matrix& distCoeffs, std::vector modelTrls, @@ -286,7 +285,6 @@ static frc::Rotation3d RVecToRotation(const cv::Mat& rvecInput) { photon::PnpResult result; result.best = best; result.bestReprojErr = error; - result.isPresent = true; return result; } } // namespace OpenCVHelp diff --git a/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h b/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h index fadc1d8b61..53df691598 100644 --- a/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h +++ b/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h @@ -47,7 +47,7 @@ static std::vector GetVisibleLayoutTags( return retVal; } -static PnpResult EstimateCamPosePNP( +static std::optional EstimateCamPosePNP( const Eigen::Matrix& cameraMatrix, const Eigen::Matrix& distCoeffs, const std::vector& visTags, @@ -76,24 +76,24 @@ static PnpResult EstimateCamPosePNP( std::vector points = OpenCVHelp::CornersToPoints(corners); if (knownTags.size() == 1) { - PnpResult camToTag = OpenCVHelp::SolvePNP_Square( - cameraMatrix, distCoeffs, tagModel.GetVertices(), points); - if (!camToTag.isPresent) { + auto camToTag = OpenCVHelp::SolvePNP_Square(cameraMatrix, distCoeffs, + tagModel.GetVertices(), points); + if (!camToTag) { return PnpResult{}; } frc::Pose3d bestPose = - knownTags[0].pose.TransformBy(camToTag.best.Inverse()); + knownTags[0].pose.TransformBy(camToTag->best.Inverse()); frc::Pose3d altPose{}; - if (camToTag.ambiguity != 0) { - altPose = knownTags[0].pose.TransformBy(camToTag.alt.Inverse()); + if (camToTag->ambiguity != 0) { + altPose = knownTags[0].pose.TransformBy(camToTag->alt.Inverse()); } frc::Pose3d o{}; PnpResult result{}; result.best = frc::Transform3d{o, bestPose}; result.alt = frc::Transform3d{o, altPose}; - result.ambiguity = camToTag.ambiguity; - result.bestReprojErr = camToTag.bestReprojErr; - result.altReprojErr = camToTag.altReprojErr; + result.ambiguity = camToTag->ambiguity; + result.bestReprojErr = camToTag->bestReprojErr; + result.altReprojErr = camToTag->altReprojErr; return result; } else { std::vector objectTrls{}; @@ -101,19 +101,18 @@ static PnpResult EstimateCamPosePNP( auto verts = tagModel.GetFieldVertices(tag.pose); objectTrls.insert(objectTrls.end(), verts.begin(), verts.end()); } - PnpResult camToOrigin = OpenCVHelp::SolvePNP_SQPNP(cameraMatrix, distCoeffs, - objectTrls, points); - if (!camToOrigin.isPresent) { - return PnpResult{}; + auto camToOrigin = OpenCVHelp::SolvePNP_SQPNP(cameraMatrix, distCoeffs, + objectTrls, points); + if (!camToOrigin) { + return std::nullopt; } else { - PnpResult result{}; - result.best = camToOrigin.best.Inverse(), - result.alt = camToOrigin.alt.Inverse(), - result.ambiguity = camToOrigin.ambiguity; - result.bestReprojErr = camToOrigin.bestReprojErr; - result.altReprojErr = camToOrigin.altReprojErr; - result.isPresent = true; - return result; + PnpResult ret{}; + ret.best = camToOrigin.best.Inverse(), + ret.alt = camToOrigin.alt.Inverse(), + ret.ambiguity = camToOrigin.ambiguity; + ret.bestReprojErr = camToOrigin.bestReprojErr; + ret.altReprojErr = camToOrigin.altReprojErr; + return ret; } } }