From 8bfc45dcd2936abd304f63d8e2de001d6019f14c Mon Sep 17 00:00:00 2001 From: "carrotmercinary@gmail.com" <167659798+yasen5@users.noreply.github.com> Date: Sat, 18 Apr 2026 23:01:49 +0000 Subject: [PATCH 01/30] Add multi camera detector --- src/camera/camera_constants.cc | 4 + src/camera/camera_constants.h | 1 + src/localization/CMakeLists.txt | 1 + src/localization/multi_camera_detector.cc | 94 +++++++++++++++++++++++ src/localization/multi_camera_detector.h | 32 ++++++++ 5 files changed, 132 insertions(+) create mode 100644 src/localization/multi_camera_detector.cc create mode 100644 src/localization/multi_camera_detector.h diff --git a/src/camera/camera_constants.cc b/src/camera/camera_constants.cc index ea107749..4c3b44cc 100644 --- a/src/camera/camera_constants.cc +++ b/src/camera/camera_constants.cc @@ -89,6 +89,10 @@ auto GetCameraConstants(const std::string& path) -> camera_constants_t { !camera_config["max_payload_size"].is_null()) { camera_constant.max_payload_size = camera_config["max_payload_size"]; } + if (camera_config.contains("use_cpu") && + !camera_config["use_cpu"].is_null()) { + camera_constant.use_cpu = camera_config["use_cpu"]; + } camera_constants.insert({camera_constant.name, camera_constant}); } return camera_constants; diff --git a/src/camera/camera_constants.h b/src/camera/camera_constants.h index 43e066b1..1e163bda 100644 --- a/src/camera/camera_constants.h +++ b/src/camera/camera_constants.h @@ -20,6 +20,7 @@ using camera_constant_t = struct CameraConstant { std::optional max_frame_size = std::nullopt; // uvc only std::optional max_payload_size = std::nullopt; // uvc only std::optional serial_id = std::nullopt; // uvc only + std::optional use_cpu = std::nullopt; // uvc only friend auto operator<<(std::ostream& os, const CameraConstant& c) -> std::ostream& { diff --git a/src/localization/CMakeLists.txt b/src/localization/CMakeLists.txt index 5a3ae30b..438596c8 100644 --- a/src/localization/CMakeLists.txt +++ b/src/localization/CMakeLists.txt @@ -1,4 +1,5 @@ cmake_minimum_required(VERSION 3.10) add_library(localization gpu_apriltag_detector.cc opencv_apriltag_detector.cc run_localization.cc nvidia_apriltag_detector.cc square_solver.cc joint_solver.cc multi_tag_solver.cc position_receiver.cc unambiguous_estimator.cc simulation_sender.cc networktable_sender.cc) +target_sources(localization PRIVATE multi_camera_detector.cc) target_link_libraries(localization PUBLIC 971apriltag utils camera wpilibc wpiutil vpi absl::status) diff --git a/src/localization/multi_camera_detector.cc b/src/localization/multi_camera_detector.cc new file mode 100644 index 00000000..161efa2e --- /dev/null +++ b/src/localization/multi_camera_detector.cc @@ -0,0 +1,94 @@ +#include "src/localization/multi_camera_detector.h" +#include "absl/status/status.h" +#include "src/camera/camera.h" +#include "src/camera/cv_camera.h" +#include "src/camera/uvc_camera.h" +#include "src/localization/gpu_apriltag_detector.h" +#include "src/localization/opencv_apriltag_detector.h" +#include "src/utils/camera_utils.h" +#include "src/utils/log.h" + +namespace localization { +MultiCameraDetector::MultiCameraDetector( + std::vector camera_constants) + : camera_constants_(std::move(camera_constants)), + timestamped_frames_(camera_constants_.size()), + tag_detections_(camera_constants_.size()) { + std::string log_path = frc::DataLogManager::GetLogDir(); + cameras_.reserve(camera_constants_.size()); + camera_threads_.reserve(camera_constants_.size()); + for (size_t i = 0; i < camera_constants_.size(); i++) { + const std::string camera_log_dest = + fmt::format("{}/{}", log_path, camera_constants_[i].name); + if (camera_constants_[i].serial_id.has_value()) { + absl::Status status; + cameras_.push_back(std::make_unique( + camera_constants_[i], status, camera_log_dest)); + if (!status.ok()) { + LOG(WARNING) << "Unable to create uvc camera for unambiguous solver: " + << status.message(); + } + } else { + cameras_.push_back(std::make_unique( + camera_constants_[i], camera_log_dest)); + } + auto intrinsics = + utils::ReadIntrinsics(camera_constants_[i].intrinsics_path.value()); + if (camera_constants_[i].use_cpu.has_value() && + camera_constants[i].use_cpu.value()) { + detectors_.push_back(std::make_unique( + camera_constants_[i].frame_width.value(), + camera_constants_[i].frame_height.value(), intrinsics)); + } else { + detectors_.push_back(std::make_unique( + camera_constants_[i].frame_width.value(), + camera_constants_[i].frame_height.value(), intrinsics)); + } + camera_threads_.emplace_back([this, i]() -> void { + while (run_cameras_) { + camera::timestamped_frame_t timestamped_frame; + timestamped_frame = cameras_[i]->GetFrame(); + if (timestamped_frame.invalid) { + continue; // this is ok because GetFrame is blocking + } + std::vector detections = + detectors_[i]->GetTagDetections(timestamped_frame); + mutex_.lock(); + timestamped_frames_[i] = std::move(timestamped_frame); + tag_detections_[i] = std::move(detections); + mutex_.unlock(); + } + }); + } +} + +auto MultiCameraDetector::GetTagDetections() + -> std::vector> { + std::vector> tag_detections; + mutex_.lock(); + tag_detections = tag_detections_; + mutex_.unlock(); + return tag_detections; +} + +// Expensive, do not use inside of a loop because of copy +auto MultiCameraDetector::GetCVFrames() -> std::vector { + std::vector frames; + frames.reserve(cameras_.size()); + mutex_.lock(); + for (size_t i = 0; i < cameras_.size(); i++) { + frames.push_back(timestamped_frames_[i].frame.clone()); + } + mutex_.unlock(); + return frames; +} + +MultiCameraDetector::~MultiCameraDetector() { + run_cameras_ = false; + for (auto& t : camera_threads_) { + if (t.joinable()) + t.join(); + } +} + +} // namespace localization diff --git a/src/localization/multi_camera_detector.h b/src/localization/multi_camera_detector.h new file mode 100644 index 00000000..cd102bd1 --- /dev/null +++ b/src/localization/multi_camera_detector.h @@ -0,0 +1,32 @@ +#pragma once +#include "src/camera/camera.h" +#include "src/camera/disk_camera.h" +#include "src/localization/apriltag_detector.h" +#include "src/localization/position.h" +#include "src/localization/position_solver.h" +#include "src/utils/pch.h" +namespace localization { + +class MultiCameraDetector { + public: + MultiCameraDetector(std::vector camera_constants); + [[nodiscard]] auto GetTagDetections() + -> std::vector>; + [[nodiscard]] auto GetCVFrames() -> std::vector; + [[nodiscard]] inline auto NumCameras() -> double { return cameras_.size(); } + ~MultiCameraDetector(); + + private: + std::vector camera_constants_; + std::vector> cameras_; + std::vector> detectors_; + std::vector + invalid_frame_start_times_; // time since the frame has not been old + std::vector timestamped_frames_; + std::vector> tag_detections_; + std::vector camera_threads_; + std::mutex mutex_; + std::atomic run_cameras_{true}; +}; + +} // namespace localization From de3aa6abc33f2742c72cdfae576532e9beeb8e71 Mon Sep 17 00:00:00 2001 From: "carrotmercinary@gmail.com" <167659798+yasen5@users.noreply.github.com> Date: Sat, 18 Apr 2026 23:31:50 +0000 Subject: [PATCH 02/30] Add streaming --- src/localization/multi_camera_detector.cc | 61 +++++++++++++++-------- src/localization/multi_camera_detector.h | 7 ++- 2 files changed, 44 insertions(+), 24 deletions(-) diff --git a/src/localization/multi_camera_detector.cc b/src/localization/multi_camera_detector.cc index 161efa2e..09f1545a 100644 --- a/src/localization/multi_camera_detector.cc +++ b/src/localization/multi_camera_detector.cc @@ -12,12 +12,16 @@ namespace localization { MultiCameraDetector::MultiCameraDetector( std::vector camera_constants) : camera_constants_(std::move(camera_constants)), + last_write_times_(camera_constants_.size()), timestamped_frames_(camera_constants_.size()), tag_detections_(camera_constants_.size()) { std::string log_path = frc::DataLogManager::GetLogDir(); cameras_.reserve(camera_constants_.size()); camera_threads_.reserve(camera_constants_.size()); + streamers_.reserve(camera_constants_.size()); for (size_t i = 0; i < camera_constants_.size(); i++) { + streamers_.emplace_back(camera_constants_[i].name, 5801 + i, 30, 1080, + 1080); const std::string camera_log_dest = fmt::format("{}/{}", log_path, camera_constants_[i].name); if (camera_constants_[i].serial_id.has_value()) { @@ -44,30 +48,16 @@ MultiCameraDetector::MultiCameraDetector( camera_constants_[i].frame_width.value(), camera_constants_[i].frame_height.value(), intrinsics)); } - camera_threads_.emplace_back([this, i]() -> void { - while (run_cameras_) { - camera::timestamped_frame_t timestamped_frame; - timestamped_frame = cameras_[i]->GetFrame(); - if (timestamped_frame.invalid) { - continue; // this is ok because GetFrame is blocking - } - std::vector detections = - detectors_[i]->GetTagDetections(timestamped_frame); - mutex_.lock(); - timestamped_frames_[i] = std::move(timestamped_frame); - tag_detections_[i] = std::move(detections); - mutex_.unlock(); - } - }); } } auto MultiCameraDetector::GetTagDetections() -> std::vector> { std::vector> tag_detections; - mutex_.lock(); - tag_detections = tag_detections_; - mutex_.unlock(); + { + std::lock_guard lock(mutex_); + tag_detections = tag_detections_; + } return tag_detections; } @@ -75,11 +65,12 @@ auto MultiCameraDetector::GetTagDetections() auto MultiCameraDetector::GetCVFrames() -> std::vector { std::vector frames; frames.reserve(cameras_.size()); - mutex_.lock(); - for (size_t i = 0; i < cameras_.size(); i++) { - frames.push_back(timestamped_frames_[i].frame.clone()); + { + std::lock_guard lock(mutex_); + for (size_t i = 0; i < cameras_.size(); i++) { + frames.push_back(timestamped_frames_[i].frame.clone()); + } } - mutex_.unlock(); return frames; } @@ -91,4 +82,30 @@ MultiCameraDetector::~MultiCameraDetector() { } } +void MultiCameraDetector::StartThreads() { + for (size_t i = 0; i < cameras_.size(); i++) { + camera_threads_.emplace_back([this, i]() -> void { + while (run_cameras_) { + camera::timestamped_frame_t timestamped_frame; + timestamped_frame = cameras_[i]->GetFrame(); + if (timestamped_frame.invalid) { + continue; // this is ok because GetFrame is blocking + } + if (timestamped_frame.timestamp - last_write_times_[i] > + 1.0 / kenforced_streamer_fps) { + streamers_[i].WriteFrame(timestamped_frame.frame); + last_write_times_[i] = timestamped_frame.timestamp; + } + std::vector detections = + detectors_[i]->GetTagDetections(timestamped_frame); + { + std::lock_guard lock(mutex_); + timestamped_frames_[i] = std::move(timestamped_frame); + tag_detections_[i] = std::move(detections); + } + } + }); + } +} + } // namespace localization diff --git a/src/localization/multi_camera_detector.h b/src/localization/multi_camera_detector.h index cd102bd1..d4c8b6e3 100644 --- a/src/localization/multi_camera_detector.h +++ b/src/localization/multi_camera_detector.h @@ -1,5 +1,6 @@ #pragma once #include "src/camera/camera.h" +#include "src/camera/cscore_streamer.h" #include "src/camera/disk_camera.h" #include "src/localization/apriltag_detector.h" #include "src/localization/position.h" @@ -14,19 +15,21 @@ class MultiCameraDetector { -> std::vector>; [[nodiscard]] auto GetCVFrames() -> std::vector; [[nodiscard]] inline auto NumCameras() -> double { return cameras_.size(); } + void StartThreads(); ~MultiCameraDetector(); private: std::vector camera_constants_; std::vector> cameras_; std::vector> detectors_; - std::vector - invalid_frame_start_times_; // time since the frame has not been old + std::vector streamers_; + std::vector last_write_times_; std::vector timestamped_frames_; std::vector> tag_detections_; std::vector camera_threads_; std::mutex mutex_; std::atomic run_cameras_{true}; + static constexpr double kenforced_streamer_fps = 30.0; }; } // namespace localization From 292d013f618fb2f4bfc9864eddd47329662c74cb Mon Sep 17 00:00:00 2001 From: "carrotmercinary@gmail.com" <167659798+yasen5@users.noreply.github.com> Date: Thu, 23 Apr 2026 02:57:47 +0000 Subject: [PATCH 03/30] Holy change, builds tho --- src/localization/multi_camera_detector.cc | 2 +- src/localization/multi_camera_detector.h | 2 +- src/localization/networktable_sender.cc | 103 +++++---- src/localization/networktable_sender.h | 3 +- src/localization/position_sender.h | 8 +- src/localization/position_solver.h | 8 + src/localization/run_localization.cc | 47 +--- src/localization/run_localization.h | 13 +- src/localization/simulation_sender.cc | 17 +- src/localization/simulation_sender.h | 3 +- src/localization/unambiguous_estimator.cc | 209 +++++------------- src/localization/unambiguous_estimator.h | 45 ++-- src/main_bot_main.cc | 12 +- src/second_bot_main.cc | 8 +- .../integration_test/localization_test.cc | 3 +- .../integration_test/localization_test2.cc | 129 +++++++++-- src/unambiguous_first.cc | 21 +- src/unambiguous_second.cc | 25 +-- 18 files changed, 297 insertions(+), 361 deletions(-) diff --git a/src/localization/multi_camera_detector.cc b/src/localization/multi_camera_detector.cc index 09f1545a..d3c7f69e 100644 --- a/src/localization/multi_camera_detector.cc +++ b/src/localization/multi_camera_detector.cc @@ -21,7 +21,7 @@ MultiCameraDetector::MultiCameraDetector( streamers_.reserve(camera_constants_.size()); for (size_t i = 0; i < camera_constants_.size(); i++) { streamers_.emplace_back(camera_constants_[i].name, 5801 + i, 30, 1080, - 1080); + 1080); // TODO move constants to steamer class const std::string camera_log_dest = fmt::format("{}/{}", log_path, camera_constants_[i].name); if (camera_constants_[i].serial_id.has_value()) { diff --git a/src/localization/multi_camera_detector.h b/src/localization/multi_camera_detector.h index d4c8b6e3..9772f81b 100644 --- a/src/localization/multi_camera_detector.h +++ b/src/localization/multi_camera_detector.h @@ -29,7 +29,7 @@ class MultiCameraDetector { std::vector camera_threads_; std::mutex mutex_; std::atomic run_cameras_{true}; - static constexpr double kenforced_streamer_fps = 30.0; + static constexpr int kenforced_streamer_fps = 30; }; } // namespace localization diff --git a/src/localization/networktable_sender.cc b/src/localization/networktable_sender.cc index c8278cb9..37d2a8c4 100644 --- a/src/localization/networktable_sender.cc +++ b/src/localization/networktable_sender.cc @@ -78,71 +78,68 @@ NetworkTableSender::NetworkTableSender(const std::string& camera_name, } void NetworkTableSender::Send( - const std::vector& detections) { + const localization::position_estimate_t& detection) { mutex_.lock(); - for (auto& detection : detections) { - std::array tag_estimation{ - detection.pose.X().value(), - detection.pose.Y().value(), - detection.pose.Rotation().Z().value(), - detection.variance, - detection.timestamp + - instance_.GetServerTimeOffset().value_or(0) / 1000000.0, - static_cast(detection.num_tags), - detection.latency, - detection.avg_tag_dist}; - - std::array tags{}; - for (int tag_id : detection.tag_ids) { - tags[tag_id] = true; - } + std::array tag_estimation{ + detection.pose.X().value(), + detection.pose.Y().value(), + detection.pose.Rotation().Z().value(), + detection.variance, + detection.timestamp + + instance_.GetServerTimeOffset().value_or(0) / 1000000.0, + static_cast(detection.num_tags), + detection.latency, + detection.avg_tag_dist}; + + std::array tags{}; + for (int tag_id : detection.tag_ids) { + tags[tag_id] = true; + } - std::array rejected_tags{}; - for (int tag_id : detection.rejected_tag_ids) { - rejected_tags[tag_id] = true; - } + std::array rejected_tags{}; + for (int tag_id : detection.rejected_tag_ids) { + rejected_tags[tag_id] = true; + } - pose_publisher_.Set( - frc::Pose2d(units::meter_t{detection.pose.X().value()}, - units::meter_t{detection.pose.Y().value()}, - units::radian_t{detection.pose.Rotation().Z().value()})); - pose3d_publisher_.Set(detection.pose); + pose_publisher_.Set( + frc::Pose2d(units::meter_t{detection.pose.X().value()}, + units::meter_t{detection.pose.Y().value()}, + units::radian_t{detection.pose.Rotation().Z().value()})); + pose3d_publisher_.Set(detection.pose); - tag_estimation_publisher_.Set(tag_estimation); + tag_estimation_publisher_.Set(tag_estimation); - tag_ids_publisher_.Set(tags); - rejected_tag_ids_publisher_.Set(rejected_tags); - varience_publisher_.Set(detection.variance); + tag_ids_publisher_.Set(tags); + rejected_tag_ids_publisher_.Set(rejected_tags); + varience_publisher_.Set(detection.variance); - latency_publisher_.Set(detection.latency); - timestamp_publisher_.Set(detection.timestamp); - num_tags_publisher_.Set(detection.num_tags); - loss_publisher_.Set(detection.loss); + latency_publisher_.Set(detection.latency); + timestamp_publisher_.Set(detection.timestamp); + num_tags_publisher_.Set(detection.num_tags); + loss_publisher_.Set(detection.loss); - if (log_) { - double adjusted_timestamp = - detection.timestamp + - instance_.GetServerTimeOffset().value_or(0) / 1e6; - auto log_time = static_cast(adjusted_timestamp * 1e6); + if (log_) { + double adjusted_timestamp = + detection.timestamp + instance_.GetServerTimeOffset().value_or(0) / 1e6; + auto log_time = static_cast(adjusted_timestamp * 1e6); - pose3d_log_->Append(detection.pose, log_time); + pose3d_log_->Append(detection.pose, log_time); - tag_estimation_log_->Append(tag_estimation, log_time); - tag_ids_log_->Append(tags, log_time); - rejected_tag_ids_log_->Append(rejected_tags, log_time); + tag_estimation_log_->Append(tag_estimation, log_time); + tag_ids_log_->Append(tags, log_time); + rejected_tag_ids_log_->Append(rejected_tags, log_time); - varience_log_->Append(detection.variance, log_time); - latency_log_->Append(detection.latency, log_time); - timestamp_log_->Append(detection.timestamp, log_time); - num_tags_log_->Append(detection.num_tags, log_time); - loss_log_->Append(detection.loss, log_time); + varience_log_->Append(detection.variance, log_time); + latency_log_->Append(detection.latency, log_time); + timestamp_log_->Append(detection.timestamp, log_time); + num_tags_log_->Append(detection.num_tags, log_time); + loss_log_->Append(detection.loss, log_time); - log_->Flush(); - } + log_->Flush(); + } - if (verbose_) { - LOG(INFO) << detection; - } + if (verbose_) { + LOG(INFO) << detection; } mutex_.unlock(); } diff --git a/src/localization/networktable_sender.h b/src/localization/networktable_sender.h index 045bf959..a3e388df 100644 --- a/src/localization/networktable_sender.h +++ b/src/localization/networktable_sender.h @@ -21,8 +21,7 @@ class NetworkTableSender : public IPositionSender { public: NetworkTableSender(const std::string& camera_name, bool verbose = false, bool sim = false); - void Send(const std::vector& detections) - override; + void Send(const localization::position_estimate_t& detection) override; private: nt::NetworkTableInstance instance_; diff --git a/src/localization/position_sender.h b/src/localization/position_sender.h index 7e37ea75..287772cc 100644 --- a/src/localization/position_sender.h +++ b/src/localization/position_sender.h @@ -6,8 +6,12 @@ namespace localization { class IPositionSender { public: - virtual auto Send( - const std::vector& detections) + void Send(const std::vector& detections) { + for (const auto& detection : detections) { + Send(detection); + } + } + virtual auto Send(const localization::position_estimate_t& detection) -> void = 0; virtual ~IPositionSender() = default; }; diff --git a/src/localization/position_solver.h b/src/localization/position_solver.h index ae984f8f..3e3fd838 100644 --- a/src/localization/position_solver.h +++ b/src/localization/position_solver.h @@ -42,4 +42,12 @@ class IPositionSolver { virtual ~IPositionSolver() = default; }; +class IJointPositionSolver { + public: + virtual auto EstimatePosition( + std::vector>&& detection_batches, + bool reject_far_tags = true) -> std::optional = 0; + virtual ~IJointPositionSolver() = default; +}; + } // namespace localization diff --git a/src/localization/run_localization.cc b/src/localization/run_localization.cc index ce8763d2..8d3afacf 100644 --- a/src/localization/run_localization.cc +++ b/src/localization/run_localization.cc @@ -18,13 +18,12 @@ namespace localization { -// TODO remove extrinsics void RunLocalization( std::unique_ptr source, std::unique_ptr detector, std::unique_ptr solver, std::vector> senders, - const std::string& extrinsics, std::optional port, bool verbose) { + std::optional port, bool verbose) { std::optional streamer = port.has_value() ? std::make_optional(camera::CscoreStreamer( @@ -51,43 +50,17 @@ void RunLocalization( } } -void RunLocalizationSimulation( - camera::CameraSource& source, - std::unique_ptr detector, - std::unique_ptr solver, - const std::string& extrinsics, std::optional port, bool verbose) { - std::error_code ec; - auto log = std::make_unique("multitag2.wpilog", ec); - if (ec) { - std::cerr << "Failed to open log: " << ec.message() << std::endl; - return; - } - log->AddStructSchema(0); - log->AddStructSchema(0); - log->AddStructSchema(0); - wpi::log::StructLogEntry pose_log(*log, "/localization/pose"); - wpi::log::DoubleLogEntry num_tags_log(*log, "/localization/num_tags"); - wpi::log::DoubleLogEntry timestamp_log(*log, "/localization/timestamp"); +void RunJointLocalization( + MultiCameraDetector& detector_source, + std::unique_ptr solver, + std::unique_ptr sender, bool verbose) { while (true) { - camera::timestamped_frame_t timestamped_frame = source.Get(); - double timestamp = timestamped_frame.timestamp; - if (timestamped_frame.invalid) { - std::cout << "Stopping log" << std::endl; - log->Stop(); - std::cout << "Stopped log" << std::endl; - return; - } - std::cout << "Reading from timestamp: " << timestamp << std::endl; - std::vector tag_detections = - detector->GetTagDetections(timestamped_frame); - std::vector position_estimates = - solver->EstimatePosition(tag_detections, false); - auto log_time = static_cast(timestamp * 1e6); - for (const auto& estimate : position_estimates) { - pose_log.Append(estimate.pose, log_time); - num_tags_log.Append(estimate.num_tags, log_time); - timestamp_log.Append(estimate.timestamp, log_time); + std::optional estimated_pose = + solver->EstimatePosition(detector_source.GetTagDetections()); + if (!estimated_pose.has_value()) { + continue; } + sender->Send(estimated_pose.value()); } } diff --git a/src/localization/run_localization.h b/src/localization/run_localization.h index ac37f9af..2429961a 100644 --- a/src/localization/run_localization.h +++ b/src/localization/run_localization.h @@ -2,6 +2,7 @@ #include "src/camera/camera_source.h" #include "src/localization/apriltag_detector.h" #include "src/localization/gpu_apriltag_detector.h" +#include "src/localization/multi_camera_detector.h" #include "src/localization/position_sender.h" #include "src/localization/position_solver.h" #include "src/utils/pch.h" @@ -26,12 +27,10 @@ void RunLocalization( std::unique_ptr detector, std::unique_ptr solver, std::vector> sender, - const std::string& extrinsics, std::optional port = std::nullopt, - bool verbose = false); -void RunLocalizationSimulation( - camera::CameraSource& source, - std::unique_ptr detector, - std::unique_ptr solver, - const std::string& extrinsics, std::optional port = std::nullopt, + std::optional port = std::nullopt, bool verbose = false); +void RunJointLocalization( + MultiCameraDetector& detector_source, + std::unique_ptr solver, + std::unique_ptr sender, bool verbose = false); } // namespace localization diff --git a/src/localization/simulation_sender.cc b/src/localization/simulation_sender.cc index 2c730223..25f3c80e 100644 --- a/src/localization/simulation_sender.cc +++ b/src/localization/simulation_sender.cc @@ -29,19 +29,12 @@ SimulationSender::SimulationSender(const std::string& name, int port) } void SimulationSender::Send( - const std::vector& detections) { - if (detections.empty()) { - return; - } - + const localization::position_estimate_t& detection) { cv::Mat canvas = field_image_.clone(); - for (const auto& detection : detections) { - DrawArrow( - canvas, - cv::Point(field_image_.cols * (detection.pose.X().value() / 16.46), - field_image_.rows * (detection.pose.Y().value() / 8.23)), - detection.pose.ToPose2d().Rotation().Radians().value()); - } + DrawArrow(canvas, + cv::Point(field_image_.cols * (detection.pose.X().value() / 16.46), + field_image_.rows * (detection.pose.Y().value() / 8.23)), + detection.pose.ToPose2d().Rotation().Radians().value()); streamer_.WriteFrame(canvas); } } // namespace localization diff --git a/src/localization/simulation_sender.h b/src/localization/simulation_sender.h index cb006438..0c52c58e 100644 --- a/src/localization/simulation_sender.h +++ b/src/localization/simulation_sender.h @@ -8,8 +8,7 @@ namespace localization { class SimulationSender : public IPositionSender { public: SimulationSender(const std::string& name, int port); - void Send(const std::vector& detections) - override; + void Send(const localization::position_estimate_t& detection) override; ~SimulationSender() override = default; private: diff --git a/src/localization/unambiguous_estimator.cc b/src/localization/unambiguous_estimator.cc index a16f1e1d..323f25db 100644 --- a/src/localization/unambiguous_estimator.cc +++ b/src/localization/unambiguous_estimator.cc @@ -29,75 +29,14 @@ namespace localization { -bool UnambiguousEstimator::log_interesting_timestamp_ = false; - UnambiguousEstimator::UnambiguousEstimator( - std::vector>& cameras, - std::optional port_start, bool verbose, - std::optional> img_dir_paths) - : port_start_(port_start), - prev_timestamps_(cameras.size()), - sim_(img_dir_paths.has_value()) { - std::string log_path = frc::DataLogManager::GetLogDir(); - auto camera_constants = camera::GetCameraConstants(); - detectors_.reserve(cameras.size()); - solvers_.reserve(cameras.size()); - if (port_start.has_value()) { - streamers_.reserve(cameras.size()); - } - std::cout << "Initializing cameras" << std::endl; - std::vector> icameras; - for (size_t i = 0; i < cameras.size(); i++) { - if (sim_) { - icameras.push_back(std::make_unique( - img_dir_paths.value()[i], cameras[i].first, 10, - interesting_timestamp_start_ - 1, interesting_timestamp_end_)); - } else { - const std::string camera_log_dest = - fmt::format("{}/{}", log_path, cameras[i].first.name); - if (cameras[i].first.serial_id.has_value()) { - absl::Status status; - icameras.push_back(std::make_unique( - cameras[i].first, status, camera_log_dest)); - if (!status.ok()) { - LOG(WARNING) << "Unable to create uvc camera for unambiguous solver: " - << status.message(); - } - } else { - icameras.push_back(std::make_unique(cameras[i].first, - camera_log_dest)); - } - std::cout << "Logging to destination: " << camera_log_dest << std::endl; - } + const std::vector& camera_constants, + bool verbose) + : verbose_(verbose) { + solvers_.reserve(camera_constants.size()); + for (const auto& camera_constant : camera_constants) { + solvers_.emplace_back(camera_constant); } - sources_ = std::make_unique(icameras, sim_); - std::cout << "Initialized cameras" << std::endl; - std::this_thread::sleep_for(std::chrono::duration(2)); - std::cout << "Initializing estimators and streamers" << std::endl; - std::vector first_frames = sources_->GetCVFrames(); - for (size_t i = 0; i < cameras.size(); i++) { - switch (cameras[i].second) { - case OPENCV_CPU: - detectors_.push_back(std::make_unique( - first_frames[i].cols, first_frames[i].rows, - utils::ReadIntrinsics(cameras[i].first.intrinsics_path.value()))); - break; - case AUSTIN_GPU: - detectors_.push_back(std::make_unique( - first_frames[i].cols, first_frames[i].rows, - utils::ReadIntrinsics(cameras[i].first.intrinsics_path.value()))); - break; - default: - LOG(FATAL) << "Invalid solver type"; - return; - } - solvers_.emplace_back(cameras[i].first); - if (port_start.has_value()) { - streamers_.emplace_back(cameras[i].first.name, port_start.value() + i, 30, - 1080, 1080); - } - } - std::cout << "Initialized estimators and streamers" << std::endl; } auto UnambiguousEstimator::Cost(const frc::Pose3d& a, const frc::Pose3d& b) @@ -177,11 +116,11 @@ auto UnambiguousEstimator::WeightedAveragePose( } auto UnambiguousEstimator::SearchSolutions( - const std::vector& all_pose_estimates_, size_t index, + const std::vector& all_pose_estimates, size_t index, std::vector& current_solution, std::vector& best_solution, double& best_cost) -> double { - if (index == all_pose_estimates_.size()) { + if (index == all_pose_estimates.size()) { double cost = ComputeCost(current_solution); if (cost < best_cost) { @@ -192,62 +131,38 @@ auto UnambiguousEstimator::SearchSolutions( return best_cost; } - const auto& pair = all_pose_estimates_[index]; + const ambiguous_estimate_t& maybe_ambiguous_estimate = + all_pose_estimates[index]; - current_solution.push_back(pair.pos1); - SearchSolutions(all_pose_estimates_, index + 1, current_solution, + current_solution.push_back(maybe_ambiguous_estimate.pos1); + SearchSolutions(all_pose_estimates, index + 1, current_solution, best_solution, best_cost); current_solution.pop_back(); - if (pair.pos2.has_value()) { - current_solution.push_back(pair.pos2.value()); - SearchSolutions(all_pose_estimates_, index + 1, current_solution, + if (maybe_ambiguous_estimate.pos2.has_value()) { + current_solution.push_back(maybe_ambiguous_estimate.pos2.value()); + SearchSolutions(all_pose_estimates, index + 1, current_solution, best_solution, best_cost); current_solution.pop_back(); } return best_cost; } -void UnambiguousEstimator::Run() { - frc::DataLogManager::Start(); - localization::NetworkTableSender position_sender("Left", false, sim_); - while (true) { - latent_estimate_t pose_estimate = GetUnambiguatedEstimate(); - if (pose_estimate.invalid) { - continue; - } - position_sender.Send( - std::vector{pose_estimate.pose_estimate}); - } -} - -auto UnambiguousEstimator::GetAmbiguousEstimates() +auto UnambiguousEstimator::GetAmbiguousEstimates( + std::vector>& detection_batches) -> std::vector { - std::vector frames = - sources_->GetTimestampedFrames(); - std::vector> usable_frames = - GetUsableFrames(frames); + std::vector>> filtered_detections = + GetFilteredDetections(detection_batches); std::vector estimates; - for (size_t i = 0; i < usable_frames.size(); ++i) { - if (!usable_frames[i].has_value()) { - continue; - } - if (sim_ && usable_frames[i]->invalid) { - frc::DataLogManager::Stop(); - std::cout << "Stopped log" << std::endl; - throw std::runtime_error("DONE"); - } - prev_timestamps_[i] = usable_frames[i]->timestamp; - - std::vector detections = - detectors_[i]->GetTagDetections(usable_frames[i].value()); - - if (detections.empty()) { + for (size_t i = 0; i < filtered_detections.size(); ++i) { + if (!filtered_detections[i].has_value()) { continue; } + prev_timestamps_[i] = filtered_detections[i]->at(0).timestamp; std::optional est = - solvers_[i].EstimatePositionAmbiguous(detections, false); + solvers_[i].EstimatePositionAmbiguous(filtered_detections[i].value(), + false); if (!est.has_value()) { continue; @@ -269,46 +184,36 @@ auto UnambiguousEstimator::GetAmbiguousEstimates() return estimates; } -auto UnambiguousEstimator::GetUsableFrames( - std::vector& frames) - -> std::vector> { - std::vector> usable_frames( - frames.size()); +auto UnambiguousEstimator::GetFilteredDetections( + std::vector>& detection_batches) + -> std::vector>> { + std::vector>> usable_detections( + detection_batches.size()); constexpr double kacceptable_frame_recency = 0.25; double latest_timestamp = -1; - for (auto& frame : frames) { - if (sim_ && (frame.invalid || frame.frame.empty())) { - std::cout << "STOPPING LOG" << std::endl; - frc::DataLogManager::Stop(); - std::exit(0); + for (auto& detection_batch : detection_batches) { + if (detection_batch.empty()) { + continue; } - if (frame.timestamp > latest_timestamp) { - latest_timestamp = frame.timestamp; + if (detection_batch[0].timestamp > latest_timestamp) { + latest_timestamp = detection_batch[0].timestamp; } - UnambiguousEstimator::log_interesting_timestamp_ = - frame.timestamp > interesting_timestamp_start_ && - frame.timestamp < interesting_timestamp_end_; } - for (size_t i = 0; i < frames.size(); i++) { - if (latest_timestamp - frames[i].timestamp < kacceptable_frame_recency) { - streamers_[i].WriteFrame(frames[i].frame); - usable_frames[i] = std::move(frames[i]); + for (size_t i = 0; i < detection_batches.size(); i++) { + if (latest_timestamp - detection_batches[i][0].timestamp < + kacceptable_frame_recency) { + usable_detections[i] = std::move(detection_batches[i]); } } - return usable_frames; + return usable_detections; } -auto UnambiguousEstimator::GetUnambiguatedEstimate() -> latent_estimate_t { - utils::Timer everything_timer("Getting estimate", false); - const auto& ambiguous_estimates = GetAmbiguousEstimates(); - std::vector all_pose_estimates_for_log; - for (const auto& est : ambiguous_estimates) { - all_pose_estimates_for_log.push_back(est.pos1.pose); - if (est.pos2.has_value()) { - all_pose_estimates_for_log.push_back(est.pos2.value().pose); - } - } +auto UnambiguousEstimator::EstimatePosition( + std::vector>&& detection_batches, + bool reject_far_tags) -> std::optional { + utils::Timer computation_timer("Getting estimate", false); + const auto& ambiguous_estimates = GetAmbiguousEstimates(detection_batches); std::vector best_solution; std::vector current_solution; use_prev_pose_ = true; @@ -317,7 +222,7 @@ auto UnambiguousEstimator::GetUnambiguatedEstimate() -> latent_estimate_t { double cost = SearchSolutions(ambiguous_estimates, 0, current_solution, best_solution, best_cost); if (best_solution.size() == 0) { - return {.invalid = true}; + return std::make_optional({.invalid = true}); } double avg_variance = 0; double avg_timestamp = 0; @@ -333,21 +238,15 @@ auto UnambiguousEstimator::GetUnambiguatedEstimate() -> latent_estimate_t { avg_timestamp /= best_solution.size(); avg_variance /= best_solution.size(); const int num_tags = tag_ids.size(); - double latency = everything_timer.Stop(); - position_estimate_t averaged_estimate = { - .pose = WeightedAveragePose(best_solution), - .variance = avg_variance, - .timestamp = avg_timestamp, - .num_tags = num_tags, - .latency = latency, - .invalid = invalid, - .loss = cost}; - prev_pose_estimate_ = std::make_optional(averaged_estimate); - return {.pose_estimate = averaged_estimate, - .latency = latency, - .best_cost = cost, - .used_prev_pose = use_prev_pose_, - .all_pose_estimates = all_pose_estimates_for_log}; + double latency = computation_timer.Stop(); + return std::make_optional( + {.pose = WeightedAveragePose(best_solution), + .variance = avg_variance, + .timestamp = avg_timestamp, + .num_tags = num_tags, + .latency = latency, + .invalid = invalid, + .loss = cost}); } } // namespace localization diff --git a/src/localization/unambiguous_estimator.h b/src/localization/unambiguous_estimator.h index 0bb51716..cfe8dfac 100644 --- a/src/localization/unambiguous_estimator.h +++ b/src/localization/unambiguous_estimator.h @@ -13,52 +13,39 @@ #include "src/utils/pch.h" namespace localization { -enum Detector { OPENCV_CPU, AUSTIN_GPU }; -using latent_estimate_t = struct LatentEstimate { - position_estimate_t pose_estimate; - double latency; - double best_cost; - bool used_prev_pose; - std::vector all_pose_estimates; - bool invalid = false; -}; -class UnambiguousEstimator { +class UnambiguousEstimator : public IJointPositionSolver { public: UnambiguousEstimator( - std::vector>& cameras, - std::optional port_start = std::nullopt, bool verbose = false, - std::optional> img_dir_paths = - std::nullopt); - void Run(); - static bool log_interesting_timestamp_; + const std::vector& camera_constants, + bool verbose = false); + auto EstimatePosition( + std::vector>&& detection_batches, + bool reject_far_tags = true) + -> std::optional override; private: - auto GetUnambiguatedEstimate() -> latent_estimate_t; static auto Cost(const frc::Pose3d& a, const frc::Pose3d& b) -> double; auto ComputeCost(const std::vector& poses) -> double; - auto WeightedAveragePose(const std::vector& solutions) - -> frc::Pose3d; + static auto WeightedAveragePose( + const std::vector& solutions) -> frc::Pose3d; auto SearchSolutions( const std::vector& all_pose_estimates, size_t index, std::vector& current_solution, std::vector& best_solution, double& best_cost) -> double; - auto GetAmbiguousEstimates() -> std::vector; - auto GetUsableFrames(std::vector& frames) - -> std::vector>; - static constexpr double interesting_timestamp_start_ = 0; // 13.265; - static constexpr double interesting_timestamp_end_ = 600; + auto GetAmbiguousEstimates( + std::vector>& detection_batches) + -> std::vector; + auto GetFilteredDetections( + std::vector>& detection_batches) + -> std::vector>>; - std::vector streamers_; - std::unique_ptr sources_; - std::vector> detectors_; std::vector solvers_; - const std::optional port_start_; std::mutex mutex_; std::vector prev_timestamps_; std::optional prev_pose_estimate_ = std::nullopt; - const bool sim_; static constexpr double kuse_prev_pose_threshold = 100; // tune bool use_prev_pose_ = false; + const bool verbose_; }; } // namespace localization diff --git a/src/main_bot_main.cc b/src/main_bot_main.cc index 332846a0..1da63f7b 100644 --- a/src/main_bot_main.cc +++ b/src/main_bot_main.cc @@ -39,9 +39,7 @@ auto main() -> int { camera_constants.at("main_bot_front").intrinsics_path.value())), std::make_unique( camera_constants.at("main_bot_front")), - std::move(front_sender), - camera_constants.at("main_bot_front").extrinsics_path.value(), 5801, - false); + std::move(front_sender), false); }); std::thread left_thread([&]() { @@ -63,9 +61,7 @@ auto main() -> int { camera_constants.at("main_bot_left").intrinsics_path.value())), std::make_unique( camera_constants.at("main_bot_left")), - std::move(left_sender), - camera_constants.at("main_bot_left").extrinsics_path.value(), 5802, - false); + std::move(left_sender), false); }); std::thread right_thread([&]() { @@ -88,9 +84,7 @@ auto main() -> int { camera_constants.at("main_bot_right").intrinsics_path.value())), std::make_unique( camera_constants.at("main_bot_right")), - std::move(right_sender), - camera_constants.at("main_bot_right").extrinsics_path.value(), 5803, - false); + std::move(right_sender), false); }); LOG(INFO) << "Started estimators"; diff --git a/src/second_bot_main.cc b/src/second_bot_main.cc index a2e305df..5c758d1a 100644 --- a/src/second_bot_main.cc +++ b/src/second_bot_main.cc @@ -37,9 +37,7 @@ auto main() -> int { .intrinsics_path.value())), std::make_unique( camera_constants.at("second_bot_left")), - std::move(left_sender), - camera_constants.at("second_bot_left").extrinsics_path.value(), 5802, - false); + std::move(left_sender), false); }); std::thread right_thread([&]() { @@ -62,9 +60,7 @@ auto main() -> int { .intrinsics_path.value())), std::make_unique( camera_constants.at("second_bot_right")), - std::move(right_sender), - camera_constants.at("second_bot_right").extrinsics_path.value(), 5803, - false); + std::move(right_sender), false); }); // TODO front camera diff --git a/src/test/integration_test/localization_test.cc b/src/test/integration_test/localization_test.cc index 80555fcc..be271c23 100644 --- a/src/test/integration_test/localization_test.cc +++ b/src/test/integration_test/localization_test.cc @@ -145,8 +145,7 @@ auto main(int argc, char** argv) -> int { frame.cols, frame.rows, utils::ReadIntrinsics(camera_constant.intrinsics_path.value())), std::make_unique(camera_constant), - std::move(senders), camera_constant.extrinsics_path.value(), - base_port + i, true); + std::move(senders), true); }); } diff --git a/src/test/integration_test/localization_test2.cc b/src/test/integration_test/localization_test2.cc index 421be66f..cdb05966 100644 --- a/src/test/integration_test/localization_test2.cc +++ b/src/test/integration_test/localization_test2.cc @@ -1,55 +1,144 @@ #include +#include +#include #include +#include #include +#include +#include #include "absl/flags/flag.h" #include "absl/flags/internal/flag.h" #include "absl/flags/parse.h" +#include "absl/log/log.h" #include "frc/DataLogManager.h" #include "src/camera/camera_constants.h" #include "src/camera/camera_source.h" #include "src/camera/disk_camera.h" #include "src/localization/multi_tag_solver.h" +#include "src/localization/networktable_sender.h" #include "src/localization/opencv_apriltag_detector.h" #include "src/localization/run_localization.h" +#include "src/localization/simulation_sender.h" #include "src/localization/unambiguous_estimator.h" #include "src/utils/camera_utils.h" #include "src/utils/log.h" -// for reference, example command: -// ./build/src/test/integration_test/l2calization_test --camera_name=main_bot_right --image_folder=logs/log181/right --speed=0.5 +// Example command: +// ./build/src/test/integration_test/localization_test --camera_name=main_bot_right --image_folder=logs/log181/right --speed=0.5 +// To each camera has two streams. 1. Raw video stream, 2. Position estimate stream. The port for raw video stream is 580x and the port for position estimate stream is 480x. x is different for each camera. It starts at 1 and counts up. For example, to view the third camera's position estimate stream, go to localhost:4803 ABSL_FLAG(std::string, image_folder, "", //NOLINT "Path to folder of test images"); ABSL_FLAG(std::optional, camera_name, std::nullopt, //NOLINT "Camera name"); -ABSL_FLAG(int, port, 5801, "Port"); //NOLINT -ABSL_FLAG(double, speed, 0.01, "Delay between frames"); //NOLINT +ABSL_FLAG(int, port, 5801, "Port"); //NOLINT +ABSL_FLAG(double, speed, 1, "Delay between frames"); //NOLINT + +auto HasRegularFiles(const std::filesystem::path& path) -> bool { + for (const auto& entry : std::filesystem::directory_iterator(path)) { + if (!entry.is_regular_file()) { + continue; + } + + std::string extension = entry.path().extension().string(); + std::transform(extension.begin(), extension.end(), extension.begin(), + [](unsigned char c) { return std::tolower(c); }); + if (extension == ".png" || extension == ".jpg" || extension == ".jpeg") { + return true; + } + } + return false; +} + +auto FindCameraFolders(const std::filesystem::path& path) + -> std::vector { + std::vector camera_folders; + for (const auto& entry : std::filesystem::directory_iterator(path)) { + if (entry.is_directory()) { + camera_folders.push_back(entry.path()); + } + } + + if (camera_folders.empty()) { + camera_folders.push_back(path); + } + + std::sort(camera_folders.begin(), camera_folders.end()); + return camera_folders; +} + +auto ResolveCameraName(const std::string& directory_name, + const camera::camera_constants_t& constants) + -> std::string { + std::string resolved_name = directory_name.rfind("main_bot_", 0) == 0 + ? directory_name + : "main_bot_" + directory_name; + + if (!constants.contains(resolved_name)) { + LOG(FATAL) << "Could not resolve camera constants name for directory: " + << directory_name + << ", expected constants entry: " << resolved_name; + } + + return resolved_name; +} auto main(int argc, char** argv) -> int { absl::ParseCommandLine(argc, argv); - const std::string image_folder = absl::GetFlag(FLAGS_image_folder); - const std::filesystem::path image_path(image_folder); - if (image_folder.empty() || !std::filesystem::exists(image_path) || - !std::filesystem::is_directory(image_path)) { + frc::DataLogManager::Start(frc::DataLogManager::GetLogDir()); + + const std::string image_folder_root = absl::GetFlag(FLAGS_image_folder); + const std::filesystem::path image_root_path(image_folder_root); + if (image_folder_root.empty() || !std::filesystem::exists(image_root_path) || + !std::filesystem::is_directory(image_root_path)) { LOG(FATAL) << "Folder empty or doesn't exist"; } + auto camera_folders = FindCameraFolders(image_root_path); auto constants = camera::GetCameraConstants(); + std::optional camera_name_override = + absl::GetFlag(FLAGS_camera_name); + if (camera_name_override.has_value() && camera_folders.size() > 1) { + LOG(FATAL) << "--camera_name may only be used with a single camera folder"; + } - std::vector> - cameras{{constants.at("main_bot_left"), localization::OPENCV_CPU}, - {constants.at("main_bot_right"), localization::OPENCV_CPU}}; - auto paths = std::make_optional>( - {image_folder + "/left", image_folder + "/right"}); + std::vector camera_constants; + camera_constants.reserve(camera_folders.size()); - // std::vector> - // cameras{{constants.at("main_bot_left"), localization::OPENCV_CPU}}; - // auto paths = std::make_optional>( - // {image_folder + "/left"}); + const int base_port = absl::GetFlag(FLAGS_port); + const double speed = absl::GetFlag(FLAGS_speed); - localization::UnambiguousEstimator estimator( - cameras, std::make_optional(5801), false, paths); - estimator.Run(); + for (auto& camera_folder : camera_folders) { + if (!HasRegularFiles(camera_folder)) { + LOG(WARNING) << "Skipping empty camera folder: " << camera_folder; + continue; + } + const std::string camera_name = + camera_name_override.has_value() + ? camera_name_override.value() + : ResolveCameraName(camera_folder.filename().string(), constants); + + if (!constants.contains(camera_name)) { + LOG(FATAL) << "Unknown camera name: " << camera_name; + } + + constants.at(camera_name); + } + std::thread thread([camera_constants] { + localization::MultiCameraDetector detector_source(camera_constants); + std::vector> senders; + senders.emplace_back(); + localization::RunJointLocalization( + detector_source, + std::make_unique(camera_constants), + std::make_unique("Joint", false, + true)); + }); + + if (camera_constants.empty()) { + LOG(FATAL) << "No readable images found in any camera folder under: " + << image_folder_root; + } } diff --git a/src/unambiguous_first.cc b/src/unambiguous_first.cc index 02260610..8859d522 100644 --- a/src/unambiguous_first.cc +++ b/src/unambiguous_first.cc @@ -1,7 +1,9 @@ #include "src/camera/camera_constants.h" #include "src/camera/camera_source.h" #include "src/camera/cv_camera.h" +#include "src/localization/multi_camera_detector.h" #include "src/localization/multi_tag_solver.h" +#include "src/localization/networktable_sender.h" #include "src/localization/opencv_apriltag_detector.h" #include "src/localization/run_localization.h" #include "src/localization/square_solver.h" @@ -18,17 +20,16 @@ auto main() -> int { LOG(INFO) << "Starting cameras"; - std::vector> - cameras{ - {camera_constants.at("main_bot_left"), - localization::Detector::AUSTIN_GPU}, - {camera_constants.at("main_bot_right"), - localization::Detector::AUSTIN_GPU}, - }; + std::vector cameras{ + camera_constants.at("main_bot_left"), + camera_constants.at("main_bot_right")}; + localization::MultiCameraDetector detector(cameras); LOG(INFO) << "Started cameras"; + std::this_thread::sleep_for(std::chrono::duration(2)); - localization::UnambiguousEstimator localizer(cameras, - std::make_optional(5801)); - localizer.Run(); + localization::UnambiguousEstimator localizer(cameras); + std::vector> joint_sender; + joint_sender.emplace_back( + std::make_unique("Left")); } diff --git a/src/unambiguous_second.cc b/src/unambiguous_second.cc index 4e5ace9b..0ee0349f 100644 --- a/src/unambiguous_second.cc +++ b/src/unambiguous_second.cc @@ -2,6 +2,7 @@ #include "src/camera/camera_source.h" #include "src/camera/cv_camera.h" #include "src/localization/multi_tag_solver.h" +#include "src/localization/networktable_sender.h" #include "src/localization/opencv_apriltag_detector.h" #include "src/localization/run_localization.h" #include "src/localization/square_solver.h" @@ -11,26 +12,24 @@ using camera::camera_constants_t; auto main() -> int { - utils::StartNetworktables(); + utils::StartNetworktables(9971); std::string log_path = frc::DataLogManager::GetLogDir(); camera_constants_t camera_constants = camera::GetCameraConstants(); LOG(INFO) << "Starting cameras"; - std::vector> - cameras{ - {camera_constants.at("second_bot_left"), - localization::Detector::AUSTIN_GPU}, - {camera_constants.at("second_bot_right"), - localization::Detector::AUSTIN_GPU}, - {camera_constants.at("second_bot_front"), - localization::Detector::AUSTIN_GPU}, - }; + std::vector cameras{ + camera_constants.at("second_bot_left"), + camera_constants.at("second_bot_right"), + camera_constants.at("second_bot_front")}; + localization::MultiCameraDetector detector(cameras); LOG(INFO) << "Started cameras"; + std::this_thread::sleep_for(std::chrono::duration(2)); - localization::UnambiguousEstimator localizer(cameras, - std::make_optional(5801)); - localizer.Run(); + localization::UnambiguousEstimator localizer(cameras); + std::vector> joint_sender; + joint_sender.emplace_back( + std::make_unique("Left")); } From 5f2db9d6c4b910e2178f75adda694c37f15afaed Mon Sep 17 00:00:00 2001 From: "carrotmercinary@gmail.com" <167659798+yasen5@users.noreply.github.com> Date: Fri, 24 Apr 2026 22:58:22 +0000 Subject: [PATCH 04/30] Works? --- src/camera/disk_camera.cc | 3 +- src/localization/multi_camera_detector.cc | 63 ++++++++++--------- src/localization/multi_camera_detector.h | 7 ++- src/localization/unambiguous_estimator.cc | 7 ++- .../integration_test/localization_test.cc | 4 +- .../integration_test/localization_test2.cc | 25 ++++---- 6 files changed, 57 insertions(+), 52 deletions(-) diff --git a/src/camera/disk_camera.cc b/src/camera/disk_camera.cc index f3663c58..d7d72fe7 100644 --- a/src/camera/disk_camera.cc +++ b/src/camera/disk_camera.cc @@ -60,7 +60,8 @@ auto DiskCamera::GetFrame() -> timestamped_frame_t { std::cout << "Finished reading all frames from DiskCamera. Folder path: " << image_folder_path_ << std::endl; frc::DataLogManager::Stop(); - return {.invalid = true}; + LOG(INFO) << "Reached the end of the file list"; + std::exit(0); } double recorded_ts = image_paths_.top().timestamp; diff --git a/src/localization/multi_camera_detector.cc b/src/localization/multi_camera_detector.cc index d3c7f69e..b35b9030 100644 --- a/src/localization/multi_camera_detector.cc +++ b/src/localization/multi_camera_detector.cc @@ -10,7 +10,8 @@ namespace localization { MultiCameraDetector::MultiCameraDetector( - std::vector camera_constants) + std::vector camera_constants, + std::optional> image_paths) : camera_constants_(std::move(camera_constants)), last_write_times_(camera_constants_.size()), timestamped_frames_(camera_constants_.size()), @@ -24,7 +25,10 @@ MultiCameraDetector::MultiCameraDetector( 1080); // TODO move constants to steamer class const std::string camera_log_dest = fmt::format("{}/{}", log_path, camera_constants_[i].name); - if (camera_constants_[i].serial_id.has_value()) { + if (image_paths.has_value()) { + cameras_.push_back(std::make_unique( + image_paths.value()[i], camera_constants_[i])); + } else if (camera_constants_[i].serial_id.has_value()) { absl::Status status; cameras_.push_back(std::make_unique( camera_constants_[i], status, camera_log_dest)); @@ -38,8 +42,8 @@ MultiCameraDetector::MultiCameraDetector( } auto intrinsics = utils::ReadIntrinsics(camera_constants_[i].intrinsics_path.value()); - if (camera_constants_[i].use_cpu.has_value() && - camera_constants[i].use_cpu.value()) { + if (true /*camera_constants_[i].use_cpu.has_value() && + camera_constants[i].use_cpu.value()*/) { detectors_.push_back(std::make_unique( camera_constants_[i].frame_width.value(), camera_constants_[i].frame_height.value(), intrinsics)); @@ -49,10 +53,35 @@ MultiCameraDetector::MultiCameraDetector( camera_constants_[i].frame_height.value(), intrinsics)); } } + for (size_t i = 0; i < cameras_.size(); i++) { + camera_threads_.emplace_back([this, i, image_paths]() -> void { + while (run_cameras_) { + camera::timestamped_frame_t timestamped_frame; + timestamped_frame = cameras_[i]->GetFrame(); + if (timestamped_frame.invalid) { + continue; // this is ok because GetFrame is blocking + } + if (timestamped_frame.timestamp - last_write_times_[i] > + 1.0 / kenforced_streamer_fps) { + streamers_[i].WriteFrame(timestamped_frame.frame); + last_write_times_[i] = timestamped_frame.timestamp; + } + std::vector detections = + detectors_[i]->GetTagDetections(timestamped_frame); + { + std::lock_guard lock(mutex_); + timestamped_frames_[i] = std::move(timestamped_frame); + tag_detections_[i] = std::move(detections); + } + has_new_detections_.release(); + } + }); + } } auto MultiCameraDetector::GetTagDetections() -> std::vector> { + has_new_detections_.acquire(); std::vector> tag_detections; { std::lock_guard lock(mutex_); @@ -82,30 +111,4 @@ MultiCameraDetector::~MultiCameraDetector() { } } -void MultiCameraDetector::StartThreads() { - for (size_t i = 0; i < cameras_.size(); i++) { - camera_threads_.emplace_back([this, i]() -> void { - while (run_cameras_) { - camera::timestamped_frame_t timestamped_frame; - timestamped_frame = cameras_[i]->GetFrame(); - if (timestamped_frame.invalid) { - continue; // this is ok because GetFrame is blocking - } - if (timestamped_frame.timestamp - last_write_times_[i] > - 1.0 / kenforced_streamer_fps) { - streamers_[i].WriteFrame(timestamped_frame.frame); - last_write_times_[i] = timestamped_frame.timestamp; - } - std::vector detections = - detectors_[i]->GetTagDetections(timestamped_frame); - { - std::lock_guard lock(mutex_); - timestamped_frames_[i] = std::move(timestamped_frame); - tag_detections_[i] = std::move(detections); - } - } - }); - } -} - } // namespace localization diff --git a/src/localization/multi_camera_detector.h b/src/localization/multi_camera_detector.h index 9772f81b..c5c729be 100644 --- a/src/localization/multi_camera_detector.h +++ b/src/localization/multi_camera_detector.h @@ -10,7 +10,9 @@ namespace localization { class MultiCameraDetector { public: - MultiCameraDetector(std::vector camera_constants); + MultiCameraDetector(std::vector camera_constants, + std::optional> + image_paths = std::nullopt); [[nodiscard]] auto GetTagDetections() -> std::vector>; [[nodiscard]] auto GetCVFrames() -> std::vector; @@ -26,9 +28,10 @@ class MultiCameraDetector { std::vector last_write_times_; std::vector timestamped_frames_; std::vector> tag_detections_; - std::vector camera_threads_; + std::vector camera_threads_; std::mutex mutex_; std::atomic run_cameras_{true}; + std::binary_semaphore has_new_detections_{false}; static constexpr int kenforced_streamer_fps = 30; }; diff --git a/src/localization/unambiguous_estimator.cc b/src/localization/unambiguous_estimator.cc index 323f25db..8b31cddf 100644 --- a/src/localization/unambiguous_estimator.cc +++ b/src/localization/unambiguous_estimator.cc @@ -32,7 +32,7 @@ namespace localization { UnambiguousEstimator::UnambiguousEstimator( const std::vector& camera_constants, bool verbose) - : verbose_(verbose) { + : prev_timestamps_(camera_constants.size()), verbose_(verbose) { solvers_.reserve(camera_constants.size()); for (const auto& camera_constant : camera_constants) { solvers_.emplace_back(camera_constant); @@ -200,8 +200,9 @@ auto UnambiguousEstimator::GetFilteredDetections( } } for (size_t i = 0; i < detection_batches.size(); i++) { - if (latest_timestamp - detection_batches[i][0].timestamp < - kacceptable_frame_recency) { + if (!detection_batches[i].empty() && + latest_timestamp - detection_batches[i][0].timestamp < + kacceptable_frame_recency) { usable_detections[i] = std::move(detection_batches[i]); } } diff --git a/src/test/integration_test/localization_test.cc b/src/test/integration_test/localization_test.cc index be271c23..1334ad4e 100644 --- a/src/test/integration_test/localization_test.cc +++ b/src/test/integration_test/localization_test.cc @@ -69,9 +69,9 @@ auto FindCameraFolders(const std::filesystem::path& path) auto ResolveCameraName(const std::string& directory_name, const camera::camera_constants_t& constants) -> std::string { - std::string resolved_name = directory_name.rfind("main_bot_", 0) == 0 + std::string resolved_name = directory_name.rfind("second_bot_", 0) == 0 ? directory_name - : "main_bot_" + directory_name; + : "second_bot_" + directory_name; if (!constants.contains(resolved_name)) { LOG(FATAL) << "Could not resolve camera constants name for directory: " diff --git a/src/test/integration_test/localization_test2.cc b/src/test/integration_test/localization_test2.cc index cdb05966..00d984e0 100644 --- a/src/test/integration_test/localization_test2.cc +++ b/src/test/integration_test/localization_test2.cc @@ -54,15 +54,15 @@ auto FindCameraFolders(const std::filesystem::path& path) -> std::vector { std::vector camera_folders; for (const auto& entry : std::filesystem::directory_iterator(path)) { + std::cout << "Considering dir: " << entry.path() << std::endl; if (entry.is_directory()) { camera_folders.push_back(entry.path()); + } else { + std::cout << "Rejected dir for being a file: " << entry.path() + << std::endl; } } - if (camera_folders.empty()) { - camera_folders.push_back(path); - } - std::sort(camera_folders.begin(), camera_folders.end()); return camera_folders; } @@ -70,9 +70,9 @@ auto FindCameraFolders(const std::filesystem::path& path) auto ResolveCameraName(const std::string& directory_name, const camera::camera_constants_t& constants) -> std::string { - std::string resolved_name = directory_name.rfind("main_bot_", 0) == 0 + std::string resolved_name = directory_name.rfind("second_bot_", 0) == 0 ? directory_name - : "main_bot_" + directory_name; + : "second_bot_" + directory_name; if (!constants.contains(resolved_name)) { LOG(FATAL) << "Could not resolve camera constants name for directory: " @@ -107,9 +107,6 @@ auto main(int argc, char** argv) -> int { std::vector camera_constants; camera_constants.reserve(camera_folders.size()); - const int base_port = absl::GetFlag(FLAGS_port); - const double speed = absl::GetFlag(FLAGS_speed); - for (auto& camera_folder : camera_folders) { if (!HasRegularFiles(camera_folder)) { LOG(WARNING) << "Skipping empty camera folder: " << camera_folder; @@ -124,12 +121,12 @@ auto main(int argc, char** argv) -> int { LOG(FATAL) << "Unknown camera name: " << camera_name; } - constants.at(camera_name); + camera_constants.push_back(constants.at(camera_name)); } - std::thread thread([camera_constants] { - localization::MultiCameraDetector detector_source(camera_constants); - std::vector> senders; - senders.emplace_back(); + std::jthread thread([camera_constants, camera_folders] { + localization::MultiCameraDetector detector_source(camera_constants, + camera_folders); + LOG(INFO) << "Created camera source"; localization::RunJointLocalization( detector_source, std::make_unique(camera_constants), From 2df055aa84d0e0863929cafba1e7b5aa23f9282f Mon Sep 17 00:00:00 2001 From: "carrotmercinary@gmail.com" <167659798+yasen5@users.noreply.github.com> Date: Sat, 25 Apr 2026 00:19:29 +0000 Subject: [PATCH 05/30] Fix invalid pose estimate return and fix testing --- constants/camera_constants.json | 14 +++++++------- src/camera/camera_constants.cc | 4 ---- src/localization/networktable_sender.cc | 2 +- src/localization/unambiguous_estimator.cc | 2 +- src/main_bot_main.cc | 8 ++------ src/pathing/simulator.cc | 4 ++-- src/test/integration_test/localization_test2.cc | 4 ++-- 7 files changed, 15 insertions(+), 23 deletions(-) diff --git a/constants/camera_constants.json b/constants/camera_constants.json index 6305002c..3d715316 100644 --- a/constants/camera_constants.json +++ b/constants/camera_constants.json @@ -33,7 +33,7 @@ "fps": null, "exposure": null, "port": 5803, - "detector_type": "austin_gpu" + "detector_type": "opencv_cpu" }, { "pipeline": "/dev/v4l/by-path/platform-3610000.usb-usb-0:2.3:1.0-video-index0", @@ -46,7 +46,7 @@ "fps": 60.0, "exposure": null, "port": 5802, - "detector_type": "austin_gpu" + "detector_type": "opencv_cpu" }, { "pipeline": "/dev/v4l/by-path/platform-3610000.usb-usb-0:2.1:1.0-video-index0", @@ -59,7 +59,7 @@ "fps": 60.0, "exposure": null, "port": 5801, - "detector_type": "austin_gpu" + "detector_type": "opencv_cpu" }, { "name": "second_bot_front", @@ -72,7 +72,7 @@ "max_payload_size": 1024, "stream_ratio": 0.5, "port": 5803, - "detector_type": "austin_gpu" + "detector_type": "opencv_cpu" }, { "name": "second_bot_right", @@ -85,7 +85,7 @@ "max_payload_size": 1024, "stream_ratio": 0.5, "port": 5802, - "detector_type": "austin_gpu" + "detector_type": "opencv_cpu" }, { "name": "second_bot_left", @@ -98,7 +98,7 @@ "max_payload_size": 1024, "stream_ratio": 0.5, "port": 5801, - "detector_type": "austin_gpu" + "detector_type": "opencv_cpu" } ] -} \ No newline at end of file +} diff --git a/src/camera/camera_constants.cc b/src/camera/camera_constants.cc index 9831a34a..79762019 100644 --- a/src/camera/camera_constants.cc +++ b/src/camera/camera_constants.cc @@ -88,10 +88,6 @@ auto GetCameraConstants(const std::string& path) -> camera_constants_t { camera_constant.detector_type = StringToDetectorType(camera_config["detector_type"]); } - if (camera_config.contains("use_cpu") && - !camera_config["use_cpu"].is_null()) { - camera_constant.use_cpu = camera_config["use_cpu"]; - } camera_constants.insert({camera_constant.name, camera_constant}); } return camera_constants; diff --git a/src/localization/networktable_sender.cc b/src/localization/networktable_sender.cc index 37d2a8c4..eb133eb8 100644 --- a/src/localization/networktable_sender.cc +++ b/src/localization/networktable_sender.cc @@ -56,7 +56,7 @@ NetworkTableSender::NetworkTableSender(const std::string& camera_name, if (sim) { std::error_code ec; - log_.emplace("/bos/logs/sim.wpilog", ec); + log_.emplace("/bos/logs/sim2.wpilog", ec); if (ec) { std::cerr << "Failed to open log: " << ec.message() << '\n'; std::exit(0); diff --git a/src/localization/unambiguous_estimator.cc b/src/localization/unambiguous_estimator.cc index 8b31cddf..b125d6a9 100644 --- a/src/localization/unambiguous_estimator.cc +++ b/src/localization/unambiguous_estimator.cc @@ -223,7 +223,7 @@ auto UnambiguousEstimator::EstimatePosition( double cost = SearchSolutions(ambiguous_estimates, 0, current_solution, best_solution, best_cost); if (best_solution.size() == 0) { - return std::make_optional({.invalid = true}); + return std::nullopt; } double avg_variance = 0; double avg_timestamp = 0; diff --git a/src/main_bot_main.cc b/src/main_bot_main.cc index 3cd75110..b41b3079 100644 --- a/src/main_bot_main.cc +++ b/src/main_bot_main.cc @@ -41,9 +41,7 @@ auto main() -> int { camera_constants.at("main_bot_left").intrinsics_path.value())), std::make_unique( camera_constants.at("main_bot_left")), - std::move(left_sender), - camera_constants.at("main_bot_left").extrinsics_path.value(), 5802, - false); + std::move(left_sender), 5802, false); }); std::thread right_thread([&]() { @@ -66,9 +64,7 @@ auto main() -> int { camera_constants.at("main_bot_right").intrinsics_path.value())), std::make_unique( camera_constants.at("main_bot_right")), - std::move(right_sender), - camera_constants.at("main_bot_right").extrinsics_path.value(), 5803, - false); + std::move(right_sender), 5803, false); }); LOG(INFO) << "Started estimators"; diff --git a/src/pathing/simulator.cc b/src/pathing/simulator.cc index 45b391e0..ccc0243f 100644 --- a/src/pathing/simulator.cc +++ b/src/pathing/simulator.cc @@ -10,7 +10,7 @@ #include "src/pathing/pathing.h" auto main() -> int { - wpi::log::DataLogBackgroundWriter log{"/root/bos/logs", "sim.wpilog"}; + wpi::log::DataLogBackgroundWriter log{"/root/bos/logs", "sim2.wpilog"}; wpi::log::StructLogEntry poseLog(log, "/sim/Pose2d"); wpi::log::DoubleLogEntry accelXLog(log, "/sim/AccelX"); @@ -146,4 +146,4 @@ auto main() -> int { log.Flush(); return 0; -} \ No newline at end of file +} diff --git a/src/test/integration_test/localization_test2.cc b/src/test/integration_test/localization_test2.cc index 00d984e0..572c6731 100644 --- a/src/test/integration_test/localization_test2.cc +++ b/src/test/integration_test/localization_test2.cc @@ -70,9 +70,9 @@ auto FindCameraFolders(const std::filesystem::path& path) auto ResolveCameraName(const std::string& directory_name, const camera::camera_constants_t& constants) -> std::string { - std::string resolved_name = directory_name.rfind("second_bot_", 0) == 0 + std::string resolved_name = directory_name.rfind("main_bot_", 0) == 0 ? directory_name - : "second_bot_" + directory_name; + : "main_bot_" + directory_name; if (!constants.contains(resolved_name)) { LOG(FATAL) << "Could not resolve camera constants name for directory: " From e86fb6c48b1bfd6be607d3b75f50c5387f730810 Mon Sep 17 00:00:00 2001 From: "carrotmercinary@gmail.com" <167659798+yasen5@users.noreply.github.com> Date: Sat, 25 Apr 2026 01:07:26 +0000 Subject: [PATCH 06/30] Cleanup --- constants/camera_constants.json | 14 +++++++------- src/localization/multi_camera_detector.h | 1 - src/localization/networktable_sender.cc | 2 +- src/localization/unambiguous_estimator.cc | 4 ++-- src/pathing/simulator.cc | 2 +- 5 files changed, 11 insertions(+), 12 deletions(-) diff --git a/constants/camera_constants.json b/constants/camera_constants.json index 3d715316..6305002c 100644 --- a/constants/camera_constants.json +++ b/constants/camera_constants.json @@ -33,7 +33,7 @@ "fps": null, "exposure": null, "port": 5803, - "detector_type": "opencv_cpu" + "detector_type": "austin_gpu" }, { "pipeline": "/dev/v4l/by-path/platform-3610000.usb-usb-0:2.3:1.0-video-index0", @@ -46,7 +46,7 @@ "fps": 60.0, "exposure": null, "port": 5802, - "detector_type": "opencv_cpu" + "detector_type": "austin_gpu" }, { "pipeline": "/dev/v4l/by-path/platform-3610000.usb-usb-0:2.1:1.0-video-index0", @@ -59,7 +59,7 @@ "fps": 60.0, "exposure": null, "port": 5801, - "detector_type": "opencv_cpu" + "detector_type": "austin_gpu" }, { "name": "second_bot_front", @@ -72,7 +72,7 @@ "max_payload_size": 1024, "stream_ratio": 0.5, "port": 5803, - "detector_type": "opencv_cpu" + "detector_type": "austin_gpu" }, { "name": "second_bot_right", @@ -85,7 +85,7 @@ "max_payload_size": 1024, "stream_ratio": 0.5, "port": 5802, - "detector_type": "opencv_cpu" + "detector_type": "austin_gpu" }, { "name": "second_bot_left", @@ -98,7 +98,7 @@ "max_payload_size": 1024, "stream_ratio": 0.5, "port": 5801, - "detector_type": "opencv_cpu" + "detector_type": "austin_gpu" } ] -} +} \ No newline at end of file diff --git a/src/localization/multi_camera_detector.h b/src/localization/multi_camera_detector.h index c5c729be..10d6b5c3 100644 --- a/src/localization/multi_camera_detector.h +++ b/src/localization/multi_camera_detector.h @@ -17,7 +17,6 @@ class MultiCameraDetector { -> std::vector>; [[nodiscard]] auto GetCVFrames() -> std::vector; [[nodiscard]] inline auto NumCameras() -> double { return cameras_.size(); } - void StartThreads(); ~MultiCameraDetector(); private: diff --git a/src/localization/networktable_sender.cc b/src/localization/networktable_sender.cc index eb133eb8..37d2a8c4 100644 --- a/src/localization/networktable_sender.cc +++ b/src/localization/networktable_sender.cc @@ -56,7 +56,7 @@ NetworkTableSender::NetworkTableSender(const std::string& camera_name, if (sim) { std::error_code ec; - log_.emplace("/bos/logs/sim2.wpilog", ec); + log_.emplace("/bos/logs/sim.wpilog", ec); if (ec) { std::cerr << "Failed to open log: " << ec.message() << '\n'; std::exit(0); diff --git a/src/localization/unambiguous_estimator.cc b/src/localization/unambiguous_estimator.cc index b125d6a9..8565b595 100644 --- a/src/localization/unambiguous_estimator.cc +++ b/src/localization/unambiguous_estimator.cc @@ -46,8 +46,8 @@ auto UnambiguousEstimator::Cost(const frc::Pose3d& a, const frc::Pose3d& b) frc::Rotation3d delta = a.Rotation() - b.Rotation(); double rotation = delta.Angle().value(); - constexpr double kRotationWeight = 3.0; // tune - return translation + kRotationWeight * rotation; + constexpr double krotation_weight = 0.1; // tune + return translation + krotation_weight * rotation; } auto UnambiguousEstimator::ComputeCost( diff --git a/src/pathing/simulator.cc b/src/pathing/simulator.cc index ccc0243f..128248cc 100644 --- a/src/pathing/simulator.cc +++ b/src/pathing/simulator.cc @@ -10,7 +10,7 @@ #include "src/pathing/pathing.h" auto main() -> int { - wpi::log::DataLogBackgroundWriter log{"/root/bos/logs", "sim2.wpilog"}; + wpi::log::DataLogBackgroundWriter log{"/root/bos/logs", "sim.wpilog"}; wpi::log::StructLogEntry poseLog(log, "/sim/Pose2d"); wpi::log::DoubleLogEntry accelXLog(log, "/sim/AccelX"); From 76c5ea89f2177d82c334033cea09bfedcfb4747f Mon Sep 17 00:00:00 2001 From: "carrotmercinary@gmail.com" <167659798+yasen5@users.noreply.github.com> Date: Sat, 25 Apr 2026 01:20:20 +0000 Subject: [PATCH 07/30] Revert change to disk camera --- src/camera/disk_camera.cc | 16 +++++++--------- src/localization/multi_camera_detector.cc | 5 +++++ 2 files changed, 12 insertions(+), 9 deletions(-) diff --git a/src/camera/disk_camera.cc b/src/camera/disk_camera.cc index d7d72fe7..eee12f85 100644 --- a/src/camera/disk_camera.cc +++ b/src/camera/disk_camera.cc @@ -23,12 +23,6 @@ DiskCamera::DiskCamera(std::string image_folder_path, double timestamp = std::stod(entry_name.erase(entry_name.size() - 4, 4)); - if (start.has_value() && timestamp < start_) { - continue; - } - if (end.has_value() && timestamp > end_) { - continue; - } // remove .png for timestamp image_paths_.push( timestamped_frame_path_t{.path = entry.path(), .timestamp = timestamp}); @@ -51,6 +45,12 @@ DiskCamera::DiskCamera(std::string image_folder_path, } image_paths_.pop(); entry.timestamp -= offset; + if (start.has_value() && entry.timestamp < start_) { + continue; + } + if (end.has_value() && entry.timestamp > end_) { + continue; + } normalized.push(entry); } image_paths_ = std::move(normalized); @@ -59,9 +59,7 @@ auto DiskCamera::GetFrame() -> timestamped_frame_t { if (image_paths_.empty()) { std::cout << "Finished reading all frames from DiskCamera. Folder path: " << image_folder_path_ << std::endl; - frc::DataLogManager::Stop(); - LOG(INFO) << "Reached the end of the file list"; - std::exit(0); + return {.invalid = true}; } double recorded_ts = image_paths_.top().timestamp; diff --git a/src/localization/multi_camera_detector.cc b/src/localization/multi_camera_detector.cc index 2bf072dc..fa8773e3 100644 --- a/src/localization/multi_camera_detector.cc +++ b/src/localization/multi_camera_detector.cc @@ -65,6 +65,11 @@ MultiCameraDetector::MultiCameraDetector( camera::timestamped_frame_t timestamped_frame; timestamped_frame = cameras_[i]->GetFrame(); if (timestamped_frame.invalid) { + if (image_paths.has_value()) { + frc::DataLogManager::Stop(); + LOG(INFO) << "Reached the end of the file list"; + std::exit(0); + } continue; // this is ok because GetFrame is blocking } if (timestamped_frame.timestamp - last_write_times_[i] > From 45446cf4eb42c7918b49c8887f07560fbc8fd3a2 Mon Sep 17 00:00:00 2001 From: "carrotmercinary@gmail.com" <167659798+yasen5@users.noreply.github.com> Date: Sat, 25 Apr 2026 21:51:47 +0000 Subject: [PATCH 08/30] Dp not working --- constants/camera_constants.json | 6 +++--- src/camera/cv_camera.cc | 9 ++++++--- src/camera/cv_camera.h | 1 + src/localization/multi_camera_detector.cc | 22 ++++++++++++++++++++-- src/localization/multi_camera_detector.h | 2 +- src/localization/position_solver.h | 2 +- src/localization/run_localization.cc | 6 +++++- src/localization/unambiguous_estimator.cc | 5 ++++- src/localization/unambiguous_estimator.h | 2 +- src/unambiguous_first.cc | 12 ++++++++---- 10 files changed, 50 insertions(+), 17 deletions(-) diff --git a/constants/camera_constants.json b/constants/camera_constants.json index 6305002c..aee1f454 100644 --- a/constants/camera_constants.json +++ b/constants/camera_constants.json @@ -46,7 +46,7 @@ "fps": 60.0, "exposure": null, "port": 5802, - "detector_type": "austin_gpu" + "detector_type": "opencv_cpu" }, { "pipeline": "/dev/v4l/by-path/platform-3610000.usb-usb-0:2.1:1.0-video-index0", @@ -59,7 +59,7 @@ "fps": 60.0, "exposure": null, "port": 5801, - "detector_type": "austin_gpu" + "detector_type": "opencv_cpu" }, { "name": "second_bot_front", @@ -101,4 +101,4 @@ "detector_type": "austin_gpu" } ] -} \ No newline at end of file +} diff --git a/src/camera/cv_camera.cc b/src/camera/cv_camera.cc index 96b6227f..e0ebeaad 100644 --- a/src/camera/cv_camera.cc +++ b/src/camera/cv_camera.cc @@ -67,8 +67,11 @@ auto CVCamera::GetFrame() -> timestamped_frame_t { timestamped_frame_t timestamped_frame; cv::Mat raw_image; if (!cap_.grab()) { - Restart(); - LOG(WARNING) << "Restarting camera"; + i_++; + if (i_ <= 5) { + Restart(); + LOG(WARNING) << "Restarting camera"; + } } timestamped_frame.timestamp = frc::Timer::GetFPGATimestamp().to(); cap_.retrieve(raw_image); @@ -76,7 +79,7 @@ auto CVCamera::GetFrame() -> timestamped_frame_t { raw_image.copyTo(timestamped_frame.frame); if (timestamped_frame.frame.empty()) { - timestamped_frame.frame = backup_image_; + timestamped_frame.invalid = true; } if (timestamped_frame.frame.channels() == 4) { cv::cvtColor(timestamped_frame.frame, timestamped_frame.frame, diff --git a/src/camera/cv_camera.h b/src/camera/cv_camera.h index 048d96b7..f8477485 100644 --- a/src/camera/cv_camera.h +++ b/src/camera/cv_camera.h @@ -21,6 +21,7 @@ class CVCamera : public ICamera { std::string pipeline_; std::optional log_path_; cv::Mat backup_image_; + int i_ = 0; }; } // namespace camera diff --git a/src/localization/multi_camera_detector.cc b/src/localization/multi_camera_detector.cc index fa8773e3..e9fbfd1a 100644 --- a/src/localization/multi_camera_detector.cc +++ b/src/localization/multi_camera_detector.cc @@ -65,6 +65,7 @@ MultiCameraDetector::MultiCameraDetector( camera::timestamped_frame_t timestamped_frame; timestamped_frame = cameras_[i]->GetFrame(); if (timestamped_frame.invalid) { + LOG(INFO) << "Skipping frame"; if (image_paths.has_value()) { frc::DataLogManager::Stop(); LOG(INFO) << "Reached the end of the file list"; @@ -79,12 +80,18 @@ MultiCameraDetector::MultiCameraDetector( } std::vector detections = detectors_[i]->GetTagDetections(timestamped_frame); + LOG(INFO) << "Received " << detections.size() << " detections"; + std::cout << has_new_detections_ << std::endl; { std::lock_guard lock(mutex_); timestamped_frames_[i] = std::move(timestamped_frame); tag_detections_[i] = std::move(detections); + LOG(INFO) << "Producer sees: " << tag_detections_[i].size(); } - has_new_detections_.release(); + LOG(INFO) << "Put in new detections"; + has_new_detections_.store(true, std::memory_order_release); + // has_new_detections_.notify_one(); + LOG(INFO) << "Added new detections"; } }); } @@ -92,12 +99,23 @@ MultiCameraDetector::MultiCameraDetector( auto MultiCameraDetector::GetTagDetections() -> std::vector> { - has_new_detections_.acquire(); + LOG(INFO) << "Trying to acquire"; + // has_new_detections_.wait(false, std::memory_order_acquire); + while (!has_new_detections_.load()) { + std::cout << "Waiting because this garbage is " + << has_new_detections_.load() << std::endl; + has_new_detections_.store(true); + std::cout << "Updated is " << has_new_detections_.load() << std::endl; + std::this_thread::sleep_for(std::chrono::duration(0.1)); + } + has_new_detections_.store(false, std::memory_order_release); + LOG(INFO) << "Acquired"; std::vector> tag_detections; { std::lock_guard lock(mutex_); tag_detections = tag_detections_; } + LOG(INFO) << "Consumer sees: " << tag_detections[0].size(); return tag_detections; } diff --git a/src/localization/multi_camera_detector.h b/src/localization/multi_camera_detector.h index 10d6b5c3..a7584776 100644 --- a/src/localization/multi_camera_detector.h +++ b/src/localization/multi_camera_detector.h @@ -30,7 +30,7 @@ class MultiCameraDetector { std::vector camera_threads_; std::mutex mutex_; std::atomic run_cameras_{true}; - std::binary_semaphore has_new_detections_{false}; + std::atomic has_new_detections_{false}; static constexpr int kenforced_streamer_fps = 30; }; diff --git a/src/localization/position_solver.h b/src/localization/position_solver.h index 3e3fd838..70ebae2b 100644 --- a/src/localization/position_solver.h +++ b/src/localization/position_solver.h @@ -45,7 +45,7 @@ class IPositionSolver { class IJointPositionSolver { public: virtual auto EstimatePosition( - std::vector>&& detection_batches, + std::vector>& detection_batches, bool reject_far_tags = true) -> std::optional = 0; virtual ~IJointPositionSolver() = default; }; diff --git a/src/localization/run_localization.cc b/src/localization/run_localization.cc index 8d3afacf..eb9b88b8 100644 --- a/src/localization/run_localization.cc +++ b/src/localization/run_localization.cc @@ -55,9 +55,13 @@ void RunJointLocalization( std::unique_ptr solver, std::unique_ptr sender, bool verbose) { while (true) { + auto detections = detector_source.GetTagDetections(); + LOG(INFO) << "Detections: " << detections.size(); std::optional estimated_pose = - solver->EstimatePosition(detector_source.GetTagDetections()); + solver->EstimatePosition(detections); + LOG(INFO) << "Estimated"; if (!estimated_pose.has_value()) { + LOG(INFO) << "Skipping"; continue; } sender->Send(estimated_pose.value()); diff --git a/src/localization/unambiguous_estimator.cc b/src/localization/unambiguous_estimator.cc index 8565b595..5bb14fad 100644 --- a/src/localization/unambiguous_estimator.cc +++ b/src/localization/unambiguous_estimator.cc @@ -211,8 +211,9 @@ auto UnambiguousEstimator::GetFilteredDetections( } auto UnambiguousEstimator::EstimatePosition( - std::vector>&& detection_batches, + std::vector>& detection_batches, bool reject_far_tags) -> std::optional { + LOG(INFO) << "Init"; utils::Timer computation_timer("Getting estimate", false); const auto& ambiguous_estimates = GetAmbiguousEstimates(detection_batches); std::vector best_solution; @@ -220,9 +221,11 @@ auto UnambiguousEstimator::EstimatePosition( use_prev_pose_ = true; double best_cost = std::numeric_limits::infinity(); + LOG(INFO) << "Searched"; double cost = SearchSolutions(ambiguous_estimates, 0, current_solution, best_solution, best_cost); if (best_solution.size() == 0) { + LOG(INFO) << "No detections"; return std::nullopt; } double avg_variance = 0; diff --git a/src/localization/unambiguous_estimator.h b/src/localization/unambiguous_estimator.h index cfe8dfac..b74a1f43 100644 --- a/src/localization/unambiguous_estimator.h +++ b/src/localization/unambiguous_estimator.h @@ -19,7 +19,7 @@ class UnambiguousEstimator : public IJointPositionSolver { const std::vector& camera_constants, bool verbose = false); auto EstimatePosition( - std::vector>&& detection_batches, + std::vector>& detection_batches, bool reject_far_tags = true) -> std::optional override; diff --git a/src/unambiguous_first.cc b/src/unambiguous_first.cc index 8859d522..5a3ee5c2 100644 --- a/src/unambiguous_first.cc +++ b/src/unambiguous_first.cc @@ -28,8 +28,12 @@ auto main() -> int { LOG(INFO) << "Started cameras"; std::this_thread::sleep_for(std::chrono::duration(2)); - localization::UnambiguousEstimator localizer(cameras); - std::vector> joint_sender; - joint_sender.emplace_back( - std::make_unique("Left")); + std::jthread thread([cameras] { + localization::MultiCameraDetector detector_source(cameras); + LOG(INFO) << "Created camera source"; + localization::RunJointLocalization( + detector_source, + std::make_unique(cameras), + std::make_unique("Left", false)); + }); } From 51244d584505700c1a18dee3c14a9b9b3de9eba0 Mon Sep 17 00:00:00 2001 From: "carrotmercinary@gmail.com" <167659798+yasen5@users.noreply.github.com> Date: Sat, 25 Apr 2026 21:55:26 +0000 Subject: [PATCH 09/30] Log ptr --- src/localization/multi_camera_detector.cc | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/localization/multi_camera_detector.cc b/src/localization/multi_camera_detector.cc index e9fbfd1a..98434974 100644 --- a/src/localization/multi_camera_detector.cc +++ b/src/localization/multi_camera_detector.cc @@ -33,8 +33,7 @@ MultiCameraDetector::MultiCameraDetector( cameras_.push_back(std::make_unique( camera_constants_[i], status, camera_log_dest)); if (!status.ok()) { - LOG(WARNING) << "Unable to create uvc camera for unambiguous solver: " - << status.message(); + LOG(WARNING) << "Unable to create uvc camera: " << status.message(); } } else { cameras_.push_back(std::make_unique( @@ -103,9 +102,8 @@ auto MultiCameraDetector::GetTagDetections() // has_new_detections_.wait(false, std::memory_order_acquire); while (!has_new_detections_.load()) { std::cout << "Waiting because this garbage is " - << has_new_detections_.load() << std::endl; - has_new_detections_.store(true); - std::cout << "Updated is " << has_new_detections_.load() << std::endl; + << has_new_detections_.load() + << " with ptr: " << &has_new_detections_ << std::endl; std::this_thread::sleep_for(std::chrono::duration(0.1)); } has_new_detections_.store(false, std::memory_order_release); From 7d2036e6f1b42d249ec38472ec6dc26a1ab76f96 Mon Sep 17 00:00:00 2001 From: "carrotmercinary@gmail.com" <167659798+yasen5@users.noreply.github.com> Date: Sun, 26 Apr 2026 22:29:08 +0000 Subject: [PATCH 10/30] Remove duplicate multicameradetector which was causing threading issues --- src/unambiguous_first.cc | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/unambiguous_first.cc b/src/unambiguous_first.cc index 5a3ee5c2..d631f34a 100644 --- a/src/unambiguous_first.cc +++ b/src/unambiguous_first.cc @@ -20,10 +20,7 @@ auto main() -> int { LOG(INFO) << "Starting cameras"; - std::vector cameras{ - camera_constants.at("main_bot_left"), - camera_constants.at("main_bot_right")}; - localization::MultiCameraDetector detector(cameras); + std::vector cameras{camera_constants.at("dev_orin")}; LOG(INFO) << "Started cameras"; std::this_thread::sleep_for(std::chrono::duration(2)); From 1ade23ffee06a93b53a273e56664473e450f8d26 Mon Sep 17 00:00:00 2001 From: "carrotmercinary@gmail.com" <167659798+yasen5@users.noreply.github.com> Date: Sun, 26 Apr 2026 22:50:43 +0000 Subject: [PATCH 11/30] Works on dev orin --- constants/camera_constants.json | 11 +++++++++-- src/localization/gpu_apriltag_detector.h | 2 +- src/localization/multi_camera_detector.cc | 15 +++++---------- src/localization/run_localization.cc | 1 - src/unambiguous_first.cc | 2 +- src/utils/nt_utils.cc | 19 ++++++++++++++----- src/utils/nt_utils.h | 1 + 7 files changed, 31 insertions(+), 20 deletions(-) diff --git a/constants/camera_constants.json b/constants/camera_constants.json index aee1f454..bbc2585b 100644 --- a/constants/camera_constants.json +++ b/constants/camera_constants.json @@ -17,10 +17,17 @@ "name": "mipi1" }, { - "pipeline": "/dev/v4l/by-path/platform-3610000.usb-usb-0:2.1:1.0-video-index0", + "pipeline": "/dev/v4l/by-path/platform-3610000.usb-usb-0:2.4:1.0-video-index0", "intrinsics_path": "/bos/constants/misc/dev_orin_intrinsics.json", "extrinsics_path": "/bos/constants/misc/dev_orin_extrinsics.json", - "name": "dev_orin" + "name": "dev_orin", + "backlight": null, + "frame_width": 1280, + "frame_height": 800, + "fps": null, + "exposure": null, + "port": 5801, + "detector_type": "austin_gpu" }, { "name": "main_bot_front", diff --git a/src/localization/gpu_apriltag_detector.h b/src/localization/gpu_apriltag_detector.h index 088cbfa7..3cf26487 100644 --- a/src/localization/gpu_apriltag_detector.h +++ b/src/localization/gpu_apriltag_detector.h @@ -24,6 +24,6 @@ class GPUAprilTagDetector : public IAprilTagDetector { apriltag_detector_t* apriltag_detector_; std::unique_ptr gpu_detector_; static constexpr vision::ImageFormat image_format = - vision::ImageFormat::MONO8; + vision::ImageFormat::YUYV422; }; } // namespace localization diff --git a/src/localization/multi_camera_detector.cc b/src/localization/multi_camera_detector.cc index 98434974..70816be1 100644 --- a/src/localization/multi_camera_detector.cc +++ b/src/localization/multi_camera_detector.cc @@ -85,11 +85,12 @@ MultiCameraDetector::MultiCameraDetector( std::lock_guard lock(mutex_); timestamped_frames_[i] = std::move(timestamped_frame); tag_detections_[i] = std::move(detections); - LOG(INFO) << "Producer sees: " << tag_detections_[i].size(); + // LOG(INFO) << "Producer sees: " << tag_detections_[i].size() + // << " with ptr: " << &has_new_detections_ << std::endl; } LOG(INFO) << "Put in new detections"; has_new_detections_.store(true, std::memory_order_release); - // has_new_detections_.notify_one(); + has_new_detections_.notify_one(); LOG(INFO) << "Added new detections"; } }); @@ -99,13 +100,7 @@ MultiCameraDetector::MultiCameraDetector( auto MultiCameraDetector::GetTagDetections() -> std::vector> { LOG(INFO) << "Trying to acquire"; - // has_new_detections_.wait(false, std::memory_order_acquire); - while (!has_new_detections_.load()) { - std::cout << "Waiting because this garbage is " - << has_new_detections_.load() - << " with ptr: " << &has_new_detections_ << std::endl; - std::this_thread::sleep_for(std::chrono::duration(0.1)); - } + has_new_detections_.wait(false, std::memory_order_acquire); has_new_detections_.store(false, std::memory_order_release); LOG(INFO) << "Acquired"; std::vector> tag_detections; @@ -113,7 +108,7 @@ auto MultiCameraDetector::GetTagDetections() std::lock_guard lock(mutex_); tag_detections = tag_detections_; } - LOG(INFO) << "Consumer sees: " << tag_detections[0].size(); + // LOG(INFO) << "Consumer sees: " << tag_detections[0].size(); return tag_detections; } diff --git a/src/localization/run_localization.cc b/src/localization/run_localization.cc index eb9b88b8..15f33ffc 100644 --- a/src/localization/run_localization.cc +++ b/src/localization/run_localization.cc @@ -56,7 +56,6 @@ void RunJointLocalization( std::unique_ptr sender, bool verbose) { while (true) { auto detections = detector_source.GetTagDetections(); - LOG(INFO) << "Detections: " << detections.size(); std::optional estimated_pose = solver->EstimatePosition(detections); LOG(INFO) << "Estimated"; diff --git a/src/unambiguous_first.cc b/src/unambiguous_first.cc index d631f34a..dede6614 100644 --- a/src/unambiguous_first.cc +++ b/src/unambiguous_first.cc @@ -13,7 +13,7 @@ using camera::camera_constants_t; auto main() -> int { - utils::StartNetworktables(9971); + utils::HostNetworktables(); std::string log_path = frc::DataLogManager::GetLogDir(); camera_constants_t camera_constants = camera::GetCameraConstants(); diff --git a/src/utils/nt_utils.cc b/src/utils/nt_utils.cc index 743ccb34..ad99a086 100644 --- a/src/utils/nt_utils.cc +++ b/src/utils/nt_utils.cc @@ -15,11 +15,10 @@ namespace utils { // Publishes logname such as log32 to networktables so we can easily find match logs static void PublishLogName() { std::string path = frc::DataLogManager::GetLogDir(); - static auto log_name_publisher = - nt::NetworkTableInstance::GetDefault() - .GetTable("Orin/") - ->GetStringTopic("LogName") - .Publish(); + static auto log_name_publisher = nt::NetworkTableInstance::GetDefault() + .GetTable("Orin/") + ->GetStringTopic("LogName") + .Publish(); log_name_publisher.Set(path); } @@ -44,6 +43,16 @@ void StartNetworktables(int team_number) { LOG(INFO) << "Connected to rio!"; } +void HostNetworktables() { + nt::NetworkTableInstance inst = nt::NetworkTableInstance::GetDefault(); + inst.StopLocal(); + inst.StopClient(); + inst.StartServer("orin_localization"); + std::string log_path = GetNewLogPath(); + LOG(INFO) << "Log path: " << log_path; + frc::DataLogManager::Start(log_path); +} + auto GetNewLogPath(const std::string& log_dir) -> std::string { int id = 0; while (fs::exists(fmt::format("/bos/logs/log{}", id))) { diff --git a/src/utils/nt_utils.h b/src/utils/nt_utils.h index 84fedd41..8c6df0cd 100644 --- a/src/utils/nt_utils.h +++ b/src/utils/nt_utils.h @@ -3,5 +3,6 @@ namespace utils { // Sets up logging and networktables. Should be called at the beggining of every robot's main void StartNetworktables(int team_number = 971); +void HostNetworktables(); auto GetNewLogPath(const std::string& log_dir = "/bos/logs") -> std::string; } // namespace utils From 8ffa73f508a0de2998d7b0ba64a0c48a4ace233a Mon Sep 17 00:00:00 2001 From: "carrotmercinary@gmail.com" <167659798+yasen5@users.noreply.github.com> Date: Sun, 26 Apr 2026 23:51:15 +0000 Subject: [PATCH 12/30] Cleanup --- src/camera/cv_camera.cc | 9 +++------ src/camera/cv_camera.h | 1 - src/localization/multi_camera_detector.cc | 10 ---------- src/localization/run_localization.cc | 2 -- src/localization/unambiguous_estimator.cc | 3 --- 5 files changed, 3 insertions(+), 22 deletions(-) diff --git a/src/camera/cv_camera.cc b/src/camera/cv_camera.cc index e0ebeaad..59f23f08 100644 --- a/src/camera/cv_camera.cc +++ b/src/camera/cv_camera.cc @@ -66,12 +66,9 @@ CVCamera::CVCamera(const CameraConstant& c, std::optional log_path) auto CVCamera::GetFrame() -> timestamped_frame_t { timestamped_frame_t timestamped_frame; cv::Mat raw_image; - if (!cap_.grab()) { - i_++; - if (i_ <= 5) { - Restart(); - LOG(WARNING) << "Restarting camera"; - } + while (!cap_.grab()) { + Restart(); + LOG(WARNING) << "Restarting camera"; } timestamped_frame.timestamp = frc::Timer::GetFPGATimestamp().to(); cap_.retrieve(raw_image); diff --git a/src/camera/cv_camera.h b/src/camera/cv_camera.h index f8477485..048d96b7 100644 --- a/src/camera/cv_camera.h +++ b/src/camera/cv_camera.h @@ -21,7 +21,6 @@ class CVCamera : public ICamera { std::string pipeline_; std::optional log_path_; cv::Mat backup_image_; - int i_ = 0; }; } // namespace camera diff --git a/src/localization/multi_camera_detector.cc b/src/localization/multi_camera_detector.cc index 70816be1..768d0c16 100644 --- a/src/localization/multi_camera_detector.cc +++ b/src/localization/multi_camera_detector.cc @@ -64,7 +64,6 @@ MultiCameraDetector::MultiCameraDetector( camera::timestamped_frame_t timestamped_frame; timestamped_frame = cameras_[i]->GetFrame(); if (timestamped_frame.invalid) { - LOG(INFO) << "Skipping frame"; if (image_paths.has_value()) { frc::DataLogManager::Stop(); LOG(INFO) << "Reached the end of the file list"; @@ -79,19 +78,13 @@ MultiCameraDetector::MultiCameraDetector( } std::vector detections = detectors_[i]->GetTagDetections(timestamped_frame); - LOG(INFO) << "Received " << detections.size() << " detections"; - std::cout << has_new_detections_ << std::endl; { std::lock_guard lock(mutex_); timestamped_frames_[i] = std::move(timestamped_frame); tag_detections_[i] = std::move(detections); - // LOG(INFO) << "Producer sees: " << tag_detections_[i].size() - // << " with ptr: " << &has_new_detections_ << std::endl; } - LOG(INFO) << "Put in new detections"; has_new_detections_.store(true, std::memory_order_release); has_new_detections_.notify_one(); - LOG(INFO) << "Added new detections"; } }); } @@ -99,16 +92,13 @@ MultiCameraDetector::MultiCameraDetector( auto MultiCameraDetector::GetTagDetections() -> std::vector> { - LOG(INFO) << "Trying to acquire"; has_new_detections_.wait(false, std::memory_order_acquire); has_new_detections_.store(false, std::memory_order_release); - LOG(INFO) << "Acquired"; std::vector> tag_detections; { std::lock_guard lock(mutex_); tag_detections = tag_detections_; } - // LOG(INFO) << "Consumer sees: " << tag_detections[0].size(); return tag_detections; } diff --git a/src/localization/run_localization.cc b/src/localization/run_localization.cc index 15f33ffc..d8a767a2 100644 --- a/src/localization/run_localization.cc +++ b/src/localization/run_localization.cc @@ -58,9 +58,7 @@ void RunJointLocalization( auto detections = detector_source.GetTagDetections(); std::optional estimated_pose = solver->EstimatePosition(detections); - LOG(INFO) << "Estimated"; if (!estimated_pose.has_value()) { - LOG(INFO) << "Skipping"; continue; } sender->Send(estimated_pose.value()); diff --git a/src/localization/unambiguous_estimator.cc b/src/localization/unambiguous_estimator.cc index 5bb14fad..8daad897 100644 --- a/src/localization/unambiguous_estimator.cc +++ b/src/localization/unambiguous_estimator.cc @@ -213,7 +213,6 @@ auto UnambiguousEstimator::GetFilteredDetections( auto UnambiguousEstimator::EstimatePosition( std::vector>& detection_batches, bool reject_far_tags) -> std::optional { - LOG(INFO) << "Init"; utils::Timer computation_timer("Getting estimate", false); const auto& ambiguous_estimates = GetAmbiguousEstimates(detection_batches); std::vector best_solution; @@ -221,11 +220,9 @@ auto UnambiguousEstimator::EstimatePosition( use_prev_pose_ = true; double best_cost = std::numeric_limits::infinity(); - LOG(INFO) << "Searched"; double cost = SearchSolutions(ambiguous_estimates, 0, current_solution, best_solution, best_cost); if (best_solution.size() == 0) { - LOG(INFO) << "No detections"; return std::nullopt; } double avg_variance = 0; From c450544a388f9b86e9338dc9f0214041503efbd3 Mon Sep 17 00:00:00 2001 From: "carrotmercinary@gmail.com" <167659798+yasen5@users.noreply.github.com> Date: Mon, 27 Apr 2026 00:10:52 +0000 Subject: [PATCH 13/30] Cleanup p2 --- constants/camera_constants.json | 17 +++++------------ src/localization/gpu_apriltag_detector.h | 2 +- src/localization/multi_camera_detector.cc | 8 ++++++-- src/localization/unambiguous_estimator.cc | 5 ++--- src/localization/unambiguous_estimator.h | 4 +--- src/test/integration_test/localization_test.cc | 4 ++-- src/test/integration_test/localization_test2.cc | 4 ---- src/unambiguous_first.cc | 2 +- 8 files changed, 18 insertions(+), 28 deletions(-) diff --git a/constants/camera_constants.json b/constants/camera_constants.json index bbc2585b..6305002c 100644 --- a/constants/camera_constants.json +++ b/constants/camera_constants.json @@ -17,17 +17,10 @@ "name": "mipi1" }, { - "pipeline": "/dev/v4l/by-path/platform-3610000.usb-usb-0:2.4:1.0-video-index0", + "pipeline": "/dev/v4l/by-path/platform-3610000.usb-usb-0:2.1:1.0-video-index0", "intrinsics_path": "/bos/constants/misc/dev_orin_intrinsics.json", "extrinsics_path": "/bos/constants/misc/dev_orin_extrinsics.json", - "name": "dev_orin", - "backlight": null, - "frame_width": 1280, - "frame_height": 800, - "fps": null, - "exposure": null, - "port": 5801, - "detector_type": "austin_gpu" + "name": "dev_orin" }, { "name": "main_bot_front", @@ -53,7 +46,7 @@ "fps": 60.0, "exposure": null, "port": 5802, - "detector_type": "opencv_cpu" + "detector_type": "austin_gpu" }, { "pipeline": "/dev/v4l/by-path/platform-3610000.usb-usb-0:2.1:1.0-video-index0", @@ -66,7 +59,7 @@ "fps": 60.0, "exposure": null, "port": 5801, - "detector_type": "opencv_cpu" + "detector_type": "austin_gpu" }, { "name": "second_bot_front", @@ -108,4 +101,4 @@ "detector_type": "austin_gpu" } ] -} +} \ No newline at end of file diff --git a/src/localization/gpu_apriltag_detector.h b/src/localization/gpu_apriltag_detector.h index 3cf26487..088cbfa7 100644 --- a/src/localization/gpu_apriltag_detector.h +++ b/src/localization/gpu_apriltag_detector.h @@ -24,6 +24,6 @@ class GPUAprilTagDetector : public IAprilTagDetector { apriltag_detector_t* apriltag_detector_; std::unique_ptr gpu_detector_; static constexpr vision::ImageFormat image_format = - vision::ImageFormat::YUYV422; + vision::ImageFormat::MONO8; }; } // namespace localization diff --git a/src/localization/multi_camera_detector.cc b/src/localization/multi_camera_detector.cc index 768d0c16..cbdd5af5 100644 --- a/src/localization/multi_camera_detector.cc +++ b/src/localization/multi_camera_detector.cc @@ -21,8 +21,12 @@ MultiCameraDetector::MultiCameraDetector( camera_threads_.reserve(camera_constants_.size()); streamers_.reserve(camera_constants_.size()); for (size_t i = 0; i < camera_constants_.size(); i++) { - streamers_.emplace_back(camera_constants_[i].name, 5801 + i, 30, 1080, - 1080); // TODO move constants to steamer class + CHECK(camera_constants_[i].frame_width.has_value() && + camera_constants_[i].frame_width.has_value()); + streamers_.emplace_back(camera_constants_[i].name, + camera_constants_[i].port.value_or(5801 + i), 30, + camera_constants_[i].frame_width, + camera_constants_[i].frame_height); const std::string camera_log_dest = fmt::format("{}/{}", log_path, camera_constants_[i].name); if (image_paths.has_value()) { diff --git a/src/localization/unambiguous_estimator.cc b/src/localization/unambiguous_estimator.cc index 8daad897..ea4a4de9 100644 --- a/src/localization/unambiguous_estimator.cc +++ b/src/localization/unambiguous_estimator.cc @@ -30,9 +30,8 @@ namespace localization { UnambiguousEstimator::UnambiguousEstimator( - const std::vector& camera_constants, - bool verbose) - : prev_timestamps_(camera_constants.size()), verbose_(verbose) { + const std::vector& camera_constants) + : prev_timestamps_(camera_constants.size()) { solvers_.reserve(camera_constants.size()); for (const auto& camera_constant : camera_constants) { solvers_.emplace_back(camera_constant); diff --git a/src/localization/unambiguous_estimator.h b/src/localization/unambiguous_estimator.h index b74a1f43..1b058b18 100644 --- a/src/localization/unambiguous_estimator.h +++ b/src/localization/unambiguous_estimator.h @@ -16,8 +16,7 @@ namespace localization { class UnambiguousEstimator : public IJointPositionSolver { public: UnambiguousEstimator( - const std::vector& camera_constants, - bool verbose = false); + const std::vector& camera_constants); auto EstimatePosition( std::vector>& detection_batches, bool reject_far_tags = true) @@ -46,6 +45,5 @@ class UnambiguousEstimator : public IJointPositionSolver { std::optional prev_pose_estimate_ = std::nullopt; static constexpr double kuse_prev_pose_threshold = 100; // tune bool use_prev_pose_ = false; - const bool verbose_; }; } // namespace localization diff --git a/src/test/integration_test/localization_test.cc b/src/test/integration_test/localization_test.cc index 1334ad4e..be271c23 100644 --- a/src/test/integration_test/localization_test.cc +++ b/src/test/integration_test/localization_test.cc @@ -69,9 +69,9 @@ auto FindCameraFolders(const std::filesystem::path& path) auto ResolveCameraName(const std::string& directory_name, const camera::camera_constants_t& constants) -> std::string { - std::string resolved_name = directory_name.rfind("second_bot_", 0) == 0 + std::string resolved_name = directory_name.rfind("main_bot_", 0) == 0 ? directory_name - : "second_bot_" + directory_name; + : "main_bot_" + directory_name; if (!constants.contains(resolved_name)) { LOG(FATAL) << "Could not resolve camera constants name for directory: " diff --git a/src/test/integration_test/localization_test2.cc b/src/test/integration_test/localization_test2.cc index 572c6731..58030649 100644 --- a/src/test/integration_test/localization_test2.cc +++ b/src/test/integration_test/localization_test2.cc @@ -23,10 +23,6 @@ #include "src/utils/camera_utils.h" #include "src/utils/log.h" -// Example command: -// ./build/src/test/integration_test/localization_test --camera_name=main_bot_right --image_folder=logs/log181/right --speed=0.5 -// To each camera has two streams. 1. Raw video stream, 2. Position estimate stream. The port for raw video stream is 580x and the port for position estimate stream is 480x. x is different for each camera. It starts at 1 and counts up. For example, to view the third camera's position estimate stream, go to localhost:4803 - ABSL_FLAG(std::string, image_folder, "", //NOLINT "Path to folder of test images"); ABSL_FLAG(std::optional, camera_name, std::nullopt, //NOLINT diff --git a/src/unambiguous_first.cc b/src/unambiguous_first.cc index dede6614..d631f34a 100644 --- a/src/unambiguous_first.cc +++ b/src/unambiguous_first.cc @@ -13,7 +13,7 @@ using camera::camera_constants_t; auto main() -> int { - utils::HostNetworktables(); + utils::StartNetworktables(9971); std::string log_path = frc::DataLogManager::GetLogDir(); camera_constants_t camera_constants = camera::GetCameraConstants(); From b5e0f241b3dafcfeb3427ce2eccb39f5f1ef18ee Mon Sep 17 00:00:00 2001 From: "carrotmercinary@gmail.com" <167659798+yasen5@users.noreply.github.com> Date: Mon, 27 Apr 2026 00:32:37 +0000 Subject: [PATCH 14/30] I love optionals --- src/localization/multi_camera_detector.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/localization/multi_camera_detector.cc b/src/localization/multi_camera_detector.cc index cbdd5af5..4193becd 100644 --- a/src/localization/multi_camera_detector.cc +++ b/src/localization/multi_camera_detector.cc @@ -25,8 +25,8 @@ MultiCameraDetector::MultiCameraDetector( camera_constants_[i].frame_width.has_value()); streamers_.emplace_back(camera_constants_[i].name, camera_constants_[i].port.value_or(5801 + i), 30, - camera_constants_[i].frame_width, - camera_constants_[i].frame_height); + camera_constants_[i].frame_width.value(), + camera_constants_[i].frame_height.value()); const std::string camera_log_dest = fmt::format("{}/{}", log_path, camera_constants_[i].name); if (image_paths.has_value()) { From 11a4dbaf4f70ff1891a3626899995ffe178b9e1f Mon Sep 17 00:00:00 2001 From: "carrotmercinary@gmail.com" <167659798+yasen5@users.noreply.github.com> Date: Mon, 27 Apr 2026 03:17:38 +0000 Subject: [PATCH 15/30] Switch back to real cameras --- src/unambiguous_first.cc | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/unambiguous_first.cc b/src/unambiguous_first.cc index d631f34a..9400050e 100644 --- a/src/unambiguous_first.cc +++ b/src/unambiguous_first.cc @@ -20,7 +20,9 @@ auto main() -> int { LOG(INFO) << "Starting cameras"; - std::vector cameras{camera_constants.at("dev_orin")}; + std::vector cameras{ + camera_constants.at("main_bot_left"), + camera_constants.at("main_bot_right")}; LOG(INFO) << "Started cameras"; std::this_thread::sleep_for(std::chrono::duration(2)); From 940a39ac93989e45f68e899245321d3f988ce6b5 Mon Sep 17 00:00:00 2001 From: "carrotmercinary@gmail.com" <167659798+yasen5@users.noreply.github.com> Date: Mon, 27 Apr 2026 04:35:55 +0000 Subject: [PATCH 16/30] Address comments --- src/localization/unambiguous_estimator.cc | 4 +--- src/localization/unambiguous_estimator.h | 1 - src/utils/nt_utils.cc | 2 +- src/utils/nt_utils.h | 2 +- 4 files changed, 3 insertions(+), 6 deletions(-) diff --git a/src/localization/unambiguous_estimator.cc b/src/localization/unambiguous_estimator.cc index ea4a4de9..021fc99d 100644 --- a/src/localization/unambiguous_estimator.cc +++ b/src/localization/unambiguous_estimator.cc @@ -30,8 +30,7 @@ namespace localization { UnambiguousEstimator::UnambiguousEstimator( - const std::vector& camera_constants) - : prev_timestamps_(camera_constants.size()) { + const std::vector& camera_constants) { solvers_.reserve(camera_constants.size()); for (const auto& camera_constant : camera_constants) { solvers_.emplace_back(camera_constant); @@ -157,7 +156,6 @@ auto UnambiguousEstimator::GetAmbiguousEstimates( if (!filtered_detections[i].has_value()) { continue; } - prev_timestamps_[i] = filtered_detections[i]->at(0).timestamp; std::optional est = solvers_[i].EstimatePositionAmbiguous(filtered_detections[i].value(), diff --git a/src/localization/unambiguous_estimator.h b/src/localization/unambiguous_estimator.h index 1b058b18..31f8cca2 100644 --- a/src/localization/unambiguous_estimator.h +++ b/src/localization/unambiguous_estimator.h @@ -41,7 +41,6 @@ class UnambiguousEstimator : public IJointPositionSolver { std::vector solvers_; std::mutex mutex_; - std::vector prev_timestamps_; std::optional prev_pose_estimate_ = std::nullopt; static constexpr double kuse_prev_pose_threshold = 100; // tune bool use_prev_pose_ = false; diff --git a/src/utils/nt_utils.cc b/src/utils/nt_utils.cc index ad99a086..7eaba3ad 100644 --- a/src/utils/nt_utils.cc +++ b/src/utils/nt_utils.cc @@ -43,7 +43,7 @@ void StartNetworktables(int team_number) { LOG(INFO) << "Connected to rio!"; } -void HostNetworktables() { +void StartNetworktablesAsHost() { nt::NetworkTableInstance inst = nt::NetworkTableInstance::GetDefault(); inst.StopLocal(); inst.StopClient(); diff --git a/src/utils/nt_utils.h b/src/utils/nt_utils.h index 8c6df0cd..a84c2f22 100644 --- a/src/utils/nt_utils.h +++ b/src/utils/nt_utils.h @@ -3,6 +3,6 @@ namespace utils { // Sets up logging and networktables. Should be called at the beggining of every robot's main void StartNetworktables(int team_number = 971); -void HostNetworktables(); +void StartNetworktablesAsHost(); auto GetNewLogPath(const std::string& log_dir = "/bos/logs") -> std::string; } // namespace utils From c01f85f424f48ced6f597d1d6b6a583e5aa712e9 Mon Sep 17 00:00:00 2001 From: "carrotmercinary@gmail.com" <167659798+yasen5@users.noreply.github.com> Date: Mon, 27 Apr 2026 04:46:26 +0000 Subject: [PATCH 17/30] Address codex comments --- src/localization/unambiguous_estimator.cc | 19 +++++++++++-------- src/main_bot_main.cc | 4 ++-- src/second_bot_main.cc | 4 ++-- .../integration_test/localization_test.cc | 2 +- src/unambiguous_first.cc | 6 ++---- src/unambiguous_second.cc | 19 +++++++++---------- 6 files changed, 27 insertions(+), 27 deletions(-) diff --git a/src/localization/unambiguous_estimator.cc b/src/localization/unambiguous_estimator.cc index 021fc99d..32340f42 100644 --- a/src/localization/unambiguous_estimator.cc +++ b/src/localization/unambiguous_estimator.cc @@ -237,14 +237,17 @@ auto UnambiguousEstimator::EstimatePosition( avg_variance /= best_solution.size(); const int num_tags = tag_ids.size(); double latency = computation_timer.Stop(); - return std::make_optional( - {.pose = WeightedAveragePose(best_solution), - .variance = avg_variance, - .timestamp = avg_timestamp, - .num_tags = num_tags, - .latency = latency, - .invalid = invalid, - .loss = cost}); + std::optional estimate = + std::make_optional( + {.pose = WeightedAveragePose(best_solution), + .variance = avg_variance, + .timestamp = avg_timestamp, + .num_tags = num_tags, + .latency = latency, + .invalid = invalid, + .loss = cost}); + prev_pose_estimate_ = estimate; + return estimate; } } // namespace localization diff --git a/src/main_bot_main.cc b/src/main_bot_main.cc index b41b3079..29ffd54a 100644 --- a/src/main_bot_main.cc +++ b/src/main_bot_main.cc @@ -41,7 +41,7 @@ auto main() -> int { camera_constants.at("main_bot_left").intrinsics_path.value())), std::make_unique( camera_constants.at("main_bot_left")), - std::move(left_sender), 5802, false); + std::move(left_sender), 5802); }); std::thread right_thread([&]() { @@ -64,7 +64,7 @@ auto main() -> int { camera_constants.at("main_bot_right").intrinsics_path.value())), std::make_unique( camera_constants.at("main_bot_right")), - std::move(right_sender), 5803, false); + std::move(right_sender), 5803); }); LOG(INFO) << "Started estimators"; diff --git a/src/second_bot_main.cc b/src/second_bot_main.cc index cd1e341e..dc76ada2 100644 --- a/src/second_bot_main.cc +++ b/src/second_bot_main.cc @@ -39,7 +39,7 @@ auto main() -> int { .intrinsics_path.value())), std::make_unique( camera_constants.at("second_bot_left")), - std::move(left_sender), false); + std::move(left_sender)); }); std::thread right_thread([&]() { @@ -62,7 +62,7 @@ auto main() -> int { .intrinsics_path.value())), std::make_unique( camera_constants.at("second_bot_right")), - std::move(right_sender), false); + std::move(right_sender)); }); // TODO front camera diff --git a/src/test/integration_test/localization_test.cc b/src/test/integration_test/localization_test.cc index be271c23..2799f961 100644 --- a/src/test/integration_test/localization_test.cc +++ b/src/test/integration_test/localization_test.cc @@ -145,7 +145,7 @@ auto main(int argc, char** argv) -> int { frame.cols, frame.rows, utils::ReadIntrinsics(camera_constant.intrinsics_path.value())), std::make_unique(camera_constant), - std::move(senders), true); + std::move(senders), std::nullopt, true); }); } diff --git a/src/unambiguous_first.cc b/src/unambiguous_first.cc index 9400050e..ae59ed1f 100644 --- a/src/unambiguous_first.cc +++ b/src/unambiguous_first.cc @@ -24,12 +24,10 @@ auto main() -> int { camera_constants.at("main_bot_left"), camera_constants.at("main_bot_right")}; - LOG(INFO) << "Started cameras"; - std::this_thread::sleep_for(std::chrono::duration(2)); - std::jthread thread([cameras] { localization::MultiCameraDetector detector_source(cameras); - LOG(INFO) << "Created camera source"; + LOG(INFO) << "Started cameras"; + std::this_thread::sleep_for(std::chrono::duration(2)); localization::RunJointLocalization( detector_source, std::make_unique(cameras), diff --git a/src/unambiguous_second.cc b/src/unambiguous_second.cc index 0ee0349f..f8ea9c92 100644 --- a/src/unambiguous_second.cc +++ b/src/unambiguous_second.cc @@ -17,19 +17,18 @@ auto main() -> int { std::string log_path = frc::DataLogManager::GetLogDir(); camera_constants_t camera_constants = camera::GetCameraConstants(); - LOG(INFO) << "Starting cameras"; - std::vector cameras{ camera_constants.at("second_bot_left"), camera_constants.at("second_bot_right"), camera_constants.at("second_bot_front")}; - localization::MultiCameraDetector detector(cameras); - - LOG(INFO) << "Started cameras"; - std::this_thread::sleep_for(std::chrono::duration(2)); - localization::UnambiguousEstimator localizer(cameras); - std::vector> joint_sender; - joint_sender.emplace_back( - std::make_unique("Left")); + std::jthread thread([cameras] { + localization::MultiCameraDetector detector_source(cameras); + LOG(INFO) << "Started cameras"; + std::this_thread::sleep_for(std::chrono::duration(2)); + localization::RunJointLocalization( + detector_source, + std::make_unique(cameras), + std::make_unique("Left", false)); + }); } From 389daf37be2c2905eb2c243b358c2a24a5f7f9b6 Mon Sep 17 00:00:00 2001 From: "carrotmercinary@gmail.com" <167659798+yasen5@users.noreply.github.com> Date: Tue, 28 Apr 2026 22:30:52 +0000 Subject: [PATCH 18/30] Tested --- constants/camera_constants.json | 13 +++++++++++-- src/camera/camera_constants.cc | 2 ++ src/camera/camera_constants.h | 1 + src/camera/uvc_camera.cc | 1 + src/localization/multi_camera_detector.cc | 5 +++-- src/localization/multi_camera_detector.h | 2 +- src/second_bot_main.cc | 2 +- src/unambiguous_first.cc | 6 ++---- src/unambiguous_second.cc | 5 +---- 9 files changed, 23 insertions(+), 14 deletions(-) diff --git a/constants/camera_constants.json b/constants/camera_constants.json index 6305002c..9ef22e72 100644 --- a/constants/camera_constants.json +++ b/constants/camera_constants.json @@ -20,7 +20,16 @@ "pipeline": "/dev/v4l/by-path/platform-3610000.usb-usb-0:2.1:1.0-video-index0", "intrinsics_path": "/bos/constants/misc/dev_orin_intrinsics.json", "extrinsics_path": "/bos/constants/misc/dev_orin_extrinsics.json", - "name": "dev_orin" + "name": "dev_orin", + "backlight": null, + "frame_width": 1280, + "frame_height": 800, + "fps": 100.0, + "exposure": null, + "port": 5801, + "detector_type": "austin_gpu", + "serial_id": "devorin", + "streamer_fps": 1 }, { "name": "main_bot_front", @@ -101,4 +110,4 @@ "detector_type": "austin_gpu" } ] -} \ No newline at end of file +} diff --git a/src/camera/camera_constants.cc b/src/camera/camera_constants.cc index 79762019..4f25e73a 100644 --- a/src/camera/camera_constants.cc +++ b/src/camera/camera_constants.cc @@ -82,6 +82,8 @@ auto GetCameraConstants(const std::string& path) -> camera_constants_t { SetConstant("stream_ratio", camera_constant.stream_ratio, camera_config); SetConstant("port", camera_constant.port, camera_config); + SetConstant("streamer_fps", camera_constant.streamer_fps, + camera_config); if (camera_config.contains("detector_type") && !camera_config["detector_type"].is_null()) { diff --git a/src/camera/camera_constants.h b/src/camera/camera_constants.h index a65c6871..2d5dc505 100644 --- a/src/camera/camera_constants.h +++ b/src/camera/camera_constants.h @@ -24,6 +24,7 @@ using camera_constant_t = struct CameraConstant { std::optional serial_id = std::nullopt; // uvc only std::optional stream_ratio = std::nullopt; std::optional port = std::nullopt; + std::optional streamer_fps = std::nullopt; DetectorType detector_type = INVALID; friend auto operator<<(std::ostream& os, const CameraConstant& c) diff --git a/src/camera/uvc_camera.cc b/src/camera/uvc_camera.cc index bd391c71..17e87e17 100644 --- a/src/camera/uvc_camera.cc +++ b/src/camera/uvc_camera.cc @@ -71,6 +71,7 @@ UVCCamera::UVCCamera(const CameraConstant& camera_constant, } res = uvc_find_device(context_, &device_, 0, 0, camera_constant_.serial_id->c_str()); + LOG(INFO) << "Serial id: " << camera_constant_.serial_id.value(); if (res != 0) { status = absl::AbortedError( fmt::format("Unable to find device for camera {} with error code {}", diff --git a/src/localization/multi_camera_detector.cc b/src/localization/multi_camera_detector.cc index 4193becd..cdf00c76 100644 --- a/src/localization/multi_camera_detector.cc +++ b/src/localization/multi_camera_detector.cc @@ -58,7 +58,7 @@ MultiCameraDetector::MultiCameraDetector( camera_constants_[i].frame_height.value(), intrinsics)); break; } - default: + case camera::INVALID: LOG(FATAL) << "Invalid detector type"; } } @@ -76,7 +76,8 @@ MultiCameraDetector::MultiCameraDetector( continue; // this is ok because GetFrame is blocking } if (timestamped_frame.timestamp - last_write_times_[i] > - 1.0 / kenforced_streamer_fps) { + 1.0 / camera_constants_[i].streamer_fps.value_or( + kdefault_stream_fps)) { streamers_[i].WriteFrame(timestamped_frame.frame); last_write_times_[i] = timestamped_frame.timestamp; } diff --git a/src/localization/multi_camera_detector.h b/src/localization/multi_camera_detector.h index a7584776..fe96fbde 100644 --- a/src/localization/multi_camera_detector.h +++ b/src/localization/multi_camera_detector.h @@ -31,7 +31,7 @@ class MultiCameraDetector { std::mutex mutex_; std::atomic run_cameras_{true}; std::atomic has_new_detections_{false}; - static constexpr int kenforced_streamer_fps = 30; + static constexpr int kdefault_stream_fps = 30; }; } // namespace localization diff --git a/src/second_bot_main.cc b/src/second_bot_main.cc index dc76ada2..386edad4 100644 --- a/src/second_bot_main.cc +++ b/src/second_bot_main.cc @@ -13,7 +13,7 @@ using camera::camera_constants_t; auto main() -> int { - utils::StartNetworktables(9971); + utils::StartNetworktables(971); std::string log_path = frc::DataLogManager::GetLogDir(); camera_constants_t camera_constants = camera::GetCameraConstants(); diff --git a/src/unambiguous_first.cc b/src/unambiguous_first.cc index ae59ed1f..a9ca6631 100644 --- a/src/unambiguous_first.cc +++ b/src/unambiguous_first.cc @@ -13,16 +13,14 @@ using camera::camera_constants_t; auto main() -> int { - utils::StartNetworktables(9971); + utils::StartNetworktablesAsHost(); std::string log_path = frc::DataLogManager::GetLogDir(); camera_constants_t camera_constants = camera::GetCameraConstants(); LOG(INFO) << "Starting cameras"; - std::vector cameras{ - camera_constants.at("main_bot_left"), - camera_constants.at("main_bot_right")}; + std::vector cameras{camera_constants.at("dev_orin")}; std::jthread thread([cameras] { localization::MultiCameraDetector detector_source(cameras); diff --git a/src/unambiguous_second.cc b/src/unambiguous_second.cc index f8ea9c92..ec353836 100644 --- a/src/unambiguous_second.cc +++ b/src/unambiguous_second.cc @@ -17,10 +17,7 @@ auto main() -> int { std::string log_path = frc::DataLogManager::GetLogDir(); camera_constants_t camera_constants = camera::GetCameraConstants(); - std::vector cameras{ - camera_constants.at("second_bot_left"), - camera_constants.at("second_bot_right"), - camera_constants.at("second_bot_front")}; + std::vector cameras{camera_constants.at("dev_orin")}; std::jthread thread([cameras] { localization::MultiCameraDetector detector_source(cameras); From dff89fbcdb3218c59da752137644ebff92327549 Mon Sep 17 00:00:00 2001 From: "carrotmercinary@gmail.com" <167659798+yasen5@users.noreply.github.com> Date: Tue, 28 Apr 2026 22:47:18 +0000 Subject: [PATCH 19/30] Fix first again --- src/unambiguous_first.cc | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/unambiguous_first.cc b/src/unambiguous_first.cc index a9ca6631..ae59ed1f 100644 --- a/src/unambiguous_first.cc +++ b/src/unambiguous_first.cc @@ -13,14 +13,16 @@ using camera::camera_constants_t; auto main() -> int { - utils::StartNetworktablesAsHost(); + utils::StartNetworktables(9971); std::string log_path = frc::DataLogManager::GetLogDir(); camera_constants_t camera_constants = camera::GetCameraConstants(); LOG(INFO) << "Starting cameras"; - std::vector cameras{camera_constants.at("dev_orin")}; + std::vector cameras{ + camera_constants.at("main_bot_left"), + camera_constants.at("main_bot_right")}; std::jthread thread([cameras] { localization::MultiCameraDetector detector_source(cameras); From 23d68d368e94799b5d561023f7428cabe13089a4 Mon Sep 17 00:00:00 2001 From: yasen5 Date: Sat, 16 May 2026 05:52:13 +0000 Subject: [PATCH 20/30] Basic math done, need to do Levenberg-Marquardt --- src/localization/joint_solver.cc | 203 ++++++++++--------------- src/localization/joint_solver.h | 16 +- src/test/unit_test/joint_solve_test.cc | 180 ++++++++++++++++++---- src/test/unit_test/matrix.cc | 17 --- src/utils/transform.cc | 54 ------- src/utils/transform.h | 31 ++-- 6 files changed, 252 insertions(+), 249 deletions(-) diff --git a/src/localization/joint_solver.cc b/src/localization/joint_solver.cc index 2f9ff596..cb2d7e46 100644 --- a/src/localization/joint_solver.cc +++ b/src/localization/joint_solver.cc @@ -1,5 +1,6 @@ #include "src/localization/joint_solver.h" #include +#include #include #include "src/utils/camera_utils.h" #include "src/utils/constants_from_json.h" @@ -9,7 +10,7 @@ namespace localization { using data_point_t = struct DataPoint { Eigen::Vector2d undistorted_point; - std::string name; + size_t source_index; Eigen::Vector4d field_to_tag_corner_homogenous; }; @@ -20,8 +21,7 @@ constexpr auto sq(double num) -> double { using frc::AprilTagFieldLayout; JointSolver::JointSolver( - const std::vector& camera_constants_, - const AprilTagFieldLayout& layout) + const std::vector& camera_constants_) : robot_to_field_(Eigen::Matrix4d::Identity()) { // clang-format off const Eigen::Matrix4d rotate_yaw_cv = (Eigen::Matrix4d() << @@ -30,7 +30,7 @@ JointSolver::JointSolver( 0, 0, -1, 0, 0, 0, 0, 1).finished(); // clang-format on - for (const frc::AprilTag& tag : layout.GetTags()) { + for (const frc::AprilTag& tag : kapriltag_layout.GetTags()) { Eigen::Matrix4d field_to_tag = tag.pose.ToMatrix(); utils::ChangeBasis(field_to_tag, utils::WPI_TO_CV); std::array field_relative_corners; @@ -54,162 +54,127 @@ JointSolver::JointSolver( .ToMatrix(); const Eigen::Matrix image_to_robot = image_to_camera * camera_to_robot; - camera_matrices_.insert( - {camera_constant.name, - {.image_to_robot = image_to_robot, - .distortion_coefficients = - utils::DistortionCoefficientsFromJson(intrinsics_json), - .camera_matrix = utils::EigenToCvMat(camera_matrix)}}); + camera_matrices_.push_back( + {.image_to_robot = image_to_robot, + .distortion_coefficients = + utils::DistortionCoefficientsFromJson(intrinsics_json), + .camera_matrix = utils::EigenToCvMat(camera_matrix)}); } } +void JointSolver::SetStartPosition(const frc::Pose3d& pose) { + robot_to_field_ = pose.ToMatrix().inverse(); + utils::ChangeBasis(robot_to_field_, utils::WPI_TO_CV); +} + auto JointSolver::EstimatePosition( - const std::map>& - all_cam_detections, - const frc::Pose3d& starting_pose) -> position_estimate_t { - if (all_cam_detections.empty()) { - return {}; + std::vector>& detection_batches, + bool reject_far_tags) -> std::optional { + if (detection_batches.empty()) { + return std::nullopt; } - robot_to_field_ = starting_pose.ToMatrix().inverse(); - utils::ChangeBasis(robot_to_field_, utils::WPI_TO_CV); std::vector data_points; - for (const auto& pair : all_cam_detections) { - const CameraMatrices& camera_mats = camera_matrices_.at(pair.first); - for (const tag_detection_t& detection : pair.second) { + for (size_t source_index = 0; source_index < detection_batches.size(); + source_index++) { + const CameraMatrices& camera_mats = camera_matrices_[source_index]; + for (const tag_detection_t& detection : detection_batches[source_index]) { std::vector undistorted_corners; cv::undistortImagePoints(detection.corners, undistorted_corners, camera_mats.camera_matrix, camera_mats.distortion_coefficients); - for (size_t i = 0; i < undistorted_corners.size(); i++) { + for (size_t corner_index = 0; corner_index < undistorted_corners.size(); + corner_index++) { Eigen::Vector2d undistorted_image_point; - undistorted_image_point << undistorted_corners[i].x, - undistorted_corners[i].y; + undistorted_image_point << undistorted_corners[corner_index].x, + undistorted_corners[corner_index].y; const data_point_t datapoint = { .undistorted_point = undistorted_image_point, - .name = pair.first, + .source_index = source_index, .field_to_tag_corner_homogenous = - tag_corners_[detection.tag_id].value()[i]}; + tag_corners_[detection.tag_id].value()[corner_index]}; data_points.push_back(datapoint); } } } + Eigen::VectorXd residual(2 * data_points.size()); + Eigen::MatrixXd d_residual_d_twist_jacobian(2 * data_points.size(), 6); + double loss = std::numeric_limits::infinity(); - int counter = 0; - const double step = 1e-7; + for (int epoch = 0; epoch < 1000; epoch++) { + for (size_t i = 0; i < data_points.size(); i++) { + const Eigen::Matrix& image_to_robot = + camera_matrices_[data_points[i].source_index].image_to_robot; - utils::TransformValues translation_and_rotation = - utils::ExtractTranslationAndRotation(robot_to_field_); - utils::TransformDecomposition decomposed_robot_to_field; + const Eigen::Vector4d robot_relative_corner = + robot_to_field_ * data_points[i].field_to_tag_corner_homogenous; - while (loss > kacceptable_reprojection_error && counter < 1000000) { - bool printed = false; - for (const data_point_t& data_point : data_points) { - decomposed_robot_to_field = utils::SeparateTranslationAndRotationMatrices( - translation_and_rotation); - const Eigen::Matrix& image_to_robot = - camera_matrices_.at(data_point.name).image_to_robot; - - const Eigen::Vector4d Rx_activation = - decomposed_robot_to_field.Rx * - data_point.field_to_tag_corner_homogenous; - const Eigen::Vector4d Ry_activation = - decomposed_robot_to_field.Ry * Rx_activation; - const Eigen::Vector4d Rz_activation = - decomposed_robot_to_field.Rz * Ry_activation; - - Eigen::Vector3d projection = image_to_robot * - decomposed_robot_to_field.translation * - Rz_activation; + Eigen::Vector3d projection = image_to_robot * robot_relative_corner; const double lambda = projection(2); projection /= lambda; - const Eigen::Vector2d projection_error( - projection.x() - data_point.undistorted_point.x(), - projection.y() - data_point.undistorted_point.y()); - loss = 0.5 * projection_error.squaredNorm(); - - if (counter % 10000 == 0 && !printed) { - printed = true; - std::cout << "loss: " << loss << std::endl; - } - - const Eigen::Vector3d d_projection( - projection_error.x() / lambda, projection_error.y() / lambda, - -(projection_error.x() * projection.x() + - projection_error.y() * projection.y()) / - lambda); - - Eigen::Vector4d accumulated_gradient = - image_to_robot.transpose() * d_projection; - - const Eigen::Matrix4d d_translation = - accumulated_gradient * Rz_activation.transpose(); - - accumulated_gradient = decomposed_robot_to_field.translation.transpose() * - accumulated_gradient; - const Eigen::Matrix4d d_Rz = - accumulated_gradient * Ry_activation.transpose(); - - accumulated_gradient = - decomposed_robot_to_field.Rz.transpose() * accumulated_gradient; - const Eigen::Matrix4d d_Ry = - accumulated_gradient * Rx_activation.transpose(); - - accumulated_gradient = - decomposed_robot_to_field.Ry.transpose() * accumulated_gradient; - const Eigen::Matrix4d d_Rx = - accumulated_gradient * - data_point.field_to_tag_corner_homogenous.transpose(); + const double x_residual = + data_points[i].undistorted_point.x() - projection.x(); + const double y_residual = + data_points[i].undistorted_point.y() - projection.y(); + residual(2 * i) = x_residual; + residual(2 * i + 1) = y_residual; // clang-format off - const Eigen::Matrix4d d_Rz_d_rz = - (Eigen::Matrix4d() << - -sin(translation_and_rotation.rz), -cos(translation_and_rotation.rz), 0, 0, - cos(translation_and_rotation.rz), -sin(translation_and_rotation.rz), 0, 0, - 0, 0, 0, 0, - 0, 0, 0, 0).finished(); - const Eigen::Matrix4d d_Ry_d_ry = - (Eigen::Matrix4d() << - -sin(translation_and_rotation.ry), 0, cos(translation_and_rotation.ry), 0, - 0, 0, 0, 0, - -cos(translation_and_rotation.ry), 0, -sin(translation_and_rotation.ry), 0, - 0, 0, 0, 0).finished(); - const Eigen::Matrix4d d_Rx_d_rx = - (Eigen::Matrix4d() << - 0, 0, 0, 0, - 0, -sin(translation_and_rotation.rx), -cos(translation_and_rotation.rx), 0, - 0, cos(translation_and_rotation.rx), -sin(translation_and_rotation.rx), 0, - 0, 0, 0, 0).finished(); + const Eigen::MatrixXd d_residual_i_d_projection_i = (Eigen::MatrixXd(2, 3) << + -1/lambda, 0, projection.x()/lambda, + 0, -1/lambda, projection.y()/lambda).finished(); // clang-format on - translation_and_rotation.rx -= - step * (d_Rx_d_rx.transpose() * d_Rx).trace(); - translation_and_rotation.ry -= - step * (d_Ry_d_ry.transpose() * d_Ry).trace(); - translation_and_rotation.rz -= - step * (d_Rz_d_rz.transpose() * d_Rz).trace(); + const Eigen::MatrixXd& d_projection_i_d_robot_relative_object_point = + image_to_robot.block<3, 3>(0, 0); + + Eigen::MatrixXd d_robot_relative_object_point_d_d_twist = + Eigen::MatrixXd(3, 6); + d_robot_relative_object_point_d_d_twist.block<3, 3>(0, 0) = + -utils::CrossProduct(robot_relative_corner.block<3, 1>(0, 0)); + d_robot_relative_object_point_d_d_twist.block<3, 3>(0, 3) = + Eigen::Matrix3d::Identity(); - translation_and_rotation.x -= step * d_translation(0, 3); - translation_and_rotation.y -= step * d_translation(1, 3); - translation_and_rotation.z -= step * d_translation(2, 3); + d_residual_d_twist_jacobian.block<2, 6>(2 * i, 0) = + d_residual_i_d_projection_i * + d_projection_i_d_robot_relative_object_point * + d_robot_relative_object_point_d_d_twist; } - counter++; + std::cout << "Residual: " << residual.cwiseSquare().sum() << std::endl; + const Eigen::VectorXd b = + -d_residual_d_twist_jacobian.transpose() * residual; + const Eigen::VectorXd partial_twist = + (d_residual_d_twist_jacobian.transpose() * d_residual_d_twist_jacobian) + .ldlt() + .solve(b); + Eigen::Matrix4d twist_expanded = Eigen::Matrix4d::Zero(); + twist_expanded.block<3, 3>(0, 0) = + utils::CrossProduct(partial_twist.block<3, 1>(0, 0)); + twist_expanded.block<3, 1>(0, 3) = partial_twist.block<3, 1>(3, 0); + robot_to_field_ = twist_expanded.exp() * robot_to_field_; } - Eigen::Matrix4d field_to_robot = - (decomposed_robot_to_field.translation * decomposed_robot_to_field.Rz * - decomposed_robot_to_field.Ry * decomposed_robot_to_field.Rx) - .inverse(); + std::cout << "Robot to field:\n" << robot_to_field_ << std::endl; + + Eigen::Matrix4d field_to_robot = robot_to_field_.inverse(); + field_to_robot(3, 0) = 0; + field_to_robot(3, 1) = 0; + field_to_robot(3, 2) = 0; + field_to_robot(3, 3) = 1; + + std::cout << "Field to robot cv:\n" << field_to_robot << std::endl; utils::PrintTransformationMatrix(utils::EigenToCvMat(field_to_robot), "Field to robot cv"); utils::ChangeBasis(field_to_robot, utils::CV_TO_WPI); + std::cout << "Field to robot wpi:\n" << field_to_robot << std::endl; utils::PrintTransformationMatrix(utils::EigenToCvMat(field_to_robot), "Field to robot wpi"); - return {.pose = frc::Pose3d(field_to_robot), .variance = 0, .timestamp = 0}; + return std::make_optional( + {.pose = frc::Pose3d(field_to_robot), .variance = 0, .timestamp = 0}); } } // namespace localization diff --git a/src/localization/joint_solver.h b/src/localization/joint_solver.h index 7dfb91f1..81948bfc 100644 --- a/src/localization/joint_solver.h +++ b/src/localization/joint_solver.h @@ -14,20 +14,20 @@ struct CameraMatrices { cv::Mat camera_matrix; }; static constexpr int kmax_tags = 50; -class JointSolver { +class JointSolver : IJointPositionSolver { public: - JointSolver(const std::vector& camera_constants_, - const frc::AprilTagFieldLayout& layout = kapriltag_layout); + JointSolver(const std::vector& camera_constants_); auto EstimatePosition( - const std::map>& - all_cam_detections, - const frc::Pose3d& starting_pose) -> position_estimate_t; - Eigen::Matrix4d robot_to_field_; + std::vector>& detection_batches, + bool reject_far_tags = true) + -> std::optional override; + void SetStartPosition(const frc::Pose3d& pose); private: static constexpr double kacceptable_reprojection_error = 0.005; - std::map camera_matrices_; + std::vector camera_matrices_; std::array>, kmax_tags> tag_corners_; + Eigen::Matrix4d robot_to_field_; }; } // namespace localization diff --git a/src/test/unit_test/joint_solve_test.cc b/src/test/unit_test/joint_solve_test.cc index 0bc15d77..d4740c9a 100644 --- a/src/test/unit_test/joint_solve_test.cc +++ b/src/test/unit_test/joint_solve_test.cc @@ -1,41 +1,163 @@ #include +#include #include "src/localization/joint_solver.h" +#include "src/localization/opencv_apriltag_detector.h" #include "src/localization/square_solver.h" +#include "src/test/unit_test/unit_test_utils.h" #include "src/utils/camera_utils.h" #include "src/utils/constants_from_json.h" #include "src/utils/transform.h" -std::vector detections( - {{.tag_id = 31, - .corners = {cv::Point2d(100.0, 200.0), cv::Point2d(200.0, 200.0), - cv::Point2d(200.0, 100.0), cv::Point2d(100.0, 100.0)}, - .timestamp = 0.0, - .confidence = 0.0}}); +class JointSolverTest : public ::testing::Test { + protected: + camera::camera_constants_t camera_constants = camera::GetCameraConstants(); -TEST(JointSolveTest, EstimatePosition) { // NOLINT - camera::camera_constant_t camera_constant = - camera::GetCameraConstants().at("dev_orin"); - localization::JointSolver joint_solver({camera_constant}); - localization::SquareSolver square_solver(camera_constant); + std::vector joint_solve_cameras = + std::vector{camera_constants.at("main_bot_left")}; + localization::SquareSolver square_solver; + localization::JointSolver joint_solver; + localization::OpenCVAprilTagDetector detector; + JointSolverTest() + : square_solver(camera_constants.at("main_bot_left")), + joint_solver(joint_solve_cameras), + detector(utils::ReadIntrinsics( + camera_constants.at("main_bot_left").intrinsics_path.value())) {} +}; - auto square_solver_solution = square_solver.EstimatePosition(detections)[0]; - utils::PrintTransformationMatrix( - utils::EigenToCvMat(square_solver_solution.pose.ToMatrix())); - std::cout << square_solver_solution.pose.ToMatrix() << std::endl; +// TEST_F(JointSolverTest, MaintainsValidEstimateOpenRotation) { // NOLINT +// const localization::position_estimate_t square_solver_solution = +// square_solver.EstimatePosition(test_utils::fake_detections, false)[0]; +// std::map> +// associated_detections; +// associated_detections.insert({config, test_utils::fake_detections}); +// const localization::position_estimate_t joint_solver_solution = +// joint_solver.EstimatePosition(associated_detections, +// square_solver_solution.pose, false); +// EXPECT_EQ(square_solver_solution, joint_solver_solution); +// } +// +// TEST_F(JointSolverTest, CloseConvergenceOpenRotation) { // NOLINT +// const frc::Transform3d joint_solve_input_noise( +// frc::Translation3d(units::meter_t{0.4}, units::meter_t{0.5}, +// units::meter_t{0.2}), +// frc::Rotation3d(units::degree_t{test_utils::deg2rad(3)}, +// units::degree_t{test_utils::deg2rad(3)}, +// units::degree_t{test_utils::deg2rad(3)})); +// const localization::position_estimate_t square_solver_solution = +// square_solver.EstimatePosition(test_utils::fake_detections, false)[0]; +// frc::Pose3d noisy_pose = +// square_solver_solution.pose.TransformBy(joint_solve_input_noise); +// std::map> +// associated_detections; +// associated_detections.insert({config, test_utils::fake_detections}); +// const localization::position_estimate_t joint_solver_solution = +// joint_solver.EstimatePosition(associated_detections, noisy_pose, false, +// true); +// // std::cout << "sq: " << square_solver_solution +// // << "\njoint: " << joint_solver_solution << std::endl; +// EXPECT_EQ(square_solver_solution, joint_solver_solution); +// } - frc::Transform3d noise( - frc::Translation3d(units::meter_t{0.067}, units::meter_t{0.1}, - units::meter_t{-0.1}), - {}); +// TEST_F(JointSolverTest, IncreasingDistanceYawOnly) { // NOLINT +// double xDiff = rand(); +// double yDiff = rand(); +// double zDiff = rand(); +// const double translationMax = std::hypot(xDiff, yDiff, zDiff) * 4.0; +// xDiff /= translationMax; +// yDiff /= translationMax; +// zDiff /= translationMax; +// double rollDiff = rand(); +// double pitchDiff = rand(); +// double yawDiff = rand(); +// const double rotationDivider = std::hypot(rollDiff, pitchDiff, yawDiff) / 4.0; +// // rollDiff /= rotationDivider; +// // pitchDiff /= rotationDivider; +// rollDiff = 0; +// pitchDiff = 0; +// yawDiff /= rotationDivider; +// localization::position_estimate_t square_solver_solution = +// square_solver.EstimatePosition(test_utils::fake_detections, false)[0]; +// std::map> +// associated_detections; +// associated_detections.insert( +// {camera_constants.at("main_bot_left"), test_utils::fake_detections}); +// localization::position_estimate_t joint_solver_solution; +// for (int i = 0; i < 15; i++) { +// const frc::Transform3d joint_solve_input_noise( +// frc::Translation3d(units::meter_t{i * xDiff}, units::meter_t{i * yDiff}, +// units::meter_t{i * zDiff}), +// frc::Rotation3d(units::degree_t{i * rollDiff}, +// units::degree_t{i * pitchDiff}, +// units::degree_t{i * yawDiff})); +// +// frc::Pose3d noisy_pose = +// square_solver_solution.pose.TransformBy(joint_solve_input_noise); +// joint_solver_solution = joint_solver.EstimatePosition( +// associated_detections, noisy_pose, true, false); +// std::cout << "iter: " << i << " sq: " << square_solver_solution +// << "\njoint: " << joint_solver_solution << std::endl; +// std::cout << "Noise" << std::endl; +// utils::PrintTransform3d(joint_solve_input_noise); +// if (square_solver_solution != joint_solver_solution) { +// joint_solver_solution = joint_solver.EstimatePosition( +// associated_detections, noisy_pose, true, true); +// std::cout << "Noisy pose" << std::endl; +// utils::PrintPose3d(noisy_pose); +// FAIL() << "Failed with above transformation: " +// << "\nJoint solve solution: " << joint_solver_solution +// << "\nSq solution: " << square_solver_solution; +// +// break; +// } +// } +// } +// +// TEST_F(JointSolverTest, MaintainsValidEstimateYawOnly) { // NOLINT +// const localization::position_estimate_t square_solver_solution = +// square_solver.EstimatePosition(test_utils::fake_detections, false)[0]; +// std::map> +// associated_detections; +// associated_detections.insert({config, test_utils::fake_detections}); +// const localization::position_estimate_t joint_solver_solution = +// joint_solver.EstimatePosition(associated_detections, +// square_solver_solution.pose, true); +// // std::cout << "sq: " << square_solver_solution +// // << "\njoint: " << joint_solver_solution << std::endl; +// EXPECT_EQ(square_solver_solution, joint_solver_solution); +// } - square_solver_solution.pose = square_solver_solution.pose.TransformBy(noise); - - std::map> - associated_detections; - associated_detections.insert({camera_constant.name, detections}); - auto joint_solver_solution = joint_solver.EstimatePosition( - associated_detections, square_solver_solution.pose); - std::cout << "Joint solver solution: " << joint_solver_solution << std::endl; - std::cout << "Square solver solution: " << square_solver_solution - << std::endl; +TEST_F(JointSolverTest, MaintainsValidEstimateRealImageYawOnly) { // NOLINT + const cv::Mat image = cv::imread("/bos/bos-logs/log181/left/11.963395.jpg"); + camera::timestamped_frame_t frame{.frame = image, .timestamp = 0.0}; + const std::vector detections = + detector.GetTagDetections(frame); + const localization::position_estimate_t square_solver_solution = + square_solver.EstimatePosition(detections, false)[0]; + std::vector> associated_detections{ + detections}; + const localization::position_estimate_t joint_solver_solution = + joint_solver.EstimatePosition(associated_detections, false).value(); + std::cout << "sq: " << square_solver_solution + << "\njoint: " << joint_solver_solution << std::endl; + EXPECT_EQ(square_solver_solution, joint_solver_solution); } + +// TEST_F(JointSolverTest, CloseConvergenceYawOnly) { // NOLINT +// const frc::Transform3d joint_solve_input_noise( +// frc::Translation3d(units::meter_t{0.4}, units::meter_t{0.5}, +// units::meter_t{0.2}), +// frc::Rotation3d(units::degree_t{0}, units::degree_t{0}, +// units::degree_t{3})); +// localization::position_estimate_t square_solver_solution = +// square_solver.EstimatePosition(test_utils::fake_detections, false)[0]; +// frc::Pose3d noisy_pose = +// square_solver_solution.pose.TransformBy(joint_solve_input_noise); +// std::map> +// associated_detections; +// associated_detections.insert({config, test_utils::fake_detections}); +// const localization::position_estimate_t joint_solver_solution = +// joint_solver.EstimatePosition(associated_detections, noisy_pose, true); +// // std::cout << "sq: " << square_solver_solution +// // << "\njoint: " << joint_solver_solution << std::endl; +// EXPECT_EQ(square_solver_solution, joint_solver_solution); +// } diff --git a/src/test/unit_test/matrix.cc b/src/test/unit_test/matrix.cc index fce9f629..9b661d4a 100644 --- a/src/test/unit_test/matrix.cc +++ b/src/test/unit_test/matrix.cc @@ -30,20 +30,3 @@ auto TransformationMatrix(const double rx, const double ry, const double rz, return A; } - -TEST(JointSolveTest, EstimatePosition) { // NOLINT - const Eigen::Matrix4d transform = - TransformationMatrix(rad(35), rad(23), rad(58), 1, 2, 3); - const utils::TransformValues decomp = - utils::ExtractTranslationAndRotation(transform); - std::cout << decomp.x << " " << decomp.y << " " << decomp.z << " " - << decomp.rx << " " << decomp.ry << " " << decomp.rz << " " - << std::endl; - const utils::TransformDecomposition decomped = - utils::SeparateTranslationAndRotationMatrices(decomp); - utils::PrintTransformationMatrix(utils::EigenToCvMat(transform), "original"); - utils::PrintTransformationMatrix( - utils::EigenToCvMat(decomped.translation * decomped.Rz * decomped.Ry * - decomped.Rx), - "new"); -} diff --git a/src/utils/transform.cc b/src/utils/transform.cc index 36116045..854480df 100644 --- a/src/utils/transform.cc +++ b/src/utils/transform.cc @@ -68,58 +68,4 @@ auto HomogenizePoint3d(cv::Point3d point) -> cv::Mat { template cv::Mat utils::EigenToCvMat>( const Eigen::MatrixBase>&); -auto ExtractTranslationAndRotation(const Eigen::Matrix4d& transform_mat) - -> TransformValues { - double x = transform_mat(0, 3); - double y = transform_mat(1, 3); - double z = transform_mat(2, 3); - - const Eigen::Matrix3d& R = transform_mat.block<3, 3>(0, 0); - - double sy = std::hypot(R(0, 0), R(1, 0)); - - bool singular = sy < 1e-6; - - double roll, pitch, yaw; - - if (!singular) { - roll = std::atan2(R(2, 1), R(2, 2)); - pitch = std::atan2(-R(2, 0), sy); - yaw = std::atan2(R(1, 0), R(0, 0)); - } else { - // Gimbal lock - roll = std::atan2(-R(1, 2), R(1, 1)); - pitch = std::atan2(-R(2, 0), sy); - yaw = 0.0; - } - - return {x, y, z, roll, pitch, yaw}; -} - -auto SeparateTranslationAndRotationMatrices( - const TransformValues& decomposition) -> TransformDecomposition { - // clang-format off - const Eigen::Matrix4d Rx = (Eigen::Matrix4d() << - 1, 0, 0, 0, - 0, cos(decomposition.rx), -sin(decomposition.rx), 0, - 0, sin(decomposition.rx), cos(decomposition.rx), 0, - 0, 0, 0, 1).finished(); - const Eigen::Matrix4d Ry = (Eigen::Matrix4d() << - cos(decomposition.ry), 0, sin(decomposition.ry), 0, - 0, 1, 0, 0, - -sin(decomposition.ry), 0, cos(decomposition.ry), 0, - 0, 0, 0, 1).finished(); - const Eigen::Matrix4d Rz = (Eigen::Matrix4d() << - cos(decomposition.rz), -sin(decomposition.rz), 0, 0, - sin(decomposition.rz), cos(decomposition.rz), 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1).finished(); - - // clang-format on - Eigen::Matrix4d translation = Eigen::Matrix4d::Identity(); - translation(0, 3) = decomposition.x; - translation(1, 3) = decomposition.y; - translation(2, 3) = decomposition.z; - return {translation, Rx, Ry, Rz}; -} } // namespace utils diff --git a/src/utils/transform.h b/src/utils/transform.h index d573be1d..ec5f81ed 100644 --- a/src/utils/transform.h +++ b/src/utils/transform.h @@ -62,28 +62,15 @@ auto inline Homogenize(const Eigen::Vector3d point) -> Eigen::Vector4d { return homogenized_point; } -struct TransformValues { - double x; - double y; - double z; - double rx; - double ry; - double rz; -}; - -struct TransformDecomposition { - Eigen::Matrix4d translation; - Eigen::Matrix4d Rx; - Eigen::Matrix4d Ry; - Eigen::Matrix4d Rz; -}; - -auto ExtractTranslationAndRotation(const Eigen::Matrix4d& transform_mat) - -> TransformValues; - -auto SeparateTranslationAndRotationMatrices(const TransformValues& combined) - -> TransformDecomposition; - +auto inline CrossProduct(const Eigen::Vector3d corner) -> Eigen::Matrix3d { + // clang-format off + return (Eigen::Matrix3d() << + 0, -corner.z(), corner.y(), + corner.z(), 0, -corner.x(), + -corner.y(), corner.x(), 0) + .finished(); + // clang-format on +} inline auto PoseOffField(frc::Pose3d pose) -> bool { constexpr double kerror_margin = 0.2; return pose.X().value() < 0 - kerror_margin || From f2cfc8baf56c68541daaaf2153d45d43693a824f Mon Sep 17 00:00:00 2001 From: yasen5 Date: Sat, 16 May 2026 18:45:33 +0000 Subject: [PATCH 21/30] Add Levenberg-Marquardt --- src/localization/joint_solver.cc | 141 +++++++++++++++---------- src/localization/joint_solver.h | 10 ++ src/test/unit_test/joint_solve_test.cc | 27 ++--- 3 files changed, 100 insertions(+), 78 deletions(-) diff --git a/src/localization/joint_solver.cc b/src/localization/joint_solver.cc index cb2d7e46..1582f79d 100644 --- a/src/localization/joint_solver.cc +++ b/src/localization/joint_solver.cc @@ -8,12 +8,6 @@ namespace localization { -using data_point_t = struct DataPoint { - Eigen::Vector2d undistorted_point; - size_t source_index; - Eigen::Vector4d field_to_tag_corner_homogenous; -}; - constexpr auto sq(double num) -> double { return num * num; } @@ -67,6 +61,55 @@ void JointSolver::SetStartPosition(const frc::Pose3d& pose) { utils::ChangeBasis(robot_to_field_, utils::WPI_TO_CV); } +void JointSolver::ComputeResidual( + const std::vector& data_points, + const Eigen::Matrix4d& robot_to_field, Eigen::VectorXd& residual, + Eigen::MatrixXd* d_residual_d_twist_jacobian) { + for (size_t i = 0; i < data_points.size(); i++) { + const Eigen::Matrix& image_to_robot = + camera_matrices_[data_points[i].source_index].image_to_robot; + + const Eigen::Vector4d robot_relative_corner = + robot_to_field * data_points[i].field_to_tag_corner_homogenous; + + Eigen::Vector3d projection = image_to_robot * robot_relative_corner; + const double lambda = projection(2); + projection /= lambda; + + const double x_residual = + data_points[i].undistorted_point.x() - projection.x(); + const double y_residual = + data_points[i].undistorted_point.y() - projection.y(); + residual(2 * i) = x_residual; + residual(2 * i + 1) = y_residual; + if (d_residual_d_twist_jacobian == nullptr) { + continue; + } + + // clang-format off + const Eigen::Matrix d_residual_i_d_projection_i = + (Eigen::MatrixXd(2, 3) << + -1/lambda, 0, projection.x()/lambda, + 0, -1/lambda, projection.y()/lambda).finished(); + // clang-format on + + const Eigen::Matrix3d& d_projection_i_d_robot_relative_object_point = + image_to_robot.block<3, 3>(0, 0); + + Eigen::Matrix d_robot_relative_object_point_d_d_twist = + Eigen::MatrixXd(3, 6); + d_robot_relative_object_point_d_d_twist.block<3, 3>(0, 0) = + -utils::CrossProduct(robot_relative_corner.block<3, 1>(0, 0)); + d_robot_relative_object_point_d_d_twist.block<3, 3>(0, 3) = + Eigen::Matrix3d::Identity(); + + d_residual_d_twist_jacobian->block<2, 6>(2 * i, 0) = + d_residual_i_d_projection_i * + d_projection_i_d_robot_relative_object_point * + d_robot_relative_object_point_d_d_twist; + } +} + auto JointSolver::EstimatePosition( std::vector>& detection_batches, bool reject_far_tags) -> std::optional { @@ -100,60 +143,42 @@ auto JointSolver::EstimatePosition( Eigen::VectorXd residual(2 * data_points.size()); Eigen::MatrixXd d_residual_d_twist_jacobian(2 * data_points.size(), 6); - double loss = std::numeric_limits::infinity(); - for (int epoch = 0; epoch < 1000; epoch++) { - for (size_t i = 0; i < data_points.size(); i++) { - const Eigen::Matrix& image_to_robot = - camera_matrices_[data_points[i].source_index].image_to_robot; - - const Eigen::Vector4d robot_relative_corner = - robot_to_field_ * data_points[i].field_to_tag_corner_homogenous; - - Eigen::Vector3d projection = image_to_robot * robot_relative_corner; - const double lambda = projection(2); - projection /= lambda; - - const double x_residual = - data_points[i].undistorted_point.x() - projection.x(); - const double y_residual = - data_points[i].undistorted_point.y() - projection.y(); - residual(2 * i) = x_residual; - residual(2 * i + 1) = y_residual; - - // clang-format off - const Eigen::MatrixXd d_residual_i_d_projection_i = (Eigen::MatrixXd(2, 3) << - -1/lambda, 0, projection.x()/lambda, - 0, -1/lambda, projection.y()/lambda).finished(); - // clang-format on - - const Eigen::MatrixXd& d_projection_i_d_robot_relative_object_point = - image_to_robot.block<3, 3>(0, 0); - - Eigen::MatrixXd d_robot_relative_object_point_d_d_twist = - Eigen::MatrixXd(3, 6); - d_robot_relative_object_point_d_d_twist.block<3, 3>(0, 0) = - -utils::CrossProduct(robot_relative_corner.block<3, 1>(0, 0)); - d_robot_relative_object_point_d_d_twist.block<3, 3>(0, 3) = - Eigen::Matrix3d::Identity(); - - d_residual_d_twist_jacobian.block<2, 6>(2 * i, 0) = - d_residual_i_d_projection_i * - d_projection_i_d_robot_relative_object_point * - d_robot_relative_object_point_d_d_twist; - } - std::cout << "Residual: " << residual.cwiseSquare().sum() << std::endl; - const Eigen::VectorXd b = + ComputeResidual(data_points, robot_to_field_, residual, + &d_residual_d_twist_jacobian); + double current_error = residual.cwiseSquare().sum(); + if (epoch % 10 == 0) + std::cout << "Current error: " << current_error << std::endl; + const Eigen::Vector b = -d_residual_d_twist_jacobian.transpose() * residual; - const Eigen::VectorXd partial_twist = - (d_residual_d_twist_jacobian.transpose() * d_residual_d_twist_jacobian) - .ldlt() - .solve(b); - Eigen::Matrix4d twist_expanded = Eigen::Matrix4d::Zero(); - twist_expanded.block<3, 3>(0, 0) = - utils::CrossProduct(partial_twist.block<3, 1>(0, 0)); - twist_expanded.block<3, 1>(0, 3) = partial_twist.block<3, 1>(3, 0); - robot_to_field_ = twist_expanded.exp() * robot_to_field_; + const Eigen::Matrix hessian = + d_residual_d_twist_jacobian.transpose() * d_residual_d_twist_jacobian; + double lambda = 1; + Eigen::Matrix hessian_diag = + hessian.diagonal().asDiagonal().toDenseMatrix(); + Eigen::Matrix4d candidate_robot_to_field; + while (true) { + const Eigen::Vector partial_twist = + (hessian + lambda * hessian_diag).ldlt().solve(b); + Eigen::Matrix4d twist_expanded = Eigen::Matrix4d::Zero(); + twist_expanded.block<3, 3>(0, 0) = + utils::CrossProduct(partial_twist.block<3, 1>(0, 0)); + twist_expanded.block<3, 1>(0, 3) = partial_twist.block<3, 1>(3, 0); + candidate_robot_to_field = twist_expanded.exp() * robot_to_field_; + ComputeResidual(data_points, candidate_robot_to_field, residual); + double candidate_error = residual.cwiseSquare().sum(); + if (candidate_error > current_error) { + lambda *= 2; + if (lambda > kmaximum_lambda) { + std::cout << "Failed" << std::endl; + return std::nullopt; + } + } else { + lambda /= 2; + robot_to_field_ = candidate_robot_to_field; + break; + } + } } std::cout << "Robot to field:\n" << robot_to_field_ << std::endl; diff --git a/src/localization/joint_solver.h b/src/localization/joint_solver.h index 81948bfc..d99db068 100644 --- a/src/localization/joint_solver.h +++ b/src/localization/joint_solver.h @@ -8,6 +8,11 @@ using json = nlohmann::json; namespace localization { +using data_point_t = struct DataPoint { + Eigen::Vector2d undistorted_point; + size_t source_index; + Eigen::Vector4d field_to_tag_corner_homogenous; +}; struct CameraMatrices { Eigen::Matrix image_to_robot; cv::Mat distortion_coefficients; @@ -22,9 +27,14 @@ class JointSolver : IJointPositionSolver { bool reject_far_tags = true) -> std::optional override; void SetStartPosition(const frc::Pose3d& pose); + void ComputeResidual(const std::vector& data_points, + const Eigen::Matrix4d& robot_to_field, + Eigen::VectorXd& residual, + Eigen::MatrixXd* d_residual_d_twist_jacobian = nullptr); private: static constexpr double kacceptable_reprojection_error = 0.005; + static constexpr double kmaximum_lambda = 1e10; std::vector camera_matrices_; std::array>, kmax_tags> tag_corners_; diff --git a/src/test/unit_test/joint_solve_test.cc b/src/test/unit_test/joint_solve_test.cc index d4740c9a..d8a4ce68 100644 --- a/src/test/unit_test/joint_solve_test.cc +++ b/src/test/unit_test/joint_solve_test.cc @@ -135,29 +135,16 @@ TEST_F(JointSolverTest, MaintainsValidEstimateRealImageYawOnly) { // NOLINT square_solver.EstimatePosition(detections, false)[0]; std::vector> associated_detections{ detections}; + const frc::Transform3d joint_solve_input_noise( + frc::Translation3d(units::meter_t{0.2}, units::meter_t{0.2}, + units::meter_t{0.2}), + frc::Rotation3d(units::degree_t{0}, units::degree_t{0}, + units::degree_t{0})); + joint_solver.SetStartPosition( + square_solver_solution.pose.TransformBy(joint_solve_input_noise)); const localization::position_estimate_t joint_solver_solution = joint_solver.EstimatePosition(associated_detections, false).value(); std::cout << "sq: " << square_solver_solution << "\njoint: " << joint_solver_solution << std::endl; EXPECT_EQ(square_solver_solution, joint_solver_solution); } - -// TEST_F(JointSolverTest, CloseConvergenceYawOnly) { // NOLINT -// const frc::Transform3d joint_solve_input_noise( -// frc::Translation3d(units::meter_t{0.4}, units::meter_t{0.5}, -// units::meter_t{0.2}), -// frc::Rotation3d(units::degree_t{0}, units::degree_t{0}, -// units::degree_t{3})); -// localization::position_estimate_t square_solver_solution = -// square_solver.EstimatePosition(test_utils::fake_detections, false)[0]; -// frc::Pose3d noisy_pose = -// square_solver_solution.pose.TransformBy(joint_solve_input_noise); -// std::map> -// associated_detections; -// associated_detections.insert({config, test_utils::fake_detections}); -// const localization::position_estimate_t joint_solver_solution = -// joint_solver.EstimatePosition(associated_detections, noisy_pose, true); -// // std::cout << "sq: " << square_solver_solution -// // << "\njoint: " << joint_solver_solution << std::endl; -// EXPECT_EQ(square_solver_solution, joint_solver_solution); -// } From 864031fe3a98556a360b37154f995cc676b06bed Mon Sep 17 00:00:00 2001 From: yasen5 Date: Sat, 16 May 2026 20:03:38 +0000 Subject: [PATCH 22/30] Fix camera being wpilib instead of cv --- src/localization/joint_solver.cc | 13 +++++++++---- src/localization/joint_solver.h | 2 +- src/test/unit_test/joint_solve_test.cc | 6 +++--- 3 files changed, 13 insertions(+), 8 deletions(-) diff --git a/src/localization/joint_solver.cc b/src/localization/joint_solver.cc index 1582f79d..28bcb4c8 100644 --- a/src/localization/joint_solver.cc +++ b/src/localization/joint_solver.cc @@ -42,10 +42,11 @@ JointSolver::JointSolver( const auto camera_matrix = utils::CameraMatrixFromJson(intrinsics_json); const Eigen::Matrix image_to_camera = camera_matrix * pi; - const Eigen::Matrix4d camera_to_robot = + Eigen::Matrix4d camera_to_robot = utils::ExtrinsicsJsonToCameraToRobot( utils::ReadExtrinsics(camera_constant.extrinsics_path.value())) .ToMatrix(); + utils::ChangeBasis(camera_to_robot, utils::WPI_TO_CV); const Eigen::Matrix image_to_robot = image_to_camera * camera_to_robot; camera_matrices_.push_back( @@ -70,7 +71,7 @@ void JointSolver::ComputeResidual( camera_matrices_[data_points[i].source_index].image_to_robot; const Eigen::Vector4d robot_relative_corner = - robot_to_field * data_points[i].field_to_tag_corner_homogenous; + robot_to_field * data_points[i].field_to_tag_corner_homogeneous_cv; Eigen::Vector3d projection = image_to_robot * robot_relative_corner; const double lambda = projection(2); @@ -133,17 +134,21 @@ auto JointSolver::EstimatePosition( const data_point_t datapoint = { .undistorted_point = undistorted_image_point, .source_index = source_index, - .field_to_tag_corner_homogenous = + .field_to_tag_corner_homogeneous_cv = tag_corners_[detection.tag_id].value()[corner_index]}; data_points.push_back(datapoint); } } } + if (data_points.empty()) { + return std::nullopt; + } + Eigen::VectorXd residual(2 * data_points.size()); Eigen::MatrixXd d_residual_d_twist_jacobian(2 * data_points.size(), 6); - for (int epoch = 0; epoch < 1000; epoch++) { + for (int epoch = 0; epoch < 1e4; epoch++) { ComputeResidual(data_points, robot_to_field_, residual, &d_residual_d_twist_jacobian); double current_error = residual.cwiseSquare().sum(); diff --git a/src/localization/joint_solver.h b/src/localization/joint_solver.h index d99db068..5bcc4f7f 100644 --- a/src/localization/joint_solver.h +++ b/src/localization/joint_solver.h @@ -11,7 +11,7 @@ namespace localization { using data_point_t = struct DataPoint { Eigen::Vector2d undistorted_point; size_t source_index; - Eigen::Vector4d field_to_tag_corner_homogenous; + Eigen::Vector4d field_to_tag_corner_homogeneous_cv; }; struct CameraMatrices { Eigen::Matrix image_to_robot; diff --git a/src/test/unit_test/joint_solve_test.cc b/src/test/unit_test/joint_solve_test.cc index d8a4ce68..48820a89 100644 --- a/src/test/unit_test/joint_solve_test.cc +++ b/src/test/unit_test/joint_solve_test.cc @@ -127,7 +127,7 @@ class JointSolverTest : public ::testing::Test { // } TEST_F(JointSolverTest, MaintainsValidEstimateRealImageYawOnly) { // NOLINT - const cv::Mat image = cv::imread("/bos/bos-logs/log181/left/11.963395.jpg"); + const cv::Mat image = cv::imread("/bos/bos-logs/log181/left/20.095369.jpg"); camera::timestamped_frame_t frame{.frame = image, .timestamp = 0.0}; const std::vector detections = detector.GetTagDetections(frame); @@ -136,8 +136,8 @@ TEST_F(JointSolverTest, MaintainsValidEstimateRealImageYawOnly) { // NOLINT std::vector> associated_detections{ detections}; const frc::Transform3d joint_solve_input_noise( - frc::Translation3d(units::meter_t{0.2}, units::meter_t{0.2}, - units::meter_t{0.2}), + frc::Translation3d(units::meter_t{0.4}, units::meter_t{0.4}, + units::meter_t{0.4}), frc::Rotation3d(units::degree_t{0}, units::degree_t{0}, units::degree_t{0})); joint_solver.SetStartPosition( From d27fa1ff8e02a79a6aa140cdffc9168713de769b Mon Sep 17 00:00:00 2001 From: yasen5 Date: Sat, 16 May 2026 22:58:06 +0000 Subject: [PATCH 23/30] ensure that file actually has detections in test --- src/test/unit_test/joint_solve_test.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/src/test/unit_test/joint_solve_test.cc b/src/test/unit_test/joint_solve_test.cc index 48820a89..59ae2378 100644 --- a/src/test/unit_test/joint_solve_test.cc +++ b/src/test/unit_test/joint_solve_test.cc @@ -131,6 +131,7 @@ TEST_F(JointSolverTest, MaintainsValidEstimateRealImageYawOnly) { // NOLINT camera::timestamped_frame_t frame{.frame = image, .timestamp = 0.0}; const std::vector detections = detector.GetTagDetections(frame); + CHECK_NE(detections.size(), 0); const localization::position_estimate_t square_solver_solution = square_solver.EstimatePosition(detections, false)[0]; std::vector> associated_detections{ From 512ee7bc076cb92483d4284a99e4b5c18de12782 Mon Sep 17 00:00:00 2001 From: yasen5 Date: Sun, 17 May 2026 01:25:19 +0000 Subject: [PATCH 24/30] Make easier to test --- src/localization/joint_solver.cc | 30 +-- src/localization/joint_solver.h | 2 +- .../integration_test/localization_test2.cc | 3 +- src/test/unit_test/joint_solve_test.cc | 178 ++++++------------ 4 files changed, 78 insertions(+), 135 deletions(-) diff --git a/src/localization/joint_solver.cc b/src/localization/joint_solver.cc index 28bcb4c8..82e65a16 100644 --- a/src/localization/joint_solver.cc +++ b/src/localization/joint_solver.cc @@ -117,11 +117,13 @@ auto JointSolver::EstimatePosition( if (detection_batches.empty()) { return std::nullopt; } + double avg_timestamp = 0; std::vector data_points; for (size_t source_index = 0; source_index < detection_batches.size(); source_index++) { const CameraMatrices& camera_mats = camera_matrices_[source_index]; for (const tag_detection_t& detection : detection_batches[source_index]) { + avg_timestamp += detection.timestamp; std::vector undistorted_corners; cv::undistortImagePoints(detection.corners, undistorted_corners, camera_mats.camera_matrix, @@ -145,15 +147,18 @@ auto JointSolver::EstimatePosition( return std::nullopt; } + avg_timestamp /= data_points.size() / 4; + Eigen::VectorXd residual(2 * data_points.size()); Eigen::MatrixXd d_residual_d_twist_jacobian(2 * data_points.size(), 6); + double current_error; for (int epoch = 0; epoch < 1e4; epoch++) { ComputeResidual(data_points, robot_to_field_, residual, &d_residual_d_twist_jacobian); - double current_error = residual.cwiseSquare().sum(); - if (epoch % 10 == 0) - std::cout << "Current error: " << current_error << std::endl; + current_error = residual.cwiseSquare().sum(); + // if (epoch % 10 == 0) + // std::cout << "Current error: " << current_error << std::endl; const Eigen::Vector b = -d_residual_d_twist_jacobian.transpose() * residual; const Eigen::Matrix hessian = @@ -186,7 +191,7 @@ auto JointSolver::EstimatePosition( } } - std::cout << "Robot to field:\n" << robot_to_field_ << std::endl; + // std::cout << "Robot to field:\n" << robot_to_field_ << std::endl; Eigen::Matrix4d field_to_robot = robot_to_field_.inverse(); field_to_robot(3, 0) = 0; @@ -194,17 +199,20 @@ auto JointSolver::EstimatePosition( field_to_robot(3, 2) = 0; field_to_robot(3, 3) = 1; - std::cout << "Field to robot cv:\n" << field_to_robot << std::endl; + // std::cout << "Field to robot cv:\n" << field_to_robot << std::endl; - utils::PrintTransformationMatrix(utils::EigenToCvMat(field_to_robot), - "Field to robot cv"); + // utils::PrintTransformationMatrix(utils::EigenToCvMat(field_to_robot), + // "Field to robot cv"); utils::ChangeBasis(field_to_robot, utils::CV_TO_WPI); - std::cout << "Field to robot wpi:\n" << field_to_robot << std::endl; - utils::PrintTransformationMatrix(utils::EigenToCvMat(field_to_robot), - "Field to robot wpi"); + // std::cout << "Field to robot wpi:\n" << field_to_robot << std::endl; + // utils::PrintTransformationMatrix(utils::EigenToCvMat(field_to_robot), + // "Field to robot wpi"); return std::make_optional( - {.pose = frc::Pose3d(field_to_robot), .variance = 0, .timestamp = 0}); + {.pose = frc::Pose3d(field_to_robot), + .timestamp = avg_timestamp, + .num_tags = static_cast(data_points.size() / 4), + .loss = current_error}); } } // namespace localization diff --git a/src/localization/joint_solver.h b/src/localization/joint_solver.h index 5bcc4f7f..a02c8d95 100644 --- a/src/localization/joint_solver.h +++ b/src/localization/joint_solver.h @@ -19,7 +19,7 @@ struct CameraMatrices { cv::Mat camera_matrix; }; static constexpr int kmax_tags = 50; -class JointSolver : IJointPositionSolver { +class JointSolver : public IJointPositionSolver { public: JointSolver(const std::vector& camera_constants_); auto EstimatePosition( diff --git a/src/test/integration_test/localization_test2.cc b/src/test/integration_test/localization_test2.cc index 58030649..5ad15e01 100644 --- a/src/test/integration_test/localization_test2.cc +++ b/src/test/integration_test/localization_test2.cc @@ -14,6 +14,7 @@ #include "src/camera/camera_constants.h" #include "src/camera/camera_source.h" #include "src/camera/disk_camera.h" +#include "src/localization/joint_solver.h" #include "src/localization/multi_tag_solver.h" #include "src/localization/networktable_sender.h" #include "src/localization/opencv_apriltag_detector.h" @@ -125,7 +126,7 @@ auto main(int argc, char** argv) -> int { LOG(INFO) << "Created camera source"; localization::RunJointLocalization( detector_source, - std::make_unique(camera_constants), + std::make_unique(camera_constants), std::make_unique("Joint", false, true)); }); diff --git a/src/test/unit_test/joint_solve_test.cc b/src/test/unit_test/joint_solve_test.cc index 59ae2378..5c522ed4 100644 --- a/src/test/unit_test/joint_solve_test.cc +++ b/src/test/unit_test/joint_solve_test.cc @@ -13,139 +13,73 @@ class JointSolverTest : public ::testing::Test { camera::camera_constants_t camera_constants = camera::GetCameraConstants(); std::vector joint_solve_cameras = - std::vector{camera_constants.at("main_bot_left")}; - localization::SquareSolver square_solver; + std::vector{ + camera_constants.at("main_bot_left"), + camera_constants.at("main_bot_right")}; + localization::SquareSolver left_square_solver; + localization::SquareSolver right_square_solver; localization::JointSolver joint_solver; - localization::OpenCVAprilTagDetector detector; + localization::OpenCVAprilTagDetector left_detector; + localization::OpenCVAprilTagDetector right_detector; JointSolverTest() - : square_solver(camera_constants.at("main_bot_left")), + : left_square_solver(camera_constants.at("main_bot_left")), + right_square_solver(camera_constants.at("main_bot_right")), joint_solver(joint_solve_cameras), - detector(utils::ReadIntrinsics( - camera_constants.at("main_bot_left").intrinsics_path.value())) {} + left_detector(utils::ReadIntrinsics( + camera_constants.at("main_bot_left").intrinsics_path.value())), + right_detector(utils::ReadIntrinsics( + camera_constants.at("main_bot_right").intrinsics_path.value())) {} }; -// TEST_F(JointSolverTest, MaintainsValidEstimateOpenRotation) { // NOLINT -// const localization::position_estimate_t square_solver_solution = -// square_solver.EstimatePosition(test_utils::fake_detections, false)[0]; -// std::map> -// associated_detections; -// associated_detections.insert({config, test_utils::fake_detections}); -// const localization::position_estimate_t joint_solver_solution = -// joint_solver.EstimatePosition(associated_detections, -// square_solver_solution.pose, false); -// EXPECT_EQ(square_solver_solution, joint_solver_solution); -// } -// -// TEST_F(JointSolverTest, CloseConvergenceOpenRotation) { // NOLINT -// const frc::Transform3d joint_solve_input_noise( -// frc::Translation3d(units::meter_t{0.4}, units::meter_t{0.5}, -// units::meter_t{0.2}), -// frc::Rotation3d(units::degree_t{test_utils::deg2rad(3)}, -// units::degree_t{test_utils::deg2rad(3)}, -// units::degree_t{test_utils::deg2rad(3)})); -// const localization::position_estimate_t square_solver_solution = -// square_solver.EstimatePosition(test_utils::fake_detections, false)[0]; -// frc::Pose3d noisy_pose = -// square_solver_solution.pose.TransformBy(joint_solve_input_noise); -// std::map> -// associated_detections; -// associated_detections.insert({config, test_utils::fake_detections}); -// const localization::position_estimate_t joint_solver_solution = -// joint_solver.EstimatePosition(associated_detections, noisy_pose, false, -// true); -// // std::cout << "sq: " << square_solver_solution -// // << "\njoint: " << joint_solver_solution << std::endl; -// EXPECT_EQ(square_solver_solution, joint_solver_solution); -// } - -// TEST_F(JointSolverTest, IncreasingDistanceYawOnly) { // NOLINT -// double xDiff = rand(); -// double yDiff = rand(); -// double zDiff = rand(); -// const double translationMax = std::hypot(xDiff, yDiff, zDiff) * 4.0; -// xDiff /= translationMax; -// yDiff /= translationMax; -// zDiff /= translationMax; -// double rollDiff = rand(); -// double pitchDiff = rand(); -// double yawDiff = rand(); -// const double rotationDivider = std::hypot(rollDiff, pitchDiff, yawDiff) / 4.0; -// // rollDiff /= rotationDivider; -// // pitchDiff /= rotationDivider; -// rollDiff = 0; -// pitchDiff = 0; -// yawDiff /= rotationDivider; -// localization::position_estimate_t square_solver_solution = -// square_solver.EstimatePosition(test_utils::fake_detections, false)[0]; -// std::map> -// associated_detections; -// associated_detections.insert( -// {camera_constants.at("main_bot_left"), test_utils::fake_detections}); -// localization::position_estimate_t joint_solver_solution; -// for (int i = 0; i < 15; i++) { -// const frc::Transform3d joint_solve_input_noise( -// frc::Translation3d(units::meter_t{i * xDiff}, units::meter_t{i * yDiff}, -// units::meter_t{i * zDiff}), -// frc::Rotation3d(units::degree_t{i * rollDiff}, -// units::degree_t{i * pitchDiff}, -// units::degree_t{i * yawDiff})); -// -// frc::Pose3d noisy_pose = -// square_solver_solution.pose.TransformBy(joint_solve_input_noise); -// joint_solver_solution = joint_solver.EstimatePosition( -// associated_detections, noisy_pose, true, false); -// std::cout << "iter: " << i << " sq: " << square_solver_solution -// << "\njoint: " << joint_solver_solution << std::endl; -// std::cout << "Noise" << std::endl; -// utils::PrintTransform3d(joint_solve_input_noise); -// if (square_solver_solution != joint_solver_solution) { -// joint_solver_solution = joint_solver.EstimatePosition( -// associated_detections, noisy_pose, true, true); -// std::cout << "Noisy pose" << std::endl; -// utils::PrintPose3d(noisy_pose); -// FAIL() << "Failed with above transformation: " -// << "\nJoint solve solution: " << joint_solver_solution -// << "\nSq solution: " << square_solver_solution; -// -// break; -// } -// } -// } -// -// TEST_F(JointSolverTest, MaintainsValidEstimateYawOnly) { // NOLINT -// const localization::position_estimate_t square_solver_solution = -// square_solver.EstimatePosition(test_utils::fake_detections, false)[0]; -// std::map> -// associated_detections; -// associated_detections.insert({config, test_utils::fake_detections}); -// const localization::position_estimate_t joint_solver_solution = -// joint_solver.EstimatePosition(associated_detections, -// square_solver_solution.pose, true); -// // std::cout << "sq: " << square_solver_solution -// // << "\njoint: " << joint_solver_solution << std::endl; -// EXPECT_EQ(square_solver_solution, joint_solver_solution); -// } - TEST_F(JointSolverTest, MaintainsValidEstimateRealImageYawOnly) { // NOLINT - const cv::Mat image = cv::imread("/bos/bos-logs/log181/left/20.095369.jpg"); - camera::timestamped_frame_t frame{.frame = image, .timestamp = 0.0}; - const std::vector detections = - detector.GetTagDetections(frame); - CHECK_NE(detections.size(), 0); - const localization::position_estimate_t square_solver_solution = - square_solver.EstimatePosition(detections, false)[0]; + const cv::Mat left_image = + cv::imread("/bos/bos-logs/log181/main_bot_left/12.899408.jpg"); + const cv::Mat right_image = + cv::imread("/bos/bos-logs/log181/main_bot_right/13.027757.jpg"); + camera::timestamped_frame_t left_frame{.frame = left_image, .timestamp = 0.0}; + camera::timestamped_frame_t right_frame{.frame = right_image, + .timestamp = 0.0}; + const std::vector left_detections = + left_detector.GetTagDetections(left_frame); + const std::vector right_detections = + right_detector.GetTagDetections(right_frame); + CHECK_NE(left_detections.size() + right_detections.size(), 0); + const std::vector + left_square_solver_solutions = + left_square_solver.EstimatePosition(left_detections, false); + const std::optional + left_square_solver_solution = + left_square_solver_solutions.size() == 0 + ? std::nullopt + : std::make_optional( + left_square_solver_solutions[0]); + const std::vector + right_square_solver_solutions = + right_square_solver.EstimatePosition(right_detections, false); + const std::optional + right_square_solver_solution = + right_square_solver_solutions.size() == 0 + ? std::nullopt + : std::make_optional( + right_square_solver_solutions[0]); std::vector> associated_detections{ - detections}; + left_detections, right_detections}; const frc::Transform3d joint_solve_input_noise( - frc::Translation3d(units::meter_t{0.4}, units::meter_t{0.4}, - units::meter_t{0.4}), + frc::Translation3d(units::meter_t{0.1}, units::meter_t{0.1}, + units::meter_t{0.1}), frc::Rotation3d(units::degree_t{0}, units::degree_t{0}, units::degree_t{0})); joint_solver.SetStartPosition( - square_solver_solution.pose.TransformBy(joint_solve_input_noise)); + (left_square_solver_solution.has_value() ? left_square_solver_solution + : right_square_solver_solution) + ->pose.TransformBy(joint_solve_input_noise)); const localization::position_estimate_t joint_solver_solution = joint_solver.EstimatePosition(associated_detections, false).value(); - std::cout << "sq: " << square_solver_solution - << "\njoint: " << joint_solver_solution << std::endl; - EXPECT_EQ(square_solver_solution, joint_solver_solution); + if (left_square_solver_solution) { + std::cout << "sq left: " << left_square_solver_solution.value(); + } + if (right_square_solver_solution) { + std::cout << "\nsq right: " << right_square_solver_solution.value(); + } + std::cout << "\njoint: " << joint_solver_solution << std::endl; } From b7bc6eeefeaf6a3aa6dd9d949ddade50f79cd2e9 Mon Sep 17 00:00:00 2001 From: yasen5 Date: Sun, 17 May 2026 02:29:27 +0000 Subject: [PATCH 25/30] Add backup solver --- src/localization/joint_solver.cc | 19 ++++++++++++++++--- src/localization/joint_solver.h | 4 +++- 2 files changed, 19 insertions(+), 4 deletions(-) diff --git a/src/localization/joint_solver.cc b/src/localization/joint_solver.cc index 82e65a16..e85fa222 100644 --- a/src/localization/joint_solver.cc +++ b/src/localization/joint_solver.cc @@ -16,7 +16,8 @@ using frc::AprilTagFieldLayout; JointSolver::JointSolver( const std::vector& camera_constants_) - : robot_to_field_(Eigen::Matrix4d::Identity()) { + : robot_to_field_(Eigen::Matrix4d::Identity()), + backup_solver_(camera_constants_) { // clang-format off const Eigen::Matrix4d rotate_yaw_cv = (Eigen::Matrix4d() << -1, 0, 0, 0, @@ -181,7 +182,7 @@ auto JointSolver::EstimatePosition( lambda *= 2; if (lambda > kmaximum_lambda) { std::cout << "Failed" << std::endl; - return std::nullopt; + return backup_solver_.EstimatePosition(detection_batches); } } else { lambda /= 2; @@ -207,9 +208,21 @@ auto JointSolver::EstimatePosition( // std::cout << "Field to robot wpi:\n" << field_to_robot << std::endl; // utils::PrintTransformationMatrix(utils::EigenToCvMat(field_to_robot), // "Field to robot wpi"); + const frc::Pose3d iteratively_solved_pose(field_to_robot); + if (current_error > kmax_acceptable_error || + utils::PoseOffField(iteratively_solved_pose)) { + std::optional backup_solution = + backup_solver_.EstimatePosition(detection_batches); + if (backup_solution.has_value()) { + SetStartPosition(backup_solution->pose); + std::cout << "Using unambiguous detector for ts: " + << backup_solution->timestamp << std::endl; + } + return backup_solution; + } return std::make_optional( - {.pose = frc::Pose3d(field_to_robot), + {.pose = iteratively_solved_pose, .timestamp = avg_timestamp, .num_tags = static_cast(data_points.size() / 4), .loss = current_error}); diff --git a/src/localization/joint_solver.h b/src/localization/joint_solver.h index a02c8d95..651545e1 100644 --- a/src/localization/joint_solver.h +++ b/src/localization/joint_solver.h @@ -3,7 +3,7 @@ #include "nlohmann/json.hpp" #include "src/camera/camera_constants.h" #include "src/localization/position_solver.h" -#include "src/localization/square_solver.h" +#include "src/localization/unambiguous_estimator.h" using json = nlohmann::json; @@ -35,9 +35,11 @@ class JointSolver : public IJointPositionSolver { private: static constexpr double kacceptable_reprojection_error = 0.005; static constexpr double kmaximum_lambda = 1e10; + static constexpr double kmax_acceptable_error = 1e3; std::vector camera_matrices_; std::array>, kmax_tags> tag_corners_; Eigen::Matrix4d robot_to_field_; + UnambiguousEstimator backup_solver_; }; } // namespace localization From 36d0c93bddb1a16ca2edd116aeb73c1964396f03 Mon Sep 17 00:00:00 2001 From: yasen5 Date: Sun, 17 May 2026 03:26:11 +0000 Subject: [PATCH 26/30] Add determinism --- src/camera/disk_camera.cc | 76 ++++++++++++++++++++++- src/camera/disk_camera.h | 6 +- src/localization/joint_solver.cc | 9 ++- src/localization/joint_solver.h | 3 +- src/localization/multi_camera_detector.cc | 64 ++++++++++++++++--- src/localization/multi_camera_detector.h | 9 ++- src/test/unit_test/joint_solve_test.cc | 6 +- 7 files changed, 153 insertions(+), 20 deletions(-) diff --git a/src/camera/disk_camera.cc b/src/camera/disk_camera.cc index eee12f85..54cad904 100644 --- a/src/camera/disk_camera.cc +++ b/src/camera/disk_camera.cc @@ -1,21 +1,82 @@ #include "disk_camera.h" +#include #include #include +#include "absl/flags/flag.h" #include "absl/log/check.h" #include "src/camera/camera_constants.h" #include "src/camera/camera_source.h" +ABSL_FLAG(bool, deterministic_disk_camera, true, // NOLINT + "Make DiskCamera replay synchronize deterministic DiskCamera " + "instances by frame instead of sleeping between timestamps."); + +namespace { +std::mutex deterministic_barrier_mutex; +std::condition_variable deterministic_barrier_cv; +size_t deterministic_barrier_active = 0; +size_t deterministic_barrier_arrived = 0; +size_t deterministic_barrier_generation = 0; + +auto RegisterDeterministicDiskCamera() -> void { + std::lock_guard lock(deterministic_barrier_mutex); + deterministic_barrier_active++; +} + +auto ReleaseDeterministicBarrierIfReady(std::unique_lock& lock) + -> void { + if (deterministic_barrier_active == 0 || + deterministic_barrier_arrived < deterministic_barrier_active) { + return; + } + deterministic_barrier_arrived = 0; + deterministic_barrier_generation++; + lock.unlock(); + deterministic_barrier_cv.notify_all(); +} + +auto DeregisterDeterministicDiskCamera(bool& registered) -> void { + if (!registered) { + return; + } + std::unique_lock lock(deterministic_barrier_mutex); + registered = false; + deterministic_barrier_active--; + ReleaseDeterministicBarrierIfReady(lock); +} + +auto WaitForDeterministicDiskCameras() -> void { + std::unique_lock lock(deterministic_barrier_mutex); + const size_t generation = deterministic_barrier_generation; + deterministic_barrier_arrived++; + if (deterministic_barrier_arrived >= deterministic_barrier_active) { + ReleaseDeterministicBarrierIfReady(lock); + return; + } + deterministic_barrier_cv.wait(lock, [generation] { + return generation != deterministic_barrier_generation || + deterministic_barrier_active == 0; + }); +} +} // namespace + namespace camera { DiskCamera::DiskCamera(std::string image_folder_path, std::optional camera_constant, double speed, std::optional start, - std::optional end) + std::optional end, bool deterministic) : speed_(speed), + deterministic_(deterministic || + absl::GetFlag(FLAGS_deterministic_disk_camera)), + deterministic_registered_(deterministic_), start_(start), end_(end), camera_constant_(std::move(camera_constant)), image_folder_path_(std::move(image_folder_path)) { + if (deterministic_registered_) { + RegisterDeterministicDiskCamera(); + } CHECK(!start_.has_value() || !end_.has_value() || end_.value() > start_.value()); for (auto& entry : std::filesystem::directory_iterator(image_folder_path_)) { @@ -55,8 +116,14 @@ DiskCamera::DiskCamera(std::string image_folder_path, } image_paths_ = std::move(normalized); } + +DiskCamera::~DiskCamera() { + DeregisterDeterministicDiskCamera(deterministic_registered_); +} + auto DiskCamera::GetFrame() -> timestamped_frame_t { if (image_paths_.empty()) { + DeregisterDeterministicDiskCamera(deterministic_registered_); std::cout << "Finished reading all frames from DiskCamera. Folder path: " << image_folder_path_ << std::endl; return {.invalid = true}; @@ -69,7 +136,12 @@ auto DiskCamera::GetFrame() -> timestamped_frame_t { .timestamp = recorded_ts + start_.value_or(0)}; image_paths_.pop(); - if (!image_paths_.empty()) { + if (deterministic_) { + WaitForDeterministicDiskCameras(); + if (image_paths_.empty()) { + DeregisterDeterministicDiskCamera(deterministic_registered_); + } + } else if (!image_paths_.empty()) { double delta = image_paths_.top().timestamp - recorded_ts; std::this_thread::sleep_for(std::chrono::duration(delta / speed_)); } diff --git a/src/camera/disk_camera.h b/src/camera/disk_camera.h index 08d98d2d..9899a2ac 100644 --- a/src/camera/disk_camera.h +++ b/src/camera/disk_camera.h @@ -28,7 +28,9 @@ class DiskCamera : public ICamera { DiskCamera(std::string image_folder_path, std::optional camera_constant = std::nullopt, double speed = 1.0, std::optional start = std::nullopt, - std::optional end = std::nullopt); + std::optional end = std::nullopt, + bool deterministic = false); + ~DiskCamera() override; auto GetFrame() -> timestamped_frame_t override; auto Restart() -> void override; auto IsDone() -> bool override { return image_paths_.empty(); } @@ -36,6 +38,8 @@ class DiskCamera : public ICamera { private: const double speed_; + const bool deterministic_; + bool deterministic_registered_; const std::optional start_, end_; std::optional camera_constant_; std::string image_folder_path_; diff --git a/src/localization/joint_solver.cc b/src/localization/joint_solver.cc index e85fa222..323e3d96 100644 --- a/src/localization/joint_solver.cc +++ b/src/localization/joint_solver.cc @@ -154,7 +154,8 @@ auto JointSolver::EstimatePosition( Eigen::MatrixXd d_residual_d_twist_jacobian(2 * data_points.size(), 6); double current_error; - for (int epoch = 0; epoch < 1e4; epoch++) { + for (int epoch = 0; epoch < 1e4 && current_error < kmin_acceptable_error; + epoch++) { ComputeResidual(data_points, robot_to_field_, residual, &d_residual_d_twist_jacobian); current_error = residual.cwiseSquare().sum(); @@ -192,6 +193,8 @@ auto JointSolver::EstimatePosition( } } + if (current_error > kmax_acceptable_error) {} + // std::cout << "Robot to field:\n" << robot_to_field_ << std::endl; Eigen::Matrix4d field_to_robot = robot_to_field_.inverse(); @@ -209,8 +212,8 @@ auto JointSolver::EstimatePosition( // utils::PrintTransformationMatrix(utils::EigenToCvMat(field_to_robot), // "Field to robot wpi"); const frc::Pose3d iteratively_solved_pose(field_to_robot); - if (current_error > kmax_acceptable_error || - utils::PoseOffField(iteratively_solved_pose)) { + if (false/*current_error > kmax_acceptable_error || + utils::PoseOffField(iteratively_solved_pose)*/) { std::optional backup_solution = backup_solver_.EstimatePosition(detection_batches); if (backup_solution.has_value()) { diff --git a/src/localization/joint_solver.h b/src/localization/joint_solver.h index 651545e1..5f9d0406 100644 --- a/src/localization/joint_solver.h +++ b/src/localization/joint_solver.h @@ -35,7 +35,8 @@ class JointSolver : public IJointPositionSolver { private: static constexpr double kacceptable_reprojection_error = 0.005; static constexpr double kmaximum_lambda = 1e10; - static constexpr double kmax_acceptable_error = 1e3; + static constexpr double kmax_acceptable_error = 1e5; + static constexpr double kmin_acceptable_error = 2; std::vector camera_matrices_; std::array>, kmax_tags> tag_corners_; diff --git a/src/localization/multi_camera_detector.cc b/src/localization/multi_camera_detector.cc index cdf00c76..99ef3731 100644 --- a/src/localization/multi_camera_detector.cc +++ b/src/localization/multi_camera_detector.cc @@ -1,4 +1,5 @@ #include "src/localization/multi_camera_detector.h" +#include "absl/flags/flag.h" #include "absl/status/status.h" #include "src/camera/camera.h" #include "src/camera/cv_camera.h" @@ -8,14 +9,24 @@ #include "src/utils/camera_utils.h" #include "src/utils/log.h" +ABSL_FLAG(bool, wait_for_all_camera_detections, true, // NOLINT + "Make MultiCameraDetector::GetTagDetections wait for one new " + "detection batch from every camera before returning."); + namespace localization { MultiCameraDetector::MultiCameraDetector( std::vector camera_constants, - std::optional> image_paths) + std::optional> image_paths, + bool wait_for_all_detections) : camera_constants_(std::move(camera_constants)), last_write_times_(camera_constants_.size()), timestamped_frames_(camera_constants_.size()), - tag_detections_(camera_constants_.size()) { + tag_detections_(camera_constants_.size()), + detection_counts_(camera_constants_.size()), + consumed_detection_counts_(camera_constants_.size()), + wait_for_all_detections_( + wait_for_all_detections || + absl::GetFlag(FLAGS_wait_for_all_camera_detections)) { std::string log_path = frc::DataLogManager::GetLogDir(); cameras_.reserve(camera_constants_.size()); camera_threads_.reserve(camera_constants_.size()); @@ -65,6 +76,16 @@ MultiCameraDetector::MultiCameraDetector( for (size_t i = 0; i < cameras_.size(); i++) { camera_threads_.emplace_back([this, i, image_paths]() -> void { while (run_cameras_) { + if (wait_for_all_detections_) { + std::unique_lock lock(mutex_); + detection_cv_.wait(lock, [this, i] { + return !run_cameras_ || + detection_counts_[i] == consumed_detection_counts_[i]; + }); + if (!run_cameras_) { + return; + } + } camera::timestamped_frame_t timestamped_frame; timestamped_frame = cameras_[i]->GetFrame(); if (timestamped_frame.invalid) { @@ -87,9 +108,9 @@ MultiCameraDetector::MultiCameraDetector( std::lock_guard lock(mutex_); timestamped_frames_[i] = std::move(timestamped_frame); tag_detections_[i] = std::move(detections); + detection_counts_[i]++; } - has_new_detections_.store(true, std::memory_order_release); - has_new_detections_.notify_one(); + detection_cv_.notify_all(); } }); } @@ -97,13 +118,37 @@ MultiCameraDetector::MultiCameraDetector( auto MultiCameraDetector::GetTagDetections() -> std::vector> { - has_new_detections_.wait(false, std::memory_order_acquire); - has_new_detections_.store(false, std::memory_order_release); std::vector> tag_detections; - { - std::lock_guard lock(mutex_); - tag_detections = tag_detections_; + std::unique_lock lock(mutex_); + if (wait_for_all_detections_) { + detection_cv_.wait(lock, [this] { + if (!run_cameras_) { + return true; + } + for (size_t i = 0; i < detection_counts_.size(); i++) { + if (detection_counts_[i] <= consumed_detection_counts_[i]) { + return false; + } + } + return true; + }); + } else { + detection_cv_.wait(lock, [this] { + if (!run_cameras_) { + return true; + } + for (size_t i = 0; i < detection_counts_.size(); i++) { + if (detection_counts_[i] > consumed_detection_counts_[i]) { + return true; + } + } + return false; + }); } + tag_detections = tag_detections_; + consumed_detection_counts_ = detection_counts_; + lock.unlock(); + detection_cv_.notify_all(); return tag_detections; } @@ -122,6 +167,7 @@ auto MultiCameraDetector::GetCVFrames() -> std::vector { MultiCameraDetector::~MultiCameraDetector() { run_cameras_ = false; + detection_cv_.notify_all(); for (auto& t : camera_threads_) { if (t.joinable()) t.join(); diff --git a/src/localization/multi_camera_detector.h b/src/localization/multi_camera_detector.h index fe96fbde..17a8e34f 100644 --- a/src/localization/multi_camera_detector.h +++ b/src/localization/multi_camera_detector.h @@ -1,4 +1,5 @@ #pragma once +#include #include "src/camera/camera.h" #include "src/camera/cscore_streamer.h" #include "src/camera/disk_camera.h" @@ -12,7 +13,8 @@ class MultiCameraDetector { public: MultiCameraDetector(std::vector camera_constants, std::optional> - image_paths = std::nullopt); + image_paths = std::nullopt, + bool wait_for_all_detections = false); [[nodiscard]] auto GetTagDetections() -> std::vector>; [[nodiscard]] auto GetCVFrames() -> std::vector; @@ -27,10 +29,13 @@ class MultiCameraDetector { std::vector last_write_times_; std::vector timestamped_frames_; std::vector> tag_detections_; + std::vector detection_counts_; + std::vector consumed_detection_counts_; std::vector camera_threads_; std::mutex mutex_; + std::condition_variable detection_cv_; std::atomic run_cameras_{true}; - std::atomic has_new_detections_{false}; + const bool wait_for_all_detections_; static constexpr int kdefault_stream_fps = 30; }; diff --git a/src/test/unit_test/joint_solve_test.cc b/src/test/unit_test/joint_solve_test.cc index 5c522ed4..dda9ac93 100644 --- a/src/test/unit_test/joint_solve_test.cc +++ b/src/test/unit_test/joint_solve_test.cc @@ -33,9 +33,9 @@ class JointSolverTest : public ::testing::Test { TEST_F(JointSolverTest, MaintainsValidEstimateRealImageYawOnly) { // NOLINT const cv::Mat left_image = - cv::imread("/bos/bos-logs/log181/main_bot_left/12.899408.jpg"); + cv::imread("/bos/bos-logs/log181/main_bot_left/5.611357.jpg"); const cv::Mat right_image = - cv::imread("/bos/bos-logs/log181/main_bot_right/13.027757.jpg"); + cv::imread("/bos/bos-logs/log181/main_bot_right/5.647740.jpg"); camera::timestamped_frame_t left_frame{.frame = left_image, .timestamp = 0.0}; camera::timestamped_frame_t right_frame{.frame = right_image, .timestamp = 0.0}; @@ -82,4 +82,6 @@ TEST_F(JointSolverTest, MaintainsValidEstimateRealImageYawOnly) { // NOLINT std::cout << "\nsq right: " << right_square_solver_solution.value(); } std::cout << "\njoint: " << joint_solver_solution << std::endl; + std::cout << "loss: " << joint_solver_solution.loss << std::endl; + std::cout << "numtags: " << joint_solver_solution.num_tags << std::endl; } From 43bff0430b7b24244f05b898d07510d951b1d7b8 Mon Sep 17 00:00:00 2001 From: yasen5 Date: Thu, 21 May 2026 02:27:14 +0000 Subject: [PATCH 27/30] Charlie commit msg --- scripts/copy_images_in_range.py | 115 ++++++++++++++++++ scripts/img_opener.py | 159 +++++++++++++++++++++---- src/localization/joint_solver.cc | 53 ++++++--- src/localization/joint_solver.h | 4 +- src/localization/position_solver.h | 1 + src/localization/run_localization.cc | 1 + src/test/unit_test/joint_solve_test.cc | 73 +++++++++++- 7 files changed, 361 insertions(+), 45 deletions(-) create mode 100755 scripts/copy_images_in_range.py diff --git a/scripts/copy_images_in_range.py b/scripts/copy_images_in_range.py new file mode 100755 index 00000000..68b43222 --- /dev/null +++ b/scripts/copy_images_in_range.py @@ -0,0 +1,115 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +import argparse +import os +import re +import shutil +import sys +from pathlib import Path + +SUPPORTED_EXTENSIONS = (".png", ".jpg", ".jpeg", ".bmp", ".gif", ".tiff") +CAMERA_DIRS = ("main_bot_left", "main_bot_right") + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser( + description=( + "Copy timestamped images from main_bot_left/main_bot_right into a " + "new folder, keeping only images inside an inclusive timestamp range." + ) + ) + parser.add_argument("source", help="Source folder containing camera folders") + parser.add_argument("destination", help="Destination folder to copy into") + parser.add_argument("start_time", type=float, help="Minimum timestamp to copy") + parser.add_argument("end_time", type=float, help="Maximum timestamp to copy") + parser.add_argument( + "--flat", + action="store_true", + help="Copy directly into destination instead of preserving camera folders", + ) + return parser.parse_args() + + +def extract_timestamp(filename: str) -> float: + match = re.search(r"\d+(\.\d+)?", filename) + if not match: + raise ValueError(f"No timestamp found in filename: {filename}") + return float(match.group()) + + +def iter_image_paths(folder: Path): + for path in sorted(folder.iterdir()): + if path.is_file() and path.name.lower().endswith(SUPPORTED_EXTENSIONS): + yield path + + +def copy_camera_folder( + source_dir: Path, + destination_dir: Path, + start_time: float, + end_time: float, +) -> tuple[int, int]: + destination_dir.mkdir(parents=True, exist_ok=True) + copied = 0 + skipped = 0 + + for image_path in iter_image_paths(source_dir): + try: + timestamp = extract_timestamp(image_path.name) + except ValueError: + skipped += 1 + continue + + if start_time <= timestamp <= end_time: + shutil.copy2(image_path, destination_dir / image_path.name) + copied += 1 + else: + skipped += 1 + + return copied, skipped + + +def main() -> int: + args = parse_args() + source = Path(args.source).expanduser() + destination = Path(args.destination).expanduser() + + if args.start_time > args.end_time: + print("Error: start_time must be <= end_time", file=sys.stderr) + return 1 + + if not source.is_dir(): + print(f"Error: source folder not found: {source}", file=sys.stderr) + return 1 + + camera_dirs = [source / camera_dir for camera_dir in CAMERA_DIRS] + if not all(camera_dir.is_dir() for camera_dir in camera_dirs): + missing = [str(camera_dir) for camera_dir in camera_dirs if not camera_dir.is_dir()] + print( + "Error: source must contain main_bot_left and main_bot_right. " + f"Missing: {', '.join(missing)}", + file=sys.stderr, + ) + return 1 + + total_copied = 0 + total_skipped = 0 + for camera_dir in camera_dirs: + output_dir = destination if args.flat else destination / camera_dir.name + copied, skipped = copy_camera_folder( + camera_dir, + output_dir, + args.start_time, + args.end_time, + ) + total_copied += copied + total_skipped += skipped + print(f"{camera_dir.name}: copied {copied}, skipped {skipped}") + + print(f"Done. Copied {total_copied} images into {destination}") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/scripts/img_opener.py b/scripts/img_opener.py index 250b06d6..b2511ac9 100644 --- a/scripts/img_opener.py +++ b/scripts/img_opener.py @@ -1,10 +1,12 @@ import os import sys import re +from bisect import bisect_left from PIL import Image, ImageTk import tkinter as tk SUPPORTED_EXTENSIONS = (".png", ".jpg", ".jpeg", ".bmp", ".gif", ".tiff") +CAMERA_DIRS = ("main_bot_left", "main_bot_right") def extract_timestamp(filename): @@ -20,24 +22,31 @@ def __init__(self, folder, start_time=0.0): self.root = tk.Tk() self.root.title("Image Viewer (Press any key to advance)") - # Collect images - items = [] - for f in os.listdir(folder): - if f.lower().endswith(SUPPORTED_EXTENSIONS): - try: - ts = extract_timestamp(f) - items.append((ts, os.path.join(folder, f))) - except ValueError: - pass + folder = resolve_folder(folder) + camera_folders = [os.path.join(folder, cam) for cam in CAMERA_DIRS] + if all(os.path.isdir(camera_folder) for camera_folder in camera_folders): + self.load_stereo_images(camera_folders, start_time) + else: + self.load_single_folder(folder, start_time) + + self.index = 0 + + self.label = tk.Label(self.root) + self.label.pack() + + self.root.bind("", self.next_image) + + self.show_image() + self.root.mainloop() + + def load_single_folder(self, folder, start_time): + self.mode = "single" + items = collect_images(folder) if not items: print("No valid timestamped images found.") sys.exit(1) - # Sort numerically - items.sort(key=lambda x: x[0]) - - # --- FILTER BY START TIME --- items = [item for item in items if item[0] >= start_time] if not items: @@ -46,22 +55,52 @@ def __init__(self, folder, start_time=0.0): self.timestamps = [x[0] for x in items] self.image_paths = [x[1] for x in items] - - # --- NORMALIZE TO FIRST DISPLAYED FRAME --- self.t0 = self.timestamps[0] self.norm_timestamps = [t - self.t0 for t in self.timestamps] - self.index = 0 + def load_stereo_images(self, camera_folders, start_time): + self.mode = "stereo" + left_items = collect_images(camera_folders[0]) + right_items = collect_images(camera_folders[1]) - self.label = tk.Label(self.root) - self.label.pack() + if not left_items or not right_items: + print("Both main_bot_left and main_bot_right need valid timestamped images.") + sys.exit(1) - self.root.bind("", self.next_image) + left_items = [item for item in left_items if item[0] >= start_time] + right_items = [item for item in right_items if item[0] >= start_time] - self.show_image() - self.root.mainloop() + if not left_items or not right_items: + print(f"No images found after start_time={start_time}") + sys.exit(1) + + all_timestamps = sorted(set(t for t, _ in left_items + right_items)) + + left_timestamps = [x[0] for x in left_items] + right_timestamps = [x[0] for x in right_items] + pairs = [] + last_paths = None + for timestamp in all_timestamps: + left = nearest_item(left_items, left_timestamps, timestamp) + right = nearest_item(right_items, right_timestamps, timestamp) + paths = (left[1], right[1]) + if paths == last_paths: + continue + pairs.append((timestamp, left, right)) + last_paths = paths + + self.timestamps = [x[0] for x in pairs] + self.image_pairs = [(x[1], x[2]) for x in pairs] + self.t0 = self.timestamps[0] + self.norm_timestamps = [t - self.t0 for t in self.timestamps] def show_image(self): + if self.mode == "stereo": + self.show_stereo_image() + else: + self.show_single_image() + + def show_single_image(self): path = self.image_paths[self.index] img = Image.open(path) @@ -79,9 +118,28 @@ def show_image(self): f"({self.index+1}/{len(self.image_paths)})" ) + def show_stereo_image(self): + (left_ts, left_path), (right_ts, right_path) = self.image_pairs[self.index] + + screen_w = self.root.winfo_screenwidth() + screen_h = self.root.winfo_screenheight() + img = make_side_by_side(left_path, right_path, screen_w, screen_h) + + self.tk_image = ImageTk.PhotoImage(img) + self.label.config(image=self.tk_image) + + norm_time = self.norm_timestamps[self.index] + frame_time = self.timestamps[self.index] + delta_ms = abs(left_ts - right_ts) * 1000 + self.root.title( + f"left {os.path.basename(left_path)} | right {os.path.basename(right_path)} | " + f"t = {norm_time:.3f}s ({frame_time:.3f}) | dt = {delta_ms:.1f}ms " + f"({self.index+1}/{len(self.image_pairs)})" + ) + def next_image(self, event=None): self.index += 1 - if self.index >= len(self.image_paths): + if self.index >= len(self.timestamps): print("Reached end of images.") self.root.quit() return @@ -89,9 +147,64 @@ def next_image(self, event=None): self.show_image() +def resolve_folder(folder): + if os.path.isdir(folder): + return folder + + scripts_parent_path = os.path.join(os.path.dirname(__file__), "..", folder) + if os.path.isdir(scripts_parent_path): + return scripts_parent_path + + print(f"Folder not found: {folder}") + sys.exit(1) + + +def collect_images(folder): + items = [] + for f in os.listdir(folder): + if f.lower().endswith(SUPPORTED_EXTENSIONS): + try: + ts = extract_timestamp(f) + items.append((ts, os.path.join(folder, f))) + except ValueError: + pass + + items.sort(key=lambda x: x[0]) + return items + + +def nearest_item(items, timestamps, timestamp): + index = bisect_left(timestamps, timestamp) + if index == 0: + return items[0] + if index == len(items): + return items[-1] + + before = items[index - 1] + after = items[index] + if abs(before[0] - timestamp) <= abs(after[0] - timestamp): + return before + return after + + +def make_side_by_side(left_path, right_path, screen_w, screen_h): + max_each_w = max(1, screen_w // 2) + left_img = Image.open(left_path).convert("RGB") + right_img = Image.open(right_path).convert("RGB") + left_img.thumbnail((max_each_w, screen_h)) + right_img.thumbnail((max_each_w, screen_h)) + + width = left_img.width + right_img.width + height = max(left_img.height, right_img.height) + combined = Image.new("RGB", (width, height), "black") + combined.paste(left_img, (0, (height - left_img.height) // 2)) + combined.paste(right_img, (left_img.width, (height - right_img.height) // 2)) + return combined + + if __name__ == "__main__": if len(sys.argv) not in (2, 3): - print("Usage: python viewer.py [start_time]") + print("Usage: python img_opener.py [start_time]") sys.exit(1) folder = sys.argv[1] diff --git a/src/localization/joint_solver.cc b/src/localization/joint_solver.cc index 323e3d96..8c94e0bb 100644 --- a/src/localization/joint_solver.cc +++ b/src/localization/joint_solver.cc @@ -58,9 +58,13 @@ JointSolver::JointSolver( } } -void JointSolver::SetStartPosition(const frc::Pose3d& pose) { - robot_to_field_ = pose.ToMatrix().inverse(); - utils::ChangeBasis(robot_to_field_, utils::WPI_TO_CV); +void JointSolver::SetStartPosition(const std::optional& pose) { + if (pose.has_value()) { + robot_to_field_ = pose->ToMatrix().inverse(); + utils::ChangeBasis(robot_to_field_, utils::WPI_TO_CV); + } else { + should_reset_ = true; + } } void JointSolver::ComputeResidual( @@ -148,19 +152,29 @@ auto JointSolver::EstimatePosition( return std::nullopt; } + if (should_reset_) { + std::optional backup_estimate_ = + backup_solver_.EstimatePosition(detection_batches); + if (backup_estimate_.has_value()) { + std::cout << "RESETTING to\t"; + utils::PrintPose3d(backup_estimate_->pose); + SetStartPosition(std::make_optional(backup_estimate_->pose)); + should_reset_ = false; + } + } + avg_timestamp /= data_points.size() / 4; Eigen::VectorXd residual(2 * data_points.size()); Eigen::MatrixXd d_residual_d_twist_jacobian(2 * data_points.size(), 6); - double current_error; + double current_error = std::numeric_limits::infinity(); - for (int epoch = 0; epoch < 1e4 && current_error < kmin_acceptable_error; - epoch++) { + for (int epoch = 0; epoch < 1e4; epoch++) { ComputeResidual(data_points, robot_to_field_, residual, &d_residual_d_twist_jacobian); current_error = residual.cwiseSquare().sum(); - // if (epoch % 10 == 0) - // std::cout << "Current error: " << current_error << std::endl; + if (debug && epoch % 10 == 0) + std::cout << "Current error: " << current_error << std::endl; const Eigen::Vector b = -d_residual_d_twist_jacobian.transpose() * residual; const Eigen::Matrix hessian = @@ -195,7 +209,8 @@ auto JointSolver::EstimatePosition( if (current_error > kmax_acceptable_error) {} - // std::cout << "Robot to field:\n" << robot_to_field_ << std::endl; + if (debug) + std::cout << "Robot to field:\n" << robot_to_field_ << std::endl; Eigen::Matrix4d field_to_robot = robot_to_field_.inverse(); field_to_robot(3, 0) = 0; @@ -203,14 +218,17 @@ auto JointSolver::EstimatePosition( field_to_robot(3, 2) = 0; field_to_robot(3, 3) = 1; - // std::cout << "Field to robot cv:\n" << field_to_robot << std::endl; - - // utils::PrintTransformationMatrix(utils::EigenToCvMat(field_to_robot), - // "Field to robot cv"); + if (debug) { + std::cout << "Field to robot cv:\n" << field_to_robot << std::endl; + utils::PrintTransformationMatrix(utils::EigenToCvMat(field_to_robot), + "Field to robot cv"); + } utils::ChangeBasis(field_to_robot, utils::CV_TO_WPI); - // std::cout << "Field to robot wpi:\n" << field_to_robot << std::endl; - // utils::PrintTransformationMatrix(utils::EigenToCvMat(field_to_robot), - // "Field to robot wpi"); + if (debug) { + std::cout << "Field to robot wpi:\n" << field_to_robot << std::endl; + utils::PrintTransformationMatrix(utils::EigenToCvMat(field_to_robot), + "Field to robot wpi"); + } const frc::Pose3d iteratively_solved_pose(field_to_robot); if (false/*current_error > kmax_acceptable_error || utils::PoseOffField(iteratively_solved_pose)*/) { @@ -224,6 +242,9 @@ auto JointSolver::EstimatePosition( return backup_solution; } + std::cout << "Pose for ts: " << avg_timestamp << ":\t"; + utils::PrintPose3d(iteratively_solved_pose); + return std::make_optional( {.pose = iteratively_solved_pose, .timestamp = avg_timestamp, diff --git a/src/localization/joint_solver.h b/src/localization/joint_solver.h index 5f9d0406..42fd127d 100644 --- a/src/localization/joint_solver.h +++ b/src/localization/joint_solver.h @@ -26,7 +26,7 @@ class JointSolver : public IJointPositionSolver { std::vector>& detection_batches, bool reject_far_tags = true) -> std::optional override; - void SetStartPosition(const frc::Pose3d& pose); + void SetStartPosition(const std::optional& pose) override; void ComputeResidual(const std::vector& data_points, const Eigen::Matrix4d& robot_to_field, Eigen::VectorXd& residual, @@ -42,5 +42,7 @@ class JointSolver : public IJointPositionSolver { tag_corners_; Eigen::Matrix4d robot_to_field_; UnambiguousEstimator backup_solver_; + bool should_reset_ = false; + const bool debug = false; }; } // namespace localization diff --git a/src/localization/position_solver.h b/src/localization/position_solver.h index 70ebae2b..83f6c46d 100644 --- a/src/localization/position_solver.h +++ b/src/localization/position_solver.h @@ -47,6 +47,7 @@ class IJointPositionSolver { virtual auto EstimatePosition( std::vector>& detection_batches, bool reject_far_tags = true) -> std::optional = 0; + virtual void SetStartPosition(const std::optional& start_pose) {} virtual ~IJointPositionSolver() = default; }; diff --git a/src/localization/run_localization.cc b/src/localization/run_localization.cc index d8a767a2..520323d9 100644 --- a/src/localization/run_localization.cc +++ b/src/localization/run_localization.cc @@ -54,6 +54,7 @@ void RunJointLocalization( MultiCameraDetector& detector_source, std::unique_ptr solver, std::unique_ptr sender, bool verbose) { + solver->SetStartPosition(std::nullopt); while (true) { auto detections = detector_source.GetTagDetections(); std::optional estimated_pose = diff --git a/src/test/unit_test/joint_solve_test.cc b/src/test/unit_test/joint_solve_test.cc index dda9ac93..88f21c04 100644 --- a/src/test/unit_test/joint_solve_test.cc +++ b/src/test/unit_test/joint_solve_test.cc @@ -33,9 +33,9 @@ class JointSolverTest : public ::testing::Test { TEST_F(JointSolverTest, MaintainsValidEstimateRealImageYawOnly) { // NOLINT const cv::Mat left_image = - cv::imread("/bos/bos-logs/log181/main_bot_left/5.611357.jpg"); + cv::imread("/bos/bos-logs/short-pmatch2/main_bot_left/3.267974.jpg"); const cv::Mat right_image = - cv::imread("/bos/bos-logs/log181/main_bot_right/5.647740.jpg"); + cv::imread("/bos/bos-logs/short-pmatch2/main_bot_right/3.271297.jpg"); camera::timestamped_frame_t left_frame{.frame = left_image, .timestamp = 0.0}; camera::timestamped_frame_t right_frame{.frame = right_image, .timestamp = 0.0}; @@ -65,10 +65,10 @@ TEST_F(JointSolverTest, MaintainsValidEstimateRealImageYawOnly) { // NOLINT std::vector> associated_detections{ left_detections, right_detections}; const frc::Transform3d joint_solve_input_noise( - frc::Translation3d(units::meter_t{0.1}, units::meter_t{0.1}, - units::meter_t{0.1}), + frc::Translation3d(units::meter_t{0.4}, units::meter_t{0.4}, + units::meter_t{0.0}), frc::Rotation3d(units::degree_t{0}, units::degree_t{0}, - units::degree_t{0})); + units::degree_t{5})); joint_solver.SetStartPosition( (left_square_solver_solution.has_value() ? left_square_solver_solution : right_square_solver_solution) @@ -85,3 +85,66 @@ TEST_F(JointSolverTest, MaintainsValidEstimateRealImageYawOnly) { // NOLINT std::cout << "loss: " << joint_solver_solution.loss << std::endl; std::cout << "numtags: " << joint_solver_solution.num_tags << std::endl; } + +TEST_F(JointSolverTest, DoubleTrouble) { // NOLINT + const cv::Mat left_image = + cv::imread("/bos/bos-logs/short-pmatch2/main_bot_left/3.267974.jpg"); + const cv::Mat right_image = + cv::imread("/bos/bos-logs/short-pmatch2/main_bot_right/3.271297.jpg"); + camera::timestamped_frame_t left_frame{.frame = left_image, .timestamp = 0.0}; + camera::timestamped_frame_t right_frame{.frame = right_image, + .timestamp = 0.0}; + const std::vector left_detections = + left_detector.GetTagDetections(left_frame); + const std::vector right_detections = + right_detector.GetTagDetections(right_frame); + CHECK_NE(left_detections.size() + right_detections.size(), 0); + const std::vector + left_square_solver_solutions = + left_square_solver.EstimatePosition(left_detections, false); + const std::optional + left_square_solver_solution = + left_square_solver_solutions.size() == 0 + ? std::nullopt + : std::make_optional( + left_square_solver_solutions[0]); + const std::vector + right_square_solver_solutions = + right_square_solver.EstimatePosition(right_detections, false); + const std::optional + right_square_solver_solution = + right_square_solver_solutions.size() == 0 + ? std::nullopt + : std::make_optional( + right_square_solver_solutions[0]); + std::vector> associated_detections{ + left_detections, right_detections}; + const frc::Pose3d start_pose( + frc::Translation3d(units::meter_t{6.062}, units::meter_t{0.15}, + units::meter_t{-0.065}), + frc::Rotation3d(units::degree_t{3.221067}, units::degree_t{3.6748235}, + units::degree_t{176.1127104})); + + const frc::Pose3d correction_noise( + frc::Translation3d(units::meter_t{6.062}, units::meter_t{0.15}, + units::meter_t{0.06}), + frc::Rotation3d(units::degree_t{3.221067}, units::degree_t{3.6748235}, + units::degree_t{176.1127104})); + joint_solver.SetStartPosition(start_pose); + const localization::position_estimate_t joint_solver_solution = + joint_solver.EstimatePosition(associated_detections, false).value(); + const localization::position_estimate_t new_solution = + joint_solver.EstimatePosition(associated_detections, false).value(); + if (left_square_solver_solution) { + std::cout << "sq left: " << left_square_solver_solution.value(); + } + if (right_square_solver_solution) { + std::cout << "\nsq right: " << right_square_solver_solution.value(); + } + std::cout << "\njoint: " << joint_solver_solution << std::endl; + std::cout << "loss: " << joint_solver_solution.loss << std::endl; + std::cout << "numtags: " << joint_solver_solution.num_tags << std::endl; + std::cout << "\njoint new: " << new_solution << std::endl; + std::cout << "loss: " << new_solution.loss << std::endl; + std::cout << "numtags: " << new_solution.num_tags << std::endl; +} From 680b61b188bf1848520e0daabb0ad603d6875463 Mon Sep 17 00:00:00 2001 From: yasen5 Date: Thu, 21 May 2026 02:50:14 +0000 Subject: [PATCH 28/30] Switch from pure joint solve to endpoint adjustment --- src/localization/joint_solver.cc | 52 ++++++++++++++------------------ 1 file changed, 23 insertions(+), 29 deletions(-) diff --git a/src/localization/joint_solver.cc b/src/localization/joint_solver.cc index 8c94e0bb..8db3aa7b 100644 --- a/src/localization/joint_solver.cc +++ b/src/localization/joint_solver.cc @@ -152,16 +152,14 @@ auto JointSolver::EstimatePosition( return std::nullopt; } - if (should_reset_) { - std::optional backup_estimate_ = - backup_solver_.EstimatePosition(detection_batches); - if (backup_estimate_.has_value()) { - std::cout << "RESETTING to\t"; - utils::PrintPose3d(backup_estimate_->pose); - SetStartPosition(std::make_optional(backup_estimate_->pose)); - should_reset_ = false; - } + std::optional backup_estimate_ = + backup_solver_.EstimatePosition(detection_batches); + if (!backup_estimate_.has_value()) { + return std::nullopt; } + bool joint_solve_failure = false; + utils::PrintPose3d(backup_estimate_->pose); + SetStartPosition(std::make_optional(backup_estimate_->pose)); avg_timestamp /= data_points.size() / 4; @@ -173,8 +171,9 @@ auto JointSolver::EstimatePosition( ComputeResidual(data_points, robot_to_field_, residual, &d_residual_d_twist_jacobian); current_error = residual.cwiseSquare().sum(); - if (debug && epoch % 10 == 0) - std::cout << "Current error: " << current_error << std::endl; + if (debug && epoch % 10 == 0) { + LOG(INFO) << "Current error: " << current_error; + } const Eigen::Vector b = -d_residual_d_twist_jacobian.transpose() * residual; const Eigen::Matrix hessian = @@ -196,8 +195,7 @@ auto JointSolver::EstimatePosition( if (candidate_error > current_error) { lambda *= 2; if (lambda > kmaximum_lambda) { - std::cout << "Failed" << std::endl; - return backup_solver_.EstimatePosition(detection_batches); + joint_solve_failure = true; } } else { lambda /= 2; @@ -207,10 +205,15 @@ auto JointSolver::EstimatePosition( } } - if (current_error > kmax_acceptable_error) {} + if (joint_solve_failure || current_error > kmax_acceptable_error) { + if (debug) { + LOG(INFO) << "Joint solve failure"; + } + return backup_estimate_; + } if (debug) - std::cout << "Robot to field:\n" << robot_to_field_ << std::endl; + LOG(INFO) << "Robot to field:\n" << robot_to_field_; Eigen::Matrix4d field_to_robot = robot_to_field_.inverse(); field_to_robot(3, 0) = 0; @@ -219,30 +222,21 @@ auto JointSolver::EstimatePosition( field_to_robot(3, 3) = 1; if (debug) { - std::cout << "Field to robot cv:\n" << field_to_robot << std::endl; + LOG(INFO) << "Field to robot cv:\n" << field_to_robot; utils::PrintTransformationMatrix(utils::EigenToCvMat(field_to_robot), "Field to robot cv"); } utils::ChangeBasis(field_to_robot, utils::CV_TO_WPI); if (debug) { - std::cout << "Field to robot wpi:\n" << field_to_robot << std::endl; + LOG(INFO) << "Field to robot wpi:\n" << field_to_robot; utils::PrintTransformationMatrix(utils::EigenToCvMat(field_to_robot), "Field to robot wpi"); } const frc::Pose3d iteratively_solved_pose(field_to_robot); - if (false/*current_error > kmax_acceptable_error || - utils::PoseOffField(iteratively_solved_pose)*/) { - std::optional backup_solution = - backup_solver_.EstimatePosition(detection_batches); - if (backup_solution.has_value()) { - SetStartPosition(backup_solution->pose); - std::cout << "Using unambiguous detector for ts: " - << backup_solution->timestamp << std::endl; - } - return backup_solution; - } - std::cout << "Pose for ts: " << avg_timestamp << ":\t"; + if (debug) { + LOG(INFO) << "Pose for ts: " << avg_timestamp << ":\t"; + } utils::PrintPose3d(iteratively_solved_pose); return std::make_optional( From b72bddf86867815e6ca6c2a16dbd4a44b9622441 Mon Sep 17 00:00:00 2001 From: yasen5 Date: Thu, 21 May 2026 03:05:30 +0000 Subject: [PATCH 29/30] Add absl flag for logging for easier comparison of solves --- src/localization/networktable_sender.cc | 3 ++- src/pathing/simulator.cc | 9 +++++++-- src/test/integration_test/localization_test2.cc | 2 +- src/utils/CMakeLists.txt | 2 +- src/utils/log_path.cc | 16 ++++++++++++++++ src/utils/log_path.h | 9 +++++++++ 6 files changed, 36 insertions(+), 5 deletions(-) diff --git a/src/localization/networktable_sender.cc b/src/localization/networktable_sender.cc index 37d2a8c4..80931c83 100644 --- a/src/localization/networktable_sender.cc +++ b/src/localization/networktable_sender.cc @@ -1,6 +1,7 @@ #include "networktable_sender.h" #include "frc/DataLogManager.h" #include "src/localization/position.h" +#include "src/utils/log_path.h" namespace localization { @@ -56,7 +57,7 @@ NetworkTableSender::NetworkTableSender(const std::string& camera_name, if (sim) { std::error_code ec; - log_.emplace("/bos/logs/sim.wpilog", ec); + log_.emplace("/bos/logs/" + utils::GetSimLogName(), ec); if (ec) { std::cerr << "Failed to open log: " << ec.message() << '\n'; std::exit(0); diff --git a/src/pathing/simulator.cc b/src/pathing/simulator.cc index 128248cc..cfddf080 100644 --- a/src/pathing/simulator.cc +++ b/src/pathing/simulator.cc @@ -7,10 +7,15 @@ #include #include #include +#include "absl/flags/parse.h" #include "src/pathing/pathing.h" +#include "src/utils/log_path.h" -auto main() -> int { - wpi::log::DataLogBackgroundWriter log{"/root/bos/logs", "sim.wpilog"}; +auto main(int argc, char** argv) -> int { + absl::ParseCommandLine(argc, argv); + + wpi::log::DataLogBackgroundWriter log{"/root/bos/logs", + utils::GetSimLogName()}; wpi::log::StructLogEntry poseLog(log, "/sim/Pose2d"); wpi::log::DoubleLogEntry accelXLog(log, "/sim/AccelX"); diff --git a/src/test/integration_test/localization_test2.cc b/src/test/integration_test/localization_test2.cc index 5ad15e01..8e260e89 100644 --- a/src/test/integration_test/localization_test2.cc +++ b/src/test/integration_test/localization_test2.cc @@ -126,7 +126,7 @@ auto main(int argc, char** argv) -> int { LOG(INFO) << "Created camera source"; localization::RunJointLocalization( detector_source, - std::make_unique(camera_constants), + std::make_unique(camera_constants), std::make_unique("Joint", false, true)); }); diff --git a/src/utils/CMakeLists.txt b/src/utils/CMakeLists.txt index 87daae36..3da5a702 100644 --- a/src/utils/CMakeLists.txt +++ b/src/utils/CMakeLists.txt @@ -1,4 +1,4 @@ -add_library(utils timer.cc nt_utils.cc camera_utils.cc log.cc constants_from_json.cc transform.cc) +add_library(utils timer.cc nt_utils.cc camera_utils.cc log.cc log_path.cc constants_from_json.cc transform.cc) target_link_libraries(utils PUBLIC wpilibc nlohmann_json::nlohmann_json absl::log absl::check absl::flags absl::flags_parse apriltag ntcore vpi) if(!ENABLE_CLANG_TIDY) diff --git a/src/utils/log_path.cc b/src/utils/log_path.cc index e69de29b..84d2726e 100644 --- a/src/utils/log_path.cc +++ b/src/utils/log_path.cc @@ -0,0 +1,16 @@ +#include "src/utils/log_path.h" + +#include + +#include "absl/flags/flag.h" + +ABSL_FLAG(std::string, sim_log_name, "sim.wpilog", // NOLINT + "Name of the simulation WPILib log file."); + +namespace utils { + +std::string GetSimLogName() { + return absl::GetFlag(FLAGS_sim_log_name); +} + +} // namespace utils diff --git a/src/utils/log_path.h b/src/utils/log_path.h index e69de29b..8b2c749c 100644 --- a/src/utils/log_path.h +++ b/src/utils/log_path.h @@ -0,0 +1,9 @@ +#pragma once + +#include + +namespace utils { + +std::string GetSimLogName(); + +} // namespace utils From 4ea96742af48041fdfaa1d76f8edd8a10d5e343e Mon Sep 17 00:00:00 2001 From: yasen5 Date: Sun, 24 May 2026 00:29:44 +0000 Subject: [PATCH 30/30] Add absl flags --- src/localization/joint_solver.cc | 21 +++++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) diff --git a/src/localization/joint_solver.cc b/src/localization/joint_solver.cc index 8db3aa7b..3e45fdf5 100644 --- a/src/localization/joint_solver.cc +++ b/src/localization/joint_solver.cc @@ -2,10 +2,19 @@ #include #include #include +#include "absl/flags/flag.h" #include "src/utils/camera_utils.h" #include "src/utils/constants_from_json.h" #include "src/utils/transform.h" +ABSL_FLAG(int, max_epochs, 10000, // NOLINT + "Max iterative epochs the solver will use"); // NOLINT +ABSL_FLAG(double, max_acceptable_error, 1e5, // NOLINT + "Maximum accepted final reprojection loss"); // NOLINT +ABSL_FLAG(double, lambda_scalar, // NOLINT + 2.0, // NOLINT + "Levenberg-Marquardt damping adjustment"); // NOLINT + namespace localization { constexpr auto sq(double num) -> double { @@ -119,6 +128,10 @@ void JointSolver::ComputeResidual( auto JointSolver::EstimatePosition( std::vector>& detection_batches, bool reject_far_tags) -> std::optional { + const int max_epochs = absl::GetFlag(FLAGS_max_epochs); + const double lambda_scalar = absl::GetFlag(FLAGS_lambda_scalar); + const double max_acceptable_error = + absl::GetFlag(FLAGS_max_acceptable_error); if (detection_batches.empty()) { return std::nullopt; } @@ -167,7 +180,7 @@ auto JointSolver::EstimatePosition( Eigen::MatrixXd d_residual_d_twist_jacobian(2 * data_points.size(), 6); double current_error = std::numeric_limits::infinity(); - for (int epoch = 0; epoch < 1e4; epoch++) { + for (int epoch = 0; epoch < max_epochs; epoch++) { ComputeResidual(data_points, robot_to_field_, residual, &d_residual_d_twist_jacobian); current_error = residual.cwiseSquare().sum(); @@ -193,19 +206,19 @@ auto JointSolver::EstimatePosition( ComputeResidual(data_points, candidate_robot_to_field, residual); double candidate_error = residual.cwiseSquare().sum(); if (candidate_error > current_error) { - lambda *= 2; + lambda *= lambda_scalar; if (lambda > kmaximum_lambda) { joint_solve_failure = true; } } else { - lambda /= 2; + lambda /= lambda_scalar; robot_to_field_ = candidate_robot_to_field; break; } } } - if (joint_solve_failure || current_error > kmax_acceptable_error) { + if (joint_solve_failure || current_error > max_acceptable_error) { if (debug) { LOG(INFO) << "Joint solve failure"; }