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/BUILD b/y2023/vision/BUILD
index 5c98d7c..2cc4367 100644
--- a/y2023/vision/BUILD
+++ b/y2023/vision/BUILD
@@ -66,6 +66,7 @@
srcs = ["vision_util.cc"],
hdrs = ["vision_util.h"],
deps = [
+ "//third_party:opencv",
"//y2023/constants:constants_fbs",
"@com_github_google_glog//:glog",
],
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);
diff --git a/y2023/vision/aprilrobotics.h b/y2023/vision/aprilrobotics.h
index 8c2c0ad..4477856 100644
--- a/y2023/vision/aprilrobotics.h
+++ b/y2023/vision/aprilrobotics.h
@@ -5,6 +5,7 @@
#include "aos/events/shm_event_loop.h"
#include "aos/network/team_number.h"
#include "aos/realtime.h"
+#include "frc971/constants/constants_sender_lib.h"
#include "frc971/vision/calibration_generated.h"
#include "frc971/vision/charuco_lib.h"
#include "frc971/vision/target_map_generated.h"
@@ -15,9 +16,8 @@
#include "third_party/apriltag/apriltag.h"
#include "third_party/apriltag/apriltag_pose.h"
#include "third_party/apriltag/tag16h5.h"
-#include "y2023/vision/april_debug_generated.h"
#include "y2023/constants/constants_generated.h"
-#include "frc971/constants/constants_sender_lib.h"
+#include "y2023/vision/april_debug_generated.h"
DECLARE_int32(team_number);
@@ -28,14 +28,20 @@
public:
AprilRoboticsDetector(aos::EventLoop *event_loop,
std::string_view channel_name);
-
~AprilRoboticsDetector();
void SetWorkerpoolAffinities();
+ // Undistorts the april tag corners using the camera calibration
+ void UndistortDetection(apriltag_detection_t *det) const;
+
std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>> DetectTags(
cv::Mat image);
+ const cv::Mat extrinsics() const { return extrinsics_; }
+ const cv::Mat intrinsics() const { return intrinsics_; }
+ const cv::Mat dist_coeffs() const { return dist_coeffs_; }
+
private:
void HandleImage(cv::Mat image, aos::monotonic_clock::time_point eof);
@@ -44,36 +50,15 @@
frc971::vision::TargetMapper::TargetId target_id,
flatbuffers::FlatBufferBuilder *fbb);
-
- static cv::Mat CameraIntrinsics(
- const frc971::vision::calibration::CameraCalibration
- *camera_calibration) {
- cv::Mat result(3, 3, CV_32F,
- const_cast<void *>(static_cast<const void *>(
- camera_calibration->intrinsics()->data())));
- result.convertTo(result, CV_64F);
- CHECK_EQ(result.total(), camera_calibration->intrinsics()->size());
-
- return result;
- }
-
- static cv::Mat CameraDistCoeffs(
- const frc971::vision::calibration::CameraCalibration
- *camera_calibration) {
- const cv::Mat result(5, 1, CV_32F,
- const_cast<void *>(static_cast<const void *>(
- camera_calibration->dist_coeffs()->data())));
- CHECK_EQ(result.total(), camera_calibration->dist_coeffs()->size());
- return result;
- }
-
apriltag_family_t *tag_family_;
apriltag_detector_t *tag_detector_;
const frc971::constants::ConstantsFetcher<Constants> calibration_data_;
const frc971::vision::calibration::CameraCalibration *calibration_;
cv::Mat intrinsics_;
- cv::Mat camera_distortion_coeffs_;
+ cv::Mat projection_matrix_;
+ cv::Mat extrinsics_;
+ cv::Mat dist_coeffs_;
aos::Ftrace ftrace_;
diff --git a/y2023/vision/calib_files/calibration_pi-971-1_cam-23-01_2013-01-18_08-56-17.194305339.json b/y2023/vision/calib_files/calibration_pi-971-1_cam-23-01_2013-01-18_08-56-17.194305339.json
new file mode 100755
index 0000000..1f30579
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-971-1_cam-23-01_2013-01-18_08-56-17.194305339.json
@@ -0,0 +1,42 @@
+{
+ "node_name": "pi1",
+ "team_number": 971,
+ "intrinsics": [
+ 913.133728,
+ 0.0,
+ 575.592285,
+ 0.0,
+ 917.686462,
+ 364.544403,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ -0.454162,
+ 0.255307,
+ -0.000166,
+ 0.008949,
+ -0.079813
+ ],
+ "fixed_extrinsics": [
+ 1.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "calibration_timestamp": 1358499377194305339,
+ "camera_id": "23-01"
+}
diff --git a/y2023/vision/target_mapping.cc b/y2023/vision/target_mapping.cc
index 43b5353..8b1e31e 100644
--- a/y2023/vision/target_mapping.cc
+++ b/y2023/vision/target_mapping.cc
@@ -76,41 +76,23 @@
}
}
-Eigen::Affine3d CameraExtrinsics(
- const calibration::CameraCalibration *camera_calibration) {
- const frc971::vision::calibration::TransformationMatrix *transform =
- camera_calibration->has_turret_extrinsics()
- ? camera_calibration->turret_extrinsics()
- : camera_calibration->fixed_extrinsics();
-
- cv::Mat result(
- 4, 4, CV_32F,
- const_cast<void *>(static_cast<const void *>(transform->data()->data())));
- result.convertTo(result, CV_64F);
- CHECK_EQ(result.total(), transform->data()->size());
-
- Eigen::Matrix4d result_eigen;
- cv::cv2eigen(result, result_eigen);
- return Eigen::Affine3d(result_eigen);
-}
-
// Get images from pi and pass apriltag positions to HandleAprilTag()
void HandlePiCaptures(
- const frc971::constants::ConstantsFetcher<Constants> &constants,
aos::EventLoop *pi_event_loop, aos::logger::LogReader *reader,
std::vector<DataAdapter::TimestampedDetection>
*timestamped_target_detections,
std::vector<std::unique_ptr<AprilRoboticsDetector>> *detectors) {
-
- const auto node_name = pi_event_loop->node()->name()->string_view();
- const calibration::CameraCalibration *calibration =
- FindCameraCalibration(constants.constants(), node_name);
- const auto extrinsics = CameraExtrinsics(calibration);
-
// TODO(milind): change to /camera once we log at full frequency
static constexpr std::string_view kImageChannel = "/camera/decimated";
- detectors->emplace_back(
- std::make_unique<AprilRoboticsDetector>(pi_event_loop, kImageChannel));
+ auto detector_ptr =
+ std::make_unique<AprilRoboticsDetector>(pi_event_loop, kImageChannel);
+ // Get the camera extrinsics
+ cv::Mat extrinsics_cv = detector_ptr->extrinsics();
+ Eigen::Matrix4d extrinsics_matrix;
+ cv::cv2eigen(extrinsics_cv, extrinsics_matrix);
+ const auto extrinsics = Eigen::Affine3d(extrinsics_matrix);
+
+ detectors->emplace_back(std::move(detector_ptr));
pi_event_loop->MakeWatcher("/camera", [=](const TargetMap &map) {
aos::distributed_clock::time_point pi_distributed_time =
@@ -140,36 +122,28 @@
aos::configuration::GetNode(reader.configuration(), "pi1");
std::unique_ptr<aos::EventLoop> pi1_event_loop =
reader.event_loop_factory()->MakeEventLoop("pi1", pi1);
- frc971::constants::ConstantsFetcher<Constants> pi1_constants(
- pi1_event_loop.get());
- HandlePiCaptures(pi1_constants, pi1_event_loop.get(), &reader,
+ HandlePiCaptures(pi1_event_loop.get(), &reader,
×tamped_target_detections, &detectors);
const aos::Node *pi2 =
aos::configuration::GetNode(reader.configuration(), "pi2");
std::unique_ptr<aos::EventLoop> pi2_event_loop =
reader.event_loop_factory()->MakeEventLoop("pi2", pi2);
- frc971::constants::ConstantsFetcher<Constants> pi2_constants(
- pi2_event_loop.get());
- HandlePiCaptures(pi2_constants, pi2_event_loop.get(), &reader,
+ HandlePiCaptures(pi2_event_loop.get(), &reader,
×tamped_target_detections, &detectors);
const aos::Node *pi3 =
aos::configuration::GetNode(reader.configuration(), "pi3");
std::unique_ptr<aos::EventLoop> pi3_event_loop =
reader.event_loop_factory()->MakeEventLoop("pi3", pi3);
- frc971::constants::ConstantsFetcher<Constants> pi3_constants(
- pi3_event_loop.get());
- HandlePiCaptures(pi3_constants, pi3_event_loop.get(), &reader,
+ HandlePiCaptures(pi3_event_loop.get(), &reader,
×tamped_target_detections, &detectors);
const aos::Node *pi4 =
aos::configuration::GetNode(reader.configuration(), "pi4");
std::unique_ptr<aos::EventLoop> pi4_event_loop =
reader.event_loop_factory()->MakeEventLoop("pi4", pi4);
- frc971::constants::ConstantsFetcher<Constants> pi4_constants(
- pi4_event_loop.get());
- HandlePiCaptures(pi4_constants, pi4_event_loop.get(), &reader,
+ HandlePiCaptures(pi4_event_loop.get(), &reader,
×tamped_target_detections, &detectors);
reader.event_loop_factory()->Run();
diff --git a/y2023/vision/vision_util.cc b/y2023/vision/vision_util.cc
index eed315a..f4937e5 100644
--- a/y2023/vision/vision_util.cc
+++ b/y2023/vision/vision_util.cc
@@ -3,6 +3,7 @@
#include "glog/logging.h"
namespace y2023::vision {
+
const frc971::vision::calibration::CameraCalibration *FindCameraCalibration(
const y2023::Constants &calibration_data, std::string_view node_name) {
CHECK(calibration_data.has_cameras());
@@ -16,4 +17,40 @@
}
LOG(FATAL) << ": Failed to find camera calibration for " << node_name;
}
+
+cv::Mat CameraExtrinsics(
+ const frc971::vision::calibration::CameraCalibration *camera_calibration) {
+ CHECK(!camera_calibration->has_turret_extrinsics())
+ << "No turret on 2023 robot";
+
+ cv::Mat result(4, 4, CV_32F,
+ const_cast<void *>(static_cast<const void *>(
+ camera_calibration->fixed_extrinsics()->data()->data())));
+ result.convertTo(result, CV_64F);
+ CHECK_EQ(result.total(),
+ camera_calibration->fixed_extrinsics()->data()->size());
+
+ return result;
+}
+
+cv::Mat CameraIntrinsics(
+ const frc971::vision::calibration::CameraCalibration *camera_calibration) {
+ cv::Mat result(3, 3, CV_32F,
+ const_cast<void *>(static_cast<const void *>(
+ camera_calibration->intrinsics()->data())));
+ result.convertTo(result, CV_64F);
+ CHECK_EQ(result.total(), camera_calibration->intrinsics()->size());
+
+ return result;
+}
+
+cv::Mat CameraDistCoeffs(
+ const frc971::vision::calibration::CameraCalibration *camera_calibration) {
+ const cv::Mat result(5, 1, CV_32F,
+ const_cast<void *>(static_cast<const void *>(
+ camera_calibration->dist_coeffs()->data())));
+ CHECK_EQ(result.total(), camera_calibration->dist_coeffs()->size());
+ return result;
+}
+
} // namespace y2023::vision
diff --git a/y2023/vision/vision_util.h b/y2023/vision/vision_util.h
index 58f9a7f..ce1a69d 100644
--- a/y2023/vision/vision_util.h
+++ b/y2023/vision/vision_util.h
@@ -2,10 +2,23 @@
#define Y2023_VISION_VISION_UTIL_H_
#include <string_view>
+#include "opencv2/imgproc.hpp"
#include "y2023/constants/constants_generated.h"
+
namespace y2023::vision {
const frc971::vision::calibration::CameraCalibration *FindCameraCalibration(
const y2023::Constants &calibration_data, std::string_view node_name);
-}
+
+cv::Mat CameraExtrinsics(
+ const frc971::vision::calibration::CameraCalibration *camera_calibration);
+
+cv::Mat CameraIntrinsics(
+ const frc971::vision::calibration::CameraCalibration *camera_calibration);
+
+cv::Mat CameraDistCoeffs(
+ const frc971::vision::calibration::CameraCalibration *camera_calibration);
+
+} // namespace y2023::vision
+
#endif // Y2023_VISION_VISION_UTIL_H_