Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-30 02:31:40 +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);

19
photon-lib/src/main/native/include/photon/PhotonCamera.h
View File

@@ -149,8 +149,23 @@ class PhotonCamera {
*/
const std::string_view GetCameraName() const;
std::optional<cv::Mat> GetCameraMatrix();
std::optional<cv::Mat> GetDistCoeffs();
using CameraMatrix = Eigen::Matrix<double, 3, 3>;
using DistortionMatrix = Eigen::Matrix<double, 8, 1>;
/**
* @brief Get the camera calibration matrix, in standard OpenCV form
*
* @return std::optional<cv::Mat>
*/
std::optional<CameraMatrix> GetCameraMatrix();
/**
* @brief Get the camera calibration distortion coefficients, in OPENCV8 form.
* Higher order terms are set to zero.
*
* @return std::optional<cv::Mat>
*/
std::optional<DistortionMatrix> GetDistCoeffs();
static void SetVersionCheckEnabled(bool enabled);

20
photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h
View File

@@ -213,8 +213,8 @@ class PhotonPoseEstimator {
*/
std::optional<EstimatedRobotPose> Update(
const PhotonPipelineResult& result,
std::optional<cv::Mat> cameraMatrixData,
std::optional<cv::Mat> coeffsData);
std::optional<PhotonCamera::CameraMatrix> cameraMatrixData,
std::optional<PhotonCamera::DistortionMatrix> coeffsData);
inline std::shared_ptr<PhotonCamera> GetCamera() { return camera; }
@@ -236,8 +236,10 @@ class PhotonPoseEstimator {
inline void InvalidatePoseCache() { poseCacheTimestamp = -1_s; }
std::optional<EstimatedRobotPose> Update(
PhotonPipelineResult result, std::optional<cv::Mat> cameraMatrixData,
std::optional<cv::Mat> coeffsData, PoseStrategy strategy);
const PhotonPipelineResult& result,
std::optional<PhotonCamera::CameraMatrix> cameraMatrixData,
std::optional<PhotonCamera::DistortionMatrix> coeffsData,
PoseStrategy strategy);
/**
* Return the estimated position of the robot with the lowest position
@@ -278,8 +280,9 @@ class PhotonPoseEstimator {
* @return the estimated position of the robot in the FCS
*/
std::optional<EstimatedRobotPose> 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);
/**
* Return the pose calculation using all targets in view in the same PNP()
@@ -289,8 +292,9 @@ class PhotonPoseEstimator {
timestamp of this estimation.
*/
std::optional<EstimatedRobotPose> 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);
/**
* Return the average of the best target poses using ambiguity as weight.

30
photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h
View File

@@ -60,8 +60,8 @@ class SimCameraProperties {
frc::Rotation2d fovHeight{
units::radian_t{std::atan(diagRatio * (height / resDiag)) * 2}};
Eigen::Matrix<double, 5, 1> newDistCoeffs;
newDistCoeffs << 0, 0, 0, 0, 0;
Eigen::Matrix<double, 8, 1> newDistCoeffs =
Eigen::Matrix<double, 8, 1>::Zero();
double cx = width / 2.0 - 0.5;
double cy = height / 2.0 - 0.5;
@@ -76,7 +76,7 @@ class SimCameraProperties {
void SetCalibration(int width, int height,
const Eigen::Matrix<double, 3, 3>& newCamIntrinsics,
const Eigen::Matrix<double, 5, 1>& newDistCoeffs) {
const Eigen::Matrix<double, 8, 1>& newDistCoeffs) {
resWidth = width;
resHeight = height;
camIntrinsics = newCamIntrinsics;
@@ -151,7 +151,7 @@ class SimCameraProperties {
Eigen::Matrix<double, 3, 3> GetIntrinsics() const { return camIntrinsics; }
Eigen::Matrix<double, 5, 1> GetDistCoeffs() const { return distCoeffs; }
Eigen::Matrix<double, 8, 1> GetDistCoeffs() const { return distCoeffs; }
units::hertz_t GetFPS() const { return 1 / frameSpeed; }
@@ -377,9 +377,9 @@ class SimCameraProperties {
(Eigen::MatrixXd(3, 3) << 328.2733242048587, 0.0, 164.8190261141906,
0.0, 318.0609794305216, 123.8633838438093, 0.0, 0.0, 1.0)
.finished(),
Eigen::Matrix<double, 5, 1>{
Eigen::Matrix<double, 8, 1>{
0.09957946553445934, -0.9166265114485799, 0.0019519890627236526,
-0.0036071725380870333, 1.5627234622420942});
-0.0036071725380870333, 1.5627234622420942, 0, 0, 0});
prop.SetCalibError(0.21, 0.0124);
prop.SetFPS(30_Hz);
prop.SetAvgLatency(30_ms);
@@ -394,9 +394,9 @@ class SimCameraProperties {
(Eigen::MatrixXd(3, 3) << 669.1428078983059, 0.0, 322.53377249329213,
0.0, 646.9843137061716, 241.26567383784163, 0.0, 0.0, 1.0)
.finished(),
Eigen::Matrix<double, 5, 1>{
Eigen::Matrix<double, 8, 1>{
0.12788470750464645, -1.2350335805796528, 0.0024990767286192732,
-0.0026958287600230705, 2.2951386729115537});
-0.0026958287600230705, 2.2951386729115537, 0, 0, 0});
prop.SetCalibError(0.26, 0.046);
prop.SetFPS(15_Hz);
prop.SetAvgLatency(65_ms);
@@ -411,9 +411,9 @@ class SimCameraProperties {
(Eigen::MatrixXd(3, 3) << 511.22843367007755, 0.0, 323.62049380211096,
0.0, 514.5452336723849, 261.8827920543568, 0.0, 0.0, 1.0)
.finished(),
Eigen::Matrix<double, 5, 1>{0.1917469998873756, -0.5142936883324216,
Eigen::Matrix<double, 8, 1>{0.1917469998873756, -0.5142936883324216,
0.012461562046896614, 0.0014084973492408186,
0.35160648971214437});
0.35160648971214437, 0, 0, 0});
prop.SetCalibError(0.25, 0.05);
prop.SetFPS(15_Hz);
prop.SetAvgLatency(35_ms);
@@ -428,9 +428,9 @@ class SimCameraProperties {
(Eigen::MatrixXd(3, 3) << 769.6873145148892, 0.0, 486.1096609458122,
0.0, 773.8164483705323, 384.66071662358354, 0.0, 0.0, 1.0)
.finished(),
Eigen::Matrix<double, 5, 1>{0.189462064814501, -0.49903003669627627,
Eigen::Matrix<double, 8, 1>{0.189462064814501, -0.49903003669627627,
0.007468423590519429, 0.002496885298683693,
0.3443122090208624});
0.3443122090208624, 0, 0, 0});
prop.SetCalibError(0.35, 0.10);
prop.SetFPS(10_Hz);
prop.SetAvgLatency(50_ms);
@@ -445,9 +445,9 @@ class SimCameraProperties {
(Eigen::MatrixXd(3, 3) << 1011.3749416937393, 0.0, 645.4955139388737,
0.0, 1008.5391755084075, 508.32877656020196, 0.0, 0.0, 1.0)
.finished(),
Eigen::Matrix<double, 5, 1>{0.13730101577061535, -0.2904345656989261,
Eigen::Matrix<double, 8, 1>{0.13730101577061535, -0.2904345656989261,
8.32475714507539E-4, -3.694397782014239E-4,
0.09487962227027584});
0.09487962227027584, 0, 0, 0});
prop.SetCalibError(0.37, 0.06);
prop.SetFPS(7_Hz);
prop.SetAvgLatency(60_ms);
@@ -463,7 +463,7 @@ class SimCameraProperties {
int resWidth;
int resHeight;
Eigen::Matrix<double, 3, 3> camIntrinsics;
Eigen::Matrix<double, 5, 1> distCoeffs;
Eigen::Matrix<double, 8, 1> distCoeffs;
double avgErrorPx;
double errorStdDevPx;
units::second_t frameSpeed{0};