From ba878f21f6dd7c30e17e6f5772a22ff0d0e09be4 Mon Sep 17 00:00:00 2001 From: Matthew Morley Date: Thu, 7 Sep 2023 20:50:59 -0400 Subject: [PATCH 1/2] [photon-lib] Fix camera/distortion matrix mixup in C++ --- .../main/native/cpp/photonlib/PhotonPoseEstimator.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp index c2dc8bac50..fef2893aa3 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp @@ -115,7 +115,7 @@ std::optional PhotonPoseEstimator::Update( std::optional PhotonPoseEstimator::Update( const PhotonPipelineResult& result, std::optional cameraMatrixData, - std::optional coeffsData) { + std::optional cameraDistCoeffs) { // Time in the past -- give up, since the following if expects times > 0 if (result.GetTimestamp() < 0_s) { return std::nullopt; @@ -136,12 +136,12 @@ std::optional PhotonPoseEstimator::Update( return std::nullopt; } - return Update(result, cameraMatrixData, coeffsData, this->strategy); + return Update(result, cameraMatrixData, cameraDistCoeffs, this->strategy); } std::optional PhotonPoseEstimator::Update( PhotonPipelineResult result, std::optional cameraMatrixData, - std::optional coeffsData, PoseStrategy strategy) { + std::optional cameraDistCoeffs, PoseStrategy strategy) { std::optional ret = std::nullopt; switch (strategy) { @@ -162,7 +162,7 @@ std::optional PhotonPoseEstimator::Update( ret = AverageBestTargetsStrategy(result); break; case ::photonlib::MULTI_TAG_PNP: - ret = MultiTagPnpStrategy(result, coeffsData, cameraMatrixData); + ret = MultiTagPnpStrategy(result, cameraMatrixData, cameraDistCoeffs); break; default: FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!", @@ -396,6 +396,8 @@ std::optional PhotonPoseEstimator::MultiTagPnpStrategy( return Update(result, camMat, distCoeffs, this->multiTagFallbackStrategy); } + printf("hello\n"); + // Use OpenCV ITERATIVE solver cv::Mat const rvec(3, 1, cv::DataType::type); cv::Mat const tvec(3, 1, cv::DataType::type); From cf05213e87416409aad18b22c9e16b0a2bae28b6 Mon Sep 17 00:00:00 2001 From: Matthew Morley Date: Thu, 7 Sep 2023 20:53:14 -0400 Subject: [PATCH 2/2] Remove print --- .../src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp index fef2893aa3..9282e3bdd5 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp @@ -396,16 +396,14 @@ std::optional PhotonPoseEstimator::MultiTagPnpStrategy( return Update(result, camMat, distCoeffs, this->multiTagFallbackStrategy); } - printf("hello\n"); - - // Use OpenCV ITERATIVE solver + // Output mats for results cv::Mat const rvec(3, 1, cv::DataType::type); cv::Mat const tvec(3, 1, cv::DataType::type); cv::solvePnP(objectPoints, imagePoints, camMat.value(), distCoeffs.value(), rvec, tvec, false, cv::SOLVEPNP_SQPNP); - Pose3d const pose = detail::ToPose3d(tvec, rvec); + const Pose3d pose = detail::ToPose3d(tvec, rvec); return photonlib::EstimatedRobotPose( pose.TransformBy(m_robotToCamera.Inverse()), result.GetTimestamp(),