Skip to content

Commit 19b4802

Browse files
mcm001mcm001
authored
Allow opencv8 distortion model in PhotonCamera (#1317)
We previously assumed only OpenCV5 but mrcal uses opencv8
1 parent fcca858 commit 19b4802

File tree

17 files changed

+172
-94
lines changed

17 files changed

+172
-94
lines changed

‎.github/workflows/build.yml

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -121,17 +121,17 @@ jobs:
121121
path: build/html
122122
build-photonlib-host:
123123
env:
124-
MACOSX_DEPLOYMENT_TARGET: 12
124+
MACOSX_DEPLOYMENT_TARGET: 13
125125
strategy:
126126
fail-fast: false
127127
matrix:
128128
include:
129129
- os: windows-2022
130130
artifact-name: Win64
131131
architecture: x64
132-
- os: macos-12
132+
- os: macos-14
133133
artifact-name: macOS
134-
architecture: x64
134+
architecture: aarch64
135135
- os: ubuntu-22.04
136136
artifact-name: Linux
137137

@@ -146,6 +146,7 @@ jobs:
146146
with:
147147
java-version: 17
148148
distribution: temurin
149+
architecture: ${{ matrix.architecture }}
149150
- run: git fetch --tags --force
150151
- run: |
151152
chmod +x gradlew

‎build.gradle

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -109,3 +109,17 @@ wrapper {
109109
ext.getCurrentArch = {
110110
return NativePlatforms.desktop
111111
}
112+
113+
subprojects {
114+
tasks.withType(JavaCompile) {
115+
options.compilerArgs.add '-XDstringConcat=inline'
116+
options.encoding = 'UTF-8'
117+
}
118+
119+
// Enables UTF-8 support in Javadoc
120+
tasks.withType(Javadoc) {
121+
options.addStringOption("charset", "utf-8")
122+
options.addStringOption("docencoding", "utf-8")
123+
options.addStringOption("encoding", "utf-8")
124+
}
125+
}

‎photon-core/src/main/java/org/photonvision/vision/target/TargetCalculations.java

Lines changed: 2 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -52,8 +52,7 @@ public static DoubleCouple calculateYawPitch(
5252

5353
if (cameraCal != null) {
5454
// undistort
55-
MatOfPoint2f temp = new MatOfPoint2f();
56-
temp.fromArray(new Point(targetCenterX, targetCenterY));
55+
MatOfPoint2f temp = new MatOfPoint2f(new Point(targetCenterX, targetCenterY));
5756
// Tighten up termination criteria
5857
var termCriteria = new TermCriteria(TermCriteria.COUNT + TermCriteria.EPS, 30, 1e-6);
5958
Calib3d.undistortImagePoints(
@@ -68,7 +67,7 @@ public static DoubleCouple calculateYawPitch(
6867

6968
// if outside of the imager, convergence fails, or really really bad user camera cal,
7069
// undistort will fail by giving us nans. at some point we should log this failure
71-
// if we can't undistort, don't change the cnter location
70+
// if we can't undistort, don't change the center location
7271
if (Float.isFinite(buff[0]) && Float.isFinite(buff[1])) {
7372
targetCenterX = buff[0];
7473
targetCenterY = buff[1];

‎photon-lib/src/main/java/org/photonvision/PhotonCamera.java

Lines changed: 13 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -46,6 +46,7 @@
4646
import edu.wpi.first.wpilibj.DriverStation;
4747
import edu.wpi.first.wpilibj.RobotController;
4848
import edu.wpi.first.wpilibj.Timer;
49+
import java.util.Arrays;
4950
import java.util.List;
5051
import java.util.Optional;
5152
import java.util.stream.Collectors;
@@ -313,10 +314,19 @@ public Optional<Matrix<N3, N3>> getCameraMatrix() {
313314
} else return Optional.empty();
314315
}
315316

316-
public Optional<Matrix<N5, N1>> getDistCoeffs() {
317+
/**
318+
* The camera calibration's distortion coefficients, in OPENCV8 form. Higher-order terms are set
319+
* to 0
320+
*/
321+
public Optional<Matrix<N8, N1>> getDistCoeffs() {
317322
var distCoeffs = cameraDistortionSubscriber.get();
318-
if (distCoeffs != null && distCoeffs.length == 5) {
319-
return Optional.of(MatBuilder.fill(Nat.N5(), Nat.N1(), distCoeffs));
323+
if (distCoeffs != null && distCoeffs.length <= 8) {
324+
// Copy into array of length 8, and explicitly null higher order terms out
325+
double[] data = new double[8];
326+
Arrays.fill(data, 0);
327+
System.arraycopy(distCoeffs, 0, data, 0, distCoeffs.length);
328+
329+
return Optional.of(MatBuilder.fill(Nat.N8(), Nat.N1(), data));
320330
} else return Optional.empty();
321331
}
322332

‎photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -36,7 +36,7 @@
3636
import edu.wpi.first.math.geometry.Translation3d;
3737
import edu.wpi.first.math.numbers.N1;
3838
import edu.wpi.first.math.numbers.N3;
39-
import edu.wpi.first.math.numbers.N5;
39+
import edu.wpi.first.math.numbers.N8;
4040
import edu.wpi.first.wpilibj.DriverStation;
4141
import java.util.ArrayList;
4242
import java.util.HashSet;
@@ -348,7 +348,7 @@ public Optional<EstimatedRobotPose> update(PhotonPipelineResult cameraResult) {
348348
public Optional<EstimatedRobotPose> update(
349349
PhotonPipelineResult cameraResult,
350350
Optional<Matrix<N3, N3>> cameraMatrix,
351-
Optional<Matrix<N5, N1>> distCoeffs) {
351+
Optional<Matrix<N8, N1>> distCoeffs) {
352352
// Time in the past -- give up, since the following if expects times > 0
353353
if (cameraResult.getTimestampSeconds() < 0) {
354354
return Optional.empty();
@@ -376,7 +376,7 @@ public Optional<EstimatedRobotPose> update(
376376
private Optional<EstimatedRobotPose> update(
377377
PhotonPipelineResult cameraResult,
378378
Optional<Matrix<N3, N3>> cameraMatrix,
379-
Optional<Matrix<N5, N1>> distCoeffs,
379+
Optional<Matrix<N8, N1>> distCoeffs,
380380
PoseStrategy strat) {
381381
Optional<EstimatedRobotPose> estimatedPose;
382382
switch (strat) {
@@ -418,7 +418,7 @@ private Optional<EstimatedRobotPose> update(
418418
private Optional<EstimatedRobotPose> multiTagOnCoprocStrategy(
419419
PhotonPipelineResult result,
420420
Optional<Matrix<N3, N3>> cameraMatrixOpt,
421-
Optional<Matrix<N5, N1>> distCoeffsOpt) {
421+
Optional<Matrix<N8, N1>> distCoeffsOpt) {
422422
if (result.getMultiTagResult().estimatedPose.isPresent) {
423423
var best_tf = result.getMultiTagResult().estimatedPose.best;
424424
var best =
@@ -440,7 +440,7 @@ private Optional<EstimatedRobotPose> multiTagOnCoprocStrategy(
440440
private Optional<EstimatedRobotPose> multiTagOnRioStrategy(
441441
PhotonPipelineResult result,
442442
Optional<Matrix<N3, N3>> cameraMatrixOpt,
443-
Optional<Matrix<N5, N1>> distCoeffsOpt) {
443+
Optional<Matrix<N8, N1>> distCoeffsOpt) {
444444
boolean hasCalibData = cameraMatrixOpt.isPresent() && distCoeffsOpt.isPresent();
445445
// cannot run multitagPNP, use fallback strategy
446446
if (!hasCalibData || result.getTargets().size() < 2) {

‎photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java

Lines changed: 28 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -41,6 +41,7 @@
4141
import java.io.IOException;
4242
import java.nio.file.Path;
4343
import java.util.ArrayList;
44+
import java.util.Arrays;
4445
import java.util.List;
4546
import java.util.Random;
4647
import org.ejml.data.DMatrix3;
@@ -71,7 +72,7 @@ public class SimCameraProperties {
7172
private int resWidth;
7273
private int resHeight;
7374
private Matrix<N3, N3> camIntrinsics;
74-
private Matrix<N5, N1> distCoeffs;
75+
private Matrix<N8, N1> distCoeffs;
7576
private double avgErrorPx;
7677
private double errorStdDevPx;
7778
// performance
@@ -132,7 +133,8 @@ public SimCameraProperties(Path path, int width, int height) throws IOException
132133
jsonIntrinsics[j] = jsonIntrinsicsNode.get(j).asDouble();
133134
}
134135
var jsonDistortNode = calib.get("distCoeffs").get("data");
135-
double[] jsonDistortion = new double[jsonDistortNode.size()];
136+
double[] jsonDistortion = new double[8];
137+
Arrays.fill(jsonDistortion, 0);
136138
for (int j = 0; j < jsonDistortNode.size(); j++) {
137139
jsonDistortion[j] = jsonDistortNode.get(j).asDouble();
138140
}
@@ -148,7 +150,7 @@ public SimCameraProperties(Path path, int width, int height) throws IOException
148150
jsonWidth,
149151
jsonHeight,
150152
MatBuilder.fill(Nat.N3(), Nat.N3(), jsonIntrinsics),
151-
MatBuilder.fill(Nat.N5(), Nat.N1(), jsonDistortion));
153+
MatBuilder.fill(Nat.N8(), Nat.N1(), jsonDistortion));
152154
setCalibError(jsonAvgError, jsonErrorStdDev);
153155
success = true;
154156
}
@@ -174,7 +176,7 @@ public void setCalibration(int resWidth, int resHeight, Rotation2d fovDiag) {
174176
var fovHeight = new Rotation2d(Math.atan(diagRatio * (resHeight / resDiag)) * 2);
175177

176178
// assume no distortion
177-
var distCoeff = VecBuilder.fill(0, 0, 0, 0, 0);
179+
var distCoeff = VecBuilder.fill(0, 0, 0, 0, 0, 0, 0, 0);
178180

179181
// assume centered principal point (pixels)
180182
double cx = resWidth / 2.0 - 0.5;
@@ -190,7 +192,7 @@ public void setCalibration(int resWidth, int resHeight, Rotation2d fovDiag) {
190192
}
191193

192194
public void setCalibration(
193-
int resWidth, int resHeight, Matrix<N3, N3> camIntrinsics, Matrix<N5, N1> distCoeffs) {
195+
int resWidth, int resHeight, Matrix<N3, N3> camIntrinsics, Matrix<N8, N1> distCoeffs) {
194196
this.resWidth = resWidth;
195197
this.resHeight = resHeight;
196198
this.camIntrinsics = camIntrinsics;
@@ -280,7 +282,7 @@ public Matrix<N3, N3> getIntrinsics() {
280282
return camIntrinsics.copy();
281283
}
282284

283-
public Vector<N5> getDistCoeffs() {
285+
public Vector<N8> getDistCoeffs() {
284286
return new Vector<>(distCoeffs);
285287
}
286288

@@ -616,7 +618,10 @@ public static SimCameraProperties PI4_LIFECAM_320_240() {
616618
-0.9166265114485799,
617619
0.0019519890627236526,
618620
-0.0036071725380870333,
619-
1.5627234622420942));
621+
1.5627234622420942,
622+
0,
623+
0,
624+
0));
620625
prop.setCalibError(0.21, 0.0124);
621626
prop.setFPS(30);
622627
prop.setAvgLatencyMs(30);
@@ -647,7 +652,10 @@ public static SimCameraProperties PI4_LIFECAM_640_480() {
647652
-1.2350335805796528,
648653
0.0024990767286192732,
649654
-0.0026958287600230705,
650-
2.2951386729115537));
655+
2.2951386729115537,
656+
0,
657+
0,
658+
0));
651659
prop.setCalibError(0.26, 0.046);
652660
prop.setFPS(15);
653661
prop.setAvgLatencyMs(65);
@@ -677,7 +685,10 @@ public static SimCameraProperties LL2_640_480() {
677685
-0.5142936883324216,
678686
0.012461562046896614,
679687
0.0014084973492408186,
680-
0.35160648971214437));
688+
0.35160648971214437,
689+
0,
690+
0,
691+
0));
681692
prop.setCalibError(0.25, 0.05);
682693
prop.setFPS(15);
683694
prop.setAvgLatencyMs(35);
@@ -708,7 +719,10 @@ public static SimCameraProperties LL2_960_720() {
708719
-0.49903003669627627,
709720
0.007468423590519429,
710721
0.002496885298683693,
711-
0.3443122090208624));
722+
0.3443122090208624,
723+
0,
724+
0,
725+
0));
712726
prop.setCalibError(0.35, 0.10);
713727
prop.setFPS(10);
714728
prop.setAvgLatencyMs(50);
@@ -739,7 +753,10 @@ public static SimCameraProperties LL2_1280_720() {
739753
-0.2904345656989261,
740754
8.32475714507539E-4,
741755
-3.694397782014239E-4,
742-
0.09487962227027584));
756+
0.09487962227027584,
757+
0,
758+
0,
759+
0));
743760
prop.setCalibError(0.37, 0.06);
744761
prop.setFPS(7);
745762
prop.setAvgLatencyMs(60);

‎photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp

Lines changed: 19 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -159,20 +159,6 @@ LEDMode PhotonCamera::GetLEDMode() const {
159159
return static_cast<LEDMode>(static_cast<int>(ledModeSub.Get()));
160160
}
161161

162-
std::optional<cv::Mat> PhotonCamera::GetCameraMatrix() {
163-
auto camCoeffs = cameraIntrinsicsSubscriber.Get();
164-
if (camCoeffs.size() == 9) {
165-
cv::Mat retVal(3, 3, CV_64FC1);
166-
for (int i = 0; i < 3; i++) {
167-
for (int j = 0; j < 3; j++) {
168-
retVal.at<double>(i, j) = camCoeffs[(j * 3) + i];
169-
}
170-
}
171-
return retVal;
172-
}
173-
return std::nullopt;
174-
}
175-
176162
void PhotonCamera::SetLEDMode(LEDMode mode) {
177163
ledModePub.Set(static_cast<int>(mode));
178164
}
@@ -181,13 +167,26 @@ const std::string_view PhotonCamera::GetCameraName() const {
181167
return cameraName;
182168
}
183169

184-
std::optional<cv::Mat> PhotonCamera::GetDistCoeffs() {
170+
std::optional<PhotonCamera::CameraMatrix> PhotonCamera::GetCameraMatrix() {
171+
auto camCoeffs = cameraIntrinsicsSubscriber.Get();
172+
if (camCoeffs.size() == 9) {
173+
PhotonCamera::CameraMatrix retVal =
174+
Eigen::Map<const PhotonCamera::CameraMatrix>(camCoeffs.data());
175+
return retVal;
176+
}
177+
return std::nullopt;
178+
}
179+
180+
std::optional<PhotonCamera::DistortionMatrix> PhotonCamera::GetDistCoeffs() {
185181
auto distCoeffs = cameraDistortionSubscriber.Get();
186-
if (distCoeffs.size() == 5) {
187-
cv::Mat retVal(5, 1, CV_64FC1);
188-
for (int i = 0; i < 5; i++) {
189-
retVal.at<double>(i, 0) = distCoeffs[i];
190-
}
182+
auto bound = distCoeffs.size();
183+
if (bound > 0 && bound <= 8) {
184+
PhotonCamera::DistortionMatrix retVal =
185+
PhotonCamera::DistortionMatrix::Zero();
186+
187+
Eigen::Map<const Eigen::VectorXd> map(distCoeffs.data(), bound);
188+
retVal.block(0, 0, bound, 1) = map;
189+
191190
return retVal;
192191
}
193192
return std::nullopt;

‎photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp

Lines changed: 25 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -50,6 +50,9 @@
5050
#include "photon/targeting/PhotonPipelineResult.h"
5151
#include "photon/targeting/PhotonTrackedTarget.h"
5252

53+
#define OPENCV_DISABLE_EIGEN_TENSOR_SUPPORT
54+
#include <opencv2/core/eigen.hpp>
55+
5356
namespace photon {
5457

5558
namespace detail {
@@ -126,8 +129,9 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
126129
}
127130

128131
std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
129-
const PhotonPipelineResult& result, std::optional<cv::Mat> cameraMatrixData,
130-
std::optional<cv::Mat> cameraDistCoeffs) {
132+
const PhotonPipelineResult& result,
133+
std::optional<PhotonCamera::CameraMatrix> cameraMatrixData,
134+
std::optional<PhotonCamera::DistortionMatrix> cameraDistCoeffs) {
131135
// Time in the past -- give up, since the following if expects times > 0
132136
if (result.GetTimestamp() < 0_s) {
133137
return std::nullopt;
@@ -152,8 +156,10 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
152156
}
153157

154158
std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
155-
PhotonPipelineResult result, std::optional<cv::Mat> cameraMatrixData,
156-
std::optional<cv::Mat> cameraDistCoeffs, PoseStrategy strategy) {
159+
const PhotonPipelineResult& result,
160+
std::optional<PhotonCamera::CameraMatrix> cameraMatrixData,
161+
std::optional<PhotonCamera::DistortionMatrix> cameraDistCoeffs,
162+
PoseStrategy strategy) {
157163
std::optional<EstimatedRobotPose> ret = std::nullopt;
158164

159165
switch (strategy) {
@@ -371,8 +377,9 @@ frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) {
371377
}
372378

373379
std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnCoprocStrategy(
374-
PhotonPipelineResult result, std::optional<cv::Mat> camMat,
375-
std::optional<cv::Mat> distCoeffs) {
380+
PhotonPipelineResult result,
381+
std::optional<PhotonCamera::CameraMatrix> camMat,
382+
std::optional<PhotonCamera::DistortionMatrix> distCoeffs) {
376383
if (result.MultiTagResult().result.isPresent) {
377384
const auto field2camera = result.MultiTagResult().result.best;
378385

@@ -388,8 +395,9 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnCoprocStrategy(
388395
}
389396

390397
std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
391-
PhotonPipelineResult result, std::optional<cv::Mat> camMat,
392-
std::optional<cv::Mat> distCoeffs) {
398+
PhotonPipelineResult result,
399+
std::optional<PhotonCamera::CameraMatrix> camMat,
400+
std::optional<PhotonCamera::DistortionMatrix> distCoeffs) {
393401
using namespace frc;
394402

395403
// Need at least 2 targets
@@ -433,8 +441,15 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
433441
cv::Mat const rvec(3, 1, cv::DataType<double>::type);
434442
cv::Mat const tvec(3, 1, cv::DataType<double>::type);
435443

436-
cv::solvePnP(objectPoints, imagePoints, camMat.value(), distCoeffs.value(),
437-
rvec, tvec, false, cv::SOLVEPNP_SQPNP);
444+
{
445+
cv::Mat cameraMatCV(camMat->rows(), camMat->cols(), CV_64F);
446+
cv::eigen2cv(*camMat, cameraMatCV);
447+
cv::Mat distCoeffsMatCV(distCoeffs->rows(), distCoeffs->cols(), CV_64F);
448+
cv::eigen2cv(*distCoeffs, distCoeffsMatCV);
449+
450+
cv::solvePnP(objectPoints, imagePoints, cameraMatCV, distCoeffsMatCV, rvec,
451+
tvec, false, cv::SOLVEPNP_SQPNP);
452+
}
438453

439454
const Pose3d pose = detail::ToPose3d(tvec, rvec);
440455

0 commit comments

Comments
 (0)