Remove redundant file from y2024
Change-Id: I95fb03bbb93b03e766ee49bfa20dbba8f979fd7a
Signed-off-by: Yash Maheshwari <yashmahe2018@gmail.com>
diff --git a/y2024/vision/localization_verifier.cc b/y2024/vision/localization_verifier.cc
deleted file mode 100644
index c16f874..0000000
--- a/y2024/vision/localization_verifier.cc
+++ /dev/null
@@ -1,108 +0,0 @@
-#include "glog/logging.h"
-
-#include "aos/init.h"
-#include "frc971/constants/constants_sender_lib.h"
-#include "frc971/control_loops/drivetrain/localization/localizer_output_generated.h"
-#include "frc971/vision/vision_generated.h"
-#include "y2023/localizer/localizer.h"
-#include "y2023/localizer/utils.h"
-
-DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
-
-DEFINE_uint64(min_april_id, 1,
- "Minimum april id to consider as part of the field and ignore");
-DEFINE_uint64(max_april_id, 8,
- "Maximum april id to consider as part of the field and ignore");
-
-// This binary allows us to place extra april tags on the field and verify
-// that we can compute their field pose correctly
-
-namespace y2023::vision {
-
-using localizer::Localizer;
-
-Localizer::Transform LocalizerOutputToTransform(
- const frc971::controls::LocalizerOutput &localizer) {
- const auto T_field_robot =
- Eigen::Translation3d(localizer.x(), localizer.y(), 0.0);
-
- Eigen::AngleAxisd robot_yaw_angle(localizer.theta(),
- Eigen::Vector3d::UnitZ());
- const auto R_field_robot = Eigen::Quaterniond(robot_yaw_angle);
- const Localizer::Transform H_field_robot =
- (T_field_robot * R_field_robot).matrix();
- return H_field_robot;
-}
-
-void HandleDetections(
- const frc971::vision::TargetMap &detections,
- const Localizer::Transform &H_robot_camera,
- aos::Fetcher<frc971::controls::LocalizerOutput> *localizer_fetcher) {
- localizer_fetcher->Fetch();
- CHECK(localizer_fetcher->get());
-
- for (const auto *target_pose : *detections.target_poses()) {
- // Only look at april tags not part of the field
- if (target_pose->id() >= FLAGS_min_april_id &&
- target_pose->id() <= FLAGS_max_april_id) {
- continue;
- }
-
- const Localizer::Transform H_camera_target =
- localizer::PoseToTransform(target_pose);
- const Localizer::Transform H_field_robot =
- LocalizerOutputToTransform(*localizer_fetcher->get());
-
- // Get the field-based target pose using the detection, extrinsics, and
- // localizer output
- const Localizer::Transform H_field_target =
- H_field_robot * H_robot_camera * H_camera_target;
-
- LOG(INFO) << "Field to target " << target_pose->id();
- LOG(INFO) << "H_field_robot " << H_field_robot;
- LOG(INFO) << "H_robot_camera " << H_robot_camera;
- LOG(INFO) << "H_camera_target " << H_camera_target;
- LOG(INFO) << "Transform: " << H_field_target;
- LOG(INFO) << "Translation: "
- << Eigen::Affine3d(H_field_target).translation();
- LOG(INFO);
- }
-}
-
-void LocalizationVerifierMain() {
- aos::FlatbufferDetachedBuffer<aos::Configuration> config =
- aos::configuration::ReadConfig(FLAGS_config);
-
- frc971::constants::WaitForConstants<Constants>(&config.message());
-
- aos::ShmEventLoop event_loop(&config.message());
-
- frc971::constants::ConstantsFetcher<Constants> constants_fetcher(&event_loop);
-
- aos::Fetcher<frc971::controls::LocalizerOutput> localizer_fetcher =
- event_loop.MakeFetcher<frc971::controls::LocalizerOutput>("/localizer");
-
- for (const auto *camera : *constants_fetcher.constants().cameras()) {
- CHECK(camera->has_calibration());
- Localizer::Transform H_robot_camera =
- frc971::control_loops::drivetrain::FlatbufferToTransformationMatrix(
- *camera->calibration()->fixed_extrinsics());
-
- const std::string_view pi_name =
- camera->calibration()->node_name()->string_view();
- event_loop.MakeWatcher(
- absl::StrCat("/", pi_name, "/camera"),
- [H_robot_camera,
- &localizer_fetcher](const frc971::vision::TargetMap &target_map) {
- HandleDetections(target_map, H_robot_camera, &localizer_fetcher);
- });
- }
- event_loop.Run();
-}
-
-} // namespace y2023::vision
-
-int main(int argc, char **argv) {
- aos::InitGoogle(&argc, &argv);
- y2023::vision::LocalizationVerifierMain();
-}