Refactor of apriltag detector to send out tags
Split into app in y2024, and libs in frc971
Added in calibration constants to y2024, including single basic camera
Modified team number parsing to work with orins
Pulled out year-by-year calibration call to a single FindCameraCalibration
call, and rest (extrinsics, intrinsics) are year-agnostic
Moving camera calibration files into constants/calib_files directory, instead
of vision/calib_files
Change-Id: Ic485086aad9665d2b571551a5afb337c9254d690
Signed-off-by: Jim Ostrowski <yimmy13@gmail.com>
diff --git a/aos/network/team_number.cc b/aos/network/team_number.cc
index 28ee67a..da6d0c5 100644
--- a/aos/network/team_number.cc
+++ b/aos/network/team_number.cc
@@ -39,8 +39,9 @@
return std::nullopt;
}
-std::optional<uint16_t> ParsePiTeamNumber(const std::string_view hostname) {
- if (hostname.substr(0, 3) != "pi-") {
+std::optional<uint16_t> ParsePiOrOrinTeamNumber(
+ const std::string_view hostname) {
+ if ((hostname.substr(0, 3) != "pi-") && (hostname.substr(0, 5) != "orin-")) {
return std::nullopt;
}
size_t first_separator = hostname.find('-');
@@ -93,9 +94,9 @@
}
}
{
- const auto result = team_number_internal::ParsePiTeamNumber(hostname);
+ const auto result = team_number_internal::ParsePiOrOrinTeamNumber(hostname);
if (result) {
- LOG(INFO) << "Pi hostname team number is: " << *result;
+ LOG(INFO) << "Pi/Orin hostname team number is: " << *result;
return *result;
}
}
@@ -122,8 +123,8 @@
void OverrideTeamNumber(uint16_t team) { override_team = team; }
-std::optional<uint16_t> ParsePiNumber(const std::string_view hostname) {
- if (hostname.substr(0, 3) != "pi-") {
+std::optional<uint16_t> ParsePiOrOrinNumber(const std::string_view hostname) {
+ if ((hostname.substr(0, 3) != "pi-") && (hostname.substr(0, 5) != "orin-")) {
return std::nullopt;
}
size_t first_separator = hostname.find('-');
diff --git a/aos/network/team_number.h b/aos/network/team_number.h
index b794309..9b9029a 100644
--- a/aos/network/team_number.h
+++ b/aos/network/team_number.h
@@ -27,15 +27,18 @@
// Guaranteed to be safe to call during static initialization time.
void OverrideTeamNumber(uint16_t team);
-// Returns the pi number for a pi formated hostname. pi-team#-pi# (pi-971-5)
-std::optional<uint16_t> ParsePiNumber(const std::string_view hostname);
+// Returns the number for a pi/orin formatted hostname. pi-team#-pi# (e.g., 5
+// for pi-971-5 or 2 for orin-9971-2)
+std::optional<uint16_t> ParsePiOrOrinNumber(const std::string_view hostname);
namespace team_number_internal {
std::optional<uint16_t> ParseRoborioTeamNumber(const std::string_view hostname);
-// Returns the team number for a pi formated hostname. pi-team#-pi#
-std::optional<uint16_t> ParsePiTeamNumber(const std::string_view hostname);
+// Returns the team number for a pi/orin formatted hostname. pi-team#-pi#
+// (e.g., 971 for pi-971-5 or 9971 for orin-9971-2)
+std::optional<uint16_t> ParsePiOrOrinTeamNumber(
+ const std::string_view hostname);
} // namespace team_number_internal
} // namespace network
diff --git a/aos/network/team_number_test.cc b/aos/network/team_number_test.cc
index 1ca3fc2..9922b1b 100644
--- a/aos/network/team_number_test.cc
+++ b/aos/network/team_number_test.cc
@@ -4,7 +4,7 @@
namespace aos::network::testing {
-using team_number_internal::ParsePiTeamNumber;
+using team_number_internal::ParsePiOrOrinTeamNumber;
using team_number_internal::ParseRoborioTeamNumber;
TEST(TeamNumberTest, Parse2015TeamNumber) {
@@ -28,23 +28,35 @@
EXPECT_FALSE(ParseRoborioTeamNumber("roboRIO--FRC"));
}
-TEST(TeamNumberTest, ParsePiTeamNumber) {
- EXPECT_EQ(971u, *ParsePiTeamNumber("pi-971-1"));
- EXPECT_EQ(8971u, *ParsePiTeamNumber("pi-8971-22"));
- EXPECT_EQ(8971u, *ParsePiTeamNumber("pi-8971-"));
+TEST(TeamNumberTest, ParsePiOrOrinTeamNumber) {
+ EXPECT_EQ(971u, *ParsePiOrOrinTeamNumber("pi-971-1"));
+ EXPECT_EQ(8971u, *ParsePiOrOrinTeamNumber("pi-8971-22"));
+ EXPECT_EQ(8971u, *ParsePiOrOrinTeamNumber("pi-8971-"));
+
+ EXPECT_EQ(971u, *ParsePiOrOrinTeamNumber("orin-971-1"));
+ EXPECT_EQ(8971u, *ParsePiOrOrinTeamNumber("orin-8971-22"));
+ EXPECT_EQ(8971u, *ParsePiOrOrinTeamNumber("orin-8971-"));
EXPECT_FALSE(ParseRoborioTeamNumber("pi"));
EXPECT_FALSE(ParseRoborioTeamNumber("pi-"));
EXPECT_FALSE(ParseRoborioTeamNumber("pi-971"));
EXPECT_FALSE(ParseRoborioTeamNumber("pi-971a-1"));
+ EXPECT_FALSE(ParseRoborioTeamNumber("orin-971-1"));
- EXPECT_EQ(1u, *ParsePiNumber("pi-971-1"));
- EXPECT_EQ(22u, *ParsePiNumber("pi-8971-22"));
+ EXPECT_EQ(1u, *ParsePiOrOrinNumber("pi-971-1"));
+ EXPECT_EQ(22u, *ParsePiOrOrinNumber("pi-8971-22"));
+ EXPECT_EQ(1u, *ParsePiOrOrinNumber("orin-971-1"));
+ EXPECT_EQ(22u, *ParsePiOrOrinNumber("orin-8971-22"));
- EXPECT_FALSE(ParsePiNumber("pi-8971-"));
- EXPECT_FALSE(ParsePiNumber("pi"));
- EXPECT_FALSE(ParsePiNumber("pi-"));
- EXPECT_FALSE(ParsePiNumber("pi-971"));
+ EXPECT_FALSE(ParsePiOrOrinNumber("pi-8971-"));
+ EXPECT_FALSE(ParsePiOrOrinNumber("pi"));
+ EXPECT_FALSE(ParsePiOrOrinNumber("pi-"));
+ EXPECT_FALSE(ParsePiOrOrinNumber("pi-971"));
+
+ EXPECT_FALSE(ParsePiOrOrinNumber("orin-8971-"));
+ EXPECT_FALSE(ParsePiOrOrinNumber("orin"));
+ EXPECT_FALSE(ParsePiOrOrinNumber("orin-"));
+ EXPECT_FALSE(ParsePiOrOrinNumber("orin-971"));
}
} // namespace aos::network::testing
diff --git a/frc971/orin/BUILD b/frc971/orin/BUILD
index 75e9dfc..6383423 100644
--- a/frc971/orin/BUILD
+++ b/frc971/orin/BUILD
@@ -43,6 +43,7 @@
#"-DDEBUG=1",
],
features = ["cuda"],
+ visibility = ["//visibility:public"],
deps = [
"//aos/time",
"//third_party:cudart",
@@ -127,16 +128,21 @@
],
)
-cc_binary(
- name = "gpu_apriltag",
+cc_library(
+ name = "gpu_apriltag_lib",
srcs = ["gpu_apriltag.cc"],
+ hdrs = ["gpu_apriltag.h"],
features = ["cuda"],
visibility = ["//visibility:public"],
deps = [
":apriltag",
"//aos:init",
"//aos/events:shm_event_loop",
+ "//frc971/constants:constants_sender_lib",
+ "//frc971/vision:calibration_fbs",
"//frc971/vision:charuco_lib",
+ "//frc971/vision:target_mapper",
+ "//frc971/vision:vision_util_lib",
"//third_party:opencv",
],
)
diff --git a/frc971/orin/argus_camera.cc b/frc971/orin/argus_camera.cc
index f5a3e13..37f7e7d 100644
--- a/frc971/orin/argus_camera.cc
+++ b/frc971/orin/argus_camera.cc
@@ -278,7 +278,8 @@
ret = NvBufSurfaceMapEglImage(surf_[i], 0);
// This check typically fails from having X forwarding enabled.
// Always call argus_camera without X forwarding.
- CHECK(ret == 0) << ": NvBufSurfaceMapEglImage failed";
+ CHECK(ret == 0) << ": NvBufSurfaceMapEglImage failed. Make sure X "
+ "forwarding is not enabled.";
egl_images_[i] = surf_[i]->surfaceList[0].mappedAddr.eglImage;
CHECK(egl_images_[i] != EGL_NO_IMAGE_KHR)
diff --git a/frc971/orin/gpu_apriltag.cc b/frc971/orin/gpu_apriltag.cc
index 4951cb6..1642aa3 100644
--- a/frc971/orin/gpu_apriltag.cc
+++ b/frc971/orin/gpu_apriltag.cc
@@ -1,27 +1,80 @@
+#include "frc971/orin/gpu_apriltag.h"
+
#include <chrono>
#include "third_party/apriltag/apriltag.h"
+#include "third_party/apriltag/apriltag_pose.h"
#include "third_party/apriltag/tag16h5.h"
+#include "third_party/apriltag/tag36h11.h"
+#include <opencv2/highgui.hpp>
#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "aos/logging/logging.h"
#include "aos/realtime.h"
+#include "frc971/constants/constants_sender_lib.h"
#include "frc971/orin/apriltag.h"
+#include "frc971/vision/calibration_generated.h"
#include "frc971/vision/charuco_lib.h"
-
-DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
-DEFINE_string(channel, "/camera1", "Channel name");
-DEFINE_double(min_decision_margin, 50.0,
- "Minimum decision margin (confidence) for an apriltag detection");
+#include "frc971/vision/vision_util_lib.h"
DEFINE_bool(debug, false, "If true, write debug images.");
+DEFINE_double(
+ max_expected_distortion, 0.314,
+ "Maximum expected value for unscaled distortion factors. Will scale "
+ "distortion factors so that this value (and a higher distortion) maps to "
+ "1.0.");
+DEFINE_double(min_decision_margin, 50.0,
+ "Minimum decision margin (confidence) for an apriltag detection");
+DEFINE_int32(pixel_border, 10,
+ "Size of image border within which to reject detected corners");
+DEFINE_uint64(pose_estimation_iterations, 50,
+ "Number of iterations for apriltag pose estimation.");
namespace frc971::apriltag {
+// Set max age on image for processing at 20 ms. For 60Hz, we should be
+// processing at least every 16.7ms
+constexpr aos::monotonic_clock::duration kMaxImageAge =
+ std::chrono::milliseconds(20);
+
namespace chrono = std::chrono;
-apriltag_detector_t *MakeTagDetector(apriltag_family_t *tag_family) {
+ApriltagDetector::ApriltagDetector(
+ aos::EventLoop *event_loop, std::string_view channel_name,
+ const frc971::vision::calibration::CameraCalibration *calibration,
+ size_t width, size_t height)
+ : tag_family_(tag36h11_create()),
+ tag_detector_(MakeTagDetector(tag_family_)),
+ gpu_detector_(width, height, tag_detector_),
+ node_name_(event_loop->node()->name()->string_view()),
+ calibration_(calibration),
+ image_callback_(
+ event_loop, channel_name,
+ [this](cv::Mat image_color_mat,
+ const aos::monotonic_clock::time_point eof) {
+ HandleImage(image_color_mat, eof);
+ },
+ kMaxImageAge),
+ target_map_sender_(
+ event_loop->MakeSender<frc971::vision::TargetMap>("/camera")),
+ image_annotations_sender_(
+ event_loop->MakeSender<foxglove::ImageAnnotations>("/camera")),
+ rejections_(0) {
+ image_callback_.set_format(frc971::vision::ImageCallback::Format::YUYV2);
+
+ extrinsics_ = frc971::vision::CameraExtrinsics(calibration_);
+ intrinsics_ = frc971::vision::CameraIntrinsics(calibration_);
+ dist_coeffs_ = frc971::vision::CameraDistCoeffs(calibration_);
+}
+
+ApriltagDetector::~ApriltagDetector() {
+ apriltag_detector_destroy(tag_detector_);
+ free(tag_family_);
+}
+
+apriltag_detector_t *ApriltagDetector::MakeTagDetector(
+ apriltag_family_t *tag_family) {
apriltag_detector_t *tag_detector = apriltag_detector_create();
apriltag_detector_add_family_bits(tag_detector, tag_family, 1);
@@ -34,94 +87,310 @@
return tag_detector;
}
-class Detector {
- public:
- Detector(aos::EventLoop *event_loop, std::string_view channel_name,
- size_t width = 1456, size_t height = 1088)
- : tag_family_(tag16h5_create()),
- tag_detector_(MakeTagDetector(tag_family_)),
- gpu_detector_(width, height, tag_detector_),
- image_callback_(
- event_loop, channel_name,
- [&](cv::Mat image_color_mat,
- const aos::monotonic_clock::time_point eof) {
- HandleImage(image_color_mat, eof);
- },
- chrono::milliseconds(45)) {
- image_callback_.set_format(frc971::vision::ImageCallback::Format::YUYV2);
+flatbuffers::Offset<frc971::vision::TargetPoseFbs>
+ApriltagDetector::BuildTargetPose(const Detection &detection,
+ flatbuffers::FlatBufferBuilder *fbb) {
+ const auto T =
+ Eigen::Translation3d(detection.pose.t->data[0], detection.pose.t->data[1],
+ detection.pose.t->data[2]);
+ const auto position_offset =
+ frc971::vision::CreatePosition(*fbb, T.x(), T.y(), T.z());
+
+ // Aprilrobotics stores the rotation matrix in row-major order
+ const auto orientation = Eigen::Quaterniond(
+ Eigen::Matrix<double, 3, 3, Eigen::RowMajor>(detection.pose.R->data));
+ const auto orientation_offset = frc971::vision::CreateQuaternion(
+ *fbb, orientation.w(), orientation.x(), orientation.y(), orientation.z());
+
+ return frc971::vision::CreateTargetPoseFbs(
+ *fbb, detection.det.id, position_offset, orientation_offset,
+ detection.det.decision_margin, detection.pose_error,
+ detection.distortion_factor, detection.pose_error_ratio);
+}
+
+void ApriltagDetector::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);
+ }
+ }
+}
+
+double ApriltagDetector::ComputeDistortionFactor(
+ const std::vector<cv::Point2f> &orig_corners,
+ const std::vector<cv::Point2f> &corners) {
+ CHECK_EQ(orig_corners.size(), 4ul);
+ CHECK_EQ(corners.size(), 4ul);
+
+ double avg_distance = 0.0;
+ for (size_t i = 0; i < corners.size(); i++) {
+ avg_distance += cv::norm(orig_corners[i] - corners[i]);
+ }
+ avg_distance /= corners.size();
+
+ // Normalize avg_distance by dividing by the image diagonal,
+ // and then the maximum expected distortion
+ double distortion_factor =
+ avg_distance /
+ cv::norm(cv::Point2d(image_size_.width, image_size_.height));
+ return std::min(distortion_factor / FLAGS_max_expected_distortion, 1.0);
+}
+
+std::vector<cv::Point2f> ApriltagDetector::MakeCornerVector(
+ const apriltag_detection_t *det) {
+ std::vector<cv::Point2f> corner_points;
+ corner_points.emplace_back(det->p[0][0], det->p[0][1]);
+ corner_points.emplace_back(det->p[1][0], det->p[1][1]);
+ corner_points.emplace_back(det->p[2][0], det->p[2][1]);
+ corner_points.emplace_back(det->p[3][0], det->p[3][1]);
+
+ return corner_points;
+}
+
+void ApriltagDetector::DestroyPose(apriltag_pose_t *pose) const {
+ matd_destroy(pose->R);
+ matd_destroy(pose->t);
+}
+
+void ApriltagDetector::HandleImage(cv::Mat color_image,
+ aos::monotonic_clock::time_point eof) {
+ const aos::monotonic_clock::time_point start_time =
+ aos::monotonic_clock::now();
+ gpu_detector_.Detect(color_image.data);
+ image_size_ = color_image.size();
+ cv::Mat image_copy;
+ if (FLAGS_visualize) {
+ // TODO: Need to figure out how to extract displayable color image from this
+ image_copy = color_image.clone();
}
- virtual ~Detector() {
- apriltag_detector_destroy(tag_detector_);
- free(tag_family_);
- }
+ const zarray_t *detections = gpu_detector_.Detections();
- void HandleImage(cv::Mat color_image,
- aos::monotonic_clock::time_point /*eof*/) {
- const aos::monotonic_clock::time_point start_time =
- aos::monotonic_clock::now();
- gpu_detector_.Detect(color_image.data);
+ aos::monotonic_clock::time_point end_time = aos::monotonic_clock::now();
- const zarray_t *detections = gpu_detector_.Detections();
+ const uint32_t min_x = FLAGS_pixel_border;
+ const uint32_t max_x = color_image.cols - FLAGS_pixel_border;
+ const uint32_t min_y = FLAGS_pixel_border;
+ const uint32_t max_y = color_image.rows - FLAGS_pixel_border;
- const aos::monotonic_clock::time_point end_time =
- aos::monotonic_clock::now();
- for (int i = 0; i < zarray_size(detections); ++i) {
- const apriltag_detection_t *gpu_detection;
+ // Define variables for storing / visualizing the output
+ std::vector<Detection> results;
+ auto builder = image_annotations_sender_.MakeBuilder();
+ std::vector<flatbuffers::Offset<foxglove::PointsAnnotation>> foxglove_corners;
- zarray_get(detections, i, &gpu_detection);
+ for (int i = 0; i < zarray_size(detections); ++i) {
+ apriltag_detection_t *gpu_detection;
- bool valid = gpu_detection->decision_margin > FLAGS_min_decision_margin;
+ zarray_get(detections, i, &gpu_detection);
- if (valid) {
- AOS_LOG(
- INFO,
- "Found GPU %s tag number %d hamming %d margin %f (%f, %f), (%f, "
- "%f), (%f, %f), (%f, %f) in %f ms\n",
- valid ? "valid" : "invalid", gpu_detection->id,
- gpu_detection->hamming, gpu_detection->decision_margin,
- gpu_detection->p[0][0], gpu_detection->p[0][1],
- gpu_detection->p[1][0], gpu_detection->p[1][1],
- gpu_detection->p[2][0], gpu_detection->p[2][1],
- gpu_detection->p[3][0], gpu_detection->p[3][1],
- std::chrono::duration<float, std::milli>(end_time - start_time)
- .count());
+ bool valid = gpu_detection->decision_margin > FLAGS_min_decision_margin;
+
+ if (valid) {
+ // Reject tags that are too close to the boundary, since they often
+ // lead to corrupt matches since part of the tag is cut off
+ if (gpu_detection->p[0][0] < min_x || gpu_detection->p[0][0] > max_x ||
+ gpu_detection->p[1][0] < min_x || gpu_detection->p[1][0] > max_x ||
+ gpu_detection->p[2][0] < min_x || gpu_detection->p[2][0] > max_x ||
+ gpu_detection->p[3][0] < min_x || gpu_detection->p[3][0] > max_x ||
+ gpu_detection->p[0][1] < min_y || gpu_detection->p[0][1] > max_y ||
+ gpu_detection->p[1][1] < min_y || gpu_detection->p[1][1] > max_y ||
+ gpu_detection->p[2][1] < min_y || gpu_detection->p[2][1] > max_y ||
+ gpu_detection->p[3][1] < min_y || gpu_detection->p[3][1] > max_y) {
+ VLOG(1) << "Rejecting detection because corner is outside pixel border";
+
+ // 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;
}
+
+ AOS_LOG(INFO,
+ "Found GPU %s tag number %d hamming %d margin %f (%f, %f), (%f, "
+ "%f), (%f, %f), (%f, %f) in %f ms\n",
+ valid ? "valid" : "invalid", gpu_detection->id,
+ gpu_detection->hamming, gpu_detection->decision_margin,
+ gpu_detection->p[0][0], gpu_detection->p[0][1],
+ gpu_detection->p[1][0], gpu_detection->p[1][1],
+ gpu_detection->p[2][0], gpu_detection->p[2][1],
+ gpu_detection->p[3][0], gpu_detection->p[3][1],
+ std::chrono::duration<float, std::milli>(end_time - start_time)
+ .count());
+
+ VLOG(1) << "Found tag number " << gpu_detection->id
+ << " hamming: " << gpu_detection->hamming
+ << " margin: " << gpu_detection->decision_margin;
+
+ // First create an apriltag_detection_info_t struct using your known
+ // parameters.
+ apriltag_detection_info_t info;
+ info.det = gpu_detection;
+ 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);
+ info.cy = intrinsics_.at<double>(1, 2);
+
+ // Send original corner points in green
+ std::vector<cv::Point2f> orig_corner_points =
+ MakeCornerVector(gpu_detection);
+ foxglove_corners.push_back(frc971::vision::BuildPointsAnnotation(
+ builder.fbb(), eof, orig_corner_points,
+ std::vector<double>{0.0, 1.0, 0.0, 0.5}));
+
+ UndistortDetection(gpu_detection);
+
+ const aos::monotonic_clock::time_point before_pose_estimation =
+ aos::monotonic_clock::now();
+
+ apriltag_pose_t pose_1;
+ apriltag_pose_t pose_2;
+ double pose_error_1;
+ double pose_error_2;
+ estimate_tag_pose_orthogonal_iteration(&info, &pose_error_1, &pose_1,
+ &pose_error_2, &pose_2,
+ FLAGS_pose_estimation_iterations);
+
+ const aos::monotonic_clock::time_point after_pose_estimation =
+ aos::monotonic_clock::now();
+ VLOG(1) << "Took "
+ << chrono::duration<double>(after_pose_estimation -
+ before_pose_estimation)
+ .count()
+ << " seconds for pose estimation";
+ VLOG(1) << "Pose err 1: " << pose_error_1;
+ VLOG(1) << "Pose err 2: " << pose_error_2;
+
+ // Send undistorted corner points in pink
+ std::vector<cv::Point2f> corner_points = MakeCornerVector(gpu_detection);
+ foxglove_corners.push_back(frc971::vision::BuildPointsAnnotation(
+ builder.fbb(), eof, corner_points,
+ std::vector<double>{1.0, 0.75, 0.8, 1.0}));
+
+ double distortion_factor =
+ ComputeDistortionFactor(orig_corner_points, corner_points);
+
+ // We get two estimates for poses.
+ // Choose the one with the lower pose estimation error
+ bool use_pose_1 = (pose_error_1 < pose_error_2);
+ auto best_pose = (use_pose_1 ? pose_1 : pose_2);
+ auto secondary_pose = (use_pose_1 ? pose_2 : pose_1);
+ double best_pose_error = (use_pose_1 ? pose_error_1 : pose_error_2);
+ double secondary_pose_error = (use_pose_1 ? pose_error_2 : pose_error_1);
+
+ CHECK_NE(best_pose_error, std::numeric_limits<double>::infinity())
+ << "Got no valid pose estimations, this should not be possible.";
+ double pose_error_ratio = best_pose_error / secondary_pose_error;
+
+ // Destroy the secondary pose if we got one
+ if (secondary_pose_error != std::numeric_limits<double>::infinity()) {
+ DestroyPose(&secondary_pose);
+ }
+
+ results.emplace_back(Detection{.det = *gpu_detection,
+ .pose = best_pose,
+ .pose_error = best_pose_error,
+ .distortion_factor = distortion_factor,
+ .pose_error_ratio = pose_error_ratio});
+
+ if (FLAGS_visualize) {
+ // Draw raw (distorted) corner points in green
+ cv::line(image_copy, orig_corner_points[0], orig_corner_points[1],
+ cv::Scalar(0, 255, 0), 2);
+ cv::line(image_copy, orig_corner_points[1], orig_corner_points[2],
+ cv::Scalar(0, 255, 0), 2);
+ cv::line(image_copy, orig_corner_points[2], orig_corner_points[3],
+ cv::Scalar(0, 255, 0), 2);
+ cv::line(image_copy, orig_corner_points[3], orig_corner_points[0],
+ cv::Scalar(0, 255, 0), 2);
+
+ // Draw undistorted corner points in red
+ cv::line(image_copy, corner_points[0], corner_points[1],
+ cv::Scalar(0, 0, 255), 2);
+ cv::line(image_copy, corner_points[2], corner_points[1],
+ cv::Scalar(0, 0, 255), 2);
+ cv::line(image_copy, corner_points[2], corner_points[3],
+ cv::Scalar(0, 0, 255), 2);
+ cv::line(image_copy, corner_points[0], corner_points[3],
+ cv::Scalar(0, 0, 255), 2);
+ }
+
+ VLOG(1) << "Found tag number " << gpu_detection->id
+ << " hamming: " << gpu_detection->hamming
+ << " margin: " << gpu_detection->decision_margin;
+ } else {
+ rejections_++;
}
}
- apriltag_family_t *tag_family_;
- apriltag_detector_t *tag_detector_;
+ if (FLAGS_visualize) {
+ // Display the result
+ // Rotate by 180 degrees to make it upright
+ // TODO: Make this an option?
+ bool flip_image_ = true;
+ if (flip_image_) {
+ cv::rotate(image_copy, image_copy, 1);
+ }
+ // TODO: Need to fix image display to handle YUYV images
+ // cv::imshow(absl::StrCat("ApriltagDetector Image ", node_name_),
+ // color_image);
+ // cv::waitKey(1);
+ }
- GpuDetector gpu_detector_;
+ const auto corners_offset = builder.fbb()->CreateVector(foxglove_corners);
+ foxglove::ImageAnnotations::Builder annotation_builder(*builder.fbb());
+ annotation_builder.add_points(corners_offset);
+ builder.CheckOk(builder.Send(annotation_builder.Finish()));
- frc971::vision::ImageCallback image_callback_;
-};
+ auto map_builder = target_map_sender_.MakeBuilder();
+ std::vector<flatbuffers::Offset<frc971::vision::TargetPoseFbs>> target_poses;
+ for (auto &detection : results) {
+ auto *fbb = map_builder.fbb();
+ auto pose = BuildTargetPose(detection, fbb);
+ DestroyPose(&detection.pose);
+ target_poses.emplace_back(pose);
+ }
+ const auto target_poses_offset =
+ map_builder.fbb()->CreateVector(target_poses);
+ auto target_map_builder =
+ map_builder.MakeBuilder<frc971::vision::TargetMap>();
+ target_map_builder.add_target_poses(target_poses_offset);
+ target_map_builder.add_monotonic_timestamp_ns(eof.time_since_epoch().count());
+ target_map_builder.add_rejections(rejections_);
+ map_builder.CheckOk(map_builder.Send(target_map_builder.Finish()));
-void AprilViewerMain() {
- aos::FlatbufferDetachedBuffer<aos::Configuration> config =
- aos::configuration::ReadConfig(FLAGS_config);
+ // TODO: Do we need to clean this up?
+ // apriltag_detections_destroy(detections);
- aos::ShmEventLoop event_loop(&config.message());
+ end_time = aos::monotonic_clock::now();
- Detector detector(&event_loop, FLAGS_channel);
+ if (FLAGS_debug) {
+ timeprofile_display(tag_detector_->tp);
+ }
- // TODO(austin): Figure out our core pinning strategy.
- // event_loop.SetRuntimeAffinity(aos::MakeCpusetFromCpus({5}));
+ VLOG(1) << "Took " << chrono::duration<double>(end_time - start_time).count()
+ << " seconds to detect overall";
- struct sched_param param;
- param.sched_priority = 21;
- PCHECK(sched_setscheduler(0, SCHED_FIFO, ¶m) == 0);
-
- // TODO(austin): Pre-warm it...
- event_loop.Run();
+ return;
+ // TODO: Need to have proper return here
+ // return {.detections = results, .rejections = rejections_};
}
} // namespace frc971::apriltag
-
-int main(int argc, char **argv) {
- aos::InitGoogle(&argc, &argv);
- frc971::apriltag::AprilViewerMain();
-
- return 0;
-}
diff --git a/frc971/orin/gpu_apriltag.h b/frc971/orin/gpu_apriltag.h
new file mode 100644
index 0000000..09c8579
--- /dev/null
+++ b/frc971/orin/gpu_apriltag.h
@@ -0,0 +1,93 @@
+
+#include <string>
+
+#include "Eigen/Dense"
+#include "opencv2/core/eigen.hpp"
+#include "opencv2/imgproc.hpp"
+#include "third_party/apriltag/apriltag.h"
+#include "third_party/apriltag/apriltag_pose.h"
+#include "third_party/apriltag/tag16h5.h"
+#include "third_party/apriltag/tag36h11.h"
+
+#include "aos/events/event_loop.h"
+#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/orin/apriltag.h"
+#include "frc971/vision/calibration_generated.h"
+#include "frc971/vision/charuco_lib.h"
+#include "frc971/vision/target_map_generated.h"
+#include "frc971/vision/target_mapper.h"
+#include "frc971/vision/vision_generated.h"
+#include "frc971/vision/visualize_robot.h"
+
+namespace frc971 {
+namespace apriltag {
+
+class ApriltagDetector {
+ public:
+ // Aprilrobotics representation of a tag detection
+ struct Detection {
+ apriltag_detection_t det;
+ apriltag_pose_t pose;
+ double pose_error;
+ double distortion_factor;
+ double pose_error_ratio;
+ };
+
+ // Class to detect Apriltags in images and publish poses of the tags
+ ApriltagDetector(
+ aos::EventLoop *event_loop, std::string_view channel_name,
+ const frc971::vision::calibration::CameraCalibration *calibration,
+ size_t width = 1456, size_t height = 1088);
+
+ ~ApriltagDetector();
+
+ // Creates the GPU-based Apriltag detector.
+ apriltag_detector_t *MakeTagDetector(apriltag_family_t *tag_family);
+
+ // Builds the output TargetPose flatbuffer from Apriltag detection.
+ flatbuffers::Offset<frc971::vision::TargetPoseFbs> BuildTargetPose(
+ const Detection &detection, flatbuffers::FlatBufferBuilder *fbb);
+
+ // Undistorts the april tag corners using the camera calibration.
+ // Returns the detections in place.
+ void 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
+ // corners).
+ double ComputeDistortionFactor(const std::vector<cv::Point2f> &orig_corners,
+ const std::vector<cv::Point2f> &corners);
+
+ // Helper function to store detection points in vector of Point2f's.
+ std::vector<cv::Point2f> MakeCornerVector(const apriltag_detection_t *det);
+
+ // Deletes the heap-allocated rotation and translation pointers in the given
+ // pose.
+ void DestroyPose(apriltag_pose_t *pose) const;
+
+ void HandleImage(cv::Mat color_image, aos::monotonic_clock::time_point eof);
+
+ private:
+ apriltag_family_t *tag_family_;
+ apriltag_detector_t *tag_detector_;
+ frc971::apriltag::GpuDetector gpu_detector_;
+ std::string_view node_name_;
+
+ const frc971::vision::calibration::CameraCalibration *calibration_;
+ cv::Mat intrinsics_;
+ cv::Mat projection_matrix_;
+ std::optional<cv::Mat> extrinsics_;
+ cv::Mat dist_coeffs_;
+ cv::Size image_size_;
+
+ frc971::vision::ImageCallback image_callback_;
+ aos::Sender<frc971::vision::TargetMap> target_map_sender_;
+ aos::Sender<foxglove::ImageAnnotations> image_annotations_sender_;
+ size_t rejections_;
+};
+
+} // namespace apriltag
+} // namespace frc971
diff --git a/frc971/vision/BUILD b/frc971/vision/BUILD
index 932261d..5d8bf7c 100644
--- a/frc971/vision/BUILD
+++ b/frc971/vision/BUILD
@@ -357,3 +357,15 @@
"@org_tuxfamily_eigen//:eigen",
],
)
+
+cc_library(
+ name = "vision_util_lib",
+ srcs = ["vision_util_lib.cc"],
+ hdrs = ["vision_util_lib.h"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/vision:calibration_fbs",
+ "//third_party:opencv",
+ "@com_github_google_glog//:glog",
+ ],
+)
diff --git a/frc971/vision/calibration_accumulator.cc b/frc971/vision/calibration_accumulator.cc
index d8abd33..2c0e603 100644
--- a/frc971/vision/calibration_accumulator.cc
+++ b/frc971/vision/calibration_accumulator.cc
@@ -163,11 +163,13 @@
HandleCharuco(rgb_image, eof, charuco_ids, charuco_corners, valid,
rvecs_eigen, tvecs_eigen);
}),
+ // TODO: Need to make this work for pi or orin
image_callback_(
image_event_loop_,
- absl::StrCat("/pi",
- std::to_string(aos::network::ParsePiNumber(pi).value()),
- image_channel),
+ absl::StrCat(
+ "/pi",
+ std::to_string(aos::network::ParsePiOrOrinNumber(pi).value()),
+ image_channel),
[this](cv::Mat rgb_image, const monotonic_clock::time_point eof) {
charuco_extractor_.HandleImage(rgb_image, eof);
}),
diff --git a/frc971/vision/intrinsics_calibration.cc b/frc971/vision/intrinsics_calibration.cc
index b4d3825..f98aadb 100644
--- a/frc971/vision/intrinsics_calibration.cc
+++ b/frc971/vision/intrinsics_calibration.cc
@@ -35,7 +35,7 @@
std::string hostname = FLAGS_pi;
if (hostname == "") {
hostname = aos::network::GetHostname();
- LOG(INFO) << "Using pi name from hostname as " << hostname;
+ LOG(INFO) << "Using pi/orin name from hostname as " << hostname;
}
CHECK(!FLAGS_base_intrinsics.empty())
<< "Need a base intrinsics json to use to auto-capture images when the "
diff --git a/frc971/vision/intrinsics_calibration_lib.cc b/frc971/vision/intrinsics_calibration_lib.cc
index ab03e01..49224f0 100644
--- a/frc971/vision/intrinsics_calibration_lib.cc
+++ b/frc971/vision/intrinsics_calibration_lib.cc
@@ -7,7 +7,7 @@
std::string_view base_intrinsics_file, bool display_undistorted,
std::string_view calibration_folder, aos::ExitHandle *exit_handle)
: pi_(pi),
- pi_number_(aos::network::ParsePiNumber(pi)),
+ pi_number_(aos::network::ParsePiOrOrinNumber(pi)),
camera_id_(camera_id),
prev_H_camera_board_(Eigen::Affine3d()),
prev_image_H_camera_board_(Eigen::Affine3d()),
@@ -25,11 +25,13 @@
HandleCharuco(rgb_image, eof, charuco_ids, charuco_corners, valid,
rvecs_eigen, tvecs_eigen);
}),
+ // TODO: Need to make this work with /pi or /orin
image_callback_(
event_loop,
- absl::StrCat("/pi",
- std::to_string(aos::network::ParsePiNumber(pi).value()),
- "/camera"),
+ absl::StrCat(
+ "/pi",
+ std::to_string(aos::network::ParsePiOrOrinNumber(pi).value()),
+ "/camera"),
[this](cv::Mat rgb_image,
const aos::monotonic_clock::time_point eof) {
charuco_extractor_.HandleImage(rgb_image, eof);
@@ -213,7 +215,7 @@
const aos::realtime_clock::time_point realtime_now =
aos::realtime_clock::now();
std::optional<uint16_t> team_number =
- aos::network::team_number_internal::ParsePiTeamNumber(pi_);
+ aos::network::team_number_internal::ParsePiOrOrinTeamNumber(pi_);
CHECK(team_number) << ": Invalid pi hostname " << pi_
<< ", failed to parse team number";
aos::FlatbufferDetachedBuffer<calibration::CameraCalibration>
diff --git a/frc971/vision/vision_util_lib.cc b/frc971/vision/vision_util_lib.cc
new file mode 100644
index 0000000..b65e883
--- /dev/null
+++ b/frc971/vision/vision_util_lib.cc
@@ -0,0 +1,46 @@
+#include "frc971/vision/vision_util_lib.h"
+
+#include "glog/logging.h"
+
+namespace frc971::vision {
+
+std::optional<cv::Mat> CameraExtrinsics(
+ const frc971::vision::calibration::CameraCalibration *camera_calibration) {
+ CHECK(!camera_calibration->has_turret_extrinsics())
+ << "Turret not currently supported";
+
+ if (!camera_calibration->has_fixed_extrinsics()) {
+ return std::nullopt;
+ }
+ CHECK(camera_calibration->fixed_extrinsics()->has_data());
+ 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 frc971::vision
diff --git a/frc971/vision/vision_util_lib.h b/frc971/vision/vision_util_lib.h
new file mode 100644
index 0000000..6fb32eb
--- /dev/null
+++ b/frc971/vision/vision_util_lib.h
@@ -0,0 +1,22 @@
+#ifndef FRC971_VISION_VISION_UTIL_LIB_H_
+#define FRC971_VISION_VISION_UTIL_LIB_H_
+#include <optional>
+#include <string_view>
+
+#include "opencv2/imgproc.hpp"
+
+#include "frc971/vision/calibration_generated.h"
+
+namespace frc971::vision {
+std::optional<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 frc971::vision
+
+#endif // FRC971_VISION_VISION_UTIL_LIB_H_
diff --git a/y2020/vision/extrinsics_calibration.cc b/y2020/vision/extrinsics_calibration.cc
index deda8c3..ddbe9af 100644
--- a/y2020/vision/extrinsics_calibration.cc
+++ b/y2020/vision/extrinsics_calibration.cc
@@ -45,7 +45,8 @@
const aos::Node *const roborio_node =
aos::configuration::GetNode(factory.configuration(), "roborio");
- std::optional<uint16_t> pi_number = aos::network::ParsePiNumber(FLAGS_pi);
+ std::optional<uint16_t> pi_number =
+ aos::network::ParsePiOrOrinNumber(FLAGS_pi);
CHECK(pi_number);
LOG(INFO) << "Pi " << *pi_number;
const aos::Node *const pi_node = aos::configuration::GetNode(
diff --git a/y2022/vision/calibrate_extrinsics.cc b/y2022/vision/calibrate_extrinsics.cc
index 0a950ab..7b53d67 100644
--- a/y2022/vision/calibrate_extrinsics.cc
+++ b/y2022/vision/calibrate_extrinsics.cc
@@ -35,7 +35,8 @@
void Main(int argc, char **argv) {
CalibrationData data;
- std::optional<uint16_t> pi_number = aos::network::ParsePiNumber(FLAGS_pi);
+ std::optional<uint16_t> pi_number =
+ aos::network::ParsePiOrOrinNumber(FLAGS_pi);
CHECK(pi_number);
const std::string pi_name = absl::StrCat("pi", *pi_number);
LOG(INFO) << "Pi " << *pi_number;
diff --git a/y2023/vision/aprilrobotics.cc b/y2023/vision/aprilrobotics.cc
index 0605eb2..342e046 100644
--- a/y2023/vision/aprilrobotics.cc
+++ b/y2023/vision/aprilrobotics.cc
@@ -24,6 +24,11 @@
namespace chrono = std::chrono;
+// Set max age on image for processing at 20 ms. For 60Hz, we should be
+// processing at least every 16.7ms
+constexpr aos::monotonic_clock::duration kMaxImageAge =
+ std::chrono::milliseconds(20);
+
AprilRoboticsDetector::AprilRoboticsDetector(aos::EventLoop *event_loop,
std::string_view channel_name,
bool flip_image)
@@ -34,11 +39,11 @@
ftrace_(),
image_callback_(
event_loop, channel_name,
- [&](cv::Mat image_color_mat,
- const aos::monotonic_clock::time_point eof) {
+ [this](cv::Mat image_color_mat,
+ const aos::monotonic_clock::time_point eof) {
HandleImage(image_color_mat, eof);
},
- chrono::milliseconds(5)),
+ kMaxImageAge),
target_map_sender_(
event_loop->MakeSender<frc971::vision::TargetMap>("/camera")),
image_annotations_sender_(
diff --git a/y2023/vision/calibrate_extrinsics.cc b/y2023/vision/calibrate_extrinsics.cc
index f0a0d71..59c27e6 100644
--- a/y2023/vision/calibrate_extrinsics.cc
+++ b/y2023/vision/calibrate_extrinsics.cc
@@ -74,7 +74,8 @@
void Main(int argc, char **argv) {
CalibrationData data;
- std::optional<uint16_t> pi_number = aos::network::ParsePiNumber(FLAGS_pi);
+ std::optional<uint16_t> pi_number =
+ aos::network::ParsePiOrOrinNumber(FLAGS_pi);
CHECK(pi_number);
const std::string pi_name = absl::StrCat("pi", *pi_number);
diff --git a/y2024/BUILD b/y2024/BUILD
index b362f6c..148c23d 100644
--- a/y2024/BUILD
+++ b/y2024/BUILD
@@ -53,6 +53,7 @@
"//aos/starter:irq_affinity",
"//aos/util:foxglove_websocket",
"//frc971/image_streamer:image_streamer",
+ "//frc971/vision:intrinsics_calibration",
"//y2023/vision:viewer",
"//y2024/constants:constants_sender",
"//y2024/vision:foxglove_image_converter",
@@ -75,8 +76,8 @@
"//aos/network:web_proxy_main",
"//aos/starter:irq_affinity",
"//frc971/orin:argus_camera",
- "//frc971/orin:gpu_apriltag",
"//y2024/orin:can_logger",
+ "//y2024/vision:apriltag_detector",
"//y2024/vision:image_logger",
],
target_compatible_with = ["//tools/platforms/hardware:raspberry_pi"],
diff --git a/y2024/constants/971.json b/y2024/constants/971.json
index 7e04438..a061f39 100644
--- a/y2024/constants/971.json
+++ b/y2024/constants/971.json
@@ -1,6 +1,11 @@
{% from 'y2024/constants/common.jinja2' import intake_pivot_zero %}
{
+ "cameras": [
+ {
+ "calibration": {% include 'y2024/constants/calib_files/calibration_orin-971-1_cam-24-00.json' %}
+ }
+ ],
"robot": {
"intake_constants": {
{% set _ = intake_pivot_zero.update(
diff --git a/y2024/constants/BUILD b/y2024/constants/BUILD
index 078d616..18e506a 100644
--- a/y2024/constants/BUILD
+++ b/y2024/constants/BUILD
@@ -41,6 +41,7 @@
"9971.json",
"common.jinja2",
"common.json",
+ "//y2024/constants/calib_files",
"//y2024/control_loops/drivetrain:drivetrain_config.json",
"//y2024/control_loops/superstructure/intake_pivot:intake_pivot_json",
"//y2024/vision/maps",
@@ -56,6 +57,7 @@
deps = [
"//frc971/control_loops:profiled_subsystem_fbs",
"//frc971/control_loops/drivetrain:drivetrain_config_fbs",
+ "//frc971/vision:calibration_fbs",
"//frc971/vision:target_map_fbs",
"//frc971/zeroing:constants_fbs",
],
diff --git a/y2024/constants/calib_files/BUILD b/y2024/constants/calib_files/BUILD
new file mode 100644
index 0000000..1f33d50
--- /dev/null
+++ b/y2024/constants/calib_files/BUILD
@@ -0,0 +1,5 @@
+filegroup(
+ name = "calib_files",
+ srcs = glob(["*.json"]),
+ visibility = ["//visibility:public"],
+)
diff --git a/y2024/constants/calib_files/calibration_orin-971-1_cam-24-00.json b/y2024/constants/calib_files/calibration_orin-971-1_cam-24-00.json
new file mode 100644
index 0000000..e083a13
--- /dev/null
+++ b/y2024/constants/calib_files/calibration_orin-971-1_cam-24-00.json
@@ -0,0 +1 @@
+{ "node_name": "orin1", "team_number": 971, "intrinsics": [ 893.617798, 0.0, 612.44397, 0.0, 893.193115, 375.196381, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ -0.483961, 0.220781, 0.84678, 0.176109, 0.868846, 0.005849, 0.495048, -0.231149, 0.104344, 0.975306, -0.194656, 0.550508, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.443805, 0.238734, 0.000133, 0.000448, -0.071068 ], "calibration_timestamp": 1358499779650270322, "camera_id": "23-09" }
diff --git a/y2024/constants/constants.fbs b/y2024/constants/constants.fbs
index ee5ff60..e648df0 100644
--- a/y2024/constants/constants.fbs
+++ b/y2024/constants/constants.fbs
@@ -1,3 +1,4 @@
+include "frc971/vision/calibration.fbs";
include "frc971/vision/target_map.fbs";
include "frc971/control_loops/profiled_subsystem.fbs";
include "frc971/zeroing/constants.fbs";
@@ -5,6 +6,10 @@
namespace y2024;
+table CameraConfiguration {
+ calibration:frc971.vision.calibration.CameraCalibration (id: 0);
+}
+
table ShotParams {
shot_velocity: double (id: 0);
shot_angle: double (id: 1);
@@ -63,6 +68,7 @@
table Constants {
robot:RobotConstants (id: 0);
common:Common (id: 1);
+ cameras:[CameraConfiguration] (id: 2);
}
root_type Constants;
diff --git a/y2024/vision/BUILD b/y2024/vision/BUILD
index 632de2b..795d9fa 100644
--- a/y2024/vision/BUILD
+++ b/y2024/vision/BUILD
@@ -33,3 +33,28 @@
"@com_github_google_glog//:glog",
],
)
+
+cc_binary(
+ name = "apriltag_detector",
+ srcs = [
+ "apriltag_detector.cc",
+ "vision_util.cc",
+ "vision_util.h",
+ ],
+ features = ["cuda"],
+ target_compatible_with = ["@platforms//cpu:arm64"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos:configuration",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//frc971/orin:gpu_apriltag_lib",
+ "//third_party:cudart",
+ "//third_party/apriltag",
+ "//y2024/constants:constants_fbs",
+ "@com_github_gflags_gflags//:gflags",
+ "@com_github_google_glog//:glog",
+ "@com_github_nvidia_cccl//:cccl",
+ "@com_github_nvidia_cuco//:cuco",
+ ],
+)
diff --git a/y2024/vision/apriltag_detector.cc b/y2024/vision/apriltag_detector.cc
new file mode 100644
index 0000000..96ff869
--- /dev/null
+++ b/y2024/vision/apriltag_detector.cc
@@ -0,0 +1,49 @@
+
+#include <string>
+
+#include "aos/init.h"
+#include "frc971/orin/gpu_apriltag.h"
+#include "y2024/constants/constants_generated.h"
+#include "y2024/vision/vision_util.h"
+
+DEFINE_string(channel, "/camera", "Channel name");
+DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
+
+void GpuApriltagDetector() {
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ frc971::constants::WaitForConstants<y2024::Constants>(&config.message());
+
+ aos::ShmEventLoop event_loop(&config.message());
+
+ const frc971::constants::ConstantsFetcher<y2024::Constants> calibration_data(
+ &event_loop);
+
+ const frc971::vision::calibration::CameraCalibration *calibration =
+ y2024::vision::FindCameraCalibration(
+ calibration_data.constants(),
+ event_loop.node()->name()->string_view());
+
+ frc971::apriltag::ApriltagDetector detector(&event_loop, FLAGS_channel,
+ calibration);
+
+ // TODO(austin): Figure out our core pinning strategy.
+ // event_loop.SetRuntimeAffinity(aos::MakeCpusetFromCpus({5}));
+
+ LOG(INFO) << "Setting scheduler priority";
+ struct sched_param param;
+ param.sched_priority = 21;
+ PCHECK(sched_setscheduler(0, SCHED_FIFO, ¶m) == 0);
+
+ LOG(INFO) << "Running event loop";
+ // TODO(austin): Pre-warm it...
+ event_loop.Run();
+} // namespace frc971::apriltag
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+ GpuApriltagDetector();
+
+ return 0;
+}
diff --git a/y2024/vision/vision_util.cc b/y2024/vision/vision_util.cc
new file mode 100644
index 0000000..4c196ce
--- /dev/null
+++ b/y2024/vision/vision_util.cc
@@ -0,0 +1,21 @@
+#include "y2024/vision/vision_util.h"
+
+#include "glog/logging.h"
+
+namespace y2024::vision {
+
+const frc971::vision::calibration::CameraCalibration *FindCameraCalibration(
+ const y2024::Constants &calibration_data, std::string_view node_name) {
+ CHECK(calibration_data.has_cameras());
+ for (const y2024::CameraConfiguration *candidate :
+ *calibration_data.cameras()) {
+ CHECK(candidate->has_calibration());
+ if (candidate->calibration()->node_name()->string_view() != node_name) {
+ continue;
+ }
+ return candidate->calibration();
+ }
+ LOG(FATAL) << ": Failed to find camera calibration for " << node_name;
+}
+
+} // namespace y2024::vision
diff --git a/y2024/vision/vision_util.h b/y2024/vision/vision_util.h
new file mode 100644
index 0000000..d8fa562
--- /dev/null
+++ b/y2024/vision/vision_util.h
@@ -0,0 +1,16 @@
+#ifndef Y2024_VISION_VISION_UTIL_H_
+#define Y2024_VISION_VISION_UTIL_H_
+#include <string_view>
+
+#include "opencv2/imgproc.hpp"
+
+#include "y2024/constants/constants_generated.h"
+
+namespace y2024::vision {
+
+const frc971::vision::calibration::CameraCalibration *FindCameraCalibration(
+ const y2024::Constants &calibration_data, std::string_view node_name);
+
+} // namespace y2024::vision
+
+#endif // Y2024_VISION_VISION_UTIL_H_
diff --git a/y2024/y2024_orin_template.json b/y2024/y2024_orin_template.json
index 04a63f0..6d378f7 100644
--- a/y2024/y2024_orin_template.json
+++ b/y2024/y2024_orin_template.json
@@ -265,8 +265,8 @@
]
},
{
- "name": "gpu_apriltag",
- "executable_name": "gpu_apriltag",
+ "name": "apriltag_detector",
+ "executable_name": "apriltag_detector",
"user": "pi",
"nodes": [
"orin{{ NUM }}"