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

Avoid allocating a new mat in solvePNP, perf. increases in group contors pipe

Browse Source
This commit is contained in:
Matt
2020-01-03 16:00:18 -08:00
parent a700001244
commit 2ce8a8dad9
2 changed files with 9 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/GroupContoursPipe.java
View File

@@ -41,6 +41,7 @@ public class GroupContoursPipe implements Pipe<List<MatOfPoint>, List<StandardCV
public Pair<List<StandardCVPipeline.TrackedTarget>, Long> run(List<MatOfPoint> input) {
long processStartNanos = System.nanoTime();
groupedContours.forEach(StandardCVPipeline.TrackedTarget::release);
groupedContours.clear();
if (input.size() > (group.equals(TargetGroup.Single) ? 0 : 1)) {
@@ -104,6 +105,9 @@ public class GroupContoursPipe implements Pipe<List<MatOfPoint>, List<StandardCV
groupedContours.add(target);
firstContour.release();
secondContour.release();
// skip the next contour because it's been grouped already
i += 1;
}

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

@@ -36,6 +36,7 @@ public class SolvePNPPipe implements Pipe<List<StandardCVPipeline.TrackedTarget>
Comparator<Point> leftRightComparator = Comparator.comparingDouble(point -> point.x);
Comparator<Point> verticalComparator = Comparator.comparingDouble(point -> point.y);
private double distanceDivisor = 1.0;
Mat scaledTvec = new Mat();
public SolvePNPPipe(StandardCVPipelineSettings settings, CameraCalibrationConfig calibration, Rotation2d tilt) {
super();
@@ -331,22 +332,20 @@ public class SolvePNPPipe implements Pipe<List<StandardCVPipeline.TrackedTarget>
// This should be pzero_world = numpy.matmul(rot_inv, -tvec)
// pzero_world = rot_inv.mul(matScale(tVec, -1));
Core.gemm(rot_inv, matScale(tVec, -1), 1, kMat, 0, pzero_world);
scaledTvec = matScale(tVec, -1);
Core.gemm(rot_inv, scaledTvec, 1, kMat, 0, pzero_world);
var angle2 = FastMath.atan2(pzero_world.get(0, 0)[0], pzero_world.get(2, 0)[0]);
var targetAngle = -angle1; // radians
var targetRotation = -angle2; // radians
//noinspection UnnecessaryLocalVariable
var targetDistance = distance * 25.4 / 1000d / distanceDivisor; // meters or whatever the calibration was in
var targetDistance = distance * 25.4 / 1000d / distanceDivisor; // This should be meters
var targetLocation = new Translation2d(targetDistance * FastMath.cos(targetAngle), targetDistance * FastMath.sin(targetAngle));
target.cameraRelativePose = new Pose2d(targetLocation, new Rotation2d(targetRotation));
target.rVector = rVec;
target.tVector = tVec;
// System.out.println("Found target at pose: " + target.cameraRelativePose.toString());
return target;
}
@@ -358,7 +357,7 @@ public class SolvePNPPipe implements Pipe<List<StandardCVPipeline.TrackedTarget>
*/
public Mat matScale(Mat src, double factor) {
Mat dst = new Mat(src.rows(),src.cols(),src.type());
Scalar s = new Scalar(factor);
Scalar s = new Scalar(factor); // TODO check if we need to add more elements to this
Core.multiply(src, s, dst);
return dst;
}