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/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();
}
}