Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-20 00:51:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Allow opencv8 distortion model in PhotonCamera (#1317)

Browse Source 
We previously assumed only OpenCV5 but mrcal uses opencv8
This commit is contained in:
Matt
2024-05-29 17:28:35 -04:00
committed by GitHub
parent fcca858a37
commit 19b4802094
17 changed files with 172 additions and 94 deletions
Download Patch File Download Diff File Expand all files Collapse all files

39
photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp
View File

@@ -159,20 +159,6 @@ LEDMode PhotonCamera::GetLEDMode() const {
return static_cast<LEDMode>(static_cast<int>(ledModeSub.Get()));
}
std::optional<cv::Mat> PhotonCamera::GetCameraMatrix() {
auto camCoeffs = cameraIntrinsicsSubscriber.Get();
if (camCoeffs.size() == 9) {
cv::Mat retVal(3, 3, CV_64FC1);
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
retVal.at<double>(i, j) = camCoeffs[(j * 3) + i];
}
}
return retVal;
}
return std::nullopt;
}
void PhotonCamera::SetLEDMode(LEDMode mode) {
ledModePub.Set(static_cast<int>(mode));
}
@@ -181,13 +167,26 @@ const std::string_view PhotonCamera::GetCameraName() const {
return cameraName;
}
std::optional<cv::Mat> PhotonCamera::GetDistCoeffs() {
std::optional<PhotonCamera::CameraMatrix> PhotonCamera::GetCameraMatrix() {
auto camCoeffs = cameraIntrinsicsSubscriber.Get();
if (camCoeffs.size() == 9) {
PhotonCamera::CameraMatrix retVal =
Eigen::Map<const PhotonCamera::CameraMatrix>(camCoeffs.data());
return retVal;
}
return std::nullopt;
}
std::optional<PhotonCamera::DistortionMatrix> PhotonCamera::GetDistCoeffs() {
auto distCoeffs = cameraDistortionSubscriber.Get();
if (distCoeffs.size() == 5) {
cv::Mat retVal(5, 1, CV_64FC1);
for (int i = 0; i < 5; i++) {
retVal.at<double>(i, 0) = distCoeffs[i];
}
auto bound = distCoeffs.size();
if (bound > 0 && bound <= 8) {
PhotonCamera::DistortionMatrix retVal =
PhotonCamera::DistortionMatrix::Zero();
Eigen::Map<const Eigen::VectorXd> map(distCoeffs.data(), bound);
retVal.block(0, 0, bound, 1) = map;
return retVal;
}
return std::nullopt;

35
photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp
View File

@@ -50,6 +50,9 @@
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
#define OPENCV_DISABLE_EIGEN_TENSOR_SUPPORT
#include <opencv2/core/eigen.hpp>
namespace photon {
namespace detail {
@@ -126,8 +129,9 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
const PhotonPipelineResult& result, std::optional<cv::Mat> cameraMatrixData,
std::optional<cv::Mat> cameraDistCoeffs) {
const PhotonPipelineResult& result,
std::optional<PhotonCamera::CameraMatrix> cameraMatrixData,
std::optional<PhotonCamera::DistortionMatrix> cameraDistCoeffs) {
// Time in the past -- give up, since the following if expects times > 0
if (result.GetTimestamp() < 0_s) {
return std::nullopt;
@@ -152,8 +156,10 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
PhotonPipelineResult result, std::optional<cv::Mat> cameraMatrixData,
std::optional<cv::Mat> cameraDistCoeffs, PoseStrategy strategy) {
const PhotonPipelineResult& result,
std::optional<PhotonCamera::CameraMatrix> cameraMatrixData,
std::optional<PhotonCamera::DistortionMatrix> cameraDistCoeffs,
PoseStrategy strategy) {
std::optional<EstimatedRobotPose> ret = std::nullopt;
switch (strategy) {
@@ -371,8 +377,9 @@ frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) {
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnCoprocStrategy(
PhotonPipelineResult result, std::optional<cv::Mat> camMat,
std::optional<cv::Mat> distCoeffs) {
PhotonPipelineResult result,
std::optional<PhotonCamera::CameraMatrix> camMat,
std::optional<PhotonCamera::DistortionMatrix> distCoeffs) {
if (result.MultiTagResult().result.isPresent) {
const auto field2camera = result.MultiTagResult().result.best;
@@ -388,8 +395,9 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnCoprocStrategy(
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
PhotonPipelineResult result, std::optional<cv::Mat> camMat,
std::optional<cv::Mat> distCoeffs) {
PhotonPipelineResult result,
std::optional<PhotonCamera::CameraMatrix> camMat,
std::optional<PhotonCamera::DistortionMatrix> distCoeffs) {
using namespace frc;
// Need at least 2 targets
@@ -433,8 +441,15 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
cv::Mat const rvec(3, 1, cv::DataType<double>::type);
cv::Mat const tvec(3, 1, cv::DataType<double>::type);
cv::solvePnP(objectPoints, imagePoints, camMat.value(), distCoeffs.value(),
rvec, tvec, false, cv::SOLVEPNP_SQPNP);
{
cv::Mat cameraMatCV(camMat->rows(), camMat->cols(), CV_64F);
cv::eigen2cv(*camMat, cameraMatCV);
cv::Mat distCoeffsMatCV(distCoeffs->rows(), distCoeffs->cols(), CV_64F);
cv::eigen2cv(*distCoeffs, distCoeffsMatCV);
cv::solvePnP(objectPoints, imagePoints, cameraMatCV, distCoeffsMatCV, rvec,
tvec, false, cv::SOLVEPNP_SQPNP);
}
const Pose3d pose = detail::ToPose3d(tvec, rvec);