Merge "Undistort april detection corners"
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 4f10499..18be258 100644
--- a/y2023/vision/target_mapping.cc
+++ b/y2023/vision/target_mapping.cc
@@ -75,41 +75,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 =
@@ -139,36 +121,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_