diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp index c2dc8bac50..9282e3bdd5 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,14 +396,14 @@ std::optional PhotonPoseEstimator::MultiTagPnpStrategy( return Update(result, camMat, distCoeffs, this->multiTagFallbackStrategy); } - // 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(),