From ca15e6660959976f2b35022661f971f481598974 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Fri, 24 Nov 2023 15:22:24 -0500 Subject: [PATCH] Update cpp --- .../native/cpp/photon/PhotonPoseEstimator.cpp | 4 +- .../main/native/include/photon/PhotonCamera.h | 1 - .../include/photon/PhotonPoseEstimator.h | 1 - .../include/photon/PhotonTargetSortMode.h | 1 - .../main/native/include/photon/PhotonUtils.h | 1 - .../photon/simulation/SimPhotonCamera.h | 1 - .../photon/simulation/SimVisionSystem.h | 1 - .../photon/simulation/SimVisionTarget.h | 1 - .../photon/targeting/MultiTargetPNPResult.cpp | 17 ++++- .../native/cpp/photon/targeting/PNPResult.cpp | 60 ---------------- .../photon/targeting/PhotonPipelineResult.cpp | 15 ++-- .../photon/targeting/MultiTargetPNPResult.h | 24 +++++-- .../include/photon/targeting/PNPResult.h | 69 ------------------- .../photon/targeting/PhotonPipelineResult.h | 9 ++- 14 files changed, 47 insertions(+), 158 deletions(-) delete mode 100644 photon-targeting/src/main/native/cpp/photon/targeting/PNPResult.cpp delete mode 100644 photon-targeting/src/main/native/include/photon/targeting/PNPResult.h diff --git a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp index f2ff30f7ea..0cf2cf0a55 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp @@ -360,8 +360,8 @@ frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) { std::optional PhotonPoseEstimator::MultiTagOnCoprocStrategy( PhotonPipelineResult result, std::optional camMat, std::optional distCoeffs) { - if (result.MultiTagResult().result.isPresent) { - const auto field2camera = result.MultiTagResult().result.best; + if (result.MultiTagResult().has_value()) { + const auto field2camera = result.MultiTagResult().value().best; const auto fieldToRobot = frc::Pose3d() + field2camera + m_robotToCamera.Inverse(); diff --git a/photon-lib/src/main/native/include/photon/PhotonCamera.h b/photon-lib/src/main/native/include/photon/PhotonCamera.h index 12adb6ad54..7e7676986a 100644 --- a/photon-lib/src/main/native/include/photon/PhotonCamera.h +++ b/photon-lib/src/main/native/include/photon/PhotonCamera.h @@ -40,7 +40,6 @@ #include #include "photon/targeting/MultiTargetPNPResult.h" -#include "photon/targeting/PNPResult.h" #include "photon/targeting/PhotonPipelineResult.h" #include "photon/targeting/PhotonTrackedTarget.h" diff --git a/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h index b2d495be80..36338e61a8 100644 --- a/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h +++ b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h @@ -32,7 +32,6 @@ #include "photon/PhotonCamera.h" #include "photon/targeting/MultiTargetPNPResult.h" -#include "photon/targeting/PNPResult.h" #include "photon/targeting/PhotonPipelineResult.h" #include "photon/targeting/PhotonTrackedTarget.h" diff --git a/photon-lib/src/main/native/include/photon/PhotonTargetSortMode.h b/photon-lib/src/main/native/include/photon/PhotonTargetSortMode.h index bbe70d960b..f0dce0233e 100644 --- a/photon-lib/src/main/native/include/photon/PhotonTargetSortMode.h +++ b/photon-lib/src/main/native/include/photon/PhotonTargetSortMode.h @@ -27,7 +27,6 @@ #include #include "photon/targeting/MultiTargetPNPResult.h" -#include "photon/targeting/PNPResult.h" #include "photon/targeting/PhotonPipelineResult.h" #include "photon/targeting/PhotonTrackedTarget.h" diff --git a/photon-lib/src/main/native/include/photon/PhotonUtils.h b/photon-lib/src/main/native/include/photon/PhotonUtils.h index de18ae8125..b0a92b6e7d 100644 --- a/photon-lib/src/main/native/include/photon/PhotonUtils.h +++ b/photon-lib/src/main/native/include/photon/PhotonUtils.h @@ -33,7 +33,6 @@ #include #include "photon/targeting/MultiTargetPNPResult.h" -#include "photon/targeting/PNPResult.h" #include "photon/targeting/PhotonPipelineResult.h" #include "photon/targeting/PhotonTrackedTarget.h" diff --git a/photon-lib/src/main/native/include/photon/simulation/SimPhotonCamera.h b/photon-lib/src/main/native/include/photon/simulation/SimPhotonCamera.h index dbd3270dfa..512da90e73 100644 --- a/photon-lib/src/main/native/include/photon/simulation/SimPhotonCamera.h +++ b/photon-lib/src/main/native/include/photon/simulation/SimPhotonCamera.h @@ -35,7 +35,6 @@ #include "photon/PhotonCamera.h" #include "photon/PhotonTargetSortMode.h" #include "photon/targeting/MultiTargetPNPResult.h" -#include "photon/targeting/PNPResult.h" #include "photon/targeting/PhotonPipelineResult.h" #include "photon/targeting/PhotonTrackedTarget.h" diff --git a/photon-lib/src/main/native/include/photon/simulation/SimVisionSystem.h b/photon-lib/src/main/native/include/photon/simulation/SimVisionSystem.h index dd1b39f194..dd0feb9ca0 100644 --- a/photon-lib/src/main/native/include/photon/simulation/SimVisionSystem.h +++ b/photon-lib/src/main/native/include/photon/simulation/SimVisionSystem.h @@ -35,7 +35,6 @@ #include "SimPhotonCamera.h" #include "SimVisionTarget.h" #include "photon/targeting/MultiTargetPNPResult.h" -#include "photon/targeting/PNPResult.h" #include "photon/targeting/PhotonPipelineResult.h" #include "photon/targeting/PhotonTrackedTarget.h" diff --git a/photon-lib/src/main/native/include/photon/simulation/SimVisionTarget.h b/photon-lib/src/main/native/include/photon/simulation/SimVisionTarget.h index 5d1975cd1a..d2aa4d9d9d 100644 --- a/photon-lib/src/main/native/include/photon/simulation/SimVisionTarget.h +++ b/photon-lib/src/main/native/include/photon/simulation/SimVisionTarget.h @@ -28,7 +28,6 @@ #include #include "photon/targeting/MultiTargetPNPResult.h" -#include "photon/targeting/PNPResult.h" #include "photon/targeting/PhotonPipelineResult.h" #include "photon/targeting/PhotonTrackedTarget.h" diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/MultiTargetPNPResult.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/MultiTargetPNPResult.cpp index dee7cc2b9d..30ca56f559 100644 --- a/photon-targeting/src/main/native/cpp/photon/targeting/MultiTargetPNPResult.cpp +++ b/photon-targeting/src/main/native/cpp/photon/targeting/MultiTargetPNPResult.cpp @@ -22,7 +22,10 @@ namespace photon { bool MultiTargetPNPResult::operator==(const MultiTargetPNPResult& other) const { - return other.result == result && other.fiducialIdsUsed == fiducialIdsUsed; + return other.best == best && + other.bestReprojErr == bestReprojErr && other.alt == alt && + other.altReprojErr == altReprojErr && other.ambiguity == ambiguity + && other.fiducialIdsUsed == fiducialIdsUsed; } } // namespace photon @@ -45,7 +48,11 @@ wpi::Protobuf::Unpack( } return photon::MultiTargetPNPResult{ - wpi::UnpackProtobuf(m->estimated_pose()), + wpi::UnpackProtobuf(m->best()), + m->best_reproj_err(), + wpi::UnpackProtobuf(m->alt()), + m->alt_reproj_err(), + m->ambiguity(), fiducialIdsUsed}; } @@ -53,7 +60,11 @@ void wpi::Protobuf::Pack( google::protobuf::Message* msg, const photon::MultiTargetPNPResult& value) { auto m = static_cast(msg); - wpi::PackProtobuf(m->mutable_estimated_pose(), value.result); + wpi::PackProtobuf(m->mutable_best(), value.best); + m->set_best_reproj_err(value.bestReprojErr); + wpi::PackProtobuf(m->mutable_alt(), value.alt); + m->set_alt_reproj_err(value.altReprojErr); + m->set_ambiguity(value.ambiguity); m->clear_fiducial_ids_used(); for (const auto& t : value.fiducialIdsUsed) { diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/PNPResult.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/PNPResult.cpp deleted file mode 100644 index 96d3b3d23b..0000000000 --- a/photon-targeting/src/main/native/cpp/photon/targeting/PNPResult.cpp +++ /dev/null @@ -1,60 +0,0 @@ -/* - * Copyright (C) Photon Vision. - * - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see . - */ - -#include "photon/targeting/PNPResult.h" - -#include "photon.pb.h" - -namespace photon { -bool PNPResult::operator==(const PNPResult& other) const { - return other.isPresent == isPresent && other.best == best && - other.bestReprojErr == bestReprojErr && other.alt == alt && - other.altReprojErr == altReprojErr && other.ambiguity == ambiguity; -} -} // namespace photon - -google::protobuf::Message* wpi::Protobuf::New( - google::protobuf::Arena* arena) { - return google::protobuf::Arena::CreateMessage< - photonvision::proto::ProtobufPNPResult>(arena); -} - -photon::PNPResult wpi::Protobuf::Unpack( - const google::protobuf::Message& msg) { - auto m = static_cast(&msg); - - if (!m->is_present()) { - return photon::PNPResult(); - } - - return photon::PNPResult{wpi::UnpackProtobuf(m->best()), - m->best_reproj_err(), - wpi::UnpackProtobuf(m->alt()), - m->alt_reproj_err(), m->ambiguity()}; -} - -void wpi::Protobuf::Pack(google::protobuf::Message* msg, - const photon::PNPResult& value) { - auto m = static_cast(msg); - - m->set_is_present(value.isPresent); - wpi::PackProtobuf(m->mutable_best(), value.best); - m->set_best_reproj_err(value.bestReprojErr); - wpi::PackProtobuf(m->mutable_alt(), value.alt); - m->set_alt_reproj_err(value.altReprojErr); - m->set_ambiguity(value.ambiguity); -} diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp index 64378dd3e7..f049702d16 100644 --- a/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp +++ b/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp @@ -45,10 +45,13 @@ wpi::Protobuf::Unpack( targets.emplace_back(wpi::UnpackProtobuf(t)); } - return photon::PhotonPipelineResult{ - units::millisecond_t{m->latency_ms()}, targets, - wpi::UnpackProtobuf( - m->multi_target_result())}; + if(m->has_multi_target_result()) { + return photon::PhotonPipelineResult{ + units::millisecond_t{m->latency_ms()}, targets, + wpi::UnpackProtobuf(m->multi_target_result())}; + } else { + return photon::PhotonPipelineResult{units::millisecond_t{m->latency_ms()}, targets}; + } } void wpi::Protobuf::Pack( @@ -62,5 +65,7 @@ void wpi::Protobuf::Pack( wpi::PackProtobuf(m->add_targets(), t); } - wpi::PackProtobuf(m->mutable_multi_target_result(), value.multitagResult); + if(value.multitagResult.has_value()) { + wpi::PackProtobuf(m->mutable_multi_target_result(), value.multitagResult.value()); + } } diff --git a/photon-targeting/src/main/native/include/photon/targeting/MultiTargetPNPResult.h b/photon-targeting/src/main/native/include/photon/targeting/MultiTargetPNPResult.h index cf5f58741a..64814ae1e6 100644 --- a/photon-targeting/src/main/native/include/photon/targeting/MultiTargetPNPResult.h +++ b/photon-targeting/src/main/native/include/photon/targeting/MultiTargetPNPResult.h @@ -21,19 +21,29 @@ #include #include -#include "PNPResult.h" - namespace photon { class MultiTargetPNPResult { public: - PNPResult result; - wpi::SmallVector fiducialIdsUsed; + frc::Transform3d best; + double bestReprojErr; + + frc::Transform3d alt; + double altReprojErr; - MultiTargetPNPResult() = default; + double ambiguity; + + wpi::SmallVector fiducialIdsUsed; - MultiTargetPNPResult(PNPResult result, + MultiTargetPNPResult(frc::Transform3d best, double bestReprojErr, + frc::Transform3d alt, double altReprojErr, + double ambiguity, wpi::SmallVector fiducialIdsUsed) - : result(result), fiducialIdsUsed(fiducialIdsUsed) {} + : best(best), + bestReprojErr(bestReprojErr), + alt(alt), + altReprojErr(altReprojErr), + ambiguity(ambiguity), + fiducialIdsUsed(fiducialIdsUsed) {} bool operator==(const MultiTargetPNPResult& other) const; }; diff --git a/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h b/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h deleted file mode 100644 index f200a3de04..0000000000 --- a/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h +++ /dev/null @@ -1,69 +0,0 @@ -/* - * Copyright (C) Photon Vision. - * - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see . - */ - -#pragma once - -#include -#include - -namespace photon { - -class PNPResult { - public: - // This could be wrapped in an std::optional, but chose to do it this way to - // mirror Java - bool isPresent; - - frc::Transform3d best; - double bestReprojErr; - - frc::Transform3d alt; - double altReprojErr; - - double ambiguity; - - // Apparently this can't be default-constructed? Things seem to have garbadge - // with the defaulted empty constructor anyhow - PNPResult() - : isPresent{false}, - best{frc::Transform3d{}}, - bestReprojErr{0}, - alt{frc::Transform3d()}, - altReprojErr{0}, - ambiguity{0} {} - - PNPResult(frc::Transform3d best, double bestReprojErr, frc::Transform3d alt, - double altReprojErr, double ambiguity) - : best(best), - bestReprojErr(bestReprojErr), - alt(alt), - altReprojErr(altReprojErr), - ambiguity(ambiguity) { - this->isPresent = true; - } - - bool operator==(const PNPResult& other) const; -}; -} // namespace photon - -template <> -struct wpi::Protobuf { - static google::protobuf::Message* New(google::protobuf::Arena* arena); - static photon::PNPResult Unpack(const google::protobuf::Message& msg); - static void Pack(google::protobuf::Message* msg, - const photon::PNPResult& value); -}; diff --git a/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h b/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h index 8c1abeb965..b70d90b232 100644 --- a/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h +++ b/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -37,7 +38,7 @@ class PhotonPipelineResult { units::millisecond_t latency = 0_s; units::second_t timestamp = -1_s; wpi::SmallVector targets; - MultiTargetPNPResult multitagResult; + std::optional multitagResult; inline static bool HAS_WARNED = false; /** @@ -103,11 +104,9 @@ class PhotonPipelineResult { units::second_t GetTimestamp() const { return timestamp; } /** - * Return the latest mulit-target result, as calculated on your coprocessor. - * Be sure to check getMultiTagResult().estimatedPose.isPresent before using - * the pose estimate! + * Return the MultiTarget Result. Empty if disabled or unable to create result. */ - const MultiTargetPNPResult& MultiTagResult() const { return multitagResult; } + std::optional MultiTagResult() { return multitagResult; } /** * Sets the timestamp in seconds