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 goodFeaturesToTrack, update test

Browse Source
This commit is contained in:
Matt
2020-02-24 22:12:42 -08:00
parent cdd01c4b19
commit ee13c0b2f9
2 changed files with 63 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

57
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/SolvePNPPipe.java
View File

@@ -14,6 +14,7 @@ import org.opencv.core.*;
import org.opencv.imgproc.Imgproc;
import java.util.*;
import java.util.stream.Collectors;
public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTarget>, Mat>, List<StandardCVPipeline.TrackedTarget>> {
@@ -108,14 +109,18 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
long processStartNanos = System.nanoTime();
var targets = imageTargetPair.getLeft();
var image = imageTargetPair.getRight();
Imgproc.cvtColor(image, greyImg, Imgproc.COLOR_BGR2GRAY);
poseList.clear();
for(var target: targets) {
var corners = find2020VisionTarget(target);//, imageTargetPair.getRight()); //find2020VisionTarget(target);// (target.leftRightDualTargetPair != null) ? findCorner2019(target) : findBoundingBoxCorners(target);
// var corners = findCorner2019(target);
if(corners == null) continue;
// use best features to track
corners = refineCornersByBestTrack(corners, greyImg, target);
// refine the estimate
corners = refineCornerEstimateSubPix(corners, image);
corners = refineCornerEstimateSubPix(corners, greyImg);
var pose = calculatePose(corners, target);
if(pose != null) poseList.add(pose);
@@ -172,7 +177,7 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
// Can be tuned to allow/disallow hulls
// Approx is the number of vertices
// Ramer–Douglas–Peucker algorithm
Imgproc.approxPolyDP(target.rawContour, polyOutput, 0.02 * peri, true);
Imgproc.approxPolyDP(target.rawContour, polyOutput, 0.01 * peri, true);
var area = Imgproc.moments(polyOutput);
@@ -292,10 +297,46 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
return boundingBoxResultMat;
}
MatOfPoint2f goodFeatureToTrackRetval = new MatOfPoint2f();
private MatOfPoint2f refineCornersByBestTrack(MatOfPoint2f corners, Mat greyImg, StandardCVPipeline.TrackedTarget target) {
MatOfPoint approxf1 = new MatOfPoint();
var origCornerList = new ArrayList<>(corners.toList());
approxf1.fromList(origCornerList.stream()
.map(it -> new Point(it.x - target.boundingRect.x, it.y - target.boundingRect.y))
.collect(Collectors.toList())
);
var croppedImage = greyImg.submat(target.boundingRect);
Imgproc.goodFeaturesToTrack(croppedImage, approxf1, 0, 0.01, 5);
// at this point corners is still unmodified so let's map it
List<Point> tempList = new ArrayList<>();
// shift all points back into global pose
var reshiftedList = approxf1.toList().stream().map(it -> new Point(it.x + target.boundingRect.x, it.y + target.boundingRect.y))
.collect(Collectors.toList());
for(Point p: origCornerList) {
// find the goodFeaturesToTrack corner closest to me
var closestPoint = reshiftedList.stream().min(Comparator.comparingDouble(p_ -> distanceBetween(p_, p)));
if(closestPoint.isEmpty()) {
tempList.add(p);
reshiftedList.remove(p);
} else {
tempList.add(closestPoint.get());
reshiftedList.remove(closestPoint.get());
}
}
goodFeatureToTrackRetval.fromList(tempList);
return goodFeatureToTrackRetval;
}
// Set the needed parameters to find the refined corners
Size winSize = new Size(12, 12);
Size winSize = new Size(4, 4);
Size zeroZone = new Size(-1, -1); // we don't need a zero zone
TermCriteria criteria = new TermCriteria(TermCriteria.EPS + TermCriteria.COUNT, 50, 0.001);
TermCriteria criteria = new TermCriteria(TermCriteria.EPS + TermCriteria.COUNT, 90, 0.001);
private boolean shouldRefineCorners = true;
@@ -305,19 +346,17 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
* TODO should this be here or before the points are chosen?
*
* @param corners the corners detected -- this mat is modified!
* @param img the image taken by the camera as color
* @param greyImg the image taken by the camera as color
* @return the updated mat, same as the corner mat passed in.
*/
private MatOfPoint2f refineCornerEstimateSubPix(MatOfPoint2f corners, Mat img) {
private MatOfPoint2f refineCornerEstimateSubPix(MatOfPoint2f corners, Mat greyImg) {
if(!shouldRefineCorners) return corners; // just return
Imgproc.cvtColor(img, greyImg, Imgproc.COLOR_BGR2GRAY);
Imgproc.cornerSubPix(greyImg, corners, winSize, zeroZone, criteria);
return corners;
}
private StandardCVPipeline.TrackedTarget calculatePose(MatOfPoint2f imageCornerPoints, StandardCVPipeline.TrackedTarget target) {
public StandardCVPipeline.TrackedTarget calculatePose(MatOfPoint2f imageCornerPoints, StandardCVPipeline.TrackedTarget target) {
if(objPointsMat.rows() != imageCornerPoints.rows() || cameraMatrix.rows() < 2 || distortionCoefficients.cols() < 4) {
System.err.println("can't do solvePNP with invalid params!");
return null;

19
chameleon-server/src/test/java/com/chameleonvision/vision/pipeline/SolvePNPtest.java
View File

@@ -40,8 +40,8 @@ public class SolvePNPtest {
var settings = new StandardCVPipelineSettings();
var calibration = new CameraCalibrationConfig(
new Size(640, 480),
new JsonMat(3, 3, 6, new double[] { 926.1016601017006, 0.0, 437.22446072361055, 0.0, 918.2612433944396, 137.8989492231747, 0.0, 0.0, 1.0 }),
new JsonMat(1, 5, 6, new double[] { -0.001917838248173303, 0.0059895823594355, -0.035282888419499406, 0.04373249383460662, 0.1732528905031391 }),
new JsonMat(3, 3, 6, new double[] { 1126.1154452525066, 0.0, 666.4172679761178, 0.0, 1088.0425532065287, 335.37748454259633, 0.0, 0.0, 1.0 }),
new JsonMat(1, 5, 6, new double[] { 0.07253724845871252, -0.664268685338307, -0.0011224914177033868, 4.8323234488098423E-4, 1.1731498589436031 }),
1.056
);
@@ -64,17 +64,24 @@ public class SolvePNPtest {
// -1.552061845576413]
Mat tvec = new Mat(3, 1, 6);
tvec.put(0, 0, 0, 40, 200); // 10ft away?
Mat rvec = new Mat(3, 1, 6);
rvec.put(0, 0, 0, 0, 0);
// tvec.put(0, 0, 1.032188152287021, -3.78145690753876, 52.32713732614368);
// rvec.put(0, 0, -3.084531365719034, -0.1446574541579896, -0.1297813889017779);
tvec.put(0, 0, 1.75, -6, 75.2);
rvec.put(0, 0, 2.79, 0.23, -0.0388);
MatOfPoint2f imagePoints = new MatOfPoint2f();
Calib3d.projectPoints(target, rvec, tvec, calibration.getCameraMatrixAsMat(), calibration.getDistortionCoeffsAsMat(), imagePoints, new Mat(), 0);
var projectedPts = imagePoints.toList();
// draw circles
for(var p: projectedPts) {
Imgproc.circle(blank, p, 3, new Scalar(0, 0, 255), 4);
}
Imgproc.line(blank, projectedPts.get(0), projectedPts.get(1), new Scalar(255, 0, 0));
Imgproc.line(blank, projectedPts.get(1), projectedPts.get(2), new Scalar(255, 0, 0));
Imgproc.line(blank, projectedPts.get(2), projectedPts.get(3), new Scalar(255, 0, 0));
Imgproc.line(blank, projectedPts.get(3), projectedPts.get(0), new Scalar(255, 0, 0));
// go backwards to solvePNP
Mat rvec_ = new Mat(), tvec_ = new Mat();
@@ -88,6 +95,10 @@ public class SolvePNPtest {
var retedTvec = tvec_.dump();
var rettedRvec = rvec_.dump();
var target_ = new StandardCVPipeline.TrackedTarget();
pipe.calculatePose(imagePoints, target_);
System.out.println(target_.cameraRelativePose);
displayImage(mat2BufferedImage(blank));
}