Undistort april detection corners
To have more accurate pose estimates (especially when the
april tag is in the corners of the image), we need to account for
distortion. Undistorting just the detected corners is the most efficient
way to do this.
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: Ic3872dee1e6fef060b49260e6abc22498f76a353
diff --git a/y2023/vision/aprilrobotics.cc b/y2023/vision/aprilrobotics.cc
index cb6a5bc..a98f269 100644
--- a/y2023/vision/aprilrobotics.cc
+++ b/y2023/vision/aprilrobotics.cc
@@ -6,6 +6,9 @@
debug, false,
"If true, dump a ton of debug and crash on the first valid detection.");
+DEFINE_double(min_decision_margin, 30.0,
+ "Minimum decision margin (confidence) for an apriltag detection");
+
namespace y2023 {
namespace vision {
@@ -40,8 +43,16 @@
// Check team string is valid
calibration_ = FindCameraCalibration(
calibration_data_.constants(), event_loop->node()->name()->string_view());
+
+ extrinsics_ = CameraExtrinsics(calibration_);
+
intrinsics_ = CameraIntrinsics(calibration_);
- camera_distortion_coeffs_ = CameraDistCoeffs(calibration_);
+ // Create an undistort projection matrix using the intrinsics
+ projection_matrix_ = cv::Mat::zeros(3, 4, CV_64F);
+ intrinsics_.rowRange(0, 3).colRange(0, 3).copyTo(
+ projection_matrix_.rowRange(0, 3).colRange(0, 3));
+
+ dist_coeffs_ = CameraDistCoeffs(calibration_);
image_callback_.set_format(frc971::vision::ImageCallback::Format::GRAYSCALE);
}
@@ -66,10 +77,10 @@
}
}
-void AprilRoboticsDetector::HandleImage(cv::Mat image_color_mat,
+void AprilRoboticsDetector::HandleImage(cv::Mat image_grayscale,
aos::monotonic_clock::time_point eof) {
std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>> detections =
- DetectTags(image_color_mat);
+ DetectTags(image_grayscale);
auto builder = target_map_sender_.MakeBuilder();
std::vector<flatbuffers::Offset<frc971::vision::TargetPoseFbs>> target_poses;
@@ -102,6 +113,28 @@
orientation_offset);
}
+void AprilRoboticsDetector::UndistortDetection(
+ apriltag_detection_t *det) const {
+ // 4 corners
+ constexpr size_t kRows = 4;
+ // 2d points
+ constexpr size_t kCols = 2;
+
+ cv::Mat distorted_points(kRows, kCols, CV_64F, det->p);
+ cv::Mat undistorted_points = cv::Mat::zeros(kRows, kCols, CV_64F);
+
+ // Undistort the april tag points
+ cv::undistortPoints(distorted_points, undistorted_points, intrinsics_,
+ dist_coeffs_, cv::noArray(), projection_matrix_);
+
+ // Copy the undistorted points into det
+ for (size_t i = 0; i < kRows; i++) {
+ for (size_t j = 0; j < kCols; j++) {
+ det->p[i][j] = undistorted_points.at<double>(i, j);
+ }
+ }
+}
+
std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>>
AprilRoboticsDetector::DetectTags(cv::Mat image) {
const aos::monotonic_clock::time_point start_time =
@@ -128,7 +161,7 @@
apriltag_detection_t *det;
zarray_get(detections, i, &det);
- if (det->decision_margin > 30) {
+ if (det->decision_margin > FLAGS_min_decision_margin) {
VLOG(1) << "Found tag number " << det->id << " hamming: " << det->hamming
<< " margin: " << det->decision_margin;
@@ -139,6 +172,7 @@
apriltag_detection_info_t info;
info.det = det;
info.tagsize = 0.1524;
+
info.fx = intrinsics_.at<double>(0, 0);
info.fy = intrinsics_.at<double>(1, 1);
info.cx = intrinsics_.at<double>(0, 2);
@@ -147,6 +181,7 @@
apriltag_pose_t pose;
double err = estimate_tag_pose(&info, &pose);
+ UndistortDetection(det);
VLOG(1) << "err: " << err;
results.emplace_back(*det, pose);