Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-19 00:41: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;