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;