Use aprilrobotics for target mapping
And delete april detection from charuco lib.
Signed-off-by: Yash Chainani <yashchainani28@gmail.com>
Change-Id: I98e6b84d99b23683f0c77c959b8bcc9af9ebc5d4
diff --git a/frc971/vision/charuco_lib.cc b/frc971/vision/charuco_lib.cc
index 80b1fd4..b8d4086 100644
--- a/frc971/vision/charuco_lib.cc
+++ b/frc971/vision/charuco_lib.cc
@@ -230,12 +230,6 @@
marker_length_ = 0.15;
square_length_ = 0.1651;
dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_250);
- } else if (target_type_ == TargetType::kAprilTag) {
- // Tag will be 6 in (15.24 cm) according to
- // https://www.firstinspires.org/robotics/frc/blog/2022-2023-approved-devices-rules-preview-and-vision-target-update
- square_length_ = 0.1524;
- dictionary_ =
- cv::aruco::getPredefinedDictionary(cv::aruco::DICT_APRILTAG_16h5);
} else {
// Bail out if it's not a supported target
LOG(FATAL) << "Target type undefined: "
@@ -435,9 +429,8 @@
<< ", not enough marker IDs for charuco board, got "
<< marker_ids.size() << ", needed " << FLAGS_min_charucos;
}
- } else if (target_type_ == TargetType::kAprilTag ||
- target_type_ == TargetType::kAruco) {
- // estimate pose for april tags doesn't return valid, so marking true
+ } else if (target_type_ == TargetType::kAruco) {
+ // estimate pose for arucos doesn't return valid, so marking true
valid = true;
std::vector<cv::Vec3d> rvecs, tvecs;
cv::aruco::estimatePoseSingleMarkers(marker_corners, square_length_,
diff --git a/frc971/vision/charuco_lib.h b/frc971/vision/charuco_lib.h
index c7269f9..de6d4fb 100644
--- a/frc971/vision/charuco_lib.h
+++ b/frc971/vision/charuco_lib.h
@@ -58,9 +58,7 @@
std::function<void(cv::Mat, aos::monotonic_clock::time_point)>
&&handle_image_fn);
- void set_format(Format format) {
- format_ = format;
- }
+ void set_format(Format format) { format_ = format; }
private:
void DisableTracing();
@@ -80,10 +78,9 @@
// Types of targets that a CharucoExtractor can detect in images
enum class TargetType : uint8_t {
- kAprilTag = 0,
- kAruco = 1,
- kCharuco = 2,
- kCharucoDiamond = 3
+ kAruco = 0,
+ kCharuco = 1,
+ kCharucoDiamond = 2
};
// Class which calls a callback each time an image arrives with the information
diff --git a/frc971/vision/target_map.fbs b/frc971/vision/target_map.fbs
index 37260a6..57627e8 100644
--- a/frc971/vision/target_map.fbs
+++ b/frc971/vision/target_map.fbs
@@ -5,8 +5,8 @@
// AprilTag ID of this target
id:uint64 (id: 0);
- // Pose of target relative to field origin.
- // To get the pose of the target in the field frame, do:
+ // Pose of target relative to either the field origin or robot.
+ // To get the pose of the target, do:
// Translation3d(x, y, z) *
// (AngleAxisd(yaw) * AngleAxisd(pitch) * AngleAxisd(roll))
x:double (id: 1);
@@ -20,12 +20,18 @@
}
// Map of all target poses on a field.
-// This would be solved for by TargetMapper
+// There are two possible uses for this:
+// 1. Static april tag poses on the field solved for by TargetMapper.
+// 2. List of detected april poses relative to the robot.
table TargetMap {
target_poses:[TargetPoseFbs] (id: 0);
- // Unique name of the field
+ // Unique name of the field (for use case 1.)
field_name:string (id: 1);
+
+ // End-of-frame timestamp for the frame with tag detections.
+ // (for use case 2.).
+ monotonic_timestamp_ns:int64 (id: 2);
}
root_type TargetMap;
diff --git a/y2022/vision/calibrate_extrinsics.cc b/y2022/vision/calibrate_extrinsics.cc
index 05d3481..b5005fe 100644
--- a/y2022/vision/calibrate_extrinsics.cc
+++ b/y2022/vision/calibrate_extrinsics.cc
@@ -19,7 +19,7 @@
DEFINE_bool(plot, false, "Whether to plot the resulting data.");
DEFINE_bool(turret, true, "If true, the camera is on the turret");
DEFINE_string(target_type, "charuco",
- "Type of target: april_tag|aruco|charuco|charuco_diamond");
+ "Type of target: aruco|charuco|charuco_diamond");
DEFINE_string(image_channel, "/camera", "Channel to listen for images on");
namespace frc971 {
@@ -68,9 +68,7 @@
factory.MakeEventLoop("calibration", pi_node);
TargetType target_type = TargetType::kCharuco;
- if (FLAGS_target_type == "april_tag") {
- target_type = TargetType::kAprilTag;
- } else if (FLAGS_target_type == "aruco") {
+ if (FLAGS_target_type == "aruco") {
target_type = TargetType::kAruco;
} else if (FLAGS_target_type == "charuco") {
target_type = TargetType::kCharuco;
@@ -78,7 +76,7 @@
target_type = TargetType::kCharucoDiamond;
} else {
LOG(FATAL) << "Unknown target type: " << FLAGS_target_type
- << ", expected: april_tag|aruco|charuco|charuco_diamond";
+ << ", expected: aruco|charuco|charuco_diamond";
}
// Now, hook Calibration up to everything.
diff --git a/y2023/vision/BUILD b/y2023/vision/BUILD
index 55390e4..2dde9a4 100644
--- a/y2023/vision/BUILD
+++ b/y2023/vision/BUILD
@@ -86,6 +86,7 @@
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//y2023:__subpackages__"],
deps = [
+ ":aprilrobotics_lib",
":calibration_data",
"//aos:init",
"//aos/events:simulated_event_loop",
diff --git a/y2023/vision/aprilrobotics.cc b/y2023/vision/aprilrobotics.cc
index 3bf0adf..fc9d799 100644
--- a/y2023/vision/aprilrobotics.cc
+++ b/y2023/vision/aprilrobotics.cc
@@ -15,8 +15,8 @@
ftrace_(),
image_callback_(event_loop, channel_name,
[&](cv::Mat image_color_mat,
- const aos::monotonic_clock::time_point /*eof*/) {
- HandleImage(image_color_mat);
+ const aos::monotonic_clock::time_point eof) {
+ HandleImage(image_color_mat, eof);
}),
target_map_sender_(
event_loop->MakeSender<frc971::vision::TargetMap>("/camera")) {
@@ -66,7 +66,8 @@
}
}
-void AprilRoboticsDetector::HandleImage(cv::Mat image_color_mat) {
+void AprilRoboticsDetector::HandleImage(cv::Mat image_color_mat,
+ aos::monotonic_clock::time_point eof) {
std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>> detections =
DetectTags(image_color_mat);
@@ -78,8 +79,8 @@
}
const auto target_poses_offset = builder.fbb()->CreateVector(target_poses);
auto target_map_builder = 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());
builder.CheckOk(builder.Send(target_map_builder.Finish()));
}
@@ -102,10 +103,10 @@
aos::monotonic_clock::now();
image_u8_t im = {
- .width = image.cols,
- .height = image.rows,
- .stride = image.cols,
- .buf = image.data,
+ .width = image.cols,
+ .height = image.rows,
+ .stride = image.cols,
+ .buf = image.data,
};
ftrace_.FormatMessage("Starting detect\n");
diff --git a/y2023/vision/aprilrobotics.h b/y2023/vision/aprilrobotics.h
index 6aafd75..a68b1d9 100644
--- a/y2023/vision/aprilrobotics.h
+++ b/y2023/vision/aprilrobotics.h
@@ -35,7 +35,7 @@
cv::Mat image);
private:
- void HandleImage(cv::Mat image);
+ void HandleImage(cv::Mat image, aos::monotonic_clock::time_point eof);
flatbuffers::Offset<frc971::vision::TargetPoseFbs> BuildTargetPose(
const apriltag_pose_t &pose,
diff --git a/y2023/vision/target_mapping.cc b/y2023/vision/target_mapping.cc
index d4c8c50..38465c9 100644
--- a/y2023/vision/target_mapping.cc
+++ b/y2023/vision/target_mapping.cc
@@ -12,14 +12,12 @@
#include "opencv2/highgui.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc.hpp"
+#include "y2023/vision/aprilrobotics.h"
#include "y2023/vision/calibration_data.h"
DEFINE_string(json_path, "target_map.json",
"Specify path for json with initial pose guesses.");
-DEFINE_int32(team_number, 7971,
- "Use the calibration for a node with this team number");
-
-DECLARE_string(image_channel);
+DECLARE_int32(team_number);
namespace y2023 {
namespace vision {
@@ -27,6 +25,7 @@
using frc971::vision::DataAdapter;
using frc971::vision::ImageCallback;
using frc971::vision::PoseUtils;
+using frc971::vision::TargetMap;
using frc971::vision::TargetMapper;
namespace calibration = frc971::vision::calibration;
@@ -40,21 +39,19 @@
// Add detected apriltag poses relative to the robot to
// timestamped_target_detections
-void HandleAprilTag(aos::distributed_clock::time_point pi_distributed_time,
- std::vector<cv::Vec4i> april_ids,
- std::vector<Eigen::Vector3d> rvecs_eigen,
- std::vector<Eigen::Vector3d> tvecs_eigen,
+void HandleAprilTag(const TargetMap &map,
+ aos::distributed_clock::time_point pi_distributed_time,
std::vector<DataAdapter::TimestampedDetection>
*timestamped_target_detections,
Eigen::Affine3d extrinsics) {
- for (size_t tag = 0; tag < april_ids.size(); tag++) {
+ for (const auto *target_pose : *map.target_poses()) {
Eigen::Translation3d T_camera_target = Eigen::Translation3d(
- tvecs_eigen[tag][0], tvecs_eigen[tag][1], tvecs_eigen[tag][2]);
- Eigen::AngleAxisd r_angle = Eigen::AngleAxisd(
- rvecs_eigen[tag].norm(), rvecs_eigen[tag] / rvecs_eigen[tag].norm());
- CHECK(rvecs_eigen[tag].norm() != 0) << "rvecs norm = 0; divide by 0";
+ target_pose->x(), target_pose->y(), target_pose->z());
+ Eigen::Quaterniond R_camera_target =
+ PoseUtils::EulerAnglesToQuaternion(Eigen::Vector3d(
+ target_pose->roll(), target_pose->pitch(), target_pose->yaw()));
- Eigen::Affine3d H_camcv_target = T_camera_target * r_angle;
+ Eigen::Affine3d H_camcv_target = T_camera_target * R_camera_target;
// With X, Y, Z being robot axes and x, y, z being camera axes,
// x = -Y, y = -Z, z = X
static const Eigen::Affine3d H_camcv_camrob =
@@ -69,12 +66,15 @@
PoseUtils::Affine3dToPose3d(H_camrob_target);
double distance_from_camera = target_pose_camera.p.norm();
+ CHECK(map.has_monotonic_timestamp_ns())
+ << "Need detection timestamps for mapping";
+
timestamped_target_detections->emplace_back(
DataAdapter::TimestampedDetection{
.time = pi_distributed_time,
.H_robot_target = H_robot_target,
.distance_from_camera = distance_from_camera,
- .id = april_ids[tag][0]});
+ .id = static_cast<TargetMapper::TargetId>(target_pose->id())});
}
}
@@ -115,49 +115,33 @@
// Get images from pi and pass apriltag positions to HandleAprilTag()
void HandlePiCaptures(
- int pi_number, aos::EventLoop *pi_event_loop,
- aos::logger::LogReader *reader,
+ aos::EventLoop *pi_event_loop, aos::logger::LogReader *reader,
std::vector<DataAdapter::TimestampedDetection>
*timestamped_target_detections,
- std::vector<std::unique_ptr<CharucoExtractor>> *charuco_extractors,
- std::vector<std::unique_ptr<ImageCallback>> *image_callbacks) {
+ std::vector<std::unique_ptr<AprilRoboticsDetector>> *detectors) {
const aos::FlatbufferSpan<calibration::CalibrationData> calibration_data(
CalibrationData());
- const calibration::CameraCalibration *calibration = FindCameraCalibration(
- &calibration_data.message(), "pi" + std::to_string(pi_number));
+
+ const auto node_name = pi_event_loop->node()->name()->string_view();
+ const calibration::CameraCalibration *calibration =
+ FindCameraCalibration(&calibration_data.message(), 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";
- charuco_extractors->emplace_back(std::make_unique<CharucoExtractor>(
- pi_event_loop,
- "pi-" + std::to_string(FLAGS_team_number) + "-" +
- std::to_string(pi_number),
- frc971::vision::TargetType::kAprilTag, kImageChannel,
- [=](cv::Mat /*rgb_image*/, aos::monotonic_clock::time_point eof,
- std::vector<cv::Vec4i> april_ids,
- std::vector<std::vector<cv::Point2f>> /*april_corners*/, bool valid,
- std::vector<Eigen::Vector3d> rvecs_eigen,
- std::vector<Eigen::Vector3d> tvecs_eigen) {
- aos::distributed_clock::time_point pi_distributed_time =
- reader->event_loop_factory()
- ->GetNodeEventLoopFactory(pi_event_loop->node())
- ->ToDistributedClock(eof);
+ detectors->emplace_back(
+ std::make_unique<AprilRoboticsDetector>(pi_event_loop, kImageChannel));
- if (valid) {
- HandleAprilTag(pi_distributed_time, april_ids, rvecs_eigen,
- tvecs_eigen, timestamped_target_detections,
- extrinsics);
- }
- }));
+ pi_event_loop->MakeWatcher("/camera", [=](const TargetMap &map) {
+ aos::distributed_clock::time_point pi_distributed_time =
+ reader->event_loop_factory()
+ ->GetNodeEventLoopFactory(pi_event_loop->node())
+ ->ToDistributedClock(aos::monotonic_clock::time_point(
+ aos::monotonic_clock::duration(map.monotonic_timestamp_ns())));
- image_callbacks->emplace_back(std::make_unique<ImageCallback>(
- pi_event_loop, kImageChannel,
- [&, charuco_extractor =
- charuco_extractors->at(charuco_extractors->size() - 1).get()](
- cv::Mat rgb_image, const aos::monotonic_clock::time_point eof) {
- charuco_extractor->HandleImage(rgb_image, eof);
- }));
+ HandleAprilTag(map, pi_distributed_time, timestamped_target_detections,
+ extrinsics);
+ });
}
void MappingMain(int argc, char *argv[]) {
@@ -170,40 +154,35 @@
aos::logger::LogReader reader(aos::logger::SortParts(unsorted_logfiles));
reader.Register();
- std::vector<std::unique_ptr<CharucoExtractor>> charuco_extractors;
- std::vector<std::unique_ptr<ImageCallback>> image_callbacks;
+ std::vector<std::unique_ptr<AprilRoboticsDetector>> detectors;
const aos::Node *pi1 =
aos::configuration::GetNode(reader.configuration(), "pi1");
std::unique_ptr<aos::EventLoop> pi1_event_loop =
reader.event_loop_factory()->MakeEventLoop("pi1", pi1);
- HandlePiCaptures(1, pi1_event_loop.get(), &reader,
- ×tamped_target_detections, &charuco_extractors,
- &image_callbacks);
+ 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);
- HandlePiCaptures(2, pi2_event_loop.get(), &reader,
- ×tamped_target_detections, &charuco_extractors,
- &image_callbacks);
+ 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);
- HandlePiCaptures(3, pi3_event_loop.get(), &reader,
- ×tamped_target_detections, &charuco_extractors,
- &image_callbacks);
+ 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);
- HandlePiCaptures(4, pi4_event_loop.get(), &reader,
- ×tamped_target_detections, &charuco_extractors,
- &image_callbacks);
+ HandlePiCaptures(pi4_event_loop.get(), &reader,
+ ×tamped_target_detections, &detectors);
reader.event_loop_factory()->Run();
@@ -211,15 +190,11 @@
DataAdapter::MatchTargetDetections(timestamped_target_detections);
frc971::vision::TargetMapper mapper(FLAGS_json_path, target_constraints);
- mapper.Solve("rapid_react");
+ mapper.Solve("charged_up");
- // Pointers need to be deleted to destruct all fetchers
- for (auto &charuco_extractor_ptr : charuco_extractors) {
- charuco_extractor_ptr.reset();
- }
-
- for (auto &image_callback_ptr : image_callbacks) {
- image_callback_ptr.reset();
+ // Clean up all the pointers
+ for (auto &detector_ptr : detectors) {
+ detector_ptr.reset();
}
}
diff --git a/y2023/y2023_pi_template.json b/y2023/y2023_pi_template.json
index 4c14031..1b31e91 100644
--- a/y2023/y2023_pi_template.json
+++ b/y2023/y2023_pi_template.json
@@ -182,7 +182,7 @@
"name": "/pi{{ NUM }}/camera",
"type": "frc971.vision.TargetMap",
"source_node": "pi{{ NUM }}",
- "frequency": 80,
+ "frequency": 30,
"num_senders": 2,
"max_size": 40000,
"logger": "LOCAL_AND_REMOTE_LOGGER",