Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[photonlib] Simulation robustness #874

Merged
merged 15 commits into from
Jul 24, 2023
Merged

[photonlib] Simulation robustness #874

Show file tree
Hide file tree
Changes from 7 commits
Commits
Show all changes
15 commits
Select commit Hold shift + click to select a range
4c2aa16
optional pnp results, pnp method simplification
amquake Jun 20, 2023
209e184
solvepnp docs clarification
amquake Jun 20, 2023
b4a0c90
spotless
amquake Jun 20, 2023
1511b2a
fix local tag image loading
amquake Jun 27, 2023
5be4557
solvepnp square NaN retry
amquake Jul 1, 2023
c9d2f1e
spotless
amquake Jul 1, 2023
2ea3453
Merge branch 'master' into 742-robustness
mcm001 Jul 3, 2023
c7cc564
Merge branch 'master' into 742-robustness
amquake Jul 23, 2023
6b22823
use condition in result instead of Optional
amquake Jul 23, 2023
4364ac2
publish sim intrinsics/distortion to camera
amquake Jul 23, 2023
1f03b96
spotless
amquake Jul 23, 2023
1e393d9
multitag pnp estimation unit test
amquake Jul 24, 2023
eb74a45
spotless
amquake Jul 24, 2023
97300db
dont use photonposeestimator
amquake Jul 24, 2023
b104ae3
spotless
amquake Jul 24, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
43 changes: 8 additions & 35 deletions photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,6 @@

package org.photonvision;

import edu.wpi.first.apriltag.AprilTag;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Pair;
Expand All @@ -45,7 +44,6 @@
import org.photonvision.estimation.VisionEstimation;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.TargetCorner;

/**
* The PhotonPoseEstimator class filters or combines readings from all the AprilTags visible at a
Expand Down Expand Up @@ -364,44 +362,19 @@ private Optional<EstimatedRobotPose> multiTagPNPStrategy(
PhotonPipelineResult result,
Optional<Matrix<N3, N3>> cameraMatrixOpt,
Optional<Matrix<N5, N1>> distCoeffsOpt) {
// Arrays we need declared up front
var visCorners = new ArrayList<TargetCorner>();
var knownVisTags = new ArrayList<AprilTag>();

if (result.getTargets().size() < 2) {
return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy);
}

boolean hasCalibData = cameraMatrixOpt.isPresent() && distCoeffsOpt.isPresent();

if (!hasCalibData) {
return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy);
}

for (var target : result.getTargets()) {
// Check if tag actually exists in the field layout
var tagPoseOpt = fieldTags.getTagPose(target.getFiducialId());
if (tagPoseOpt.isEmpty()) {
reportFiducialPoseError(target.getFiducialId());
continue;
}

// Now that we know tag is valid, add detected corners
visCorners.addAll(target.getDetectedCorners());

// actual layout poses of visible tags -- not exposed, so have to recreate
var tagPose = tagPoseOpt.get();
knownVisTags.add(new AprilTag(target.getFiducialId(), tagPose));
}

if (result.getTargets().size() < 2 || knownVisTags.size() < 2) {
// Run fallback strategy instead
// cannot run multitagPNP, use fallback strategy
if (!hasCalibData || result.getTargets().size() < 2) {
return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy);
}

var pnpResults =
VisionEstimation.estimateCamPoseSqpnp(
cameraMatrixOpt.get(), distCoeffsOpt.get(), visCorners, knownVisTags);
VisionEstimation.estimateCamPosePNP(
cameraMatrixOpt.get(), distCoeffsOpt.get(), result.getTargets(), fieldTags)
.orElse(null);
// try fallback strategy if solvePNP fails for some reason
if (pnpResults == null)
return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy);
var best =
new Pose3d()
.plus(pnpResults.best) // field-to-camera
Expand Down
249 changes: 147 additions & 102 deletions photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import java.util.Optional;
import org.ejml.simple.SimpleMatrix;
import org.opencv.calib3d.Calib3d;
import org.opencv.core.Core;
Expand Down Expand Up @@ -373,9 +374,10 @@ public static double getContourAreaPx(List<TargetCorner> corners) {
}

/**
* Finds the transformation(s) that map the camera's pose to the target pose. The camera's pose
* Finds the transformation(s) that map the camera's pose to the target's pose. The camera's pose
* relative to the target is determined by the supplied 3d points of the target's model and their
* associated 2d points imaged by the camera.
* associated 2d points imaged by the camera. The supplied model translations must be relative to
* the target's pose.
*
* <p>For planar targets, there may be an alternate solution which is plausible given the 2d image
* points. This has an associated "ambiguity" which describes the ratio of reprojection error
Expand All @@ -384,8 +386,8 @@ public static double getContourAreaPx(List<TargetCorner> corners) {
* <p>This method is intended for use with individual AprilTags, and will not work unless 4 points
* are provided.
*
* @param cameraMatrix the camera intrinsics matrix in standard opencv form
* @param distCoeffs the camera distortion matrix in standard opencv form
* @param cameraMatrix The camera intrinsics matrix in standard opencv form
* @param distCoeffs The camera distortion matrix in standard opencv form
* @param modelTrls The translations of the object corners. These should have the object pose as
* their origin. These must come in a specific, pose-relative order (in NWU):
* <ul>
Expand All @@ -398,127 +400,170 @@ public static double getContourAreaPx(List<TargetCorner> corners) {
* @param imageCorners The projection of these 3d object points into the 2d camera image. The
* order should match the given object point translations.
* @return The resulting transformation that maps the camera pose to the target pose and the
* ambiguity if an alternate solution is available.
* ambiguity if an alternate solution is available, or an empty Optional if solvePNP fails.
*/
public static PNPResults solvePNP_SQUARE(
public static Optional<PNPResults> solvePNP_SQUARE(
Matrix<N3, N3> cameraMatrix,
Matrix<N5, N1> distCoeffs,
List<Translation3d> modelTrls,
List<TargetCorner> imageCorners) {
// IPPE_SQUARE expects our corners in a specific order
modelTrls = reorderCircular(modelTrls, true, -1);
imageCorners = reorderCircular(imageCorners, true, -1);
// translate to opencv classes
var objectPoints = translationToTvec(modelTrls.toArray(new Translation3d[0]));
var imagePoints = targetCornersToMat(imageCorners);
var cameraMatrixMat = matrixToMat(cameraMatrix.getStorage());
var distCoeffsMat = matrixToMat(distCoeffs.getStorage());
// solvepnp inputs
MatOfPoint3f objectPoints = new MatOfPoint3f();
MatOfPoint2f imagePoints = new MatOfPoint2f();
MatOfDouble cameraMatrixMat = new MatOfDouble();
MatOfDouble distCoeffsMat = new MatOfDouble();
var rvecs = new ArrayList<Mat>();
var tvecs = new ArrayList<Mat>();
var rvec = Mat.zeros(3, 1, CvType.CV_32F);
var tvec = Mat.zeros(3, 1, CvType.CV_32F);
var reprojectionError = new Mat();
// calc rvecs/tvecs and associated reprojection error from image points
Calib3d.solvePnPGeneric(
objectPoints,
imagePoints,
cameraMatrixMat,
distCoeffsMat,
rvecs,
tvecs,
false,
Calib3d.SOLVEPNP_IPPE_SQUARE,
rvec,
tvec,
reprojectionError);

float[] errors = new float[2];
reprojectionError.get(0, 0, errors);
// convert to wpilib coordinates
var best = new Transform3d(tvecToTranslation(tvecs.get(0)), rvecToRotation(rvecs.get(0)));

Transform3d alt = null;
if (tvecs.size() > 1) {
alt = new Transform3d(tvecToTranslation(tvecs.get(1)), rvecToRotation(rvecs.get(1)));
Mat rvec = Mat.zeros(3, 1, CvType.CV_32F);
Mat tvec = Mat.zeros(3, 1, CvType.CV_32F);
Mat reprojectionError = Mat.zeros(2, 1, CvType.CV_32F);
try {
// IPPE_SQUARE expects our corners in a specific order
modelTrls = reorderCircular(modelTrls, true, -1);
imageCorners = reorderCircular(imageCorners, true, -1);
// translate to opencv classes
translationToTvec(modelTrls.toArray(new Translation3d[0])).assignTo(objectPoints);
targetCornersToMat(imageCorners).assignTo(imagePoints);
matrixToMat(cameraMatrix.getStorage()).assignTo(cameraMatrixMat);
matrixToMat(distCoeffs.getStorage()).assignTo(distCoeffsMat);

float[] errors = new float[2];
Transform3d best = null;
Transform3d alt = null;

for (int tries = 0; tries < 2; tries++) {
// calc rvecs/tvecs and associated reprojection error from image points
Calib3d.solvePnPGeneric(
objectPoints,
imagePoints,
cameraMatrixMat,
distCoeffsMat,
rvecs,
tvecs,
false,
Calib3d.SOLVEPNP_IPPE_SQUARE,
rvec,
tvec,
reprojectionError);

reprojectionError.get(0, 0, errors);
// convert to wpilib coordinates
best = new Transform3d(tvecToTranslation(tvecs.get(0)), rvecToRotation(rvecs.get(0)));

if (tvecs.size() > 1) {
alt = new Transform3d(tvecToTranslation(tvecs.get(1)), rvecToRotation(rvecs.get(1)));
}

// check if we got a NaN result
if (!Double.isNaN(errors[0])) break;
else { // add noise and retry
double[] br = imagePoints.get(0, 0);
br[0] -= 0.001;
br[1] -= 0.001;
imagePoints.put(0, 0, br);
}
}

// check if solvePnP failed with NaN results and retrying failed
if (Double.isNaN(errors[0])) throw new Exception("SolvePNP_SQUARE NaN result");

if (alt != null)
return Optional.of(new PNPResults(best, alt, errors[0] / errors[1], errors[0], errors[1]));
else return Optional.of(new PNPResults(best, errors[0]));
}
// solvePnP failed
catch (Exception e) {
System.err.println("SolvePNP_SQUARE failed!");
e.printStackTrace();
return Optional.empty();
} finally {
// release our Mats from native memory
objectPoints.release();
imagePoints.release();
cameraMatrixMat.release();
distCoeffsMat.release();
for (var v : rvecs) v.release();
for (var v : tvecs) v.release();
rvec.release();
tvec.release();
reprojectionError.release();
}

// release our Mats from native memory
objectPoints.release();
imagePoints.release();
cameraMatrixMat.release();
distCoeffsMat.release();
for (var v : rvecs) v.release();
for (var v : tvecs) v.release();
rvec.release();
tvec.release();
reprojectionError.release();

if (alt != null) return new PNPResults(best, alt, errors[0] / errors[1], errors[0], errors[1]);
else return new PNPResults(best, errors[0]);
}

/**
* Finds the transformation that maps the camera's pose to the target pose. The camera's pose
* relative to the target is determined by the supplied 3d points of the target's model and their
* associated 2d points imaged by the camera.
* Finds the transformation that maps the camera's pose to the origin of the supplied object. An
* "object" is simply a set of known 3d translations that correspond to the given 2d points. If,
* for example, the object translations are given relative to close-right corner of the blue
* alliance(the default origin), a camera-to-origin transformation is returned. If the
* translations are relative to a target's pose, a camera-to-target transformation is returned.
*
* <p>This method is intended for use with multiple targets and has no alternate solutions. There
* must be at least 3 points.
* <p>There must be at least 3 points to use this method. This does not return an alternate
* solution-- if you are intending to use solvePNP on a single AprilTag, see {@link
* #solvePNP_SQUARE} instead.
*
* @param cameraMatrix the camera intrinsics matrix in standard opencv form
* @param distCoeffs the camera distortion matrix in standard opencv form
* @param cameraMatrix The camera intrinsics matrix in standard opencv form
* @param distCoeffs The camera distortion matrix in standard opencv form
* @param objectTrls The translations of the object corners, relative to the field.
* @param imageCorners The projection of these 3d object points into the 2d camera image. The
* order should match the given object point translations.
* @return The resulting transformation that maps the camera pose to the target pose. If the 3d
* model points are supplied relative to the origin, this transformation brings the camera to
* the origin.
*/
public static PNPResults solvePNP_SQPNP(
public static Optional<PNPResults> solvePNP_SQPNP(
Matrix<N3, N3> cameraMatrix,
Matrix<N5, N1> distCoeffs,
List<Translation3d> objectTrls,
List<TargetCorner> imageCorners) {
// translate to opencv classes
var objectPoints = translationToTvec(objectTrls.toArray(new Translation3d[0]));
var imagePoints = targetCornersToMat(imageCorners);
var cameraMatrixMat = matrixToMat(cameraMatrix.getStorage());
var distCoeffsMat = matrixToMat(distCoeffs.getStorage());
var rvecs = new ArrayList<Mat>();
var tvecs = new ArrayList<Mat>();
var rvec = Mat.zeros(3, 1, CvType.CV_32F);
var tvec = Mat.zeros(3, 1, CvType.CV_32F);
var reprojectionError = new Mat();
// calc rvec/tvec from image points
Calib3d.solvePnPGeneric(
objectPoints,
imagePoints,
cameraMatrixMat,
distCoeffsMat,
rvecs,
tvecs,
false,
Calib3d.SOLVEPNP_SQPNP,
rvec,
tvec,
reprojectionError);

float[] error = new float[1];
reprojectionError.get(0, 0, error);
// convert to wpilib coordinates
var best = new Transform3d(tvecToTranslation(tvecs.get(0)), rvecToRotation(rvecs.get(0)));

// release our Mats from native memory
objectPoints.release();
imagePoints.release();
cameraMatrixMat.release();
distCoeffsMat.release();
for (var v : rvecs) v.release();
for (var v : tvecs) v.release();
rvec.release();
tvec.release();
reprojectionError.release();

return new PNPResults(best, error[0]);
try {
// translate to opencv classes
MatOfPoint3f objectPoints = translationToTvec(objectTrls.toArray(new Translation3d[0]));
MatOfPoint2f imagePoints = targetCornersToMat(imageCorners);
MatOfDouble cameraMatrixMat = matrixToMat(cameraMatrix.getStorage());
MatOfDouble distCoeffsMat = matrixToMat(distCoeffs.getStorage());
var rvecs = new ArrayList<Mat>();
var tvecs = new ArrayList<Mat>();
Mat rvec = Mat.zeros(3, 1, CvType.CV_32F);
Mat tvec = Mat.zeros(3, 1, CvType.CV_32F);
Mat reprojectionError = new Mat();
// calc rvec/tvec from image points
Calib3d.solvePnPGeneric(
objectPoints,
imagePoints,
cameraMatrixMat,
distCoeffsMat,
rvecs,
tvecs,
false,
Calib3d.SOLVEPNP_SQPNP,
rvec,
tvec,
reprojectionError);

float[] error = new float[1];
reprojectionError.get(0, 0, error);
// convert to wpilib coordinates
var best = new Transform3d(tvecToTranslation(tvecs.get(0)), rvecToRotation(rvecs.get(0)));

// release our Mats from native memory
objectPoints.release();
imagePoints.release();
cameraMatrixMat.release();
distCoeffsMat.release();
for (var v : rvecs) v.release();
for (var v : tvecs) v.release();
rvec.release();
tvec.release();
reprojectionError.release();

// check if solvePnP failed with NaN results
if (Double.isNaN(error[0])) throw new Exception("SolvePNP_SQPNP NaN result");

return Optional.of(new PNPResults(best, error[0]));
} catch (Exception e) {
System.err.println("SolvePNP_SQPNP failed!");
e.printStackTrace();
return Optional.empty();
}
}
}
Loading
Loading