Raising error if our Undistort of image points doesn't converge.
This can lead to really bad estimates near the corners of the image
Skipping the pose calculation, and sending the rejected points to foxglove,
as we do for other problems
Change-Id: Ibc4a7c709e0024dd6f55108b0e11d19dfad66786
Signed-off-by: Jim Ostrowski <yimmy13@gmail.com>
diff --git a/frc971/orin/apriltag.h b/frc971/orin/apriltag.h
index 8b5f0b2..7c0a721 100644
--- a/frc971/orin/apriltag.h
+++ b/frc971/orin/apriltag.h
@@ -193,7 +193,9 @@
distortion_coefficients_ = distortion_coefficients;
}
- static void UnDistort(double *u, double *v, const CameraMatrix *camera_matrix,
+ // Undistort pixels based on our camera model, using iterative algorithm
+ // Returns false if we fail to converge
+ static bool UnDistort(double *u, double *v, const CameraMatrix *camera_matrix,
const DistCoeffs *distortion_coefficients);
private:
diff --git a/frc971/orin/apriltag_detect.cc b/frc971/orin/apriltag_detect.cc
index 235a6d4..26de7fa 100644
--- a/frc971/orin/apriltag_detect.cc
+++ b/frc971/orin/apriltag_detect.cc
@@ -334,9 +334,10 @@
// We're undistorting using math found from this github page
// https://yangyushi.github.io/code/2020/03/04/opencv-undistort.html
-void GpuDetector::UnDistort(double *u, double *v,
+bool GpuDetector::UnDistort(double *u, double *v,
const CameraMatrix *camera_matrix,
const DistCoeffs *distortion_coefficients) {
+ bool converged = true;
const double k1 = distortion_coefficients->k1;
const double k2 = distortion_coefficients->k2;
const double p1 = distortion_coefficients->p1;
@@ -377,6 +378,7 @@
yP = (y0 - tangential_dy) * radial_distortion_inv;
if (iterations > kUndistortIterationThreshold) {
+ converged = false;
break;
}
@@ -397,6 +399,8 @@
*u = xP * fx + cx;
*v = yP * fy + cy;
+
+ return converged;
}
// Mostly stolen from aprilrobotics, but modified to implement the dewarp.
diff --git a/frc971/orin/gpu_apriltag.cc b/frc971/orin/gpu_apriltag.cc
index 19bee47..87d5f49 100644
--- a/frc971/orin/gpu_apriltag.cc
+++ b/frc971/orin/gpu_apriltag.cc
@@ -137,17 +137,19 @@
detection.distortion_factor, detection.pose_error_ratio);
}
-void ApriltagDetector::UndistortDetection(apriltag_detection_t *det) const {
+bool ApriltagDetector::UndistortDetection(apriltag_detection_t *det) const {
// Copy the undistorted points into det
+ bool converged = true;
for (size_t i = 0; i < 4; i++) {
double u = det->p[i][0];
double v = det->p[i][1];
- GpuDetector::UnDistort(&u, &v, &distortion_camera_matrix_,
- &distortion_coefficients_);
+ converged &= GpuDetector::UnDistort(&u, &v, &distortion_camera_matrix_,
+ &distortion_coefficients_);
det->p[i][0] = u;
det->p[i][1] = v;
}
+ return converged;
}
double ApriltagDetector::ComputeDistortionFactor(
@@ -275,7 +277,20 @@
builder.fbb(), eof, orig_corner_points,
std::vector<double>{0.0, 1.0, 0.0, 0.5}));
- UndistortDetection(gpu_detection);
+ bool converged = UndistortDetection(gpu_detection);
+
+ if (!converged) {
+ VLOG(1) << "Rejecting detection because Undistort failed to coverge";
+
+ // Send rejected corner points to foxglove in red
+ std::vector<cv::Point2f> rejected_corner_points =
+ MakeCornerVector(gpu_detection);
+ foxglove_corners.push_back(frc971::vision::BuildPointsAnnotation(
+ builder.fbb(), eof, rejected_corner_points,
+ std::vector<double>{1.0, 0.0, 0.0, 0.5}));
+ rejections_++;
+ continue;
+ }
// We're setting this here to use the undistorted corner points in pose
// estimation.
diff --git a/frc971/orin/gpu_apriltag.h b/frc971/orin/gpu_apriltag.h
index 5569e5a..f02a5a3 100644
--- a/frc971/orin/gpu_apriltag.h
+++ b/frc971/orin/gpu_apriltag.h
@@ -53,7 +53,8 @@
// Undistorts the april tag corners using the camera calibration.
// Returns the detections in place.
- void UndistortDetection(apriltag_detection_t *det) const;
+ // Returns false if any of the corner undistortions fail to converge
+ bool UndistortDetection(apriltag_detection_t *det) const;
// Computes the distortion effect on this detection taking the scaled average
// delta between orig_corners (distorted corners) and corners (undistorted