Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-22 01:11:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Bugfixes in solve pnp drawing

Browse Source 
* bugfixed

* Fix unit mismatch in frctargets

Co-authored-by: Matt <matthew.morley.ca@gmail.com>
This commit is contained in:
oriagranat9
2020-01-26 22:25:40 -08:00
committed by GitHub
parent 770e1f060c
commit b073231330
3 changed files with 19 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/StandardCVPipeline.java
View File

@@ -135,10 +135,11 @@ public class StandardCVPipeline extends CVPipeline<StandardCVPipelineResult, Sta
if (solvePNPPipe == null)
solvePNPPipe = new SolvePNPPipe(settings, cameraCapture.getCurrentCalibrationData(), cameraCapture.getProperties().getTilt());
if (drawSolvePNPPipe == null)
drawSolvePNPPipe = new DrawSolvePNPPipe(cameraCapture.getCurrentCalibrationData());
drawSolvePNPPipe = new DrawSolvePNPPipe(settings, cameraCapture.getCurrentCalibrationData());
solvePNPPipe.setConfig(settings, cameraCapture.getCurrentCalibrationData(), cameraCapture.getProperties().getTilt());
drawSolvePNPPipe.setConfig(cameraCapture.getCurrentCalibrationData());
drawSolvePNPPipe.setConfig(settings);
}

59
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/DrawSolvePNPPipe.java
View File

@@ -19,70 +19,37 @@ import java.util.stream.Collectors;
public class DrawSolvePNPPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.TrackedTarget>>, Mat> {
private MatOfPoint3f boxCornerMat = new MatOfPoint3f();
private MatOfPoint tempMatOfPoints = new MatOfPoint();
public Scalar green = Helpers.colorToScalar(Color.GREEN);
public Scalar blue = Helpers.colorToScalar(Color.BLUE);
public Scalar red = Helpers.colorToScalar(Color.RED);
public Scalar orange = Helpers.colorToScalar(Color.orange);
public DrawSolvePNPPipe(CameraCalibrationConfig settings) {
public DrawSolvePNPPipe(StandardCVPipelineSettings standardCVPipelineSettings, CameraCalibrationConfig settings) {
setConfig(settings);
// setObjectBox(14.5, 6, 2);
set2020Box();
setBox(standardCVPipelineSettings.targetCornerMat);
}
public void setObjectBox(double targetWidth, double targetHeight, double targetDepth) {
// implementation from 5190 Green Hope Falcons
private void setBox(MatOfPoint3f mat) {
boxCornerMat.release();
boxCornerMat = new MatOfPoint3f(
new Point3(-targetWidth/2d, -targetHeight/2d, 0),
new Point3(-targetWidth/2d, targetHeight/2d, 0),
new Point3(targetWidth/2d, targetHeight/2d, 0),
new Point3(targetWidth/2d, -targetHeight/2d, 0),
new Point3(-targetWidth/2d, -targetHeight/2d, -targetDepth),
new Point3(-targetWidth/2d, targetHeight/2d, -targetDepth),
new Point3(targetWidth/2d, targetHeight/2d, -targetDepth),
new Point3(targetWidth/2d, -targetHeight/2d, -targetDepth)
);
}
private MatOfPoint3f lastCornerListPtr = null;
public void setConfig(StandardCVPipelineSettings settings) {
// boxCornerMat.fromList(settings.targetCornerMat);
if(settings.targetCornerMat == lastCornerListPtr) {
lastCornerListPtr = settings.targetCornerMat;
return;
}
var list = settings.targetCornerMat.toList();
var list = mat.toList();
var auxList = list.stream().map(it -> new Point3(it.x, it.y, it.z - 6)).collect(Collectors.toList());
var finalList = new ArrayList<>(list);
finalList.addAll(auxList);
boxCornerMat.fromList(finalList);
}
public void set2020Box() {
boxCornerMat.release();
boxCornerMat.fromList(List.of(
new Point3(-19.625, 0, 0),
new Point3(-9.819867, -17, 0),
new Point3(9.819867, -17, 0),
new Point3(19.625, 0, 0),
new Point3(-19.625, 0, -6),
new Point3(-9.819867, -17, -6),
new Point3(9.819867, -17, -6),
new Point3(19.625, 0, -6)
));
public void setConfig(StandardCVPipelineSettings settings) {
setBox(settings.targetCornerMat);
}
private Mat cameraMatrix = new Mat();
private MatOfDouble distortionCoefficients = new MatOfDouble();
public void setConfig(CameraCalibrationConfig config) {
if(config == null) {
if (config == null) {
System.err.println("got passed a null config! Returning...");
return;
}
@@ -101,17 +68,17 @@ public class DrawSolvePNPPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.
long processStartNanos = System.nanoTime();
var image = targets.getLeft();
for(var it : targets.getRight()) {
for (var it : targets.getRight()) {
try {
Calib3d.projectPoints(boxCornerMat, it.rVector, it.tVector, this.cameraMatrix, this.distortionCoefficients, imagePoints, new Mat() , 0);
Calib3d.projectPoints(boxCornerMat, it.rVector, it.tVector, this.cameraMatrix, this.distortionCoefficients, imagePoints, new Mat(), 0);
} catch (Exception e) {
e.printStackTrace();
}
var pts = imagePoints.toList();
// draw left and right targets if possible
if(it.leftRightDualTargetPair != null) {
if (it.leftRightDualTargetPair != null) {
var left = it.leftRightDualTargetPair.getLeft();
var right = it.leftRightDualTargetPair.getRight();
Imgproc.rectangle(image, left.tl(), left.br(), new Scalar(200, 200, 0), 4);
@@ -120,7 +87,7 @@ public class DrawSolvePNPPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.
// draw poly dp
var list = it.approxPoly.toList();
for(int i = 0; i < list.size(); i++) {
for (int i = 0; i < list.size(); i++) {
var next = (i == list.size() - 1) ? list.get(0) : list.get(i + 1);
Imgproc.line(image, list.get(i), next, red, 2);
}
@@ -129,7 +96,7 @@ public class DrawSolvePNPPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.
Imgproc.circle(image, it.minAreaRect.center, 5, red);
// draw corners
for(int i = 0; i < it.imageCornerPoints.rows(); i++) {
for (int i = 0; i < it.imageCornerPoints.rows(); i++) {
var point = new Point(it.imageCornerPoints.get(i, 0));
Imgproc.circle(image, point, 4, green, 5);
}