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

Use box corner setting for solvePNP draw result

Browse Source
This commit is contained in:
Matt
2020-01-23 23:31:11 -08:00
parent 371429692d
commit c990e91f6a
2 changed files with 35 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -4,6 +4,7 @@ import com.chameleonvision.config.CameraCalibrationConfig;
import com.chameleonvision.util.Helpers;
import com.chameleonvision.vision.pipeline.Pipe;
import com.chameleonvision.vision.pipeline.impl.StandardCVPipeline;
import com.chameleonvision.vision.pipeline.impl.StandardCVPipelineSettings;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.calib3d.Calib3d;
import org.opencv.core.*;
@@ -11,7 +12,9 @@ import org.opencv.core.Point;
import org.opencv.imgproc.Imgproc;
import java.awt.*;
import java.util.ArrayList;
import java.util.List;
import java.util.stream.Collectors;
public class DrawSolvePNPPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.TrackedTarget>>, Mat> {
@@ -45,9 +48,25 @@ public class DrawSolvePNPPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.
);
}
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 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 = new MatOfPoint3f(
boxCornerMat.fromList(List.of(
new Point3(-19.625, 0, 0),
new Point3(-9.819867, -17, 0),
new Point3(9.819867, -17, 0),
@@ -56,7 +75,7 @@ public class DrawSolvePNPPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.
new Point3(-9.819867, -17, -6),
new Point3(9.819867, -17, -6),
new Point3(19.625, 0, -6)
);
));
}
private Mat cameraMatrix = new Mat();