mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-24 01:31:44 +00:00
Use nicer array syntax
This commit is contained in:
@@ -205,8 +205,8 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
|
||||
ts.cameraIntrinsicsPublisher.accept(fsp.cameraCalibration.getIntrinsicsArr());
|
||||
ts.cameraDistortionPublisher.accept(fsp.cameraCalibration.getDistCoeffsArr());
|
||||
} else {
|
||||
ts.cameraIntrinsicsPublisher.accept(new double[] {});
|
||||
ts.cameraDistortionPublisher.accept(new double[] {});
|
||||
ts.cameraIntrinsicsPublisher.accept(new double[0]);
|
||||
ts.cameraDistortionPublisher.accept(new double[0]);
|
||||
}
|
||||
|
||||
ts.heartbeatPublisher.set(acceptedResult.sequenceID);
|
||||
|
||||
@@ -59,13 +59,12 @@ public class ArucoDetectionPipe
|
||||
for (var detection : detections) {
|
||||
double[] xCorners = detection.getXCorners();
|
||||
double[] yCorners = detection.getYCorners();
|
||||
Point[] cornerPoints =
|
||||
new Point[] {
|
||||
new Point(xCorners[0], yCorners[0]),
|
||||
new Point(xCorners[1], yCorners[1]),
|
||||
new Point(xCorners[2], yCorners[2]),
|
||||
new Point(xCorners[3], yCorners[3])
|
||||
};
|
||||
Point[] cornerPoints = {
|
||||
new Point(xCorners[0], yCorners[0]),
|
||||
new Point(xCorners[1], yCorners[1]),
|
||||
new Point(xCorners[2], yCorners[2]),
|
||||
new Point(xCorners[3], yCorners[3])
|
||||
};
|
||||
double bltr =
|
||||
Math.hypot(
|
||||
cornerPoints[2].x - cornerPoints[0].x, cornerPoints[2].y - cornerPoints[0].y);
|
||||
|
||||
@@ -32,14 +32,13 @@ import org.photonvision.vision.target.TrackedTarget;
|
||||
public class DrawCalibrationPipe
|
||||
extends MutatingPipe<
|
||||
Pair<Mat, List<TrackedTarget>>, DrawCalibrationPipe.DrawCalibrationPipeParams> {
|
||||
Scalar[] chessboardColors =
|
||||
new Scalar[] {
|
||||
ColorHelper.colorToScalar(Color.RED, 0.4),
|
||||
ColorHelper.colorToScalar(Color.ORANGE, 0.4),
|
||||
ColorHelper.colorToScalar(Color.GREEN, 0.4),
|
||||
ColorHelper.colorToScalar(Color.BLUE, 0.4),
|
||||
ColorHelper.colorToScalar(Color.MAGENTA, 0.4),
|
||||
};
|
||||
Scalar[] chessboardColors = {
|
||||
ColorHelper.colorToScalar(Color.RED, 0.4),
|
||||
ColorHelper.colorToScalar(Color.ORANGE, 0.4),
|
||||
ColorHelper.colorToScalar(Color.GREEN, 0.4),
|
||||
ColorHelper.colorToScalar(Color.BLUE, 0.4),
|
||||
ColorHelper.colorToScalar(Color.MAGENTA, 0.4),
|
||||
};
|
||||
|
||||
@Override
|
||||
protected Void process(Pair<Mat, List<TrackedTarget>> in) {
|
||||
|
||||
@@ -37,19 +37,18 @@ public class PipelineProfiler {
|
||||
* output) 14 - draw2dTargetsPipe (on input) 15 - draw2dTargetsPipe (on output) 16 -
|
||||
* draw3dTargetsPipe (OPTIONAL, on input) 17 - draw3dTargetsPipe (OPTIONAL, on output)
|
||||
*/
|
||||
private static final String[] ReflectivePipeNames =
|
||||
new String[] {
|
||||
"RotateImage",
|
||||
"HSV",
|
||||
"FindContours",
|
||||
"SpeckleReject",
|
||||
"FilterContours",
|
||||
"GroupContours",
|
||||
"SortContours",
|
||||
"Collect2dTargets",
|
||||
"CornerDetection",
|
||||
"SolvePNP",
|
||||
};
|
||||
private static final String[] ReflectivePipeNames = {
|
||||
"RotateImage",
|
||||
"HSV",
|
||||
"FindContours",
|
||||
"SpeckleReject",
|
||||
"FilterContours",
|
||||
"GroupContours",
|
||||
"SortContours",
|
||||
"Collect2dTargets",
|
||||
"CornerDetection",
|
||||
"SolvePNP",
|
||||
};
|
||||
|
||||
public static final int ReflectivePipeCount = ReflectivePipeNames.length;
|
||||
|
||||
|
||||
@@ -122,11 +122,9 @@ public class TrackedTarget implements Releasable {
|
||||
tvec.put(
|
||||
0,
|
||||
0,
|
||||
new double[] {
|
||||
bestPose.getTranslation().getX(),
|
||||
bestPose.getTranslation().getY(),
|
||||
bestPose.getTranslation().getZ()
|
||||
});
|
||||
bestPose.getTranslation().getX(),
|
||||
bestPose.getTranslation().getY(),
|
||||
bestPose.getTranslation().getZ());
|
||||
setCameraRelativeTvec(tvec);
|
||||
|
||||
// Opencv expects a 3d vector with norm = angle and direction = axis
|
||||
@@ -136,13 +134,12 @@ public class TrackedTarget implements Releasable {
|
||||
}
|
||||
|
||||
double[] corners = tagDetection.getCorners();
|
||||
Point[] cornerPoints =
|
||||
new Point[] {
|
||||
new Point(corners[0], corners[1]),
|
||||
new Point(corners[2], corners[3]),
|
||||
new Point(corners[4], corners[5]),
|
||||
new Point(corners[6], corners[7])
|
||||
};
|
||||
Point[] cornerPoints = {
|
||||
new Point(corners[0], corners[1]),
|
||||
new Point(corners[2], corners[3]),
|
||||
new Point(corners[4], corners[5]),
|
||||
new Point(corners[6], corners[7])
|
||||
};
|
||||
m_targetCorners = List.of(cornerPoints);
|
||||
MatOfPoint contourMat = new MatOfPoint(cornerPoints);
|
||||
m_approximateBoundingPolygon = new MatOfPoint2f(cornerPoints);
|
||||
@@ -198,13 +195,12 @@ public class TrackedTarget implements Releasable {
|
||||
double[] xCorners = result.getXCorners();
|
||||
double[] yCorners = result.getYCorners();
|
||||
|
||||
Point[] cornerPoints =
|
||||
new Point[] {
|
||||
new Point(xCorners[0], yCorners[0]),
|
||||
new Point(xCorners[1], yCorners[1]),
|
||||
new Point(xCorners[2], yCorners[2]),
|
||||
new Point(xCorners[3], yCorners[3])
|
||||
};
|
||||
Point[] cornerPoints = {
|
||||
new Point(xCorners[0], yCorners[0]),
|
||||
new Point(xCorners[1], yCorners[1]),
|
||||
new Point(xCorners[2], yCorners[2]),
|
||||
new Point(xCorners[3], yCorners[3])
|
||||
};
|
||||
m_targetCorners = List.of(cornerPoints);
|
||||
MatOfPoint contourMat = new MatOfPoint(cornerPoints);
|
||||
m_approximateBoundingPolygon = new MatOfPoint2f(cornerPoints);
|
||||
@@ -236,11 +232,9 @@ public class TrackedTarget implements Releasable {
|
||||
tvec.put(
|
||||
0,
|
||||
0,
|
||||
new double[] {
|
||||
bestPose.getTranslation().getX(),
|
||||
bestPose.getTranslation().getY(),
|
||||
bestPose.getTranslation().getZ()
|
||||
});
|
||||
bestPose.getTranslation().getX(),
|
||||
bestPose.getTranslation().getY(),
|
||||
bestPose.getTranslation().getZ());
|
||||
setCameraRelativeTvec(tvec);
|
||||
|
||||
var rvec = new Mat(3, 1, CvType.CV_64FC1);
|
||||
|
||||
@@ -123,7 +123,7 @@ public class CalibrationRotationPipeTest {
|
||||
0.09625562194891251,
|
||||
-0.1860797479660746
|
||||
}),
|
||||
new double[] {},
|
||||
new double[0],
|
||||
List.of(),
|
||||
new Size(),
|
||||
1,
|
||||
@@ -288,7 +288,7 @@ public class CalibrationRotationPipeTest {
|
||||
0.04625562194891251,
|
||||
-0.0860797479660746
|
||||
}),
|
||||
new double[] {},
|
||||
new double[0],
|
||||
List.of(),
|
||||
new Size(),
|
||||
1,
|
||||
|
||||
@@ -138,7 +138,7 @@ public class PhotonCamera implements AutoCloseable {
|
||||
.getRawTopic("rawBytes")
|
||||
.subscribe(
|
||||
PhotonPipelineResult.photonStruct.getTypeString(),
|
||||
new byte[] {},
|
||||
new byte[0],
|
||||
PubSubOption.periodic(0.01),
|
||||
PubSubOption.sendAll(true),
|
||||
PubSubOption.pollStorage(20));
|
||||
|
||||
@@ -202,24 +202,20 @@ public class SimCameraProperties {
|
||||
this.distCoeffs = distCoeffs;
|
||||
|
||||
// left, right, up, and down view planes
|
||||
var p =
|
||||
new Translation3d[] {
|
||||
new Translation3d(
|
||||
1,
|
||||
new Rotation3d(0, 0, getPixelYaw(0).plus(new Rotation2d(-Math.PI / 2)).getRadians())),
|
||||
new Translation3d(
|
||||
1,
|
||||
new Rotation3d(
|
||||
0, 0, getPixelYaw(resWidth).plus(new Rotation2d(Math.PI / 2)).getRadians())),
|
||||
new Translation3d(
|
||||
1,
|
||||
new Rotation3d(
|
||||
0, getPixelPitch(0).plus(new Rotation2d(Math.PI / 2)).getRadians(), 0)),
|
||||
new Translation3d(
|
||||
1,
|
||||
new Rotation3d(
|
||||
0, getPixelPitch(resHeight).plus(new Rotation2d(-Math.PI / 2)).getRadians(), 0))
|
||||
};
|
||||
Translation3d[] p = {
|
||||
new Translation3d(
|
||||
1, new Rotation3d(0, 0, getPixelYaw(0).plus(new Rotation2d(-Math.PI / 2)).getRadians())),
|
||||
new Translation3d(
|
||||
1,
|
||||
new Rotation3d(
|
||||
0, 0, getPixelYaw(resWidth).plus(new Rotation2d(Math.PI / 2)).getRadians())),
|
||||
new Translation3d(
|
||||
1, new Rotation3d(0, getPixelPitch(0).plus(new Rotation2d(Math.PI / 2)).getRadians(), 0)),
|
||||
new Translation3d(
|
||||
1,
|
||||
new Rotation3d(
|
||||
0, getPixelPitch(resHeight).plus(new Rotation2d(-Math.PI / 2)).getRadians(), 0))
|
||||
};
|
||||
viewplanes.clear();
|
||||
for (Translation3d translation3d : p) {
|
||||
viewplanes.add(
|
||||
|
||||
@@ -248,13 +248,12 @@ public class VisionEstimation {
|
||||
}
|
||||
|
||||
// fx fy cx cy
|
||||
double[] cameraCal =
|
||||
new double[] {
|
||||
cameraMatrix.get(0, 0),
|
||||
cameraMatrix.get(1, 1),
|
||||
cameraMatrix.get(0, 2),
|
||||
cameraMatrix.get(1, 2),
|
||||
};
|
||||
double[] cameraCal = {
|
||||
cameraMatrix.get(0, 0),
|
||||
cameraMatrix.get(1, 1),
|
||||
cameraMatrix.get(0, 2),
|
||||
cameraMatrix.get(1, 2),
|
||||
};
|
||||
|
||||
var guess2 = robotPoseSeed.toPose2d();
|
||||
|
||||
|
||||
@@ -50,10 +50,9 @@ public class ConstrainedSolvepnpTest {
|
||||
|
||||
@Test
|
||||
public void smoketest() {
|
||||
double[] cameraCal =
|
||||
new double[] {
|
||||
600, 600, 300, 150,
|
||||
};
|
||||
double[] cameraCal = {
|
||||
600, 600, 300, 150,
|
||||
};
|
||||
|
||||
var field2points =
|
||||
MatBuilder.fill(
|
||||
@@ -86,7 +85,7 @@ public class ConstrainedSolvepnpTest {
|
||||
MatBuilder.fill(Nat.N4(), Nat.N4(), 0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 1);
|
||||
|
||||
// Initial guess for optimization
|
||||
double[] x_guess = new double[] {0.2, 0.1, -.05};
|
||||
double[] x_guess = {0.2, 0.1, -.05};
|
||||
|
||||
var ret =
|
||||
ConstrainedSolvepnpJni.do_optimization(
|
||||
|
||||
Reference in New Issue
Block a user