();
float[] data = new float[(int) matInput.total() * matInput.channels()];
matInput.get(0, 0, data);
for (int i = 0; i < (int) matInput.total(); i++) {
corners.add(new TargetCorner(data[0 + 2 * i], data[1 + 2 * i]));
}
return corners;
}
/**
* Reorders the list, optionally indexing backwards and wrapping around to the last element after
* the first, and shifting all indices in the direction of indexing.
*
* e.g.
*
*
({1,2,3}, false, 1) == {2,3,1}
*
*
({1,2,3}, true, 0) == {1,3,2}
*
*
({1,2,3}, true, 1) == {3,2,1}
*
* @param Element type
* @param elements list elements
* @param backwards If indexing should happen in reverse (0, size-1, size-2, ...)
* @param shiftStart How much the initial index should be shifted (instead of starting at index 0,
* start at shiftStart, negated if backwards)
* @return Reordered list
*/
public static List reorderCircular(List elements, boolean backwards, int shiftStart) {
int size = elements.size();
int dir = backwards ? -1 : 1;
var reordered = new ArrayList<>(elements);
for (int i = 0; i < size; i++) {
int index = (i * dir + shiftStart * dir) % size;
if (index < 0) index = size + index;
reordered.set(i, elements.get(index));
}
return reordered;
}
/**
* Convert a rotation delta from EDN to NWU. For example, if you have a rotation X,Y,Z {1, 0, 0}
* in EDN, this would be {0, -1, 0} in NWU.
*/
private static Rotation3d rotationEDNtoNWU(Rotation3d rot) {
return EDN_TO_NWU.unaryMinus().plus(rot.plus(EDN_TO_NWU));
}
/**
* Convert a rotation delta from NWU to EDN. For example, if you have a rotation X,Y,Z {1, 0, 0}
* in NWU, this would be {0, 0, 1} in EDN.
*/
private static Rotation3d rotationNWUtoEDN(Rotation3d rot) {
return NWU_TO_EDN.unaryMinus().plus(rot.plus(NWU_TO_EDN));
}
/**
* Convert a translation from EDN to NWU. For example, if you have a translation X,Y,Z {1, 0, 0}
* in EDN, this would be {0, -1, 0} in NWU.
*/
private static Translation3d translationEDNtoNWU(Translation3d trl) {
return trl.rotateBy(EDN_TO_NWU);
}
/**
* Convert a translation from NWU to EDN. For example, if you have a translation X,Y,Z {1, 0, 0}
* in NWU, this would be {0, 0, 1} in EDN.
*/
private static Translation3d translationNWUtoEDN(Translation3d trl) {
return trl.rotateBy(NWU_TO_EDN);
}
/**
* Distort a list of points in pixels using the OPENCV5/8 models. See image-rotation.md or
* https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html for the math here.
*
* @param pointsList the undistorted points
* @param cameraMatrix standard OpenCV camera mat
* @param distCoeffs standard OpenCV distortion coefficients. Must OPENCV5 or OPENCV8
*/
public static List distortPoints(
List pointsList, Mat cameraMatrix, Mat distCoeffs) {
var ret = new ArrayList();
var cx = cameraMatrix.get(0, 2)[0];
var cy = cameraMatrix.get(1, 2)[0];
var fx = cameraMatrix.get(0, 0)[0];
var fy = cameraMatrix.get(1, 1)[0];
var k1 = distCoeffs.get(0, 0)[0];
var k2 = distCoeffs.get(0, 1)[0];
var p1 = distCoeffs.get(0, 2)[0];
var p2 = distCoeffs.get(0, 3)[0];
var k3 = distCoeffs.get(0, 4)[0];
double k4 = 0;
double k5 = 0;
double k6 = 0;
if (distCoeffs.cols() == 8) {
k4 = distCoeffs.get(0, 5)[0];
k5 = distCoeffs.get(0, 6)[0];
k6 = distCoeffs.get(0, 7)[0];
}
for (Point point : pointsList) {
// To relative coordinates
double xprime = (point.x - cx) / fx; // cx, cy is the center of distortion
double yprime = (point.y - cy) / fy;
double r_sq = xprime * xprime + yprime * yprime; // square of the radius from center
// Radial distortion
double radialDistortion =
(1 + k1 * r_sq + k2 * r_sq * r_sq + k3 * r_sq * r_sq * r_sq)
/ (1 + k4 * r_sq + k5 * r_sq * r_sq + k6 * r_sq * r_sq * r_sq);
double xDistort = xprime * radialDistortion;
double yDistort = yprime * radialDistortion;
// Tangential distortion
xDistort = xDistort + (2 * p1 * xprime * yprime + p2 * (r_sq + 2 * xprime * xprime));
yDistort = yDistort + (p1 * (r_sq + 2 * yprime * yprime) + 2 * p2 * xprime * yprime);
// Back to absolute coordinates.
xDistort = xDistort * fx + cx;
yDistort = yDistort * fy + cy;
ret.add(new Point(xDistort, yDistort));
}
return ret;
}
/**
* Project object points from the 3d world into the 2d camera image. The camera
* properties(intrinsics, distortion) determine the results of this projection.
*
* @param cameraMatrix the camera intrinsics matrix in standard opencv form
* @param distCoeffs the camera distortion matrix in standard opencv form
* @param camRt The change in basis from world coordinates to camera coordinates. See {@link
* RotTrlTransform3d#makeRelativeTo(Pose3d)}.
* @param objectTranslations The 3d points to be projected
* @return The 2d points in pixels which correspond to the camera's image of the 3d points
*/
public static Point[] projectPoints(
Matrix cameraMatrix,
Matrix distCoeffs,
RotTrlTransform3d camRt,
List objectTranslations) {
// translate to opencv classes
var objectPoints = translationToTvec(objectTranslations.toArray(new Translation3d[0]));
// opencv rvec/tvec describe a change in basis from world to camera
var rvec = rotationToRvec(camRt.getRotation());
var tvec = translationToTvec(camRt.getTranslation());
var cameraMatrixMat = matrixToMat(cameraMatrix.getStorage());
var distCoeffsMat = new MatOfDouble(matrixToMat(distCoeffs.getStorage()));
var imagePoints = new MatOfPoint2f();
// project to 2d
Calib3d.projectPoints(objectPoints, rvec, tvec, cameraMatrixMat, distCoeffsMat, imagePoints);
var points = imagePoints.toArray();
// release our Mats from native memory
objectPoints.release();
rvec.release();
tvec.release();
cameraMatrixMat.release();
distCoeffsMat.release();
imagePoints.release();
return points;
}
/**
* Undistort 2d image points using a given camera's intrinsics and distortion.
*
* 2d image points from {@link #projectPoints(Matrix, Matrix, RotTrlTransform3d, List)
* projectPoints()} will naturally be distorted, so this operation is important if the image
* points need to be directly used (e.g. 2d yaw/pitch).
*
* @param cameraMatrix The camera intrinsics matrix in standard opencv form
* @param distCoeffs The camera distortion matrix in standard opencv form
* @param points The distorted image points
* @return The undistorted image points
*/
public static Point[] undistortPoints(
Matrix cameraMatrix, Matrix distCoeffs, Point[] points) {
var distMat = new MatOfPoint2f(points);
var undistMat = new MatOfPoint2f();
var cameraMatrixMat = matrixToMat(cameraMatrix.getStorage());
var distCoeffsMat = matrixToMat(distCoeffs.getStorage());
Calib3d.undistortImagePoints(distMat, undistMat, cameraMatrixMat, distCoeffsMat);
var undistPoints = undistMat.toArray();
distMat.release();
undistMat.release();
cameraMatrixMat.release();
distCoeffsMat.release();
return undistPoints;
}
/**
* Gets the (upright) rectangle which bounds this contour.
*
* Note that rectangle size and position are stored with ints and do not have sub-pixel
* accuracy.
*
* @param points The points to be bounded
* @return Rectangle bounding the given points
*/
public static Rect getBoundingRect(Point[] points) {
var pointMat = new MatOfPoint2f(points);
var rect = Imgproc.boundingRect(pointMat);
pointMat.release();
return rect;
}
/**
* Gets the rotated rectangle with minimum area which bounds this contour.
*
*
Note that rectangle size and position are stored with floats and have sub-pixel accuracy.
*
* @param points The points to be bounded
* @return Rotated rectangle bounding the given points
*/
public static RotatedRect getMinAreaRect(Point[] points) {
var pointMat = new MatOfPoint2f(points);
var rect = Imgproc.minAreaRect(pointMat);
pointMat.release();
return rect;
}
/**
* Gets the convex hull contour (the outline) of a list of points.
*
* @param points The input contour
* @return The subset of points defining the convex hull. Note that these use ints and not floats.
*/
public static Point[] getConvexHull(Point[] points) {
var pointMat = new MatOfPoint(points);
// outputHull gives us indices (of corn) that make a convex hull contour
var outputHull = new MatOfInt();
Imgproc.convexHull(pointMat, outputHull);
int[] indices = outputHull.toArray();
outputHull.release();
pointMat.release();
var convexPoints = new Point[indices.length];
for (int i = 0; i < indices.length; i++) {
convexPoints[i] = points[indices[i]];
}
return convexPoints;
}
/**
* 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. The supplied model translations must be relative to
* the target's pose.
*
*
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
* between the "best" and "alternate" solution.
*
*
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 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):
*
* - Point 0: [0, -squareLength / 2, squareLength / 2]
*
- Point 1: [0, squareLength / 2, squareLength / 2]
*
- Point 2: [0, squareLength / 2, -squareLength / 2]
*
- Point 3: [0, -squareLength / 2, -squareLength / 2]
*
*
* @param imagePoints 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.
*/
public static Optional solvePNP_SQUARE(
Matrix cameraMatrix,
Matrix distCoeffs,
List modelTrls,
Point[] imagePoints) {
// solvepnp inputs
MatOfPoint3f objectMat = new MatOfPoint3f();
MatOfPoint2f imageMat = new MatOfPoint2f();
MatOfDouble cameraMatrixMat = new MatOfDouble();
MatOfDouble distCoeffsMat = new MatOfDouble();
var rvecs = new ArrayList();
var tvecs = new ArrayList();
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);
imagePoints = reorderCircular(Arrays.asList(imagePoints), true, -1).toArray(Point[]::new);
// translate to opencv classes
translationToTvec(modelTrls.toArray(new Translation3d[0])).assignTo(objectMat);
imageMat.fromArray(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(
objectMat,
imageMat,
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 = imageMat.get(0, 0);
br[0] -= 0.001;
br[1] -= 0.001;
imageMat.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 PnpResult(best, alt, errors[0] / errors[1], errors[0], errors[1]));
else return Optional.empty();
}
// solvePnP failed
catch (Exception e) {
System.err.println("SolvePNP_SQUARE failed!");
e.printStackTrace();
return Optional.empty();
} finally {
// release our Mats from native memory
objectMat.release();
imageMat.release();
cameraMatrixMat.release();
distCoeffsMat.release();
for (var v : rvecs) v.release();
for (var v : tvecs) v.release();
rvec.release();
tvec.release();
reprojectionError.release();
}
}
/**
* 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.
*
* 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 objectTrls The translations of the object corners, relative to the field.
* @param imagePoints 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 Optional solvePNP_SQPNP(
Matrix cameraMatrix,
Matrix distCoeffs,
List objectTrls,
Point[] imagePoints) {
try {
// translate to opencv classes
MatOfPoint3f objectMat = translationToTvec(objectTrls.toArray(new Translation3d[0]));
MatOfPoint2f imageMat = new MatOfPoint2f(imagePoints);
Mat cameraMatrixMat = matrixToMat(cameraMatrix.getStorage());
Mat distCoeffsMat = matrixToMat(distCoeffs.getStorage());
var rvecs = new ArrayList();
var tvecs = new ArrayList();
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(
objectMat,
imageMat,
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
objectMat.release();
imageMat.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 PnpResult(best, error[0]));
} catch (Exception e) {
System.err.println("SolvePNP_SQPNP failed!");
e.printStackTrace();
return Optional.empty();
}
}
}