diff --git a/.github/workflows/test-single-platform.yml b/.github/workflows/test-single-platform.yml index 6b570d7..840f0fe 100644 --- a/.github/workflows/test-single-platform.yml +++ b/.github/workflows/test-single-platform.yml @@ -12,7 +12,8 @@ jobs: steps: - name: Checkout code uses: actions/checkout@v4 - + with: + submodules: recursive - name: Set up Docker Buildx uses: docker/setup-buildx-action@v3 diff --git a/.gitignore b/.gitignore index a57b149..7102495 100644 --- a/.gitignore +++ b/.gitignore @@ -3,4 +3,7 @@ build install log .cache -.devcontainer \ No newline at end of file +.devcontainer +docker-compose.yml +Dockerfile +.env \ No newline at end of file diff --git a/.vscode/extensions.json b/.vscode/extensions.json new file mode 100644 index 0000000..16e940b --- /dev/null +++ b/.vscode/extensions.json @@ -0,0 +1,5 @@ +{ + "recommendations": [ + "gruntfuggly.todo-tree" + ] +} \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 145ffe0..8e80351 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -39,6 +39,7 @@ target_link_libraries(alliance_auto_aim PUBLIC TBB::tbb openvino::runtime Ceres::ceres + yaml-cpp ) # 安装规则 diff --git a/configs/example.yaml b/configs/example.yaml new file mode 100644 index 0000000..cdeb620 --- /dev/null +++ b/configs/example.yaml @@ -0,0 +1,44 @@ +#####-----classfier parameters-----##### +# classify_model: +# model_image_width: +# model_image_height: +# +#####-----identifier parameters-----##### +threshold: 150 +max_angle_error: 45 #degree +min_lightbar_ratio: 1.5 +max_lightbar_ratio: 20 +min_lightbar_length: 8 +max_armor_ratio: 5 +min_armor_ratio: 1 +max_side_ratio: 1.5 +min_confidence: 0.8 +max_rectangular_error: 25 #degree + +#####-----target_snapshot_manager parameters-----##### +decision_speed: 7 # rad/s +high_speed_delay_time: 0.050 # s +low_speed_delay_time: 0.015 # s + +#####-----fire_decision parameters-----##### +auto_fire: true +first_tolerance: 100 # 近距离射击容差，degree +second_tolerance: 100 # 远距离射击容差，degree +judge_distance: 2 #距离判断阈值 + +#####-----fire_controller parameters-----##### +control_delay_s: 0.1 + +#####-----aiming_solver parameters-----##### +yaw_offset: 1.5 # degree +pitch_offset: -0.5 # degree +bullet_speed: 26 + +#####-----aime_point_chooser parameters-----##### +comming_angle: 60 # degree +leaving_angle: 20 # degree + +#####-----solver parameters-----##### +R_muzzle2camera: +t_muzzle2camera: + diff --git a/env/ubuntu22.04.sh b/env/ubuntu22.04.sh index c838f97..0a0e144 100644 --- a/env/ubuntu22.04.sh +++ b/env/ubuntu22.04.sh @@ -13,7 +13,7 @@ rm GPG-PUB-KEY-INTEL-SW-PRODUCTS.PUB echo "deb https://apt.repos.intel.com/openvino ubuntu22 main" | sudo tee /etc/apt/sources.list.d/intel-openvino.list sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-keys BAC6F0C353D04109 sudo apt-get update -sudo apt-get install -y libtbb-dev libeigen3-dev libopencv-dev openvino gcc-13 g++-13 libceres-dev libdwarf-dev libbackward-cpp-dev binutils-dev libdw-dev libunwind-dev libfmt-dev libspdlog-dev +sudo apt-get install -y libtbb-dev libeigen3-dev libopencv-dev openvino gcc-13 g++-13 libceres-dev libdwarf-dev libbackward-cpp-dev binutils-dev libdw-dev libunwind-dev libfmt-dev libspdlog-dev libyaml-cpp-dev sudo ln -s /usr/include/libdwarf/libdwarf.h /usr/include/libdwarf.h sudo ln -s /usr/include/libdwarf/dwarf.h /usr/include/dwarf.h diff --git a/include/data/fire_control.hpp b/include/data/fire_control.hpp index 567c618..110af04 100644 --- a/include/data/fire_control.hpp +++ b/include/data/fire_control.hpp @@ -1,12 +1,14 @@ #pragma once +#include "data/time_stamped.hpp" + #include #include #include namespace world_exe::data { struct FireControl { - time_t time_stamp; + data::TimeStamp time_stamp; /// 以ROS系，光轴方向为x, 上为z, /// 四个点为： 1->左上 2->右上 3->右下 4->左下, /// 点实际坐标为opencv系下的像素坐标 diff --git a/include/data/predictor_update_package.hpp b/include/data/predictor_update_package.hpp new file mode 100644 index 0000000..cae7c75 --- /dev/null +++ b/include/data/predictor_update_package.hpp @@ -0,0 +1,44 @@ +#pragma once + +#include "data/sync_data.hpp" +#include "interfaces/armor_in_camera.hpp" + +#include "data/time_stamped.hpp" +#include + +namespace world_exe::data { + +struct PredictorUpdatePackage final{ +public: + + /** + * @brief 传感器数据获取时的时间戳 + */ + const data::TimeStamp& GetTimeStamp() const{ return data1_.camera_capture_begin_time_stamp; }; + + /** + * @brief 求解好的装甲板三维信息 + * + * @return std::shared_ptr + */ + std::shared_ptr GetArmors() const{return data2_;}; + + /** + * @brief 相机坐标系到世界坐标系的仿射变换 + * + * @return Eigen::Affine3d + */ + Eigen::Affine3d GetCameraToWorld() const {return data1_.camera_to_gimbal;}; + + // but why? + PredictorUpdatePackage(const data::CameraGimbalMuzzleSyncData& data1, std::shared_ptr data2) + : data1_(data1) + , data2_(data2){ + } + PredictorUpdatePackage() = delete; + ~PredictorUpdatePackage() = default; +private: + const data::CameraGimbalMuzzleSyncData& data1_; + const std::shared_ptr data2_; +}; +} \ No newline at end of file diff --git a/include/data/sync_data.hpp b/include/data/sync_data.hpp index 765bc4e..cc5a21d 100644 --- a/include/data/sync_data.hpp +++ b/include/data/sync_data.hpp @@ -1,10 +1,11 @@ #pragma once +#include "data/time_stamped.hpp" #include #include namespace world_exe::data { struct CameraGimbalMuzzleSyncData { - std::time_t camera_capture_begin_time_stamp; + data::TimeStamp camera_capture_begin_time_stamp; Eigen::Affine3d camera_to_gimbal; Eigen::Affine3d gimbal_to_muzzle; }; diff --git a/include/data/time_stamped.hpp b/include/data/time_stamped.hpp new file mode 100644 index 0000000..85d7e25 --- /dev/null +++ b/include/data/time_stamped.hpp @@ -0,0 +1,36 @@ +#pragma once + +#include +#include +namespace world_exe::data { +/** + * @brief 时间戳，通常是SteadyClock + */ +struct TimeStamp { + +public: + + inline TimeStamp() : stamp_(){} + inline TimeStamp(const TimeStamp& stamp): stamp_(stamp.stamp_) {} + inline TimeStamp(const std::chrono::nanoseconds& stamp): stamp_(stamp) {} + inline TimeStamp(const std::chrono::seconds& stamp): stamp_(std::chrono::duration_cast(stamp)) {} + + inline constexpr double to_seconds() const{return stamp_.count() * 1e-9;} + inline constexpr double to_nanosec() const{return stamp_.count();} + + auto operator<=>(const TimeStamp& other) const noexcept = default; + inline TimeStamp operator- (const TimeStamp& other) const noexcept {return TimeStamp{stamp_ - other.stamp_};} + inline TimeStamp operator+ (const TimeStamp& other) const noexcept {return TimeStamp{stamp_ + other.stamp_};} + inline TimeStamp operator* (const double& ratio) const noexcept {return from_nanosec(stamp_.count() * ratio);} + inline TimeStamp operator/ (const double& ratio) const noexcept {return from_nanosec(stamp_.count() / ratio);} + + template + static inline TimeStamp from_seconds(const T time){return TimeStamp{std::chrono::nanoseconds(static_cast(time * 1e9))};} + template + static inline TimeStamp from_nanosec(const T time){return TimeStamp{std::chrono::nanoseconds(static_cast(time))};} + +private: + std::chrono::nanoseconds stamp_; + +}; +} \ No newline at end of file diff --git a/include/interfaces/armor_in_camera.hpp b/include/interfaces/armor_in_camera.hpp index dbb3b94..7083950 100644 --- a/include/interfaces/armor_in_camera.hpp +++ b/include/interfaces/armor_in_camera.hpp @@ -1,10 +1,12 @@ #pragma once #include "data/armor_camera_spacing.hpp" +#include "data/time_stamped.hpp" #include "enum/armor_id.hpp" -#include "interfaces/time_stamped.hpp" +#include "data/time_stamped.hpp" #include + namespace world_exe::interfaces { /** @@ -16,7 +18,7 @@ namespace world_exe::interfaces { class IArmorInCamera { public: /// 获取时间戳，标志其内容装甲板的准确时间点 - COMBINE_TIME_STAMPED; + virtual const data::TimeStamp& GetTimeStamp() const = 0; /// 获取某个车辆ID的装甲板集合 virtual const std::vector& GetArmors( diff --git a/include/interfaces/armor_in_gimbal_control.hpp b/include/interfaces/armor_in_gimbal_control.hpp index f2da7be..182f44f 100644 --- a/include/interfaces/armor_in_gimbal_control.hpp +++ b/include/interfaces/armor_in_gimbal_control.hpp @@ -2,7 +2,7 @@ #include "data/armor_gimbal_control_spacing.hpp" #include "enum/armor_id.hpp" -#include "time_stamped.hpp" +#include "data/time_stamped.hpp" #include namespace world_exe::interfaces { @@ -17,7 +17,7 @@ class IArmorInGimbalControl { public: /// 获取时间戳，标志其内容装甲板的准确时间点 - COMBINE_TIME_STAMPED; + virtual const data::TimeStamp& GetTimeStamp() const = 0; /// 获取某个车辆ID的装甲板集合 virtual const std::vector& GetArmors( diff --git a/include/interfaces/armor_in_image.hpp b/include/interfaces/armor_in_image.hpp index ba3ecec..92a60b9 100644 --- a/include/interfaces/armor_in_image.hpp +++ b/include/interfaces/armor_in_image.hpp @@ -2,7 +2,7 @@ #include "data/armor_image_spaceing.hpp" #include "enum/armor_id.hpp" -#include "time_stamped.hpp" +#include "data/time_stamped.hpp" #include #include @@ -21,7 +21,7 @@ class IArmorInImage { public: /// 获取时间戳，标志其内容装甲板的准确时间点 - COMBINE_TIME_STAMPED; + virtual const data::TimeStamp& GetTimeStamp() const = 0; /** * @brief 获取某个车辆ID的装甲板集合 diff --git a/include/interfaces/fire_controller.hpp b/include/interfaces/fire_controller.hpp index 901dd68..4c5a3e2 100644 --- a/include/interfaces/fire_controller.hpp +++ b/include/interfaces/fire_controller.hpp @@ -2,6 +2,7 @@ #include "data/fire_control.hpp" #include "enum/car_id.hpp" +#include #include namespace world_exe::interfaces { @@ -17,7 +18,7 @@ class IFireControl { * @param time_duration 额外时间差 典型值：当前时刻到当前帧传感器传入内容的时间差 * @return const data::FireControl */ - virtual const data::FireControl CalculateTarget(const std::time_t& time_duration) const = 0; + virtual const data::FireControl CalculateTarget(const std::chrono::seconds& time_duration) const = 0; /** * @brief 获取当前火控系统锁定的车辆ID diff --git a/include/interfaces/predictor.hpp b/include/interfaces/predictor.hpp index 248ed8b..9824f66 100644 --- a/include/interfaces/predictor.hpp +++ b/include/interfaces/predictor.hpp @@ -1,6 +1,7 @@ #pragma once #include "armor_in_gimbal_control.hpp" +#include "data/time_stamped.hpp" #include "enum/armor_id.hpp" #include @@ -24,7 +25,7 @@ class IPredictor { * @return const IArmorInGimbalControl& */ virtual std::shared_ptr Predictor( - const std::time_t& time_stamp) const = 0; + const data::TimeStamp& time_stamp) const = 0; virtual ~IPredictor() = default; }; diff --git a/include/interfaces/predictor_update_package.hpp b/include/interfaces/predictor_update_package.hpp deleted file mode 100644 index ea0eda6..0000000 --- a/include/interfaces/predictor_update_package.hpp +++ /dev/null @@ -1,36 +0,0 @@ -#pragma once - -#include "./armor_in_camera.hpp" - -#include "interfaces/time_stamped.hpp" -#include - -namespace world_exe::interfaces { - -/** - * @brief 给出滤波器需要的所有数据 - * @todo 应该使用结构体，但是先写着无所谓 - */ -class IPreDictorUpdatePackage { -public: - /** - * @brief 传感器数据获取时的时间戳 - */ - COMBINE_TIME_STAMPED; - /** - * @brief 求解好的装甲板三维信息 - * - * @return std::shared_ptr - */ - virtual std::shared_ptr GetArmors() const = 0; - - /** - * @brief 相机坐标系到世界坐标系的仿射变换 - * - * @return Eigen::Affine3d - */ - virtual Eigen::Affine3d GetTransform() const = 0; - - virtual ~IPreDictorUpdatePackage() = default; -}; -} \ No newline at end of file diff --git a/include/interfaces/sync_block.hpp b/include/interfaces/sync_block.hpp index 3f0a6a4..36b3612 100644 --- a/include/interfaces/sync_block.hpp +++ b/include/interfaces/sync_block.hpp @@ -1,6 +1,6 @@ #pragma once -#include +#include "data/time_stamped.hpp" namespace world_exe::interfaces { /** @@ -12,9 +12,9 @@ template class ISyncBlock { public: - virtual void set_data(const T& camera_data); + virtual void set_data(const T& camera_data) = 0; - virtual std::tuple get_data(time_t timestamp); + virtual std::tuple get_data(const data::TimeStamp& timestamp) = 0; virtual ~ISyncBlock() = default; }; diff --git a/include/interfaces/target_predictor.hpp b/include/interfaces/target_predictor.hpp index b7a3a44..c2bb915 100644 --- a/include/interfaces/target_predictor.hpp +++ b/include/interfaces/target_predictor.hpp @@ -1,6 +1,7 @@ #pragma once #include "armor_in_gimbal_control.hpp" +#include "data/time_stamped.hpp" #include "enum/armor_id.hpp" #include "interfaces/predictor.hpp" #include @@ -22,7 +23,7 @@ class ITargetPredictor { * @return std::shared_ptr */ virtual std::shared_ptr Predict( - const enumeration::ArmorIdFlag& id, const std::time_t& time_stamp) = 0; + const enumeration::ArmorIdFlag& id, const data::TimeStamp& time_stamp) = 0; /** * @brief 按照传入的id生成IPredictor diff --git a/include/interfaces/time_stamped.hpp b/include/interfaces/time_stamped.hpp deleted file mode 100644 index 28bfe28..0000000 --- a/include/interfaces/time_stamped.hpp +++ /dev/null @@ -1,17 +0,0 @@ -#pragma once - -#include - -#define COMBINE_TIME_STAMPED virtual const ITimeStamped& GetTimeStamped() const = 0 - -namespace world_exe::interfaces { -/** - * @brief 时间戳，通常是SteadyClock - */ -class ITimeStamped { -public: - virtual const std::time_t& GetTimeStamp() const = 0; - - virtual ~ITimeStamped() = default; -}; -} \ No newline at end of file diff --git a/include/sync/hik_camera_syncdata.hpp b/include/sync/hik_camera_syncdata.hpp deleted file mode 100644 index c955ef3..0000000 --- a/include/sync/hik_camera_syncdata.hpp +++ /dev/null @@ -1,27 +0,0 @@ -#pragma once -#include "opencv2/core/mat.hpp" - -#include "data/sync_data.hpp" -#include "sync/sync_data.hpp" - -namespace world_exe::sync { - -struct HikCameraSyncData final - : public world_exe::core::SyncData { -public: - std::tuple load() override { - return { mat_, data_ }; - }; - void store(cv::Mat arg1, world_exe::data::CameraGimbalMuzzleSyncData arg2) override { - arg1.copyTo(mat_); - std::memcpy((void*)&data_, (void*)&arg2, sizeof(data_)); - }; - explicit HikCameraSyncData(const cv::Mat& mat) - : mat_ { mat.clone() } - , data_ {} { } - -private: - cv::Mat mat_; - world_exe::data::CameraGimbalMuzzleSyncData data_; -}; -} // namespace world_exe::sync \ No newline at end of file diff --git a/include/sync/sync_data.hpp b/include/sync/sync_data.hpp deleted file mode 100644 index 9ba1ad6..0000000 --- a/include/sync/sync_data.hpp +++ /dev/null @@ -1,13 +0,0 @@ -#pragma once - -#include - -namespace world_exe::core { - -template struct SyncData { - - virtual std::tuple load() = 0; - virtual void store(Args... args) = 0; - virtual ~SyncData() = default; -}; -} \ No newline at end of file diff --git a/src/playground/HZA/identifier/armor_identifier_detail.cpp b/src/playground/HZA/identifier/armor_identifier_detail.cpp index 4773d71..66e5c63 100644 --- a/src/playground/HZA/identifier/armor_identifier_detail.cpp +++ b/src/playground/HZA/identifier/armor_identifier_detail.cpp @@ -1,3 +1,4 @@ +#include "armor_image_detail.hpp" #include "armor_identifier_detail.hpp" #include #include diff --git a/src/playground/HZA/identifier/armor_identifier_detail.hpp b/src/playground/HZA/identifier/armor_identifier_detail.hpp index d5cc035..222df90 100644 --- a/src/playground/HZA/identifier/armor_identifier_detail.hpp +++ b/src/playground/HZA/identifier/armor_identifier_detail.hpp @@ -1,6 +1,6 @@ #pragma once #include "interfaces/identifier.hpp" -#include "armor_image_detail.hpp" + namespace world_exe::interfaces::detail { class ArmorIdentifier : public world_exe::interfaces::IIdentifier { diff --git a/src/playground/HZA/identifier/armor_image_detail.cpp b/src/playground/HZA/identifier/armor_image_detail.cpp index 6b02a55..a6198cd 100644 --- a/src/playground/HZA/identifier/armor_image_detail.cpp +++ b/src/playground/HZA/identifier/armor_image_detail.cpp @@ -15,8 +15,7 @@ const std::vector& ArmorInImage::GetArmors(const enumer return result; } -const ITimeStamped& ArmorInImage::GetTimeStamped() const { - time_stamped_.GetTimeStamp(); +const data::TimeStamp& ArmorInImage::GetTimeStamp() const { return time_stamped_; } } \ No newline at end of file diff --git a/src/playground/HZA/identifier/armor_image_detail.hpp b/src/playground/HZA/identifier/armor_image_detail.hpp index 83cb71a..4d569cf 100644 --- a/src/playground/HZA/identifier/armor_image_detail.hpp +++ b/src/playground/HZA/identifier/armor_image_detail.hpp @@ -1,7 +1,6 @@ #pragma once #include "interfaces/armor_in_image.hpp" -#include "interfaces/time_stamped.hpp" -#include "time_stamped_detail.hpp" +#include "data/time_stamped.hpp" namespace world_exe::interfaces::detail{ class ArmorInImage : public world_exe::interfaces::IArmorInImage { @@ -9,11 +8,11 @@ namespace world_exe::interfaces::detail{ ArmorInImage(const std::vector& armors_) : armors_(armors_) {} virtual ~ArmorInImage() = default; - const ITimeStamped& GetTimeStamped() const override; + const data::TimeStamp& GetTimeStamp() const override; const std::vector armors_; const std::vector& GetArmors(const enumeration::ArmorIdFlag& armor_id) const override; private: - TimeStamped time_stamped_; + data::TimeStamp time_stamped_{}; }; } \ No newline at end of file diff --git a/src/playground/HZA/identifier/time_stamped_detail.cpp b/src/playground/HZA/identifier/time_stamped_detail.cpp deleted file mode 100644 index f77d1a4..0000000 --- a/src/playground/HZA/identifier/time_stamped_detail.cpp +++ /dev/null @@ -1,10 +0,0 @@ -#include "time_stamped_detail.hpp" - -namespace world_exe::interfaces::detail -{ - const std::time_t& TimeStamped::GetTimeStamp() const - { - //不是很清楚具体实现是什么 - return time_stamp_; - } -} \ No newline at end of file diff --git a/src/playground/HZA/identifier/time_stamped_detail.hpp b/src/playground/HZA/identifier/time_stamped_detail.hpp deleted file mode 100644 index 1ae4c19..0000000 --- a/src/playground/HZA/identifier/time_stamped_detail.hpp +++ /dev/null @@ -1,14 +0,0 @@ -#pragma once -#include "interfaces/time_stamped.hpp" -#include -namespace world_exe::interfaces::detail { -class TimeStamped : public world_exe::interfaces::ITimeStamped { -public: - TimeStamped() = default; - virtual ~TimeStamped() = default; - const std::time_t& GetTimeStamp() const override; - -private: - std::time_t time_stamp_; -}; -} \ No newline at end of file diff --git a/src/tongji/auto_aim_system.cpp b/src/tongji/auto_aim_system.cpp new file mode 100644 index 0000000..9789324 --- /dev/null +++ b/src/tongji/auto_aim_system.cpp @@ -0,0 +1,104 @@ +#include "auto_aim_system.hpp" + +#include +#include + +#include "../v1/sync/syncer.hpp" +#include "core/event_bus.hpp" +#include "data/predictor_update_package.hpp" +#include "parameters/params_system_v1.hpp" +#include "parameters/profile.hpp" +#include "tongji/fire_controller/fire_controller.hpp" +#include "tongji/predictor/car_predictor/car_predictor_manager.hpp" +#include "tongji/solver/solver.hpp" +#include "tongji/state_machine/state_machine.hpp" +#include "v1/identifier/identifier.hpp" + +namespace world_exe::tongji { +using namespace std::chrono; + +class AutoAimSystem::Impl { +public: + Impl(const bool& debug) + : debug(debug) + , config_path_("../../configs/example.yaml") { + + identifier_ = std::make_unique( + parameters::ParamsForSystemV1::szu_model_path(), + parameters::ParamsForSystemV1::device(), parameters::HikCameraProfile::get_width(), + parameters::HikCameraProfile::get_height()); + pnp_solver_ = std::make_unique(); + live_target_manager_ = std::make_shared(config_path_); + state_machine_ = std::make_shared(); + fire_controller_ = std::make_unique( + config_path_, state_machine_, live_target_manager_); + time_stamp_ = std::chrono::steady_clock::now(); + syncer_ = std::make_unique(seconds(2), 6e-6); + + core::EventBus::Subscript(parameters::ParamsForSystemV1::raw_image_event, + [this](const auto& mat) { Solve(mat); }); + core::EventBus::Subscript( + parameters::ParamsForSystemV1::camera_capture_transforms, + [this](const auto& pkg) { SetTransfroms(pkg); }); + } + + auto Solve(const cv::Mat& raw) -> void { + const auto& [armors_in_image, flag] = identifier_->identify(raw); + if (flag == enumeration::ArmorIdFlag::None) return; + state_machine_->Update(armors_in_image, + std::chrono::duration_cast( + std::chrono::steady_clock::now() - time_stamp_)); + + // 这里使用 any_clock::now 也可以，但是时间系统的转换和同步我希望是单独的部分 + const auto& [pack, check] = syncer_->get_data(armors_in_image->GetTimeStamp()); + if (!check) return; + + const auto R_camera2gimbal = pack.camera_to_gimbal.rotation(); + const auto t_camera2gimbal = pack.camera_to_gimbal.translation(); + + pnp_solver_->SetCamera2Gimbal(R_camera2gimbal, t_camera2gimbal); + const auto& armors_in_camera = pnp_solver_->SolvePnp(armors_in_image); + + auto combined = std::make_shared(pack, armors_in_camera); + + live_target_manager_->Update(combined); + + state_machine_ = std::make_shared(); + + const auto target_id = state_machine_->GetAllowdToFires(); + + const auto gimbal_yaw = R_camera2gimbal.eulerAngles(2, 1, 0)[0]; + fire_controller_->UpdateGimbalPosition(gimbal_yaw); + + /// 这里应该有一个线程进行稳定的输出之类的 + /// 轨迹规划器没有实现，先不管 + + core::EventBus::Publish( + parameters::ParamsForSystemV1::fire_control_event, GetControlCommand()); + } + + void SetTransfroms(const data::CameraGimbalMuzzleSyncData& data) { syncer_->set_data(data); } + + data::FireControl GetControlCommand() { + fire_controller_->GetAttackCarId(); + return fire_controller_->CalculateTarget( + std::chrono::duration_cast(std::chrono::steady_clock::now() - time_stamp_)); + } + +private: + bool debug; + const std::string config_path_; + + std::chrono::steady_clock::time_point time_stamp_; + std::unique_ptr identifier_; + std::unique_ptr pnp_solver_; + std::shared_ptr state_machine_; + std::shared_ptr live_target_manager_; + std::unique_ptr syncer_; + std::unique_ptr fire_controller_; +}; + +AutoAimSystem::AutoAimSystem(const bool& debug) + : pimpl_(std::make_unique(debug)) { } +AutoAimSystem::~AutoAimSystem() = default; +} diff --git a/src/tongji/auto_aim_system.hpp b/src/tongji/auto_aim_system.hpp new file mode 100644 index 0000000..daec9f4 --- /dev/null +++ b/src/tongji/auto_aim_system.hpp @@ -0,0 +1,18 @@ +#pragma once + +#include +namespace world_exe::tongji { + +class AutoAimSystem final { +public: + explicit AutoAimSystem(const bool& debug); + ~AutoAimSystem(); + + AutoAimSystem(const AutoAimSystem&) = delete; + AutoAimSystem& operator=(const AutoAimSystem&) = delete; + +private: + class Impl; + std::unique_ptr pimpl_; +}; +} \ No newline at end of file diff --git a/src/tongji/fire_controller/aim_point_chooser.hpp b/src/tongji/fire_controller/aim_point_chooser.hpp new file mode 100644 index 0000000..51e2c71 --- /dev/null +++ b/src/tongji/fire_controller/aim_point_chooser.hpp @@ -0,0 +1,92 @@ +#pragma once + +#include "data/armor_gimbal_control_spacing.hpp" +#include "enum/car_id.hpp" +#include "util/math.hpp" + +#include + +namespace world_exe::tongji::fire_control { + +using CarIDFlag = enumeration::CarIDFlag; + +class AimPointChooser { +public: + AimPointChooser(const std::string& config_path) { + auto yaml = YAML::LoadFile(config_path); + comming_angle_ = yaml["comming_angle"].as() / 57.3; // degree to rad + leaving_angle_ = yaml["leaving_angle"].as() / 57.3; // degree to rad + } + + std::pair ChooseAimArmor( + const Eigen::Vector& ekf_x, + const std::vector armors, const CarIDFlag& single_id) { + const auto armor_num = armors.size(); + int chosen_id = -1; + + // 整车旋转中心的球坐标yaw + const auto center_yaw = std::atan2(ekf_x[2], ekf_x[0]); + + std::vector> delta_angle_list; + for (int i = 0; i < armor_num; i++) { + auto delta_angle = util::math::clamp_pm_pi( + util::math::get_yaw_from_quaternion(armors[i].orientation) - center_yaw); + delta_angle_list.emplace_back(std::make_tuple(i, delta_angle)); + } + std::sort(delta_angle_list.begin(), delta_angle_list.end(), + [](const auto& a, const auto& b) { return std::get<1>(a) > std::get<1>(b); }); + + // 不考虑小陀螺 + if (std::abs(ekf_x[8]) <= 2 && single_id != CarIDFlag::Outpost) { + // 选择在可射击范围内的装甲板 + std::vector id_list; + for (const auto& [id, delta_angle] : delta_angle_list) { + if (std::abs(delta_angle) > 60 / 57.3) continue; + id_list.emplace_back(id); + } + + if (id_list.size() == 1) { + chosen_id = id_list[0]; + lock_id_ = -1; + } else if (id_list.size() > 1) { + // 未处于锁定模式时，选择delta_angle绝对值较小的装甲板，进入锁定模式 + if (lock_id_ == -1) lock_id_ = id_list[0]; + chosen_id = lock_id_; + } else { + lock_id_ = -1; + chosen_id = lock_id_; + } + } else { + // 小陀螺 + double coming_angle = (single_id == CarIDFlag::Outpost) ? 70 / 57.3 : comming_angle_; + double leaving_angle = (single_id == CarIDFlag::Outpost) ? 30 / 57.3 : leaving_angle_; + + // 在小陀螺时，一侧的装甲板不断出现，另一侧的装甲板不断消失，显然前者被打中的概率更高 + // + for (const auto& [id, delta_angle] : delta_angle_list) { + if (std::abs(delta_angle) > coming_angle) continue; + if ((ekf_x[7] > 0 && delta_angle < leaving_angle) + || (ekf_x[7] < 0 && delta_angle > -leaving_angle)) { + chosen_id = id; + break; + } + } + } + + if (chosen_id == -1) { + return { false, armors[std::get<0>(delta_angle_list.front())] }; + } + + return { + true, + armors[chosen_id], + }; + } + +private: + double comming_angle_ = 60 / 57.3; // degree + double leaving_angle_ = 20 / 57.3; // degree + int lock_id_ = -1; +}; + +} diff --git a/src/tongji/fire_controller/aim_solver.hpp b/src/tongji/fire_controller/aim_solver.hpp new file mode 100644 index 0000000..cdeeb8f --- /dev/null +++ b/src/tongji/fire_controller/aim_solver.hpp @@ -0,0 +1,114 @@ +#pragma once + +#include +#include +#include +#include + +#include +#include + +#include "../predictor/car_predictor/car_predictor.hpp" +#include "aim_point_chooser.hpp" +#include "tongji/predictor/kalman_filter/extended_kalman_filter.hpp" +#include "tongji/predictor/kalman_filter/predict_model.hpp" +#include "trajectory.hpp" + +namespace world_exe::tongji::fire_control { + +struct AimSolution { + bool valid; + double yaw; + double pitch; + Eigen::Vector3d aim_point; // 最终瞄准点（世界坐标 + 装甲板yaw） + double horizon_distance = 0; // 无人机专有 +}; + +class AimingSolver { +public: + using PredictorModel = predictor::EKFModel<11, 4>; + using EKF = predictor::ExtendedKalmanFilter; + + AimingSolver(const std::string& config_path, const double& gravity = 9.7833) + : aim_point_chooser_(std::make_unique(config_path)) + , g_(gravity) { + + auto yaml = YAML::LoadFile(config_path); + yaw_offset_ = yaml["yaw_offset"].as() / 57.3; // degree to rad + pitch_offset_ = yaml["pitch_offset"].as() / 57.3; // degree to rad + bullet_speed_ = yaml["bullet_speed"].as(); + } + + AimSolution SolveAimSolution(std::shared_ptr snapshot, + data::TimeStamp time_stamp, const double& control_delay_s) { + + // 迭代求解飞行时间 (最多10次，收敛条件：相邻两次fly_time差 <0.001) + double prev_fly_time_s; + Eigen::Vector3d final_aim_point; + TrajectoryResult final_trajectory; + bool converged = false; + // HACK:不同击打点影响飞行时间的迭代，需要根据整车的状态（转速和坐标）来选择击打点，不得已将指针转换为派生类 + auto snapshot_derived = std::dynamic_pointer_cast(snapshot); + + // 预测目标在未来 dt时间后的位置 + for (int i = 0; i < 10; ++i) { + const auto& dt = control_delay_s + prev_fly_time_s; + const auto& armors = + snapshot->Predictor(time_stamp + data::TimeStamp::from_seconds(dt)); + + const auto& aim_point = SelectPredictedAim(snapshot_derived->GetPredictedX(dt), + armors->GetArmors(snapshot->GetId()), snapshot->GetId()); + if (!aim_point.has_value()) + return { false, std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), { }, + 0 }; // failed: no valid aim point + const auto traj = SolveTrajectory(aim_point.value(), bullet_speed_); + if (!traj.has_value()) + return { false, std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), { }, + 0 }; // failed: trajectory unsolvable + + if (i > 0 && std::abs(traj->fly_time - prev_fly_time_s) < 0.001) { + final_aim_point = *aim_point; + final_trajectory = *traj; + converged = true; + break; + } + prev_fly_time_s = traj->fly_time; + } + if (!converged) + return { false, std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), { }, + 0 }; // failed: trajectory did not converge + + const auto xyz = final_aim_point; + const double yaw = std::atan2(xyz.y(), xyz.x()) + yaw_offset_; + const double pitch = -(final_trajectory.pitch + pitch_offset_); + return { true, yaw, pitch, final_aim_point }; + } + +private: + std::optional SelectPredictedAim(const EKF::XVec& ekf_x, + const std::vector& armors, const CarIDFlag& id) const { + + const auto& [selectable, aim_point_in_gimbal] = + aim_point_chooser_->ChooseAimArmor(ekf_x, armors, id); + + if (!selectable) return std::nullopt; + return aim_point_in_gimbal.position; + } + + std::optional SolveTrajectory( + const Eigen::Vector3d& xyz, const double& bullet_speed) const { + double d = std::hypot(xyz.x(), xyz.y()); + auto result = TrajectorySolver::SolveTrajectory(bullet_speed, d, xyz.z(), g_); + return result.solvable ? std::optional { result } : std::nullopt; + } + + double yaw_offset_, pitch_offset_; + double bullet_speed_; + const double g_; + + std::unique_ptr aim_point_chooser_; +}; +} diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index 6443340..1e3a21e 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -1,114 +1,108 @@ #include "fire_controller.hpp" +#include #include -#include + +#include #include "../identifier/identified_armor.hpp" -#include "../predictor/live_target_manager/live_target_manager.hpp" -#include "../predictor/target_snapshot_manager/target_snapshot_manager.hpp" #include "../state_machine/state_machine.hpp" +#include "aim_solver.hpp" #include "data/fire_control.hpp" #include "fire_decision.hpp" #include "interfaces/target_predictor.hpp" +#include "tongji/predictor/car_predictor/car_predictor_manager.hpp" namespace world_exe::tongji::fire_control { -using TargetSnapshotManager = predictor::TargetSnapshotManager; -using StateMachine = state_machine::StateMachine; -using IdentifiedArmor = identifier::IdentifiedArmor; -using CarIDFlag = enumeration::CarIDFlag; -using LiveTargetManager = predictor::LiveTargetManager; -using TimeStamp = time_stamp::TimeStamp; +using StateMachine = state_machine::StateMachine; +using IdentifiedArmor = identifier::IdentifiedArmor; +using CarIDFlag = enumeration::CarIDFlag; +using CarPredictorManager = predictor ::CarPredictorManager; +using TimeStamp = data::TimeStamp; class FireController::Impl { public: - Impl(bool auto_fire, const double& control_delay_in_second, const double& bullet_speed, - double yaw_offset, double pitch_offset, - std::shared_ptr state_machine, + Impl(const std::string& config_path, std::shared_ptr state_machine, std::shared_ptr live_target_manager) - : control_delay_(control_delay_in_second) - , bullet_speed_(bullet_speed) - , fire_decision_(std::make_unique(auto_fire)) + : locked_target(CarIDFlag::None) + , firable_(false) + , aiming_solver_(std::make_unique(config_path)) + , fire_decision_(std::make_unique(config_path)) , state_machine_(state_machine) - , live_target_manager_(std::move(live_target_manager)) { } + , live_target_manager_(live_target_manager) { + + auto yaml = YAML::LoadFile(config_path); + control_delay_s_ = yaml["control_delay_s"].as(); + } - // TODO:std::time_t - const data ::FireControl CalculateTarget(const std ::time_t& time_duration) const { + const data ::FireControl CalculateTarget( + const std::chrono::seconds& time_from_tracker_timepoint) const { - if (!identified_armors_ || !fire_decision_ || !state_machine_ || !live_target_manager_) + if (!fire_decision_ || !state_machine_ || !live_target_manager_) return { .fire_allowance = false }; - auto converged_cars = state_machine_->GetAllowdToFires(); - auto snapshot_manager = live_target_manager_->GetPredictor(converged_cars); + const auto& lockable_target = state_machine_->GetAllowdToFires(); + const auto& snapshot_manager = live_target_manager_->GetPredictor(lockable_target); if (!snapshot_manager) - return data::FireControl { .time_stamp = time_stamp_.GetTimeStamp(), + return data::FireControl { .time_stamp = + data::TimeStamp { time_from_tracker_timepoint }, .gimbal_dir = Eigen::Vector3d::Constant(std::numeric_limits::quiet_NaN()), .fire_allowance = false }; - - auto armors_in_gimbal_control = snapshot_manager->Predictor(control_delay_); - allowed_target_id_ = state_machine_->GetAllowdToFires(); - - auto target_gimbal_spacing = - armors_in_gimbal_control->GetArmors(allowed_target_id_).front(); - - auto gimbal_command = - std::dynamic_pointer_cast(snapshot_manager)->GetGimbalCommand(); - - auto fire_command = - fire_decision_->ShouldFire(gimbal_command, target_gimbal_spacing.position); - firable_ = fire_command; + // const auto& armors_in_gimbal = snapshot_manager->Predictor(time_from_tracker_timepoint); + // TODO:这里不应该指针转换 + const auto& aim_solution = aiming_solver_->SolveAimSolution( + snapshot_manager, time_from_tracker_timepoint, control_delay_s_); + + const auto gimbal_command = GimbalCommand { aim_solution.yaw, aim_solution.pitch }; + const auto target_pos = Eigen::Vector3d { aim_solution.aim_point }; + auto fire_command = aim_solution.valid + ? fire_decision_->ShouldFire(gimbal_yaw_, gimbal_command, target_pos) + : false; + firable_ = fire_command; data::FireControl result; result.fire_allowance = fire_command; result.gimbal_dir << gimbal_command.yaw, gimbal_command.pitch, 0; - result.time_stamp = time_stamp_.GetTimeStamp(); + result.time_stamp = data::TimeStamp { time_from_tracker_timepoint }; return result; } const CarIDFlag GetAttackCarId() const { - if (firable_) { } - return allowed_target_id_; + if (firable_) return locked_target; + return CarIDFlag::None; } - void Update(std::shared_ptr armors, const double& gimbal_yaw) { - time_stamp_.SetTimeStamp(std::time(nullptr)); - UpdateIdentifiedArmor(armors); - UpdateGimbalPosition(gimbal_yaw); - } + void UpdateGimbalPosition(const double& gimbal_yaw) { gimbal_yaw_ = gimbal_yaw; }; private: - void UpdateIdentifiedArmor(std::shared_ptr armors) { - identified_armors_ = armors; - } - void UpdateGimbalPosition(const double& gimbal_yaw) { gimbal_yaw_ = gimbal_yaw; }; - TimeStamp GetTimeStamp() const { return time_stamp_; } + double control_delay_s_; double gimbal_yaw_; - double control_delay_; - double bullet_speed_; - mutable CarIDFlag allowed_target_id_; - mutable double firable_ { false }; - - std::shared_ptr identified_armors_; + CarIDFlag locked_target; + mutable double firable_; + std::unique_ptr aiming_solver_; std::unique_ptr fire_decision_; - time_stamp::TimeStamp time_stamp_ { std::time(nullptr) }; - std::shared_ptr state_machine_; std::shared_ptr live_target_manager_; }; -FireController::FireController(std::shared_ptr state_machine, bool auto_fire, - const double& control_delay_in_second, const double& bullet_speed, double yaw_offset, - double pitch_offset, std::shared_ptr live_target_manager) - : pimpl_(std::make_unique(auto_fire, control_delay_in_second, bullet_speed, yaw_offset, - pitch_offset, state_machine, live_target_manager)) { } +FireController::FireController(const std::string& config_path, + std::shared_ptr state_machine, + std::shared_ptr live_target_manager) + : pimpl_(std::make_unique(config_path, state_machine, live_target_manager)) { } +FireController::~FireController() = default; -const data ::FireControl FireController::CalculateTarget(const std ::time_t& time_duration) const { +const data ::FireControl FireController::CalculateTarget( + const std::chrono::seconds& time_duration) const { return pimpl_->CalculateTarget(time_duration); } - const CarIDFlag FireController::GetAttackCarId() const { return pimpl_->GetAttackCarId(); } -} \ No newline at end of file +void FireController::UpdateGimbalPosition(const double& gimbal_yaw) { + return pimpl_->UpdateGimbalPosition(gimbal_yaw); +}; + +} diff --git a/src/tongji/fire_controller/fire_controller.hpp b/src/tongji/fire_controller/fire_controller.hpp index 19343e0..ffa7d15 100644 --- a/src/tongji/fire_controller/fire_controller.hpp +++ b/src/tongji/fire_controller/fire_controller.hpp @@ -1,9 +1,8 @@ #pragma once +#include #include -#include "../time_stamp/time_stamp.hpp" -#include "interfaces/armor_in_image.hpp" #include "interfaces/car_state.hpp" #include "interfaces/fire_controller.hpp" #include "interfaces/target_predictor.hpp" @@ -12,20 +11,25 @@ namespace world_exe::tongji::fire_control { class FireController final : public interfaces::IFireControl { public: - FireController(std::shared_ptr state_machine, bool auto_fire, - const double& control_delay_in_second, const double& bullet_speed, double yaw_offset, - double pitch_offset, std::shared_ptr live_target_manager); + FireController(const std::string& config_path, + std::shared_ptr state_machine, + std::shared_ptr live_target_manager); + ~FireController(); - const data ::FireControl CalculateTarget(const std ::time_t& time_duration) const override; + const data ::FireControl CalculateTarget( + const std::chrono::seconds& time_duration) const override; const enumeration ::CarIDFlag GetAttackCarId() const override; - void Update(std::shared_ptr armors, const double& gimbal_yaw); + void UpdateGimbalPosition(const double& gimbal_yaw); - time_stamp::TimeStamp GetTimeStamp() const; + FireController(const FireController&) = delete; + FireController& operator=(const FireController&) = delete; + FireController(FireController&&) noexcept = default; + FireController& operator=(FireController&&) noexcept = default; private: class Impl; std::unique_ptr pimpl_; }; -} \ No newline at end of file +} diff --git a/src/tongji/fire_controller/fire_decision.hpp b/src/tongji/fire_controller/fire_decision.hpp index 46088e8..39125c5 100644 --- a/src/tongji/fire_controller/fire_decision.hpp +++ b/src/tongji/fire_controller/fire_decision.hpp @@ -1,25 +1,31 @@ #pragma once +#include #include #include -#include "tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp" +#include namespace world_exe::tongji::fire_control { +struct GimbalCommand { + double yaw; + double pitch; +}; class FireDecision { public: - explicit FireDecision(const bool& auto_fire, const double& first_tolerance = 5, - const double& second_tolerance = 2, const double& judge_distance = 3) - : auto_fire_(auto_fire) - , last_gimbal_command_({ std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN() }) - , first_tolerance_(first_tolerance) - , second_tolerance_(second_tolerance) - , judge_distance_(judge_distance) { } - - bool ShouldFire( - predictor::GimbalCommand gimbal_command, const Eigen::Vector3d& valid_target_pos) { + explicit FireDecision(const std::string& config_path) + : last_gimbal_command_({ std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN() }) { + auto yaml = YAML::LoadFile(config_path); + auto_fire_ = yaml["auto_fire"].as(); + first_tolerance_ = yaml["first_tolerance"].as() / 57.3; // degree to rad + second_tolerance_ = yaml["second_tolerance"].as() / 57.3; // degree to rad + judge_distance_ = yaml["judge_distance"].as(); + } + + bool ShouldFire(const double& gimbal_yaw, GimbalCommand gimbal_command, + const Eigen::Vector3d& valid_target_pos) { if (!auto_fire_) return false; const auto& tolerance = std::sqrt(valid_target_pos.x() * valid_target_pos.x() @@ -28,9 +34,9 @@ class FireDecision { ? second_tolerance_ : first_tolerance_; - if (std::abs(last_gimbal_command_.yaw - gimbal_yaw_) + if (std::abs(last_gimbal_command_.yaw - gimbal_yaw) < tolerance * 2 // 此时认为command突变不应该射击 - && std::abs(gimbal_yaw_ - last_gimbal_command_.yaw) < tolerance) { + && std::abs(gimbal_yaw - last_gimbal_command_.yaw) < tolerance) { last_gimbal_command_ = gimbal_command; return true; } @@ -40,13 +46,11 @@ class FireDecision { private: bool auto_fire_; - predictor::GimbalCommand last_gimbal_command_; - - double gimbal_yaw_; + GimbalCommand last_gimbal_command_; double first_tolerance_ { 5 }; // 近距离射击容差，degree double second_tolerance_ { 2 }; // 远距离射击容差，degree double judge_distance_ { 3 }; // 距离判断阈值 }; -} \ No newline at end of file +} diff --git a/src/tongji/predictor/target_snapshot_manager/trajectory.hpp b/src/tongji/fire_controller/trajectory.hpp similarity index 96% rename from src/tongji/predictor/target_snapshot_manager/trajectory.hpp rename to src/tongji/fire_controller/trajectory.hpp index ff5adc1..2b9c5e1 100644 --- a/src/tongji/predictor/target_snapshot_manager/trajectory.hpp +++ b/src/tongji/fire_controller/trajectory.hpp @@ -1,7 +1,7 @@ #pragma once #include -namespace world_exe::tongji::predictor { +namespace world_exe::tongji::fire_control { struct TrajectoryResult { bool solvable = true; diff --git a/src/tongji/identifier/classifier.hpp b/src/tongji/identifier/classifier.hpp index 47924f3..ca8e919 100644 --- a/src/tongji/identifier/classifier.hpp +++ b/src/tongji/identifier/classifier.hpp @@ -5,16 +5,23 @@ #include #include #include +#include +#include +#include #include "enum/armor_id.hpp" namespace world_exe::tongji::identifier { + +// TODO:早期代码，和深大模型不适配，需要TODOTODO class Classifier final { public: - explicit Classifier( - const std::string& model_path, int model_image_width, int model_image_height) - : model_image_width_(model_image_width) - , model_image_height_(model_image_height) { + explicit Classifier(const std::string& config_path) { + const auto yaml = YAML::Load(config_path); + const auto model_path = yaml["classify_model"].as(); + model_image_width_ = yaml["model_image_width"].as(); + model_image_height_ = yaml["model_image_height"].as(); + net_ = cv::dnn::readNetFromONNX(model_path); auto ovmodel = core_.read_model(model_path); compiled_model_ = core_.compile_model( @@ -147,7 +154,7 @@ class Classifier final { ov::Core core_; ov::CompiledModel compiled_model_; - const int model_image_height_ = 640; - const int model_image_width_ = 640; + int model_image_height_ = 640; + int model_image_width_ = 640; }; } \ No newline at end of file diff --git a/src/tongji/predictor/live_target_manager/decider.cpp b/src/tongji/identifier/decider.cpp similarity index 96% rename from src/tongji/predictor/live_target_manager/decider.cpp rename to src/tongji/identifier/decider.cpp index 4db8573..994bf39 100644 --- a/src/tongji/predictor/live_target_manager/decider.cpp +++ b/src/tongji/identifier/decider.cpp @@ -8,7 +8,7 @@ #include "data/armor_image_spaceing.hpp" #include "enum/armor_id.hpp" -namespace world_exe::tongji::predictor { +namespace world_exe::tongji::identifier { class Decider::Impl { public: @@ -37,7 +37,7 @@ class Decider::Impl { } private: - using ArmorPriority = world_exe::tongji::predictor::ArmorPriority; + using ArmorPriority = world_exe::tongji::identifier::ArmorPriority; using ArmorId = world_exe::enumeration::ArmorIdFlag; using PriorityMap = std::unordered_map; diff --git a/src/tongji/predictor/live_target_manager/decider.hpp b/src/tongji/identifier/decider.hpp similarity index 92% rename from src/tongji/predictor/live_target_manager/decider.hpp rename to src/tongji/identifier/decider.hpp index 0a44730..4fef243 100644 --- a/src/tongji/predictor/live_target_manager/decider.hpp +++ b/src/tongji/identifier/decider.hpp @@ -8,7 +8,7 @@ #include "data/armor_image_spaceing.hpp" #include "enum/armor_id.hpp" -namespace world_exe::tongji::predictor { +namespace world_exe::tongji::identifier { enum PriorityMode { MODE_ONE = 1, MODE_TWO }; diff --git a/src/tongji/identifier/identified_armor.hpp b/src/tongji/identifier/identified_armor.hpp index 14295e5..079c0c4 100644 --- a/src/tongji/identifier/identified_armor.hpp +++ b/src/tongji/identifier/identified_armor.hpp @@ -2,12 +2,12 @@ #include "enum/armor_id.hpp" #include "interfaces/armor_in_image.hpp" -#include "interfaces/time_stamped.hpp" +#include "data/time_stamped.hpp" #include "util/index.hpp" namespace world_exe::tongji::identifier { -class IdentifiedArmor final : public interfaces::IArmorInImage, public interfaces::ITimeStamped { +class IdentifiedArmor final : public interfaces::IArmorInImage { public: explicit IdentifiedArmor(const std::vector& armors) { for (const auto& armor : armors) { @@ -15,9 +15,7 @@ class IdentifiedArmor final : public interfaces::IArmorInImage, public interface } } - const interfaces::ITimeStamped& GetTimeStamped() const override { return *this; } - - const std::time_t& GetTimeStamp() const override { return time_stamp_; }; + const data::TimeStamp& GetTimeStamp() const override { return time_stamp_; } const std::vector& GetArmors( const enumeration::ArmorIdFlag& armor_id) const override { @@ -29,7 +27,7 @@ class IdentifiedArmor final : public interfaces::IArmorInImage, public interface } private: - std::time_t time_stamp_ { std::time(nullptr) }; + data::TimeStamp time_stamp_ { std::chrono::steady_clock::now().time_since_epoch() }; std::array, 8> armors_; }; } \ No newline at end of file diff --git a/src/tongji/identifier/identifier.cpp b/src/tongji/identifier/identifier.cpp index f70af8f..2f716f0 100644 --- a/src/tongji/identifier/identifier.cpp +++ b/src/tongji/identifier/identifier.cpp @@ -5,7 +5,7 @@ #include #include #include -#include +#include #include #include @@ -14,41 +14,39 @@ #include #include #include +#include +#include +#include "../identifier/classifier.hpp" #include "data/armor_image_spaceing.hpp" #include "enum/armor_id.hpp" #include "identified_armor.hpp" #include "interfaces/armor_in_image.hpp" -#include "../identifier/classifier.hpp" #include "util/logger.hpp" #include "util/stringifier.hpp" namespace world_exe::tongji::identifier { - class Identifier::Impl { public: - explicit Impl(const std::string& model_path, const int& model_image_width, - const int& model_image_height, const double& threshold, const double& max_angle_error, - const double& min_lightbar_ratio, const double& max_lightbar_ratio, - const double& min_lightbar_length, const double& max_armor_ratio, - const double& min_armor_ratio, const double& max_side_ratio, const double& min_confidence, - const double& max_rectangular_error, const std::string& save_path, const bool& debug = true, - const bool& record = true) - : classifier_(model_path, model_image_width, model_image_height) - , threshold_(threshold) - , max_angle_error_(max_angle_error / 57.3) // degree to rad - , min_lightbar_ratio_(min_lightbar_ratio) - , max_lightbar_ratio_(max_lightbar_ratio) - , min_lightbar_length_(min_lightbar_length) - , max_armor_ratio_(max_armor_ratio) - , min_armor_ratio_(min_armor_ratio) - , max_side_ratio_(max_side_ratio) - , min_confidence_(min_confidence) - , max_rectangular_error_(max_rectangular_error / 57.3) // degree to rad - , save_path_(std::move(save_path)) + explicit Impl(const std::string& config_path, const std::string& save_path, const bool& debug, + const bool& record) + : classifier_(std::make_unique(config_path)) + , save_path_(save_path) , debug_(debug) , record_(record) , target_color_(blue) { + const auto yaml = YAML::Load(config_path); + + threshold_ = yaml["threshold"].as(); + max_angle_error_ = yaml["max_angle_error"].as() / 57.3; // degree to rad + min_lightbar_ratio_ = yaml["min_lightbar_ratio"].as(); + max_lightbar_ratio_ = yaml["max_lightbar_ratio"].as(); + min_lightbar_length_ = yaml["min_lightbar_length"].as(); + max_armor_ratio_ = yaml["max_armor_ratio"].as(); + min_armor_ratio_ = yaml["min_armor_ratio"].as(); + max_side_ratio_ = yaml["max_side_ratio"].as(); + min_confidence_ = yaml["min_confidence"].as(); + max_rectangular_error_ = yaml["max_rectangular_error"].as() / 57.3; // degree to rad if (!std::filesystem::exists(save_path_)) std::filesystem::create_directories(save_path_); } @@ -131,7 +129,7 @@ class Identifier::Impl { armor_candidate.pattern = GetPattern(bgr_img, *left, *right); - classifier_.Classify( + classifier_->Classify( armor_candidate.pattern, armor_candidate.id, armor_candidate.confidence); if (!CheckName( @@ -322,10 +320,10 @@ class Identifier::Impl { } private: - Classifier classifier_; + std::unique_ptr classifier_; double threshold_; - double max_angle_error_; + double max_angle_error_; // rad double min_lightbar_ratio_, max_lightbar_ratio_; double min_lightbar_length_; double min_armor_ratio_, max_armor_ratio_; @@ -340,10 +338,15 @@ class Identifier::Impl { std::string save_path_; }; +Identifier::Identifier(const std::string& config_path, const std::string& save_path, + const bool& debug, const bool& record) + : pimpl_(std::make_unique(config_path, save_path, debug, record)) { } +Identifier::~Identifier() = default; + const std::tuple, enumeration::CarIDFlag> Identifier::identify(const cv::Mat& input_image) { return pimpl_->Identify(input_image); } void Identifier::SetTargetColor(Color target_color) { return pimpl_->SetTargetColor(target_color); } -} \ No newline at end of file +} diff --git a/src/tongji/identifier/identifier.hpp b/src/tongji/identifier/identifier.hpp index 01e73a4..85dcc65 100644 --- a/src/tongji/identifier/identifier.hpp +++ b/src/tongji/identifier/identifier.hpp @@ -37,9 +37,11 @@ struct Lightbar { this->ratio = length / width; } }; + class Identifier final : public interfaces::IIdentifier { public: - Identifier(const std::string& model_path, int model_image_width, int model_image_height); + explicit Identifier(const std::string& config_path, const std::string& save_path, + const bool& debug = true, const bool& record = false); ~Identifier(); const std::tuple, enumeration::CarIDFlag> @@ -47,9 +49,14 @@ class Identifier final : public interfaces::IIdentifier { void SetTargetColor(Color target_color); + Identifier(const Identifier&) = delete; + Identifier& operator=(const Identifier&) = delete; + Identifier(Identifier&&) noexcept = default; + Identifier& operator=(Identifier&&) noexcept = default; + private: class Impl; std::unique_ptr pimpl_; }; -} \ No newline at end of file +} diff --git a/src/tongji/predictor/live_target_manager/tracker.hpp b/src/tongji/identifier/tracker.hpp similarity index 66% rename from src/tongji/predictor/live_target_manager/tracker.hpp rename to src/tongji/identifier/tracker.hpp index c30b2eb..aca3538 100644 --- a/src/tongji/predictor/live_target_manager/tracker.hpp +++ b/src/tongji/identifier/tracker.hpp @@ -3,18 +3,14 @@ #include #include #include -#include #include -#include "../../identifier/armor_filter.hpp" -#include "../../identifier/identified_armor.hpp" -#include "../../time_stamp/time_stamp.hpp" -#include "../target_snapshot_manager/target_snapshot.hpp" -#include "../target_snapshot_manager/target_snapshot_manager.hpp" +#include "armor_filter.hpp" #include "decider.hpp" #include "enum/armor_id.hpp" +#include "identified_armor.hpp" -namespace world_exe::tongji::predictor { +namespace world_exe::tongji::identifier { enum class TrackState { Lost, // @@ -25,25 +21,23 @@ enum class TrackState { }; class Tracker final { - using TargetSnapshotManager = world_exe::tongji::predictor::TargetSnapshotManager; - using TargetSnapshot = world_exe::tongji::predictor::TargetSnapshot; - using ArmorInImage = world_exe::tongji::identifier::IdentifiedArmor; + using ArmorInImage = world_exe::tongji::identifier::IdentifiedArmor; public: Tracker() : armor_filter_(std::make_unique()) - , decider_(std::make_unique()) - , last_track_timestamp_(std::time(nullptr)) { } + , decider_(std::make_unique()) { } ~Tracker() = default; auto SelectTrackingTargetID(const std::shared_ptr& armors_in_image, - const std::time_t& now) noexcept -> enumeration::ArmorIdFlag const { - CheckCameraOffline(now); - last_track_timestamp_.SetTimeStamp(now); + const std::chrono::milliseconds& duration_from_last_update) noexcept + + -> enumeration::ArmorIdFlag const { + CheckCameraOffline(duration_from_last_update); auto filtered_ids = enumeration::ArmorIdFlag::None; - auto detected_ids = enumeration::ArmorIdFlag::None; + std::vector filtered_armors; for (uint32_t i = 0; i < static_cast(enumeration::ArmorIdFlag::Count); ++i) { auto id = static_cast( @@ -52,12 +46,9 @@ class Tracker final { if (armors_in_image->GetArmors(id).empty()) continue; // 图像中出现的装甲板 - auto armors = armors_in_image->GetArmors(id); - detected_ids = static_cast( - static_cast(detected_ids) | static_cast(id)); - + auto armors = armors_in_image->GetArmors(id); // 对从图像识别到的装甲板进行过滤 - filtered_armors = std::move(armor_filter_->FilterArmor(std::move(armors))); + filtered_armors = armor_filter_->FilterArmor(armors); if (!filtered_armors.empty()) { filtered_ids = static_cast(static_cast(filtered_ids) @@ -65,12 +56,15 @@ class Tracker final { } } - UpdateState(!(detected_ids == enumeration::ArmorIdFlag::None)); + UpdateState(!(filtered_ids == enumeration::ArmorIdFlag::None)); tracking_car_id_ = decider_->GetBestArmor(filtered_armors); return tracking_car_id_; } + TrackState GetState() const { return state_; } + +private: void UpdateState(bool found) { switch (state_) { case TrackState::Lost: { @@ -133,14 +127,8 @@ class Tracker final { } } - TrackState GetState() const { return state_; } - -private: - void CheckCameraOffline(const std::time_t& now) { - // TODO:If the underlying timestamp is std::time_t, then this if branch will never be - // entered - if (state_ != TrackState::Lost - && static_cast(now - last_track_timestamp_.GetTimeStamp()) < 0.1) + void CheckCameraOffline(const std::chrono::milliseconds duration_from_last_update) { + if (state_ != TrackState::Lost && (duration_from_last_update > timeout_sec_)) SetState(TrackState::Lost); } @@ -158,15 +146,14 @@ class Tracker final { std::unique_ptr armor_filter_; std::unique_ptr decider_; - int detect_count_ = 0; - int temp_lost_count_ = 0; - int max_temp_lost_count_ = 15; - const int min_detect_count_ = 5; - const int outpost_max_temp_lost_count_ = 75; - const int normal_max_temp_lost_count_ = max_temp_lost_count_; - const int max_switch_count_ = 200; - - time_stamp::TimeStamp last_track_timestamp_; + int detect_count_ = 0; + int temp_lost_count_ = 0; + int max_temp_lost_count_ = 15; + const int min_detect_count_ = 5; + const int outpost_max_temp_lost_count_ = 75; + const int normal_max_temp_lost_count_ = max_temp_lost_count_; + const int max_switch_count_ = 200; + const std::chrono::milliseconds timeout_sec_ = std::chrono::milliseconds(100); }; -} \ No newline at end of file +} diff --git a/src/tongji/predictor/car_predictor/car_predictor.hpp b/src/tongji/predictor/car_predictor/car_predictor.hpp new file mode 100644 index 0000000..05136f7 --- /dev/null +++ b/src/tongji/predictor/car_predictor/car_predictor.hpp @@ -0,0 +1,153 @@ +#pragma once + +#include +#include +#include +#include + +#include + +#include "../in_gimbal_control_armor.hpp" +#include "../kalman_filter/extended_kalman_filter.hpp" +#include "../kalman_filter/predict_model.hpp" +#include "data/armor_gimbal_control_spacing.hpp" +#include "data/time_stamped.hpp" +#include "enum/car_id.hpp" +#include "interfaces/predictor.hpp" + +namespace world_exe::tongji::predictor { + +class CarPredictor final : public interfaces::IPredictor { +public: + using PredictorModel = EKFModel<11, 4>; + using EKF = ExtendedKalmanFilter; + + explicit CarPredictor( + const EKF& ekf, const PredictorModel& model, const data::TimeStamp& time_stamp) + : ekf_(ekf) + , model_(model) + , time_stamp_(time_stamp) { } + + explicit CarPredictor(const Eigen::Vector3d& armor_xyz_in_gimbal, + const Eigen::Vector3d& armor_ypr_in_gimbal, const enumeration::CarIDFlag& car_id, + const data::TimeStamp& time_stamp) + : time_stamp_(time_stamp) + , model_(car_id) + , car_id_(car_id) { + // x vx y vy z vz a w r l h + // a: angle + // w: angular velocity + // l: r2 - r1 + // h: z2 - z1 + auto center_x = + armor_xyz_in_gimbal[0] + model_.GetRadius() * std::cos(armor_ypr_in_gimbal[0]); + auto center_y = + armor_xyz_in_gimbal[1] + model_.GetRadius() * std::sin(armor_ypr_in_gimbal[0]); + auto center_z = armor_xyz_in_gimbal[2]; + + EKF::XVec x0 { center_x, 0, center_y, 0, center_z, 0, armor_ypr_in_gimbal[0], 0, + model_.GetRadius(), 0, 0 }; + + EKF::PMat P0 = model_.GetP0Dig().asDiagonal(); + ekf_.emplace(x0, P0, model_); // 初始化滤波器（预测量、预测量协方差） + } + + const enumeration ::ArmorIdFlag& GetId() const override { return car_id_; } + + std ::shared_ptr Predictor( + const data ::TimeStamp& time_stamp) const override { + const auto ekf_x = this->GetPredictedX((time_stamp - time_stamp_).to_seconds()); + std::vector armors; + for (int id = 0; id < model_.GetArmorNum(); id++) { + auto angle = util::math::clamp_pm_pi(ekf_x[6] + id * 2 * CV_PI / model_.GetArmorNum()); + auto xyz = model_.h_armor_xyz(ekf_x, id); + + data::ArmorGimbalControlSpacing armor; + armor.id = model_.GetID(); + armor.position = xyz; + armor.orientation = util::math::euler_to_quaternion(angle, 15. / 180. * CV_PI, 0); + armors.emplace_back(std::move(armor)); + } + return std::make_shared(armors, time_stamp_); + } + + EKF::XVec GetEkfX() const { return ekf_->x; } + auto GetModel() const -> const PredictorModel { return model_; } + auto GetEkf() const -> const EKF { return ekf_.value(); } + + data::TimeStamp LastSeen() const { return time_stamp_; } + + auto GetPredictedXYZAList(const double& dt) -> std::vector const { + const auto [x_n, P_n] = ekf_->PredictOnce(dt); + return model_.GetArmorXYZAList(x_n); + } + + auto GetPredictedX(const double& dt) const -> const EKF::XVec { + const auto& [x_n, P_n] = ekf_->PredictOnce(dt); + return x_n; + } + + void Update(const data::TimeStamp time_stamp, const Eigen::Vector3d& armor_xyz_in_gimbal, + const Eigen::Vector3d& armor_ypr_in_gimbal, const Eigen::Vector3d& armor_ypd_in_gimbal) { + + // 装甲板匹配 + int id = model_.MatchArmor( + ekf_->x, armor_xyz_in_gimbal, armor_ypr_in_gimbal, armor_ypd_in_gimbal); + last_id_ = id; + update_count_++; + + Update_ypda(armor_xyz_in_gimbal, armor_ypr_in_gimbal, armor_ypd_in_gimbal, id, + (time_stamp - time_stamp_).to_seconds()); + + time_stamp_ = time_stamp; + } + + bool IsConverged() const { + auto r_ok = ekf_->x[8] > 0.05 && ekf_->x[8] < 0.5; + auto l_ok = ekf_->x[8] + ekf_->x[9] > 0.05 && ekf_->x[8] + ekf_->x[9] < 0.5; + + if (r_ok && l_ok) return false; + // util::logger::logger()->debug("[Target] r={:.3f}, l={:.3f}", ekf_->x[8], ekf_->x[9]); + return true; + } + auto IsAppeared() -> bool { + const int required_count = (model_.GetID() == enumeration::CarIDFlag::Outpost) ? 10 : 3; + return update_count_ > required_count; + } + +private: + void Update_ypda(const Eigen::Vector3d& armor_xyz_in_gimbal, + const Eigen::Vector3d& armor_ypr_in_gimbal, const Eigen::Vector3d& armor_ypd_in_gimbal, + const int& id, const double& dt) { + // 观测jacobi + auto H = model_.H(ekf_->x, id); + auto R = model_.R(armor_xyz_in_gimbal, armor_ypr_in_gimbal, armor_ypd_in_gimbal, id); + + const Eigen::Vector3d& ypd = armor_ypd_in_gimbal; + const Eigen::Vector3d& ypr = armor_ypr_in_gimbal; + + // 获得观测量 + EKF::ZVec z(4); + z << ypd[0], ypd[1], ypd[2], ypr[0]; + + ekf_->Update(dt, z, H, R, id); + + // 前哨站转速特判 + if (model_.GetID() == enumeration::CarIDFlag::Outpost) { + constexpr double max_outpost_w = 2.51; + if (std::abs(ekf_->x[7]) > 2.0) { + ekf_->x[7] = ekf_->x[7] > 0 ? max_outpost_w : -max_outpost_w; + } + } + } + + data::TimeStamp time_stamp_; + PredictorModel model_; + std::optional ekf_; + enumeration::CarIDFlag car_id_; + + int last_id_ = -1; + int update_count_ = 0; + const double max_allowed_failure_rate_ = 0.4; +}; +} diff --git a/src/tongji/predictor/car_predictor/car_predictor_manager.cpp b/src/tongji/predictor/car_predictor/car_predictor_manager.cpp new file mode 100644 index 0000000..1eaca95 --- /dev/null +++ b/src/tongji/predictor/car_predictor/car_predictor_manager.cpp @@ -0,0 +1,111 @@ +#include "car_predictor_manager.hpp" + +#include +#include +#include +#include + +#include "../in_gimbal_control_armor.hpp" + +#include "car_predictor.hpp" +#include "data/predictor_update_package.hpp" +#include "data/time_stamped.hpp" +#include "enum/armor_id.hpp" +#include "enum/car_id.hpp" +#include "util/index.hpp" +#include "util/math.hpp" + +namespace world_exe::tongji::predictor { + +class CarPredictorManager::Impl { +public: + Impl(const std::string& config_path, const double& timeout_sec) + : targets_map_() + , last_update_timestamp_(data::TimeStamp { }) + , config_path_(config_path) { } + + std::shared_ptr Predict( + const enumeration::ArmorIdFlag& flag, const data::TimeStamp& time_stamp) { + std::vector result; + + for (const auto& car_id : util::enumeration::ExpandArmorIdFlags(flag)) { + if (const auto it = targets_map_.find(car_id); it != targets_map_.end()) { + const auto& [id, predictor] = *it; + if (!predictor->IsConverged()) { + continue; + } + if (predictor->IsConverged()) { + auto spacings = predictor->Predictor(time_stamp); + result.insert(result.end(), spacings->GetArmors(id).begin(), + spacings->GetArmors(id).end()); + } + } + } + + return std::make_shared(result, time_stamp); + } + + std::shared_ptr GetPredictor( + const enumeration::ArmorIdFlag& flag) const { + const auto& it = targets_map_.find(flag); + if (it == targets_map_.end()) return nullptr; + const auto& [id, predictor] = *it; + return std::make_shared( + predictor->GetEkf(), predictor->GetModel(), predictor->LastSeen()); + } + + void Update(std::shared_ptr data) { + last_update_timestamp_ = data->GetTimeStamp(); + + const Eigen::Affine3d transform = data->GetCameraToWorld(); + const auto armors_interface = data->GetArmors(); + + for (int i = 0; i < 8; i++) { + auto id = static_cast( + static_cast(enumeration::CarIDFlag::Hero) << i); + + const auto& armors = armors_interface->GetArmors(id); + if (armors.empty()) continue; + + for (const auto& armor : armors) { + if (!targets_map_.contains(armor.id)) { + targets_map_.try_emplace(armor.id, + std::make_unique(armor.position, + util::math::quaternion_to_euler(armor.orientation, 2, 1, 0), armor.id, + data->GetTimeStamp())); + } else { + targets_map_.at(armor.id)->Update(data->GetTimeStamp(), armor.position, + util::math::quaternion_to_euler(armor.orientation, 2, 1, 0), + util::math::xyz2ypd(armor.position)); + } + } + + std::erase_if(targets_map_, [](const auto& pair) { return pair.second->IsAppeared(); }); + } + } + +private: + std::unordered_map> targets_map_; + data::TimeStamp last_update_timestamp_; + + const std::string config_path_; +}; + +CarPredictorManager::CarPredictorManager(const std::string& config_path, const double& timeout_sec) + : pimpl_(std::make_unique(config_path, timeout_sec)) { } +CarPredictorManager::~CarPredictorManager() = default; + +std ::shared_ptr CarPredictorManager::Predict( + const enumeration ::ArmorIdFlag& id, const data::TimeStamp& time_stamp) { + return pimpl_->Predict(id, time_stamp); +} +std ::shared_ptr CarPredictorManager::GetPredictor( + const enumeration ::ArmorIdFlag& id) const { + return pimpl_->GetPredictor(id); +} + +void CarPredictorManager::Update(std::shared_ptr data) { + return pimpl_->Update(data); +} + +} diff --git a/src/tongji/predictor/car_predictor/car_predictor_manager.hpp b/src/tongji/predictor/car_predictor/car_predictor_manager.hpp new file mode 100644 index 0000000..d6190ba --- /dev/null +++ b/src/tongji/predictor/car_predictor/car_predictor_manager.hpp @@ -0,0 +1,34 @@ +#pragma once + +#include + +#include "data/predictor_update_package.hpp" +#include "data/time_stamped.hpp" +#include "enum/armor_id.hpp" +#include "interfaces/target_predictor.hpp" + +namespace world_exe::tongji::predictor { + +class CarPredictorManager final : public interfaces::ITargetPredictor { +public: + CarPredictorManager(const std::string& config_path, const double& timeout_sec = 0.1); + ~CarPredictorManager(); + + std ::shared_ptr Predict( + const enumeration ::ArmorIdFlag& id, const data::TimeStamp& time_stamp) override; + std ::shared_ptr GetPredictor( + const enumeration ::ArmorIdFlag& id) const override; + + void Update(std::shared_ptr data); + + CarPredictorManager(const CarPredictorManager&) = delete; + CarPredictorManager& operator=(const CarPredictorManager&) = delete; + CarPredictorManager(CarPredictorManager&&) noexcept = default; + CarPredictorManager& operator=(CarPredictorManager&&) noexcept = default; + +private: + class Impl; + std::unique_ptr pimpl_; +}; + +} diff --git a/src/tongji/predictor/in_gimbal_control_armor.hpp b/src/tongji/predictor/in_gimbal_control_armor.hpp index 9dbb878..6d7402f 100644 --- a/src/tongji/predictor/in_gimbal_control_armor.hpp +++ b/src/tongji/predictor/in_gimbal_control_armor.hpp @@ -1,11 +1,10 @@ #pragma once #include -#include #include -#include "../time_stamp/time_stamp.hpp" #include "data/armor_gimbal_control_spacing.hpp" +#include "data/time_stamped.hpp" #include "enum/armor_id.hpp" #include "interfaces/armor_in_gimbal_control.hpp" @@ -13,24 +12,25 @@ namespace world_exe::tongji::predictor { class InGimbalControlArmor final : public interfaces::IArmorInGimbalControl { public: - InGimbalControlArmor(const std::unordered_map>& all_armors, - const std::time_t& time_stamp) - : armors_map_(std::move(all_armors)) + explicit InGimbalControlArmor(const std::vector& all_armors, + const data::TimeStamp& time_stamp) + : armors_(all_armors) , time_stamp_(time_stamp) { } const std ::vector& GetArmors( const enumeration ::ArmorIdFlag& armor_id) const override { - auto it = armors_map_.find(armor_id); - return it != armors_map_.end() ? it->second : empty; + return armors_; } - const interfaces::ITimeStamped& GetTimeStamped() const override { return time_stamp_; } + const data::TimeStamp& GetTimeStamp() const override { return time_stamp_; } + + InGimbalControlArmor(const InGimbalControlArmor&) = delete; + InGimbalControlArmor& operator=(const InGimbalControlArmor&) = delete; + InGimbalControlArmor(InGimbalControlArmor&&) noexcept = default; + InGimbalControlArmor& operator=(InGimbalControlArmor&&) noexcept = default; private: - time_stamp::TimeStamp time_stamp_; - std::unordered_map> - armors_map_; - static inline const std::vector empty={}; + data::TimeStamp time_stamp_; + std::vector armors_; }; } \ No newline at end of file diff --git a/src/tongji/predictor/kalman_filter/extended_kalman_filter.hpp b/src/tongji/predictor/kalman_filter/extended_kalman_filter.hpp index ce90cfd..ec494a4 100644 --- a/src/tongji/predictor/kalman_filter/extended_kalman_filter.hpp +++ b/src/tongji/predictor/kalman_filter/extended_kalman_filter.hpp @@ -1,110 +1,144 @@ #pragma once +#include #include -#include -#include #include #include namespace world_exe::tongji::predictor { -template // -class ExtendedKalmanFilter { +template +concept EKFModelTypes = requires { + { T::xn } -> std::same_as; + { T::zn } -> std::same_as; + + typename T::XVec; + typename T::ZVec; + typename T::AMat; + typename T::PMat; + typename T::RMat; + typename T::QMat; + typename T::HMat; +}; + +template class ExtendedKalmanFilter { public: - using XVec = Eigen::Matrix; - using ZVec = Eigen::Matrix; - using AMat = Eigen::Matrix; - using PMat = Eigen::Matrix; - using PDig = Eigen::Matrix; - using RMat = Eigen::Matrix; - using RDig = Eigen::Matrix; - using QMat = Eigen::Matrix; - using HMat = Eigen::Matrix; + static constexpr int xn = EKFModel::xn; + static constexpr int zn = EKFModel::zn; + + using XVec = EKFModel::XVec; + using ZVec = EKFModel::ZVec; + using AMat = EKFModel::AMat; + using PMat = EKFModel::PMat; + using PDig = EKFModel::PDig; + using RMat = EKFModel::RMat; + using RDig = EKFModel::RDig; + using QMat = EKFModel::QMat; + using HMat = EKFModel::HMat; XVec x; PMat P; - ExtendedKalmanFilter() = default; - - ExtendedKalmanFilter( - const XVec& x0, const PMat& P0, - std::function x_add = [](const XVec& a, const XVec& b) { - return a + b; - }) { - data["residual_yaw"] = 0.0; - data["residual_pitch"] = 0.0; - data["residual_distance"] = 0.0; - data["residual_angle"] = 0.0; - data["nis"] = 0.0; - data["nees"] = 0.0; - data["nis_fail"] = 0.0; - data["nees_fail"] = 0.0; - data["recent_nis_failures"] = 0.0; + ExtendedKalmanFilter(const XVec& x0, const PMat& P0, const EKFModel& model) + : x(x0) + , P(P0) + , model_(model) { + // data["residual_yaw"] = 0.0; + // data["residual_pitch"] = 0.0; + // data["residual_distance"] = 0.0; + // data["residual_angle"] = 0.0; + // data["nis"] = 0.0; + // data["nees"] = 0.0; + // data["nis_fail"] = 0.0; + // data["nees_fail"] = 0.0; + // data["recent_nis_failures"] = 0.0; } - XVec Update( - const double& dt, const AMat& A, const QMat& Q, - std::function f, const ZVec& z, const HMat& H, - const RMat& R, std::function h, - std::function z_subtract = - [](const ZVec& a, const ZVec& b) { return a - b; }) { + // 无副作用，不修改x，仅预测 + auto PredictOnce(const double& dt) const -> const std::pair { + const auto A = model_.A(dt); + const auto Q = model_.Q(dt); + const auto x_n = model_.f(x, dt); + const auto P_n = A * P * A.transpose() + Q; - auto x_n = f(x, dt); + return { x_n, P_n }; + } - auto P_n = A * P * A.transpose() + Q; + auto Update(const double& dt, const ZVec& z, const HMat& H, const RMat& R, const int& id) + -> const XVec { + const auto [x_prior, P_prior] = PredictOnce(dt); + const auto z_prior = model_.h(x_prior, id); - auto residual = z_subtract(z, h(x_n)); + const auto residual = model_.z_subtract(z, z_prior); - auto S = H * P * H.transpose() + R; + const auto S = H * P_prior * H.transpose() + R; - auto K = P_n * H.transpose() * S.inverse(); + const auto K = P_prior * H.transpose() * S.inverse(); - x = x_add(x, K * residual); + x = x_prior; + x = model_.x_add(x, K * residual); // Stable Compution of the Posterior Covariance // https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/07-Kalman-Filter-Math.ipynb P = (I - K * H) * P * (I - K * H).transpose() + K * R * K.transpose(); - /// 卡方检验 - // 新增检验 - double nis = residual.transpose() * S.inverse() * residual; + const auto P_inverse = P_prior.inverse(); + const auto error = x - x_prior; - // 卡方检验阈值（自由度=4，取置信水平95%） - constexpr double nis_threshold = 0.711; + RecordStatistics(residual, S, error, P_inverse); - if (nis > nis_threshold) nis_count_++, data["nis_fail"] = 1; - total_count_++; - last_nis = nis; + return x; + } - recent_nis_failures.push_back(nis > nis_threshold ? 1 : 0); + auto IsDiverse() const -> bool const { return CalculateFailureRate() >= max_failure_rate; } - if (recent_nis_failures.size() > window_size) { - recent_nis_failures.pop_front(); - } +private: + auto CalculateFailureRate() const -> bool const { + int failures = std::accumulate(recent_nis_failures.begin(), recent_nis_failures.end(), 0); + return (double)failures / window_size; + } + + auto GetRecentNISFailureRate() const -> double const { + if (recent_nis_failures.empty()) return 0.0; int recent_failures = std::accumulate(recent_nis_failures.begin(), recent_nis_failures.end(), 0); - double recent_rate = static_cast(recent_failures) / recent_nis_failures.size(); + return (double)recent_failures / recent_nis_failures.size(); + } - data["residual_yaw"] = residual[0]; - data["residual_pitch"] = residual[1]; - data["residual_distance"] = residual[2]; - data["residual_angle"] = residual[3]; - data["nis"] = nis; - data["recent_nis_failures"] = recent_rate; + auto RecordStatistics( + const ZVec& residual, const RMat& S, const XVec& error, const PMat& P_inverse) -> void { - return x; + /// 卡方检验 + // 新增检验 + double nis = residual.transpose() * S.inverse() * residual; + double nees = (error).transpose() * P.inverse() * error; + + total_count_++; + last_nis = nis; + last_nees = nees; + + if (nis > nis_threshold_) nis_count_++; + if (nees > nees_threshold_) nees_count_++; + recent_nis_failures.push_back(nis > nis_threshold_ ? 1 : 0); + if (recent_nis_failures.size() > window_size) { + recent_nis_failures.pop_front(); + } } - std::map data; // 卡方检验数据 + const Eigen::Matrix I; + const EKFModel& model_; + + // 卡方检验阈值（自由度=4，取置信水平95%） + const double nis_threshold_ = 0.711; + const double nees_threshold_ = 0.711; + const double max_failure_rate = 0.4; + + // std::map data; // 卡方检验数据 std::deque recent_nis_failures { 0 }; size_t window_size = 100; double last_nis; - -private: - Eigen::Matrix I; - std::function x_add; - + double last_nees; int nees_count_ = 0; int nis_count_ = 0; int total_count_ = 0; diff --git a/src/tongji/predictor/kalman_filter/predict_model.hpp b/src/tongji/predictor/kalman_filter/predict_model.hpp index 5af8395..e811698 100644 --- a/src/tongji/predictor/kalman_filter/predict_model.hpp +++ b/src/tongji/predictor/kalman_filter/predict_model.hpp @@ -1,20 +1,37 @@ #pragma once -#include +#include #include #include #include #include "enum/car_id.hpp" -#include "extended_kalman_filter.hpp" #include "util/math.hpp" - namespace world_exe::tongji::predictor { -class PredictModel { +template +concept PositiveInteger = std::integral && T > 0; + +template + requires PositiveInteger && PositiveInteger + +class EKFModel { public: - PredictModel(const enumeration::CarIDFlag& car_id) + static constexpr int xn = N_STATE; + static constexpr int zn = N_MEAS; + + using XVec = Eigen::Matrix; + using ZVec = Eigen::Matrix; + using AMat = Eigen::Matrix; + using PMat = Eigen::Matrix; + using PDig = Eigen::Matrix; + using RMat = Eigen::Matrix; + using RDig = Eigen::Matrix; + using QMat = Eigen::Matrix; + using HMat = Eigen::Matrix; + + EKFModel(const enumeration::CarIDFlag& car_id) : car_id_(car_id) { bool is_balance = (car_id == enumeration::CarIDFlag::InfantryIII @@ -50,34 +67,58 @@ class PredictModel { } } - auto GetP0Dig() const { return P0_dig_; } - auto GetRadius() const { return radius_; } - auto GetID() const { return car_id_; } - int GetArmorNum() const { return armor_num_; } + auto GetP0Dig() const -> const PDig { return P0_dig_; } + auto GetRadius() const -> const double { return radius_; } + auto GetID() const -> const auto { return car_id_; } + auto GetArmorNum() const -> const int { return armor_num_; } // 防止夹角求和出现异常值 - std::function::XVec( - const ExtendedKalmanFilter<11, 4>::XVec&, const ExtendedKalmanFilter<11, 4>::XVec&)> - x_add = [](const ExtendedKalmanFilter<11, 4>::XVec& a, - const ExtendedKalmanFilter<11, 4>::XVec& b) { - ExtendedKalmanFilter<11, 4>::XVec c = a + b; - c(6) = util::math::clamp_pm_pi(c(6)); - return c; - }; + constexpr auto x_add(const XVec& a, const XVec& b) const -> const auto { + XVec c = a + b; + c(6) = util ::math::clamp_pm_pi(c(6)); + return c; + } + + constexpr auto z_substract(const ZVec& a, const ZVec& b) const -> const auto { + auto c = a - b; + c(0) = util::math::clamp_pm_pi(c(0)); + c(1) = util::math::clamp_pm_pi(c(1)); + c(3) = util::math::clamp_pm_pi(c(3)); + return c; + } + + auto A(double dt) const -> auto const { + // 状态转移矩阵 + AMat _A; + // clang-format off + _A<< + 1, dt, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1, dt, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 1, dt, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 1, dt, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1; + //clang-format on + return _A; + } + // 防止夹角求和出现异常值 - std::function::XVec( - const ExtendedKalmanFilter<11, 4>::XVec& x, const double& dt)> - f = [this](const ExtendedKalmanFilter<11, 4>::XVec& x, - const double& dt) -> ExtendedKalmanFilter<11, 4>::XVec { - ExtendedKalmanFilter<11, 4>::XVec x_prior = A(dt) * x; - x_prior(6) = util::math::clamp_pm_pi(x_prior(6)); + auto f(const XVec& x, const double& dt)const ->const auto{ + XVec x_prior = this->A(dt) * x; + x_prior(6) = util::math::clamp_pm_pi(x_prior(6)); return x_prior; }; - int MatchArmor(const ExtendedKalmanFilter<11, 4>::XVec& x, - const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, - const Eigen::Vector3d& armor_ypd_in_world) const { + + auto MatchArmor(const XVec& x, const Eigen::Vector3d& armor_xyz_in_gimbal, + const Eigen::Vector3d& armor_ypr_in_gimbal, const Eigen::Vector3d& armor_ypd_in_gimbal) const + ->const int { const auto& xyza_list = GetArmorXYZAList(x); std::vector> xyza_i_list; @@ -98,8 +139,8 @@ class PredictModel { for (int i = 0; i < std::min(3, armor_num_); ++i) { const auto& xyza = xyza_i_list[i].first; auto ypd = util::math::xyz2ypd(xyza.head(3)); - double error = std::abs(util::math::clamp_pm_pi(armor_ypr_in_world(0) - xyza(3))) - + std::abs(util::math::clamp_pm_pi(armor_ypd_in_world(0) - ypd(0))); + double error = std::abs(util::math::clamp_pm_pi(armor_ypr_in_gimbal(0) - xyza(3))) + + std::abs(util::math::clamp_pm_pi(armor_ypd_in_gimbal(0) - ypd(0))); if (error < min_error) { min_error = error; @@ -110,7 +151,7 @@ class PredictModel { } // 计算出装甲板中心的坐标（考虑长短轴） - Eigen::Vector3d h_armor_xyz(const ExtendedKalmanFilter<11, 4>::XVec& x, int id) const { + auto h_armor_xyz(const XVec& x, int id) const ->const auto { auto angle = util::math::clamp_pm_pi(x(6) + id * 2 * CV_PI / armor_num_); auto use_l_h = (armor_num_ == 4) && (id == 1 || id == 3); @@ -119,144 +160,120 @@ class PredictModel { auto armor_y = x(2) - r * std::sin(angle); auto armor_z = (use_l_h) ? x(4) + x(10) : x(4); - return { armor_x, armor_y, armor_z }; + return Eigen::Vector3d { armor_x, armor_y, armor_z }; } - ExtendedKalmanFilter<11, 4>::HMat H(const ExtendedKalmanFilter<11, 4>::XVec& x, int id) const { + constexpr auto H(const XVec& x, int id) const->HMat const { auto angle = util::math::clamp_pm_pi(x(6) + id * 2 * CV_PI / armor_num_); - auto use_l_h = (armor_num_ == 4) && (id == 1 || id == 3); + auto cos_angle=std::cos(angle); + auto sin_angle=std::sin(angle); + auto use_l_h = (armor_num_ == 4) && (id == 1 || id == 3); auto r = (use_l_h) ? x(8) + x(9) : x(8); - auto dx_da = r * std::sin(angle); - auto dy_da = -r * std::cos(angle); - auto dx_dr = -std::cos(angle); - auto dy_dr = -std::sin(angle); - auto dx_dl = (use_l_h) ? -std::cos(angle) : 0.0; - auto dy_dl = (use_l_h) ? -std::sin(angle) : 0.0; + auto dx_da = r * sin_angle; + auto dy_da = -r *cos_angle; + + auto dx_dr = -cos_angle; + auto dy_dr = -sin_angle; + auto dx_dl = (use_l_h) ? -cos_angle : 0.0; + auto dy_dl = (use_l_h) ? -sin_angle : 0.0; auto dz_dh = (use_l_h) ? 1.0 : 0.0; // clang-format off - Eigen::MatrixXd H_armor_xyza{ - {1, 0, 0, 0, 0, 0, dx_da, 0, dx_dr, dx_dl, 0}, - {0, 0, 1, 0, 0, 0, dy_da, 0, dy_dr, dy_dl, 0}, - {0, 0, 0, 0, 1, 0, 0, 0, 0, 0, dz_dh}, - {0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0} - }; + Eigen::Matrix + H_armor_xyza{ + {1, 0, 0, 0, 0, 0, dx_da, 0, dx_dr, dx_dl, 0}, + {0, 0, 1, 0, 0, 0, dy_da, 0, dy_dr, dy_dl, 0}, + {0, 0, 0, 0, 1, 0, 0, 0, 0, 0, dz_dh}, + {0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0} + }; // clang-format on - Eigen::VectorXd armor_xyz = h_armor_xyz(x, id); - Eigen::MatrixXd H_armor_ypd = util::math::xyz2ypd_jacobian(armor_xyz); + auto armor_xyz = h_armor_xyz(x, id); + auto H_armor_ypd = util::math::xyz2ypd_jacobian(armor_xyz); // clang-format off - Eigen::Matrix H_armor_ypda{ - {H_armor_ypd(0, 0), H_armor_ypd(0, 1), H_armor_ypd(0, 2), 0}, - {H_armor_ypd(1, 0), H_armor_ypd(1, 1), H_armor_ypd(1, 2), 0}, - {H_armor_ypd(2, 0), H_armor_ypd(2, 1), H_armor_ypd(2, 2), 0}, - { 0, 0, 0, 1} - }; + Eigen::MatrixH_armor_ypda; + H_armor_ypda<< + H_armor_ypd(0, 0), H_armor_ypd(0, 1), H_armor_ypd(0, 2), 0, + H_armor_ypd(1, 0), H_armor_ypd(1, 1), H_armor_ypd(1, 2), 0, + H_armor_ypd(2, 0), H_armor_ypd(2, 1), H_armor_ypd(2, 2), 0, + 0 , 0, 0, 1; // clang-format on - return H_armor_ypda * H_armor_xyza; } - ExtendedKalmanFilter<11, 4>::RMat R(const Eigen::Vector3d& armor_xyz_in_world, - const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world, - int id) { + auto R(const Eigen::Vector3d& armor_xyz_in_gimbal, const Eigen::Vector3d& armor_ypr_in_gimbal, + const Eigen::Vector3d& armor_ypd_in_gimbal, int id) const -> RMat const { // Eigen::VectorXd R_dig{{4e-3, 4e-3, 1, 9e-2}}; - auto center_yaw = std::atan2(armor_xyz_in_world(1), armor_xyz_in_world(0)); - auto delta_angle = util::math::clamp_pm_pi(armor_ypr_in_world(0) - center_yaw); - ExtendedKalmanFilter<11, 4>::RDig R_dig(4); + auto center_yaw = std::atan2(armor_xyz_in_gimbal(1), armor_xyz_in_gimbal(0)); + auto delta_angle = util::math::clamp_pm_pi(armor_ypr_in_gimbal(0) - center_yaw); + RDig R_dig(4); R_dig << 4e-3, 4e-3, log(std::abs(delta_angle) + 1) + 1, - log(std::abs(armor_ypd_in_world(2)) + 1) / 200 + 9e-2; + log(std::abs(armor_ypd_in_gimbal(2)) + 1) / 200 + 9e-2; // 测量过程噪声偏差的方差 return R_dig.asDiagonal(); } - ExtendedKalmanFilter<11, 4>::AMat A(double dt) const { - // 状态转移矩阵 - ExtendedKalmanFilter<11, 4>::AMat _A; - // clang-format off - _A<< - 1, dt, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 1, dt, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 1, dt, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 1, dt, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1; - //clang-format on - return _A; - } - - -ExtendedKalmanFilter<11, 4>::QMat Q(double dt) const { + auto Q(const double& dt) const -> const auto { // Piecewise White Noise Model // https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/07-Kalman-Filter-Math.ipynb - double v1, v2; - if (car_id_ == enumeration::CarIDFlag::Outpost) { - v1 = 10; // 前哨站加速度方差 - v2 = 0.1; // 前哨站角加速度方差 - } else { - v1 = 100; // 加速度方差 - v2 = 400; // 角加速度方差 - } - auto a = dt * dt * dt * dt / 4; - auto b = dt * dt * dt / 2; - auto c = dt * dt; - - // 预测过程噪声偏差的方差 - Eigen::Matrix _Q; - // clang-format off - _Q + double v1, v2; + if (car_id_ == enumeration::CarIDFlag::Outpost) { + v1 = 10; // 前哨站加速度方差 + v2 = 0.1; // 前哨站角加速度方差 + } else { + v1 = 100; // 加速度方差 + v2 = 400; // 角加速度方差 + } + auto a = dt * dt * dt * dt / 4; + auto b = dt * dt * dt / 2; + auto c = dt * dt; + + // 预测过程噪声偏差的方差 + QMat _Q; + // clang-format off + _Q << - a * v1, b * v1, 0, 0, 0, 0, 0, 0, 0, 0, 0, - b * v1, c * v1, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, a * v1, b * v1, 0, 0, 0, 0, 0, 0, 0, - 0, 0, b * v1, c * v1, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, a * v1, b * v1, 0, 0, 0, 0, 0, - 0, 0, 0, 0, b * v1, c * v1, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, a * v2, b * v2, 0, 0, 0, - 0, 0, 0, 0, 0, 0, b * v2, c * v2, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; + a * v1, b * v1, 0, 0, 0, 0, 0, 0, 0, 0, 0, + b * v1, c * v1, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, a * v1, b * v1, 0, 0, 0, 0, 0, 0, 0, + 0, 0, b * v1, c * v1, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, a * v1, b * v1, 0, 0, 0, 0, 0, + 0, 0, 0, 0, b * v1, c * v1, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, a * v2, b * v2, 0, 0, 0, + 0, 0, 0, 0, 0, 0, b * v2, c * v2, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; // clang-format on return _Q; } - std::function::ZVec(const ExtendedKalmanFilter<11, 4>::XVec&, int)> - h = [this](const ExtendedKalmanFilter<11, 4>::XVec& x, int id) { - Eigen::Vector3d xyz = this->h_armor_xyz(x, id); - Eigen::Vector3d ypd = util::math::xyz2ypd(xyz); - double angle = util::math::clamp_pm_pi(x(6) + id * 2 * CV_PI / this->armor_num_); - return Eigen::Vector4d { ypd(0), ypd(1), ypd(2), angle }; - }; + constexpr auto h(const XVec& x, const int& id) const -> const auto { + auto xyz = this->h_armor_xyz(x, id); + auto ypd = util::math::xyz2ypd(xyz); + auto angle = util::math::clamp_pm_pi(x(6) + id * 2 * CV_PI / this->armor_num_); + return Eigen::Vector4d { ypd(0), ypd(1), ypd(2), angle }; + } // 防止夹角求差出现异常值 - std::function::ZVec( - const ExtendedKalmanFilter<11, 4>::ZVec&, const ExtendedKalmanFilter<11, 4>::ZVec&)> - z_subtract = [](const ExtendedKalmanFilter<11, 4>::ZVec& a, - const ExtendedKalmanFilter<11, 4>::ZVec& b) { - ExtendedKalmanFilter<11, 4>::ZVec c = a - b; - c(0) = util::math::clamp_pm_pi(c(0)); - c(1) = util::math::clamp_pm_pi(c(1)); - c(3) = util::math::clamp_pm_pi(c(3)); - return c; - }; + constexpr auto z_subtract(const ZVec& a, const ZVec& b) const -> const auto { + ZVec c = a - b; + c(0) = util::math::clamp_pm_pi(c(0)); + c(1) = util::math::clamp_pm_pi(c(1)); + c(3) = util::math::clamp_pm_pi(c(3)); + return c; + }; - std::vector GetArmorXYZAList( - const ExtendedKalmanFilter<11, 4>::XVec& ekf_x) const { + constexpr auto GetArmorXYZAList(const XVec& ekf_x) const -> const auto { std::vector _armor_xyza_list; for (int i = 0; i < armor_num_; i++) { - auto angle = util::math::clamp_pm_pi(ekf_x(6) + i * 2 * CV_PI / armor_num_); - Eigen::Vector3d xyz = h_armor_xyz(ekf_x, i); + auto angle = util::math::clamp_pm_pi(ekf_x(6) + i * 2 * CV_PI / armor_num_); + auto xyz = h_armor_xyz(ekf_x, i); _armor_xyza_list.push_back({ xyz(0), xyz(1), xyz(2), angle }); } return _armor_xyza_list; @@ -265,9 +282,7 @@ ExtendedKalmanFilter<11, 4>::QMat Q(double dt) const { private: int armor_num_; enumeration::CarIDFlag car_id_; - - ExtendedKalmanFilter<11, 4>::PDig P0_dig_; + PDig P0_dig_; double radius_; }; - -} \ No newline at end of file +} diff --git a/src/tongji/predictor/live_target_manager/live_target.hpp b/src/tongji/predictor/live_target_manager/live_target.hpp deleted file mode 100644 index d9369a4..0000000 --- a/src/tongji/predictor/live_target_manager/live_target.hpp +++ /dev/null @@ -1,139 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -#include "../../time_stamp/time_stamp.hpp" -#include "../kalman_filter/extended_kalman_filter.hpp" -#include "../kalman_filter/predict_model.hpp" -#include "data/armor_gimbal_control_spacing.hpp" -#include "enum/car_id.hpp" - -namespace world_exe::tongji::predictor { - -class LiveTarget { -public: - LiveTarget(const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, - const enumeration::CarIDFlag& car_id) - : last_see_time_stamp_(std::time(nullptr)) - , model_(car_id) { - - // x vx y vy z vz a w r l h - // a: angle - // w: angular velocity - // l: r2 - r1 - // h: z2 - z1 - auto center_x = - armor_xyz_in_world[0] + model_.GetRadius() * std::cos(armor_ypr_in_world[0]); - auto center_y = - armor_xyz_in_world[1] + model_.GetRadius() * std::sin(armor_ypr_in_world[0]); - auto center_z = armor_xyz_in_world[2]; - - ExtendedKalmanFilter<11, 4>::XVec x0; - x0 << center_x, 0, center_y, 0, center_z, 0, armor_ypr_in_world[0], 0, model_.GetRadius(), - 0, 0; - - ExtendedKalmanFilter<11, 4>::PMat P0 = model_.GetP0Dig().asDiagonal(); - ekf_ = ExtendedKalmanFilter<11, 4>( - x0, P0, model_.x_add); // 初始化滤波器（预测量、预测量协方差） - } - - ExtendedKalmanFilter<11, 4>::XVec GetEkfX() const { return ekf_.x; } - ExtendedKalmanFilter<11, 4>::PDig GetP0Dig() const { return model_.GetP0Dig(); } - const PredictModel& GetModel() const { return model_; } - time_stamp::TimeStamp LastSeen() const { return time_stamp::TimeStamp(last_see_time_stamp_); } - - std::vector GetArmorGimbalControlSpacings() const { - std::vector armors; - for (int id = 0; id < model_.GetArmorNum(); id++) { - auto angle = - util::math::clamp_pm_pi(this->ekf_.x[6] + id * 2 * CV_PI / model_.GetArmorNum()); - auto xyz = model_.h_armor_xyz(this->ekf_.x, id); - - data::ArmorGimbalControlSpacing armor; - armor.id = model_.GetID(); - armor.position = xyz; - armor.orientation = util::math::euler_to_quaternion(angle, 15. / 180. * CV_PI, 0); - armors.emplace_back(std::move(armor)); - } - return armors; - } - - void Update(const double& dt, const Eigen::Vector3d& armor_xyz_in_world, - const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) { - // 装甲板匹配 - int id = - model_.MatchArmor(ekf_.x, armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world); - last_id = id; - update_count++; - - Update_ypda(armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world, id, dt); - - last_see_time_stamp_ = std::time(nullptr); - } - - bool IsConverged() const { return EvaluateConvergence(); } - -private: - bool EvaluateConvergence() const { - // 前哨站特殊判断 - const int required_count = (model_.GetID() == enumeration::CarIDFlag::Outpost) ? 10 : 3; - if (update_count < required_count) return false; - if (EvaluateDivergence()) return false; - - auto nis_failures = - std::accumulate(ekf_.recent_nis_failures.begin(), ekf_.recent_nis_failures.end(), 0); - if (nis_failures > 0.4 * ekf_.window_size) return false; - - return true; - } - - bool EvaluateDivergence() const { - auto r_ok = ekf_.x[8] > 0.05 && ekf_.x[8] < 0.5; - auto l_ok = ekf_.x[8] + ekf_.x[9] > 0.05 && ekf_.x[8] + ekf_.x[9] < 0.5; - - if (r_ok && l_ok) return false; - // util::logger::logger()->debug("[Target] r={:.3f}, l={:.3f}", ekf_.x[8], ekf_.x[9]); - return true; - } - - void Update_ypda(const Eigen::Vector3d& armor_xyz_in_world, - const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world, - const int& id, const double& dt) { - // 观测jacobi - auto H = model_.H(ekf_.x, id); - auto R = model_.R(armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world, id); - auto A = model_.A(dt); - auto Q = model_.Q(dt); - auto f = model_.f; - auto h = [this, id](const ExtendedKalmanFilter<11, 4>::XVec& x) { return model_.h(x, id); }; - auto z_subtract = model_.z_subtract; - - const Eigen::Vector3d& ypd = armor_ypd_in_world; - const Eigen::Vector3d& ypr = armor_ypr_in_world; - - // 获得观测量 - ExtendedKalmanFilter<11, 4>::ZVec z(4); - z << ypd[0], ypd[1], ypd[2], ypr[0]; - - ekf_.Update(dt, A, Q, f, z, H, R, h, z_subtract); - - // 前哨站转速特判 - if (model_.GetID() == enumeration::CarIDFlag::Outpost && EvaluateConvergence()) { - constexpr double max_outpost_w = 2.51; - if (std::abs(ekf_.x[7]) > 2.0) { - ekf_.x[7] = ekf_.x[7] > 0 ? max_outpost_w : -max_outpost_w; - } - } - } - - std::time_t last_see_time_stamp_; - ExtendedKalmanFilter<11, 4> ekf_; - PredictModel model_; - - int last_id = -1; - int update_count = 0; -}; -} \ No newline at end of file diff --git a/src/tongji/predictor/live_target_manager/live_target_manager.cpp b/src/tongji/predictor/live_target_manager/live_target_manager.cpp deleted file mode 100644 index ad20219..0000000 --- a/src/tongji/predictor/live_target_manager/live_target_manager.cpp +++ /dev/null @@ -1,153 +0,0 @@ -#include "live_target_manager.hpp" - -#include -#include -#include -#include - -#include "../in_gimbal_control_armor.hpp" -#include "../target_snapshot_manager/target_snapshot_manager.hpp" -#include "enum/armor_id.hpp" -#include "enum/car_id.hpp" -#include "interfaces/predictor_update_package.hpp" -#include "live_target.hpp" -#include "tracker.hpp" -#include "util/index.hpp" -#include "util/math.hpp" - -namespace world_exe::tongji::predictor { - -class LiveTargetManager::Impl { -public: - Impl(const double& time_delay, const double& yaw_offset, const double& pitch_offset, - double timeout_sec = 0.1) - : targets_map_() - , tracker_(std::make_unique()) - , last_update_timestamp_(std::time(nullptr)) - , tracking_id_(enumeration::CarIDFlag::None) - , time_delay_(time_delay) - , yaw_offset_(yaw_offset) - , pitch_offset_(pitch_offset) - , timeout_sec_(timeout_sec) { } - - std::shared_ptr Predict( - const enumeration::ArmorIdFlag& flag, const std::time_t& time_stamp) { - std::unordered_map> - result; - - for (auto id : util::enumeration::ExpandArmorIdFlags(flag)) { - auto it = targets_map_.find(id); - if (it != targets_map_.end() && it->second && it->second->IsConverged()) { - auto spacings = it->second->GetArmorGimbalControlSpacings(); - result[id] = spacings; - } - } - - return std::make_shared(result, time_stamp); - } // TODO: ** 目前 ** 我认为这个函数是多余的 - - std::shared_ptr GetPredictor( - const enumeration::ArmorIdFlag& flag) const { - - if (targets_map_.empty()) return nullptr; // TODO - - return std::make_shared( - flag, targets_map_, last_update_timestamp_, bullet_speed_, yaw_offset_, pitch_offset_); - } - - void Update(std::shared_ptr data, - const std::shared_ptr& armors_in_image, const std::time_t& now, - const double& bullet_speed) { - - UpdateTimeStamp(data->GetTimeStamped().GetTimeStamp()); - UpdateTargetMap(data, now); - UpdateTarget(data, armors_in_image, now); - UpdateBulletSpeed(bullet_speed); - } - - auto GetAllowedTargetID() const -> enumeration::CarIDFlag const { - if (targets_map_.at(tracking_id_)->IsConverged()) { - return tracking_id_; - } - return enumeration::CarIDFlag::None; - } - -private: - void UpdateBulletSpeed(const double& bullet_speed) { bullet_speed_ = bullet_speed; } - void UpdateTimeStamp(const time_t& time_stamp) { last_update_timestamp_ = time_stamp; } - void UpdateTargetMap( - std::shared_ptr data, const std::time_t& now) { - const Eigen::Affine3d transform = data->GetTransform(); - const Eigen::Matrix3d rotation_matrix = transform.linear(); - const auto armors_interface = data->GetArmors(); - - targets_map_.clear(); - for (int i; i < static_cast(enumeration::CarIDFlag::Count); i++) { - auto id = static_cast( - static_cast(enumeration::CarIDFlag::Hero) << i); - - const auto& armors_list = armors_interface->GetArmors(id); - if (armors_list.empty()) return; - - const auto& armor = armors_list.front(); - const Eigen::Vector3d xyz_in_world = transform * armor.position; - const Eigen::Vector3d ypr_in_world = rotation_matrix.eulerAngles(2, 1, 0); // ZYX - targets_map_[id] = - std::move(std::make_shared(xyz_in_world, ypr_in_world, id)); - } - } - - void UpdateTarget(std::shared_ptr data, - const std::shared_ptr& armors_in_image, const std::time_t& now) { - const Eigen::Affine3d transform = data->GetTransform(); - const Eigen::Matrix3d rotation_matrix = transform.linear(); - const auto armors_interface = data->GetArmors(); - - tracking_id_ = tracker_->SelectTrackingTargetID(armors_in_image, now); - - const auto& armors_list = armors_interface->GetArmors(tracking_id_); - if (armors_list.empty()) return; - - const auto& armor = armors_list.front(); - const Eigen::Vector3d xyz_in_world = transform * armor.position; - const Eigen::Vector3d ypr_in_world = rotation_matrix.eulerAngles(2, 1, 0); // ZYX - targets_map_[tracking_id_]->Update(static_cast(now - last_update_timestamp_), - xyz_in_world, ypr_in_world, util::math::xyz2ypd(xyz_in_world)); - } - - std::unordered_map> targets_map_; - std::unique_ptr tracker_; - std::time_t last_update_timestamp_; - enumeration::CarIDFlag tracking_id_; - - double bullet_speed_; - const double time_delay_; - const double yaw_offset_; - const double pitch_offset_; - - const double timeout_sec_; -}; - -LiveTargetManager::LiveTargetManager(const double& time_delay, const double& yaw_offset, - const double& pitch_offset, double timeout_sec) - : pimpl_(std::make_unique(time_delay, yaw_offset, pitch_offset, timeout_sec)) { } -LiveTargetManager::~LiveTargetManager() = default; - -std ::shared_ptr LiveTargetManager::Predict( - const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) { - return pimpl_->Predict(id, time_stamp); -} -std ::shared_ptr LiveTargetManager::GetPredictor( - const enumeration ::ArmorIdFlag& id) const { - return pimpl_->GetPredictor(id); -} - -void LiveTargetManager::Update(std::shared_ptr data, - const std::shared_ptr& armors_in_image, const std::time_t& now, - const double& bullet_speed) { - return pimpl_->Update(data, armors_in_image, now, bullet_speed); -} -auto LiveTargetManager::GetAllowedTargetID() const -> enumeration::ArmorIdFlag const { - return pimpl_->GetAllowedTargetID(); -} -} \ No newline at end of file diff --git a/src/tongji/predictor/live_target_manager/live_target_manager.hpp b/src/tongji/predictor/live_target_manager/live_target_manager.hpp deleted file mode 100644 index 88ebfce..0000000 --- a/src/tongji/predictor/live_target_manager/live_target_manager.hpp +++ /dev/null @@ -1,34 +0,0 @@ -#pragma once - -#include - -#include "enum/armor_id.hpp" -#include "interfaces/armor_in_image.hpp" -#include "interfaces/predictor_update_package.hpp" -#include "interfaces/target_predictor.hpp" - -namespace world_exe::tongji::predictor { - -class LiveTargetManager final : public interfaces::ITargetPredictor { -public: - LiveTargetManager(const double& time_delay, const double& yaw_offset, - const double& pitch_offset, double timeout_sec = 0.1); - ~LiveTargetManager(); - - std ::shared_ptr Predict( - const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) override; - std ::shared_ptr GetPredictor( - const enumeration ::ArmorIdFlag& id) const override; - - void Update(std::shared_ptr data, - const std::shared_ptr& armors_in_image, const std::time_t& now, - const double& bullet_speed); - - auto GetAllowedTargetID() const -> enumeration::ArmorIdFlag const; - -private: - class Impl; - std::unique_ptr pimpl_; -}; - -} \ No newline at end of file diff --git a/src/tongji/predictor/target_snapshot_manager/aim_point_chooser.hpp b/src/tongji/predictor/target_snapshot_manager/aim_point_chooser.hpp deleted file mode 100644 index 2f2c15d..0000000 --- a/src/tongji/predictor/target_snapshot_manager/aim_point_chooser.hpp +++ /dev/null @@ -1,89 +0,0 @@ -#pragma once - -#include "enum/car_id.hpp" -#include "util/math.hpp" - -namespace world_exe::tongji::predictor { - -using CarIDFlag = enumeration::CarIDFlag; - -class AimPointChooser { -public: - AimPointChooser( - const double& comming_angle = 60 / 57.3, const double& leaving_angle = 20 / 57.3) - : comming_angle_(comming_angle) - , leaving_angle_(leaving_angle) { } - - std::pair ChooseAimArmor(const Eigen::Vector& ekf_x, - const std::vector& xyza_list, const CarIDFlag& single_id) { - const auto armor_num = xyza_list.size(); - int chosen_id = -1; - - // 整车旋转中心的球坐标yaw - const auto center_yaw = std::atan2(ekf_x[2], ekf_x[0]); - - // 如果delta_angle为0，则该装甲板中心和整车中心的连线在世界坐标系的xy平面过原点 - std::vector delta_angle_list; - for (int i = 0; i < armor_num; i++) { - auto delta_angle = util::math::clamp_pm_pi(xyza_list[i][3] - center_yaw); - delta_angle_list.emplace_back(delta_angle); - } - - // 不考虑小陀螺 - if (std::abs(ekf_x[8]) <= 2 && single_id != CarIDFlag::Outpost) { - // 选择在可射击范围内的装甲板 - std::vector id_list; - for (int i = 0; i < armor_num; i++) { - if (std::abs(delta_angle_list[i]) > 60 / 57.3) continue; - id_list.push_back(i); - } - - if (id_list.size() == 1) { - chosen_id = id_list[0]; - lock_id_ = -1; - } else if (id_list.size() > 1) { - const int id0 = id_list[0], id1 = id_list[1]; - // 未处于锁定模式时，选择delta_angle绝对值较小的装甲板，进入锁定模式 - if (lock_id_ != id0 && lock_id_ != id1) - lock_id_ = (std::abs(delta_angle_list[id0]) < std::abs(delta_angle_list[id1])) - ? id0 - : id1; - chosen_id = lock_id_; - } else { - chosen_id = -1; - } - } else { - // 小陀螺 - double coming_angle = - (single_id == CarIDFlag::Outpost) ? 70 / 57.3 : comming_angle_; - double leaving_angle = - (single_id == CarIDFlag::Outpost) ? 30 / 57.3 : leaving_angle_; - - // 在小陀螺时，一侧的装甲板不断出现，另一侧的装甲板不断消失，显然前者被打中的概率更高 - for (int i = 0; i < armor_num; i++) { - if (std::abs(delta_angle_list[i]) > coming_angle) continue; - if ((ekf_x[7] > 0 && delta_angle_list[i] < leaving_angle) - || (ekf_x[7] < 0 && delta_angle_list[i] > -leaving_angle)) { - chosen_id = i; - break; - } - } - } - - if (chosen_id == -1) { - return { false, xyza_list[0] }; - } - - return { - true, - xyza_list[chosen_id], - }; - } - -private: - double comming_angle_ = 60 / 57.3; // degree - double leaving_angle_ = 20 / 57.3; // degree - int lock_id_ = -1; -}; - -} diff --git a/src/tongji/predictor/target_snapshot_manager/aim_solver.hpp b/src/tongji/predictor/target_snapshot_manager/aim_solver.hpp deleted file mode 100644 index 4c9122e..0000000 --- a/src/tongji/predictor/target_snapshot_manager/aim_solver.hpp +++ /dev/null @@ -1,88 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -#include "aim_point_chooser.hpp" -#include "target_snapshot.hpp" -#include "trajectory.hpp" - -namespace world_exe::tongji::predictor { - -using TargetSnapshot = predictor::TargetSnapshot; - -struct AimSolution { - bool valid; - double yaw; - double pitch; - Eigen::Vector4d aim_point; // 最终瞄准点（世界坐标 + 装甲板yaw） - double horizon_distance = 0; // 无人机专有 -}; - -class AimingSolver { -public: - AimingSolver( - const double& yaw_offset, const double& pitch_offset, const double& gravity = 9.7833) - : aim_point_chooser_(std::make_unique()) - , yaw_offset_(yaw_offset / 57.3) // degree to rad - , pitch_offset_(pitch_offset / 57.3) // degree to rad - , g_(gravity) { } - - AimSolution SolveAimSolution(const std::shared_ptr& snapshot, - const double& bullet_speed, const double& time_delay) { - if (!snapshot) return { false, 0, 0, { }, 0 }; - - // 迭代求解飞行时间 (最多10次，收敛条件：相邻两次fly_time差 <0.001) - double prev_fly_time = 0; - Eigen::Vector4d final_aim_point; - TrajectoryResult final_trajectory; - bool converged = false; - - // 预测目标在未来 dt时间后的位置 - for (int i = 0; i < 10; ++i) { - double dt = time_delay + prev_fly_time; - const auto aim = SelectPredictedAim(snapshot, dt); - if (!aim.has_value()) return { false, 0, 0, { }, 0 }; // failed: no valid aim point - - const auto traj = SolveTrajectory(aim->head(3), bullet_speed); - if (!traj.has_value()) return { false, 0, 0, { }, 0 }; // failed: trajectory unsolvable - - if (i > 0 && std::abs(traj->fly_time - prev_fly_time) < 0.001) { - final_aim_point = *aim; - final_trajectory = *traj; - converged = true; - break; - } - prev_fly_time = traj->fly_time; - } - if (!converged) return { false, 0, 0, { }, 0 }; // failed: trajectory did not converge - - const auto xyz = final_aim_point.head(3); - const double yaw = std::atan2(xyz.y(), xyz.x()) + yaw_offset_; - const double pitch = -(final_trajectory.pitch + pitch_offset_); - return { true, yaw, pitch, final_aim_point }; - } - -private: - std::optional SelectPredictedAim( - const std::shared_ptr& snapshot, const double& dt) const { - const auto& [selectable, aim_point] = aim_point_chooser_->ChooseAimArmor( - snapshot->Predict(dt), snapshot->GetPredictedXYZAList(dt), snapshot->GetID()); - return selectable ? std::optional { aim_point } : std::nullopt; - } - - std::optional SolveTrajectory( - const Eigen::Vector3d& xyz, const double& bullet_speed) const { - double d = std::hypot(xyz.x(), xyz.y()); - auto result = TrajectorySolver::SolveTrajectory(bullet_speed, d, xyz.z(), g_); - return result.solvable ? std::optional { result } : std::nullopt; - } - - double yaw_offset_, pitch_offset_; - const double g_; - - std::unique_ptr aim_point_chooser_; -}; -} \ No newline at end of file diff --git a/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp b/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp deleted file mode 100644 index a1fa6bf..0000000 --- a/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp +++ /dev/null @@ -1,54 +0,0 @@ -#pragma once - -#include - -#include "../../time_stamp/time_stamp.hpp" -#include "../kalman_filter/extended_kalman_filter.hpp" -#include "../kalman_filter/predict_model.hpp" -#include "../live_target_manager/live_target.hpp" - -namespace world_exe::tongji::predictor { - -class TargetSnapshot { -public: - TargetSnapshot(const LiveTarget& target) - : model_(target.GetModel()) - , ekf_(target.GetEkfX(), target.GetP0Dig().asDiagonal(), model_.x_add) - , time_stamp_(target.LastSeen()) { } - - // std::vector GetArmorGimbalControlSpacings() const { - // std::vector armors; - // for (int id = 0; id < model_.GetArmorNum(); id++) { - // auto angle = - // util::math::clamp_pm_pi(this->ekf_.x[6] + id * 2 * CV_PI / model_.GetArmorNum()); - // auto xyz = model_.h_armor_xyz(this->ekf_.x, id); - - // data::ArmorGimbalControlSpacing armor; - // armor.id = model_.GetID(); - // armor.position = xyz; - // armor.orientation = util::math::euler_to_quaternion(angle, 15. / 180. * CV_PI, 0); - // armors.emplace_back(std::move(armor)); - // } - // return armors; - // } - - auto GetPredictedXYZAList(const double& dt) -> std::vector const { - return model_.GetArmorXYZAList(this->Predict(dt)); - } - - auto GetTimeStamp() const { return time_stamp_; } - auto GetID() const { return model_.GetID(); } - auto GetEkfX() const { return ekf_.x; } - - auto Predict(const double& dt) -> Eigen::Vector const { - auto predicted_x = model_.f(ekf_.x, dt); - return predicted_x; - } - -private: - PredictModel model_; - ExtendedKalmanFilter<11, 4> ekf_; - time_stamp::TimeStamp time_stamp_; -}; - -} \ No newline at end of file diff --git a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp deleted file mode 100644 index ad9a586..0000000 --- a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp +++ /dev/null @@ -1,99 +0,0 @@ -#include "target_snapshot_manager.hpp" - -#include -#include -#include - -#include "../in_gimbal_control_armor.hpp" -#include "../live_target_manager/live_target.hpp" -#include "aim_solver.hpp" -#include "data/armor_gimbal_control_spacing.hpp" -#include "enum/enum_tools.hpp" - -namespace world_exe::tongji::predictor { - -class TargetSnapshotManager::Impl { -public: - Impl(const enumeration::ArmorIdFlag& id, - const std::unordered_map>& - live_target_map, - const std::time_t& now, const double& bullet_speed, const double& yaw_offset, - const double& pitch_offset) - : aim_solver_(std::make_unique(yaw_offset, pitch_offset)) - - , now_(now) - , ids_(id) - , bullet_speed_(bullet_speed) - , snapshots_(BuildSnapshots(live_target_map)) - , gimbal_command_({ std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN() }) { } - - const enumeration::ArmorIdFlag& GetId() const { return ids_; } - - std::shared_ptr Predictor( - const std::time_t& time_stamp) const { - - std::unordered_map> - result; - - for (const auto& [id, snapshot] : snapshots_) { - auto aim_solution = - aim_solver_->SolveAimSolution(std::make_unique(snapshot), - bullet_speed_, snapshot.GetTimeStamp().GetTimeStamp()); - - if (!aim_solution.valid) continue; - - auto target_pos = aim_solution.aim_point.head<3>(); - auto armor_yaw = aim_solution.aim_point[3]; - result.emplace(id, - std::vector { - data::ArmorGimbalControlSpacing { id, target_pos, - util::math::euler_to_quaternion(armor_yaw, 15. / 180. * CV_PI, 0) } }); - - gimbal_command_.yaw = aim_solution.yaw; - gimbal_command_.pitch = aim_solution.pitch; - } - return std::make_shared(result, time_stamp); - } - - auto GetGimbalCommand() const -> GimbalCommand const { return gimbal_command_; } - -private: - static std::unordered_map BuildSnapshots( - const std::unordered_map>& input) { - std::unordered_map result; - for (const auto& [id, target] : input) { - if (target) { - result.emplace(id, TargetSnapshot(*target)); - } - } - return result; - } - - std::unique_ptr aim_solver_; - const std::unordered_map snapshots_; - const std::time_t& now_; - const enumeration::ArmorIdFlag ids_; - const double bullet_speed_; - mutable GimbalCommand gimbal_command_; -}; - -TargetSnapshotManager::TargetSnapshotManager(const enumeration::ArmorIdFlag& id, - const std::unordered_map>& - live_target_map, - const std::time_t& now, const double& bullet_speed, const double& yaw_offset, - const double& pitch_offset) - : pimpl_(std::make_unique( - id, live_target_map, now, bullet_speed, yaw_offset, pitch_offset)) { } -TargetSnapshotManager::~TargetSnapshotManager() = default; - -const enumeration ::ArmorIdFlag& TargetSnapshotManager::GetId() const { return pimpl_->GetId(); } -std ::shared_ptr TargetSnapshotManager::Predictor( - const std ::time_t& time_stamp) const { - return pimpl_->Predictor(time_stamp); -} - -auto TargetSnapshotManager::GetGimbalCommand() const -> GimbalCommand const { - return pimpl_->GetGimbalCommand(); -} -} \ No newline at end of file diff --git a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp deleted file mode 100644 index 3176e9a..0000000 --- a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp +++ /dev/null @@ -1,36 +0,0 @@ -#pragma once - -#include -#include - -#include "../live_target_manager/live_target.hpp" -#include "enum/armor_id.hpp" -#include "interfaces/predictor.hpp" - -namespace world_exe::tongji::predictor { - -struct GimbalCommand { - double yaw; - double pitch; -}; - -class TargetSnapshotManager final : public interfaces::IPredictor { -public: - TargetSnapshotManager(const enumeration::ArmorIdFlag& id, - const std::unordered_map>& - live_target_map, - const std::time_t& now, const double& bullet_speed, const double& yaw_offset, - const double& pitch_offset); - ~TargetSnapshotManager(); - - const enumeration ::ArmorIdFlag& GetId() const override; - std ::shared_ptr Predictor( - const std ::time_t& time_stamp) const override; - - auto GetGimbalCommand() const -> GimbalCommand const; - -private: - class Impl; - std::unique_ptr pimpl_; -}; -} diff --git a/src/tongji/solver/reprojection_util.hpp b/src/tongji/solver/reprojection_util.hpp new file mode 100644 index 0000000..109163a --- /dev/null +++ b/src/tongji/solver/reprojection_util.hpp @@ -0,0 +1,120 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include + +#include "data/armor_image_spaceing.hpp" +#include "parameters/profile.hpp" +#include "parameters/rm_parameters.hpp" +#include "util/coordinate.hpp" +#include "util/math.hpp" + +namespace world_exe::tongji::solver { + +class ReprojectionUtil { +public: + ReprojectionUtil() = default; + ~ReprojectionUtil() = default; + + double CalculateReprojectionError(const Eigen::Matrix3d& R_camera2gimbal, + const Eigen::Vector3d& t_gimbal2camera, + const world_exe::data::ArmorImageSpacing& armor_in_image, + const Eigen::Vector3d& armor_xyz_in_gimbal, const double& armor_yaw, + const double& armor_pitch, const double& inclined) const { + + auto image_points = ReprojectArmor(R_camera2gimbal, t_gimbal2camera, armor_xyz_in_gimbal, + armor_yaw, armor_pitch, armor_in_image.isLargeArmor); + + auto error = 0.0; + for (int i = 0; i < 4; i++) { + error += cv::norm(armor_in_image.image_points[i] - image_points[i]); + } + + // SJTU_cost + // auto cost = SJTU_cost(image_points, armor_in_image.image_points, inclined); + return error; + } + +private: + std::vector ReprojectArmor(const Eigen::Matrix3d& R_camera2gimbal, + const Eigen::Vector3d& t_gimbal2camera, const Eigen::Vector3d& armor_xyz_in_gimbal, + const double& armor_yaw, const double& armor_pitch, const bool& is_large) const { + + auto sin_yaw = std::sin(armor_yaw); + auto cos_yaw = std::cos(armor_yaw); + auto sin_pitch = std::sin(armor_pitch); + auto cos_pitch = std::cos(armor_pitch); + + // clang-format off + const Eigen::Matrix3d R_armor2gimbal { + {cos_yaw * cos_pitch, -sin_yaw, cos_yaw * sin_pitch}, + {sin_yaw * cos_pitch, cos_yaw, sin_yaw * sin_pitch}, + { -sin_pitch, 0, cos_pitch} + }; + // clang-format on + + // get R_armor2camera t_armor2camera + const Eigen::Vector3d& t_armor2gimbal = armor_xyz_in_gimbal; + Eigen::Matrix3d R_armor2camera = R_camera2gimbal.transpose() * R_armor2gimbal; + Eigen::Matrix3d R_armor2camera_cv = util::coordinate::ros2opencv_rotation(R_armor2camera); + + Eigen::Vector3d t_armor2camera = + R_camera2gimbal.transpose() * (armor_xyz_in_gimbal) + t_gimbal2camera; + Eigen::Vector3d t_armor2camera_cv = util::coordinate::ros2opencv_position(t_armor2camera); + + // get rvec tvec + cv::Vec3d rvec; + cv::Mat _R_armor2camera_cv; + cv::eigen2cv(R_armor2camera_cv, _R_armor2camera_cv); + cv::Rodrigues(_R_armor2camera_cv, rvec); + cv::Vec3d tvec(t_armor2camera_cv[0], t_armor2camera_cv[1], t_armor2camera_cv[2]); + + std::vector image_points; + const auto& object_points = (is_large) + ? parameters::Robomaster::LargeArmorObjectPointsOpencv + : parameters::Robomaster::NormalArmorObjectPointsOpencv; + cv::projectPoints(object_points, rvec, tvec, + parameters::HikCameraProfile::get_intrinsic_parameters(), + parameters::HikCameraProfile::get_distortion_parameters(), image_points); + return image_points; + } + + double SJTU_cost(const std::vector& cv_refs, + const std::vector& cv_pts, const double& inclined) const { + std::size_t size = cv_refs.size(); + std::vector refs; + std::vector pts; + for (std::size_t i = 0u; i < size; ++i) { + refs.emplace_back(cv_refs[i].x, cv_refs[i].y); + pts.emplace_back(cv_pts[i].x, cv_pts[i].y); + } + double cost = 0.; + for (std::size_t i = 0u; i < size; ++i) { + std::size_t p = (i + 1u) % size; + // i - p 构成线段。过程：先移动起点，再补长度，再旋转 + Eigen::Vector2d ref_d = refs[p] - refs[i]; // 标准 + Eigen::Vector2d pt_d = pts[p] - pts[i]; + // 长度差代价 + 起点差代价(1 / 2)（0 度左右应该抛弃) + double pixel_dis = // dis 是指方差平面内到原点的距离 + (0.5 * ((refs[i] - pts[i]).norm() + (refs[p] - pts[p]).norm()) + + std::fabs(ref_d.norm() - pt_d.norm())) + / ref_d.norm(); + double angular_dis = + ref_d.norm() * util::math::get_abs_angle(ref_d, pt_d) / ref_d.norm(); + // 平方可能是为了配合 sin 和 cos + // 弧度差代价（0 度左右占比应该大） + double cost_i = util::math::square(pixel_dis * std::sin(inclined)) + + util::math::square(angular_dis * std::cos(inclined)) + * 2.0; // DETECTOR_ERROR_PIXEL_BY_SLOPE + // 重投影像素误差越大，越相信斜率 + cost += std::sqrt(cost_i); + } + return cost; + } +}; +} diff --git a/src/tongji/solver/solved_armor.hpp b/src/tongji/solver/solved_armor.hpp index 43a79eb..7e6fe34 100644 --- a/src/tongji/solver/solved_armor.hpp +++ b/src/tongji/solver/solved_armor.hpp @@ -4,30 +4,27 @@ #include #include "interfaces/armor_in_camera.hpp" -#include "interfaces/time_stamped.hpp" +#include "data/time_stamped.hpp" #include "util/index.hpp" namespace world_exe::tongji::solver { -class SolvedArmor final : public interfaces::IArmorInCamera, public interfaces::ITimeStamped { +class SolvedArmor final : public interfaces::IArmorInCamera { public: - explicit SolvedArmor(const std::vector& armors) { + explicit SolvedArmor(const std::vector& armors, data::TimeStamp when_image_come) + : time_stamp_(when_image_come) { for (const auto& armor : armors) { armors_[util::enumeration::GetIndex(armor.id)].emplace_back(armor); } - time_stamp_ = 0; } - const interfaces::ITimeStamped& GetTimeStamped() const override { return *this; } - - const std::time_t& GetTimeStamp() const override { return time_stamp_; }; - const std::vector& GetArmors( const enumeration::ArmorIdFlag& armor_id) const override { return armors_[util::enumeration::GetIndex(armor_id)]; } + const data::TimeStamp& GetTimeStamp() const override { return time_stamp_; } private: - std::time_t time_stamp_ { 0 }; std::array, 8> armors_; + data::TimeStamp time_stamp_; }; -} \ No newline at end of file +} diff --git a/src/tongji/solver/solver.cpp b/src/tongji/solver/solver.cpp index 2202025..a9a91d4 100644 --- a/src/tongji/solver/solver.cpp +++ b/src/tongji/solver/solver.cpp @@ -1,14 +1,24 @@ #include "solver.hpp" +#include +#include #include +#include +#include + #include #include +#include +#include #include "data/armor_camera_spacing.hpp" -#include "data/armor_gimbal_control_spacing.hpp" +#include "data/armor_image_spaceing.hpp" +#include "data/time_stamped.hpp" +#include "enum/armor_id.hpp" #include "parameters/profile.hpp" #include "parameters/rm_parameters.hpp" #include "solved_armor.hpp" +#include "tongji/solver/reprojection_util.hpp" #include "util/coordinate.hpp" #include "util/index.hpp" #include "util/math.hpp" @@ -16,298 +26,137 @@ namespace world_exe::tongji::solver { class Solver::Impl { public: - Impl(Eigen::Matrix3d R_camera2gimbal, Eigen::Matrix3d R_gimbal2world, - Eigen::Vector3d t_camera2gimbal) - : R_camera2gimbal_(R_camera2gimbal) - , R_gimbal2world_(R_gimbal2world) - , t_camera2gimbal_(t_camera2gimbal) { } + explicit Impl() + : R_camera2gimbal_(Eigen::Matrix3d::Zero()) + , t_camera2gimbal_(Eigen::Vector3d::Zero()) + , reprojection_util_(std::make_unique()) { } - std::shared_ptr SolvePnp( + std::shared_ptr EstimateAllArmorPoses( std::shared_ptr armors_in_image) { std::vector armor_plates; for (int i = 0; i < static_cast(enumeration::ArmorIdFlag::Count); i++) { const auto& armor_id = util::enumeration::GetArmorIdFlag(i); const auto& armors = armors_in_image->GetArmors(armor_id); + for (const auto& armor : armors) { - auto solved_armor = Solve(armor); - if (solved_armor.has_value()) { - armor_plates.emplace_back(solved_armor.value()); - } + auto solved_armor = EstimatePose(armor); + armor_plates.emplace_back(solved_armor); } } - return std::make_shared(armor_plates); + return std::make_shared(armor_plates, armors_in_image->GetTimeStamp()); } - std::optional Solve( - const world_exe::data::ArmorImageSpacing& armor_in_image) const { - const auto& object_points = armor_in_image.isLargeArmor - ? parameters::Robomaster::LargeArmorObjectPointsOpencv - : parameters::Robomaster::NormalArmorObjectPointsOpencv; - - cv::Vec3d rvec, tvec; - cv::solvePnP(object_points, armor_in_image.image_points, - parameters::HikCameraProfile::get_intrinsic_parameters(), - parameters::HikCameraProfile::get_distortion_parameters(), rvec, tvec, false, - cv::SOLVEPNP_IPPE); - - Eigen::Vector3d xyz_in_camera; - cv::cv2eigen(tvec, xyz_in_camera); - - cv::Mat rmat; - cv::Rodrigues(rvec, rmat); - Eigen::Matrix3d R_armor2camera; - cv::cv2eigen(rmat, R_armor2camera); - - Eigen::Quaterniond orientation_in_camera(R_armor2camera); - - world_exe::data::ArmorCameraSpacing armor_in_camera; - auto orientation_ros = util::coordinate::opencv2ros_orientation(orientation_in_camera); - orientation_ros.normalize(); - armor_in_camera.id = armor_in_image.id; - armor_in_camera.position = util::coordinate::opencv2ros_position(xyz_in_camera); - armor_in_camera.orientation = orientation_ros; - - if (armor_in_camera.position.norm() > MaxArmorDistance) { - return {}; - } - return armor_in_camera; + auto SetCamera2Gimbal( + const Eigen::Matrix3d& R_camera2gimbal, const Eigen::Vector3d& t_camera2gimbal) -> void { + R_camera2gimbal_ = R_camera2gimbal; + t_camera2gimbal_ = t_camera2gimbal; } - world_exe::data::ArmorGimbalControlSpacing OptimizeYaw( - const world_exe::data::ArmorImageSpacing& armor_in_image, - const world_exe::data::ArmorCameraSpacing& armor_in_camera) const { - - Eigen::Vector3d armor_in_camera_ocv = - util::coordinate::ros2opencv_position(armor_in_camera.position); - Eigen::Vector3d armor_xyz_in_gimbal = - R_camera2gimbal_ * armor_in_camera_ocv + t_camera2gimbal_; - Eigen::Vector3d armor_xyz_in_world = R_gimbal2world_ * armor_xyz_in_gimbal; // why no t? - - Eigen::Quaterniond armor_orientation_in_camera_ocv = - util::coordinate::ros2opencv_orientation(armor_in_camera.orientation); - Eigen::Matrix3d R_armor2camera_ocv = armor_orientation_in_camera_ocv.toRotationMatrix(); - Eigen::Matrix3d R_armor2gimbal = R_camera2gimbal_ * R_armor2camera_ocv; - Eigen::Matrix3d R_armor2world_initial = R_gimbal2world_ * R_armor2gimbal; - - Eigen::Vector3d armor_ypr_in_world = - util::math::matrix_to_euler(R_armor2world_initial, 2, 1, 0); - // double initial_yaw = armor_ypr_in_world[0]; - - Eigen::Vector3d gimbal_ypr = util::math::matrix_to_euler(R_gimbal2world_, 2, 1, 0); + auto Camera2Gimbal(const Eigen::Vector3d& xyz_in_camera) const -> const auto { + Eigen::Vector3d xyz_in_gimbal = + R_camera2gimbal_.transpose() * xyz_in_camera + t_camera2gimbal_; + return xyz_in_gimbal; + } + auto CalculateOptimizeYaw(const data::ArmorImageSpacing& armor_in_image, + const Eigen::Vector3d& armor_xyz_in_gimbal, const double& gimbal_yaw, + const double& initial_armor_yaw_in_gimbal) const -> const double { constexpr double SEARCH_RANGE = 140; // degree - auto yaw0 = util::math::clamp_pm_pi(gimbal_ypr[0] - SEARCH_RANGE / 2 * CV_PI / 180.0); + const auto yaw0 = util::math::clamp_pm_pi(gimbal_yaw - SEARCH_RANGE / 2 * CV_PI / 180.0); auto min_error = 1e10; - auto best_yaw = armor_ypr_in_world[0]; + auto best_yaw = initial_armor_yaw_in_gimbal; + + auto pitch = + (armor_in_image.id == enumeration::ArmorIdFlag::Outpost) ? -15.0 * CV_PI / 180.0 : 15.0; for (int i = 0; i < SEARCH_RANGE; i++) { double yaw = util::math::clamp_pm_pi(yaw0 + i * CV_PI / 180.0); - auto error = ArmorReprojectionError( - armor_in_image, armor_xyz_in_world, yaw, (i - SEARCH_RANGE / 2) * CV_PI / 180.0); + + auto error = reprojection_util_->CalculateReprojectionError(R_camera2gimbal_, + t_camera2gimbal_, armor_in_image, armor_xyz_in_gimbal, yaw, pitch, + (i - SEARCH_RANGE / 2) * CV_PI / 180.0); if (error < min_error) { min_error = error; best_yaw = yaw; } } - - armor_ypr_in_world[0] = best_yaw; - - Eigen::Matrix3d R_armor2world_optimized = util::math::euler_to_matrix(armor_ypr_in_world); - - Eigen::Quaterniond orientation_in_world(R_armor2world_optimized); - orientation_in_world.normalize(); - - world_exe::data::ArmorGimbalControlSpacing result; - result.id = armor_in_camera.id; - result.position = armor_xyz_in_world; - result.orientation = orientation_in_world; - return result; + return best_yaw; } - const std::time_t& GetTimeStamp() const { return time_stamp_; } + const data::TimeStamp GetTimeStamp() const { + return data::TimeStamp(std::chrono::steady_clock::now().time_since_epoch()); + } private: - std::vector ReprojectArmor(const Eigen::Vector3d& xyz_in_world, const double& yaw, - const bool& is_large, const enumeration::ArmorIdFlag& id) const { - auto sin_yaw = std::sin(yaw); - auto cos_yaw = std::cos(yaw); - - auto pitch = (id == enumeration::ArmorIdFlag::Outpost) ? -15.0 * CV_PI / 180.0 - : 15.0 * CV_PI / 180.0; - auto sin_pitch = std::sin(pitch); - auto cos_pitch = std::cos(pitch); - - // clang-format off - const Eigen::Matrix3d R_armor2world { - {cos_yaw * cos_pitch, -sin_yaw, cos_yaw * sin_pitch}, - {sin_yaw * cos_pitch, cos_yaw, sin_yaw * sin_pitch}, - { -sin_pitch, 0, cos_pitch} - }; - // clang-format on - - // get R_armor2camera t_armor2camera - const Eigen::Vector3d& t_armor2world = xyz_in_world; - Eigen::Matrix3d R_armor2camera = - R_camera2gimbal_.transpose() * R_gimbal2world_.transpose() * R_armor2world; - Eigen::Vector3d t_armor2camera = R_camera2gimbal_.transpose() - * (R_gimbal2world_.transpose() * t_armor2world - t_camera2gimbal_); + data::ArmorCameraSpacing EstimatePose( + const world_exe::data::ArmorImageSpacing& armor_in_image) const { + const auto& [xyz_in_camera, R_armor2camera] = EstimatePnp(armor_in_image); - // get rvec tvec - cv::Vec3d rvec; - cv::Mat R_armor2camera_cv; - cv::eigen2cv(R_armor2camera, R_armor2camera_cv); - cv::Rodrigues(R_armor2camera_cv, rvec); - cv::Vec3d tvec(t_armor2camera[0], t_armor2camera[1], t_armor2camera[2]); + data::ArmorCameraSpacing pose; + pose.id = armor_in_image.id; + pose.orientation = Eigen::Quaterniond(R_armor2camera).normalized(); + pose.position = xyz_in_camera; + return pose; + } - // reproject - std::vector image_points; - const auto& object_points = (is_large) + auto EstimatePnp(const world_exe::data::ArmorImageSpacing& armor_in_image) const + -> const std::tuple { + const auto& object_points = armor_in_image.isLargeArmor ? parameters::Robomaster::LargeArmorObjectPointsOpencv : parameters::Robomaster::NormalArmorObjectPointsOpencv; - cv::projectPoints(object_points, rvec, tvec, - parameters::HikCameraProfile::get_intrinsic_parameters(), - parameters::HikCameraProfile::get_distortion_parameters(), image_points); - return image_points; - } - - double ArmorReprojectionError(const world_exe::data::ArmorImageSpacing& armor_in_image, - const Eigen::Vector3d& armor_xyz_in_world, const double& yaw, - const double& inclined) const { - - auto image_points = - ReprojectArmor(armor_xyz_in_world, yaw, armor_in_image.isLargeArmor, armor_in_image.id); - auto error = 0.0; - for (int i = 0; i < 4; i++) - error += cv::norm(armor_in_image.image_points[i] - image_points[i]); - // auto error = SJTU_cost(image_points, armor_in_image.image_points, inclined); - - return error; - } - - double OupostReprojectionError(world_exe::data::ArmorImageSpacing armor_in_image, - Eigen::Vector3d armor_xyz_in_world, const double& pitch) const { - - // solve - const auto& object_points = (armor_in_image.isLargeArmor) - ? world_exe::parameters::Robomaster::LargeArmorObjectPointsOpencv - : world_exe::parameters::Robomaster::NormalArmorObjectPointsOpencv; cv::Vec3d rvec, tvec; cv::solvePnP(object_points, armor_in_image.image_points, - world_exe::parameters::HikCameraProfile::get_intrinsic_parameters(), + parameters::HikCameraProfile::get_intrinsic_parameters(), parameters::HikCameraProfile::get_distortion_parameters(), rvec, tvec, false, cv::SOLVEPNP_IPPE); - Eigen::Vector3d xyz_in_camera; - cv::cv2eigen(tvec, xyz_in_camera); + // 1. P_C_cv -> P_C_ros (位置) + Eigen::Vector3d xyz_in_camera_cv; + cv::cv2eigen(tvec, xyz_in_camera_cv); + const auto xyz_in_camera = util::coordinate::opencv2ros_position(xyz_in_camera_cv); + // 2. R_A->C_cv -> R_A->C_ros (姿态) cv::Mat rmat; cv::Rodrigues(rvec, rmat); - Eigen::Matrix3d R_armor2camera; - cv::cv2eigen(rmat, R_armor2camera); - Eigen::Matrix3d R_armor2gimbal = R_camera2gimbal_ * R_armor2camera; - Eigen::Matrix3d R_armor2world = R_gimbal2world_ * R_armor2gimbal; - auto armor_ypr_in_gimbal = util::math::matrix_to_euler(R_armor2gimbal, 2, 1, 0); - auto armor_ypr_in_world = util::math::matrix_to_euler(R_armor2world, 2, 1, 0); - - auto yaw = armor_ypr_in_world[0]; - auto xyz_in_world = armor_xyz_in_world; - - auto sin_yaw = std::sin(yaw); - auto cos_yaw = std::cos(yaw); + Eigen::Matrix3d R_armor2camera_cv; + cv::cv2eigen(rmat, R_armor2camera_cv); + const auto R_armor2camera = util::coordinate::opencv2ros_rotation(R_armor2camera_cv); - auto sin_pitch = std::sin(pitch); - auto cos_pitch = std::cos(pitch); - - // clang-format off - const Eigen::Matrix3d _R_armor2world { - {cos_yaw * cos_pitch, -sin_yaw, cos_yaw * sin_pitch}, - {sin_yaw * cos_pitch, cos_yaw, sin_yaw * sin_pitch}, - { -sin_pitch, 0, cos_pitch} - }; - // clang-format on - - // get R_armor2camera t_armor2camera - const Eigen::Vector3d& t_armor2world = xyz_in_world; - Eigen::Matrix3d _R_armor2camera = - R_camera2gimbal_.transpose() * R_gimbal2world_.transpose() * _R_armor2world; - Eigen::Vector3d t_armor2camera = R_camera2gimbal_.transpose() - * (R_gimbal2world_.transpose() * t_armor2world - t_camera2gimbal_); - - // get rvec tvec - cv::Vec3d _rvec; - cv::Mat R_armor2camera_cv; - cv::eigen2cv(_R_armor2camera, R_armor2camera_cv); - cv::Rodrigues(R_armor2camera_cv, _rvec); - cv::Vec3d _tvec(t_armor2camera[0], t_armor2camera[1], t_armor2camera[2]); - - // reproject - std::vector image_points; - cv::projectPoints(object_points, _rvec, _tvec, - parameters::HikCameraProfile::get_intrinsic_parameters(), - parameters::HikCameraProfile::get_distortion_parameters(), image_points); - - auto error = 0.0; - for (int i = 0; i < 4; i++) - error += cv::norm(armor_in_image.image_points[i] - image_points[i]); - return error; - } - - double SJTU_cost(const std::vector& cv_refs, - const std::vector& cv_pts, const double& inclined) const { - std::size_t size = cv_refs.size(); - std::vector refs; - std::vector pts; - for (std::size_t i = 0u; i < size; ++i) { - refs.emplace_back(cv_refs[i].x, cv_refs[i].y); - pts.emplace_back(cv_pts[i].x, cv_pts[i].y); - } - double cost = 0.; - for (std::size_t i = 0u; i < size; ++i) { - std::size_t p = (i + 1u) % size; - // i - p 构成线段。过程：先移动起点，再补长度，再旋转 - Eigen::Vector2d ref_d = refs[p] - refs[i]; // 标准 - Eigen::Vector2d pt_d = pts[p] - pts[i]; - // 长度差代价 + 起点差代价(1 / 2)（0 度左右应该抛弃) - double pixel_dis = // dis 是指方差平面内到原点的距离 - (0.5 * ((refs[i] - pts[i]).norm() + (refs[p] - pts[p]).norm()) - + std::fabs(ref_d.norm() - pt_d.norm())) - / ref_d.norm(); - double angular_dis = - ref_d.norm() * util::math::get_abs_angle(ref_d, pt_d) / ref_d.norm(); - // 平方可能是为了配合 sin 和 cos - // 弧度差代价（0 度左右占比应该大） - double cost_i = util::math::square(pixel_dis * std::sin(inclined)) - + util::math::square(angular_dis * std::cos(inclined)) - * 2.0; // DETECTOR_ERROR_PIXEL_BY_SLOPE - // 重投影像素误差越大，越相信斜率 - cost += std::sqrt(cost_i); - } - return cost; + return { xyz_in_camera, R_armor2camera }; } private: Eigen::Matrix3d R_camera2gimbal_; - Eigen::Matrix3d R_gimbal2world_; Eigen::Vector3d t_camera2gimbal_; - inline constexpr static const double MaxArmorDistance = 15.0; - std::time_t time_stamp_ { 0 }; + std::unique_ptr reprojection_util_; }; -Solver::Solver(Eigen::Matrix3d R_camera2gimbal, Eigen::Matrix3d R_gimbal2world, - Eigen::Vector3d t_camera2gimbal) - : pimpl_(std::make_unique(R_camera2gimbal, R_gimbal2world, t_camera2gimbal)) { } +Solver::Solver() + : pimpl_(std::make_unique()) { } Solver::~Solver() = default; -const std::time_t& Solver::GetTimeStamp() const { return pimpl_->GetTimeStamp(); } - std::shared_ptr Solver::SolvePnp( std::shared_ptr armors_in_image) { - return pimpl_->SolvePnp(armors_in_image); + return pimpl_->EstimateAllArmorPoses(armors_in_image); +} +void Solver::SetCamera2Gimbal( + const Eigen::Matrix3d& R_camera2gimbal, const Eigen::Vector3d& t_camera2gimbal) { + pimpl_->SetCamera2Gimbal(R_camera2gimbal, t_camera2gimbal); } -} \ No newline at end of file +auto Solver::CalculateOptimizeYaw(const data::ArmorImageSpacing& armor_in_image, + const Eigen::Vector3d& armor_xyz_in_gimbal, const double& gimbal_yaw, + const double& initial_armor_yaw_in_gimbal) const -> const double { + return pimpl_->CalculateOptimizeYaw( + armor_in_image, armor_xyz_in_gimbal, gimbal_yaw, initial_armor_yaw_in_gimbal); +} + +auto Solver::Camera2Gimbal(const Eigen::Vector3d& xyz_in_camera) const -> const auto { + return pimpl_->Camera2Gimbal(xyz_in_camera); +} +} diff --git a/src/tongji/solver/solver.hpp b/src/tongji/solver/solver.hpp index 5f6c2f4..5247343 100644 --- a/src/tongji/solver/solver.hpp +++ b/src/tongji/solver/solver.hpp @@ -1,24 +1,34 @@ #pragma once #include "interfaces/pnp_solver.hpp" -#include "interfaces/time_stamped.hpp" namespace world_exe::tongji::solver { -class Solver final : public interfaces::IPnpSolver, public interfaces::ITimeStamped { +class Solver final : public interfaces::IPnpSolver { public: - explicit Solver(Eigen::Matrix3d R_camera2gimbal, Eigen::Matrix3d R_gimbal2world, - Eigen::Vector3d t_camera2gimbal); + explicit Solver(); ~Solver(); std::shared_ptr SolvePnp( - std::shared_ptr armor) override; + std::shared_ptr armors) override; - const std::time_t& GetTimeStamp() const override; + void SetCamera2Gimbal( + const Eigen::Matrix3d& R_camera2gimbal, const Eigen::Vector3d& t_camera2gimbal); + + auto Camera2Gimbal(const Eigen::Vector3d& xyz_in_camera) const -> const auto; + auto CalculateOptimizeYaw(const data::ArmorImageSpacing& armor_in_image, + const Eigen::Vector3d& armor_xyz_in_gimbal, const double& gimbal_yaw, + const double& initial_armor_yaw_in_gimbal) const -> const double; + + Solver(const Solver&) = delete; + Solver& operator=(const Solver&) = delete; + Solver(Solver&&) noexcept = default; + Solver& operator=(Solver&&) noexcept = default; private: class Impl; + std::unique_ptr pimpl_; }; -} \ No newline at end of file +} diff --git a/src/tongji/state_machine/state_machine.cpp b/src/tongji/state_machine/state_machine.cpp index 990bf99..284e855 100644 --- a/src/tongji/state_machine/state_machine.cpp +++ b/src/tongji/state_machine/state_machine.cpp @@ -2,37 +2,39 @@ #include +#include "../identifier/tracker.hpp" #include "enum/car_id.hpp" -#include "interfaces/target_predictor.hpp" -#include "tongji/predictor/live_target_manager/live_target_manager.hpp" namespace world_exe::tongji::state_machine { class StateMachine::Impl { public: - Impl(std::shared_ptr live_target_manager) - : live_target_manager_(std::move(live_target_manager)) { } + Impl() + : tracker_(std::make_unique()) + , target_id_(enumeration::CarIDFlag::None) { } - const enumeration::CarIDFlag& GetAllowdToFires() const { return target_ids_; } + const enumeration::CarIDFlag& GetAllowdToFires() const { return target_id_; } - void Update() { - auto live_target_manager = - std::dynamic_pointer_cast(live_target_manager_); - target_ids_ = live_target_manager->GetAllowedTargetID(); + void Update(std::shared_ptr armors_in_image, + const std::chrono::milliseconds& duration_from_last_update) { + target_id_ = tracker_->SelectTrackingTargetID(armors_in_image, duration_from_last_update); } private: - std::shared_ptr live_target_manager_; - enumeration::CarIDFlag target_ids_; + std::unique_ptr tracker_; + enumeration::CarIDFlag target_id_; }; -StateMachine::StateMachine( - std::shared_ptr live_target_manager) - : pimpl_(std::make_unique(live_target_manager)) { } -StateMachine::~StateMachine() = default; +StateMachine::StateMachine() + : pimpl_(std::make_unique()) { } +StateMachine::~StateMachine() { }; const enumeration::CarIDFlag& StateMachine::GetAllowdToFires() const { return pimpl_->GetAllowdToFires(); } +void StateMachine::Update(std::shared_ptr armors_in_image, + const std::chrono::milliseconds& duration_from_last_update) { + return pimpl_->Update(armors_in_image, duration_from_last_update); +} } diff --git a/src/tongji/state_machine/state_machine.hpp b/src/tongji/state_machine/state_machine.hpp index c3417e3..14747b5 100644 --- a/src/tongji/state_machine/state_machine.hpp +++ b/src/tongji/state_machine/state_machine.hpp @@ -4,8 +4,8 @@ #include #include "enum/car_id.hpp" +#include "interfaces/armor_in_image.hpp" #include "interfaces/car_state.hpp" -#include "interfaces/target_predictor.hpp" namespace world_exe::tongji::state_machine { class StateMachine final : public interfaces::ICarState { @@ -14,10 +14,17 @@ class StateMachine final : public interfaces::ICarState { ~StateMachine(); const enumeration ::CarIDFlag& GetAllowdToFires() const override; - StateMachine(std::shared_ptr live_target_manager); + + void Update(std::shared_ptr armors_in_image, + const std::chrono::milliseconds& duration_from_last_update); + + StateMachine(const StateMachine&) = delete; + StateMachine& operator=(const StateMachine&) = delete; + StateMachine(StateMachine&&) noexcept = default; + StateMachine& operator=(StateMachine&&) noexcept = default; private: class Impl; std::unique_ptr pimpl_; }; -} \ No newline at end of file +} diff --git a/src/tongji/time_stamp/time_stamp.hpp b/src/tongji/time_stamp/time_stamp.hpp deleted file mode 100644 index aa93ae0..0000000 --- a/src/tongji/time_stamp/time_stamp.hpp +++ /dev/null @@ -1,21 +0,0 @@ -#pragma once - -#include "interfaces/time_stamped.hpp" - -namespace world_exe::tongji::time_stamp { -class TimeStamp : public interfaces::ITimeStamped { -public: - TimeStamp(const std::time_t& time_stamp) - : time_stamp_(time_stamp) { } - - void SetTimeStamp(const std::time_t& time_stamp) { time_stamp_ = time_stamp; } - - double SecondsSince(const TimeStamp& other) const { - return std::difftime(time_stamp_, other.time_stamp_); - } - const std::time_t& GetTimeStamp() const override { return time_stamp_; }; - -private: - std::time_t time_stamp_; -}; -} \ No newline at end of file diff --git a/src/util/coordinate.hpp b/src/util/coordinate.hpp index a0c6e96..4f632e0 100644 --- a/src/util/coordinate.hpp +++ b/src/util/coordinate.hpp @@ -16,10 +16,6 @@ static inline Eigen::Matrix3d opencv2ros_rotation(const Eigen::Matrix3d& rotatio } // clangd-format on -static inline Eigen::Quaterniond opencv2ros_orientation(const Eigen::Quaterniond& quat) { - return Eigen::Quaterniond(opencv2ros_rotation(quat.toRotationMatrix())); -} - static inline Eigen::Vector3d ros2opencv_position(const Eigen::Vector3d& position) { return Eigen::Vector3d(-position.y(), -position.z(), position.x()); } @@ -32,8 +28,4 @@ static inline Eigen::Matrix3d ros2opencv_rotation(const Eigen::Matrix3d& rotatio return t.transpose() * rotation * t; } -static inline Eigen::Quaterniond ros2opencv_orientation(const Eigen::Quaterniond& quat) { - return Eigen::Quaterniond(ros2opencv_rotation(quat.toRotationMatrix())); -} - } \ No newline at end of file diff --git a/src/v1/auto_aim_system_v1.cpp b/src/v1/auto_aim_system_v1.cpp index 7a2849b..818b989 100644 --- a/src/v1/auto_aim_system_v1.cpp +++ b/src/v1/auto_aim_system_v1.cpp @@ -7,8 +7,7 @@ #include "identifier/identifier.hpp" #include "interfaces/armor_in_camera.hpp" #include "interfaces/armor_in_image.hpp" -#include "interfaces/predictor_update_package.hpp" -#include "interfaces/time_stamped.hpp" +#include "data/predictor_update_package.hpp" #include "parameters/params_system_v1.hpp" #include "parameters/profile.hpp" #include "parameters/rm_parameters.hpp" @@ -33,24 +32,6 @@ using namespace v1; using namespace parameters; using namespace std::chrono; -class Combined final : public interfaces::IPreDictorUpdatePackage, interfaces::ITimeStamped{ -public: - virtual const std::time_t& GetTimeStamp() const{ return data1_.camera_capture_begin_time_stamp; }; - const world_exe::interfaces::ITimeStamped& GetTimeStamped() const {return *this;} - std::shared_ptr GetArmors() const{return data2_;}; - Eigen::Affine3d GetTransform() const {return data1_.camera_to_gimbal;}; - - // but why? - Combined(const data::CameraGimbalMuzzleSyncData& data1, std::shared_ptr data2) - : data1_(data1) - , data2_(data2){ - } - Combined() = delete; - ~Combined() = default; -private: - const data::CameraGimbalMuzzleSyncData& data1_; - const std::shared_ptr data2_; -}; class world_exe::v1::SystemV1::Impl{ public: @@ -94,18 +75,19 @@ class world_exe::v1::SystemV1::Impl{ const auto& solved = armor_pnp->SolvePnp(armors); - const auto& [pack, check] = sync->get_data(solved->GetTimeStamped().GetTimeStamp()); + const auto& [pack, check] = sync->get_data(solved->GetTimeStamp()); if(!check) [[unlikely]] return; time_point_ = std::chrono::steady_clock::now(); state_machine ->Update(flag); const auto& fire_targets = state_machine->GetAllowdToFires(); - auto combined = std::make_shared(pack, solved); + auto combined = std::make_shared(pack, solved); predictor ->Update(combined); - const auto& time = combined->GetTimeStamped().GetTimeStamp(); + const auto& time = combined->GetTimeStamp(); const auto& armor3d = predictor->Predict(fire_targets,time); fire_control ->set_armor(armor3d); + fire_control ->SetPredictor(predictor->GetPredictor(fire_targets)); core::EventBus::Publish(ParamsForSystemV1::fire_control_event, control()); @@ -124,7 +106,7 @@ class world_exe::v1::SystemV1::Impl{ core::EventBus::Publish>( parameters::ParamsForSystemV1::armors_in_camera_pnp_event, solved); - core::EventBus::Publish>( + core::EventBus::Publish>( parameters::ParamsForSystemV1::tracker_update_event, combined); core::EventBus::Publish( @@ -137,7 +119,7 @@ class world_exe::v1::SystemV1::Impl{ } data::FireControl control(){ - return fire_control->CalculateTarget((std::chrono::steady_clock::now() - time_point_).count()); + return fire_control->CalculateTarget(std::chrono::duration_cast(std::chrono::steady_clock::now() - time_point_)); } Impl(const Impl&) = delete; diff --git a/src/v1/fire_controller/fire_controller.cpp b/src/v1/fire_controller/fire_controller.cpp index ba9f866..de57210 100644 --- a/src/v1/fire_controller/fire_controller.cpp +++ b/src/v1/fire_controller/fire_controller.cpp @@ -1,10 +1,11 @@ #include "./fire_controller.hpp" +#include "data/time_stamped.hpp" #include "enum/enum_tools.hpp" #include "interfaces/armor_in_gimbal_control.hpp" -#include "interfaces/car_state.hpp" #include "interfaces/predictor.hpp" #include "trajectory.hpp" +#include #include #include @@ -45,8 +46,8 @@ class world_exe::v1::fire_control::TracingFireControl::Impl { predictor_ = predictor; }; - const world_exe::data::FireControl CalculateTarget(const std::time_t& time_duration) { - time_t fly_time = 0; + const world_exe::data::FireControl CalculateTarget(const std::chrono::seconds& time_duration) { + std::chrono::seconds fly_time{0}; const auto& pre1 = predictor_->Predictor(fly_time + time_duration + control_delay_); const auto& pre2 = pre1->GetArmors(predictor_->GetId()); double min_angular_dis = 1e9; @@ -72,13 +73,13 @@ class world_exe::v1::fire_control::TracingFireControl::Impl { trajectory_solver::gravity_only(armors[index].position, velocity_begin_, gravity_); } - return { .time_stamp = fly_time + time_duration + control_delay_, .fire_allowance = true }; + return { .time_stamp = data::TimeStamp{std::chrono::nanoseconds(fly_time + time_duration + control_delay_)}, .fire_allowance = true }; } private: world_exe::enumeration::CarIDFlag tracing_ = enumeration::CarIDFlag::None; time_t time_predict_point_; - const time_t control_delay_; + const std::chrono::seconds control_delay_; const double velocity_begin_; const double gravity_; const world_exe::data::FireControl no_allow_ { .fire_allowance = false }; @@ -88,7 +89,7 @@ class world_exe::v1::fire_control::TracingFireControl::Impl { const world_exe::data::FireControl // world_exe::v1::fire_control::TracingFireControl::CalculateTarget( - const std::time_t& time_duration) const { + const std::chrono::seconds& time_duration) const { return pimpl_->CalculateTarget(time_duration); } diff --git a/src/v1/fire_controller/fire_controller.hpp b/src/v1/fire_controller/fire_controller.hpp index d1e2910..171c0e6 100644 --- a/src/v1/fire_controller/fire_controller.hpp +++ b/src/v1/fire_controller/fire_controller.hpp @@ -1,14 +1,14 @@ #pragma once #include "data/fire_control.hpp" -#include "interfaces/car_state.hpp" #include "interfaces/fire_controller.hpp" #include "interfaces/predictor.hpp" +#include #include namespace world_exe::v1::fire_control { class TracingFireControl final : public interfaces::IFireControl { public: - const data::FireControl CalculateTarget(const std::time_t& time_duration) const override; + const data::FireControl CalculateTarget(const std::chrono::seconds& time_duration) const override; const enumeration::CarIDFlag GetAttackCarId() const override; void set_armor(const std::shared_ptr& armors); void SetPredictor(const std::shared_ptr& predictor); diff --git a/src/v1/identifier/identifier.cpp b/src/v1/identifier/identifier.cpp index be64c60..c4f615a 100644 --- a/src/v1/identifier/identifier.cpp +++ b/src/v1/identifier/identifier.cpp @@ -16,6 +16,7 @@ #include "openvino/core/preprocess/pre_post_process.hpp" #include "openvino/runtime/core.hpp" +#include #include namespace world_exe::v1::identifier { @@ -74,10 +75,13 @@ class Identifier::Impl { */ std::tuple, enumeration::CarIDFlag> Identify( const cv::Mat& input_image) { + // 首先使用深度学习模型进行装甲板检测得到roi区域 const auto armor_infos = model_infer(input_image); // 然后进行灯条匹配验证 - return matchPlate(input_image, armor_infos); + auto [a, b] = matchPlate(input_image, armor_infos); + a->time_stamp_ = std::chrono::steady_clock::now().time_since_epoch(); + return {a, b}; } /** @@ -228,7 +232,7 @@ class Identifier::Impl { , angle_(angle) { } }; - std::tuple, enumeration::CarIDFlag> matchPlate( + std::tuple, enumeration::CarIDFlag> matchPlate( const cv::Mat& img, const std::vector& armor_infos) { if (armor_infos.empty()) return {}; diff --git a/src/v1/identifier/identifier_armor.hpp b/src/v1/identifier/identifier_armor.hpp index d6a6877..310adf5 100644 --- a/src/v1/identifier/identifier_armor.hpp +++ b/src/v1/identifier/identifier_armor.hpp @@ -1,21 +1,21 @@ #pragma once #include "interfaces/armor_in_image.hpp" -#include "interfaces/time_stamped.hpp" +#include "data/time_stamped.hpp" #include "util/index.hpp" +#include namespace world_exe::v1::identifier { -class IdentifierArmor : public interfaces::IArmorInImage, public interfaces::ITimeStamped { +class IdentifierArmor final : public interfaces::IArmorInImage { public: IdentifierArmor() = default; - IdentifierArmor(const std::vector& armors) { + IdentifierArmor(const std::vector& armors) + { for (const auto armor : armors) armors_[util::enumeration::GetIndex(armor.id)].emplace_back(armor); } - const interfaces::ITimeStamped& GetTimeStamped() const override { return *this; } - - const std::time_t& GetTimeStamp() const override { return time_stamp_; }; + const data::TimeStamp& GetTimeStamp() const override { return time_stamp_; } void SetArmors(const std::vector& armors) { for (auto& armors : armors_) @@ -29,8 +29,9 @@ class IdentifierArmor : public interfaces::IArmorInImage, public interfaces::ITi return armors_[util::enumeration::GetIndex(armor_id)]; } + data::TimeStamp time_stamp_ {}; + private: - std::time_t time_stamp_ { 0 }; std::array, 8> armors_; }; } \ No newline at end of file diff --git a/src/v1/pnpsolver/armor_pnp_solver.cpp b/src/v1/pnpsolver/armor_pnp_solver.cpp index 7f2185c..3f82652 100644 --- a/src/v1/pnpsolver/armor_pnp_solver.cpp +++ b/src/v1/pnpsolver/armor_pnp_solver.cpp @@ -5,7 +5,7 @@ #include "armor_pnp_solver.hpp" #include "interfaces/armor_in_camera.hpp" -#include "interfaces/time_stamped.hpp" +#include "data/time_stamped.hpp" #include "parameters/profile.hpp" #include "util/index.hpp" @@ -55,17 +55,15 @@ class ArmorIPPEPnPSolver::Impl { const std::vector& NormalArmorObjectPoints_; }; -class ArmorInCamera final : public world_exe::interfaces::IArmorInCamera, - world_exe::interfaces::ITimeStamped { +class ArmorInCamera final : public world_exe::interfaces::IArmorInCamera { public: - const world_exe::interfaces::ITimeStamped& GetTimeStamped() const override { return *this; } + const world_exe::data::TimeStamp& GetTimeStamp() const override { return time_stampe; } const std::vector& GetArmors( const world_exe::enumeration::ArmorIdFlag& armor_id) const override { return armors[world_exe::util::enumeration::GetIndex(armor_id)]; } - const std::time_t& GetTimeStamp() const override { return time_stampe; } - std::time_t time_stampe = 0; + world_exe::data::TimeStamp time_stampe{}; std::vector armors[static_cast(world_exe::enumeration::ArmorIdFlag::Count)]; @@ -86,8 +84,6 @@ std::shared_ptr ArmorIPPEPnPSolver::Solve return armors_; } -const std::time_t& ArmorIPPEPnPSolver::GetTimeStamp() const { return time_point_; } - ArmorIPPEPnPSolver::ArmorIPPEPnPSolver(const std::vector& LargeArmorObjectPoints, const std::vector& NormalArmorObjectPoints) : pimpl_(std::make_unique(LargeArmorObjectPoints, NormalArmorObjectPoints)) { } diff --git a/src/v1/pnpsolver/armor_pnp_solver.hpp b/src/v1/pnpsolver/armor_pnp_solver.hpp index 9f0684f..c9d5d87 100644 --- a/src/v1/pnpsolver/armor_pnp_solver.hpp +++ b/src/v1/pnpsolver/armor_pnp_solver.hpp @@ -1,4 +1,5 @@ #pragma once +#include #include #include @@ -7,20 +8,19 @@ namespace world_exe::v1::pnpsolver { -class ArmorIPPEPnPSolver final : public interfaces::IPnpSolver, public interfaces::ITimeStamped { +class ArmorIPPEPnPSolver final : public interfaces::IPnpSolver { public: ArmorIPPEPnPSolver(const std::vector& LargeArmorObjectPointsOpencv, const std::vector& NormalArmorObjectPointsOpencv); ~ArmorIPPEPnPSolver(); - void set_time_point(const std::time_t& time_point); + void set_time_point(const std::chrono::nanoseconds& time_point); std::shared_ptr SolvePnp( std::shared_ptr armor) override; - const std::time_t& GetTimeStamp() const override; private: class Impl; std::unique_ptr pimpl_; - std::time_t time_point_; + std::chrono::nanoseconds time_point_; }; } \ No newline at end of file diff --git a/src/v1/predictor/car/car_predictor.cpp b/src/v1/predictor/car/car_predictor.cpp index cc67c1f..43c674f 100644 --- a/src/v1/predictor/car/car_predictor.cpp +++ b/src/v1/predictor/car/car_predictor.cpp @@ -1,26 +1,24 @@ #include "car_predictor.hpp" #include "../predict_armor_in_gimbal_control.hpp" -#include "interfaces/car_state.hpp" -#include "interfaces/time_stamped.hpp" -#include "v1/predictor/predict_time_stamp.hpp" +#include "data/time_stamped.hpp" #include namespace world_exe::v1::predictor { class CarPredictor::Impl { public: Impl(const enumeration::CarIDFlag& id, const CarPredictEkf& ekf, - const interfaces::ITimeStamped& create_time_stamp) + const data::TimeStamp& create_time_stamp) : id_(id) , ekf_(ekf) { - create_time_stamp_.SetTimeStamp(create_time_stamp.GetTimeStamp()); + create_time_stamp_ = create_time_stamp; } const enumeration::ArmorIdFlag& GetId() const { return id_; } - std::shared_ptr Predictor(const std::time_t& time_stamp) { + std::shared_ptr Predictor(const data::TimeStamp& time_stamp) { auto ptr = std::make_shared(); ptr->SetWithSingleId(ekf_.get_predict_output_armor( - id_, (time_stamp - create_time_stamp_.GetTimeStamp()) / 1.e9), + id_, (time_stamp - create_time_stamp_).to_seconds()), time_stamp); return ptr; } @@ -29,22 +27,22 @@ class CarPredictor::Impl { inline void SetEkf(const CarPredictEkf& ekf) { ekf_ = ekf; } - inline void SetTimeStamp(const interfaces::ITimeStamped& time_stamp) { - create_time_stamp_.SetTimeStamp(time_stamp.GetTimeStamp()); + inline void SetTimeStamp(const data::TimeStamp& time_stamp) { + create_time_stamp_ = time_stamp; } private: enumeration::CarIDFlag id_; CarPredictEkf ekf_; - PredictTimeStamp create_time_stamp_; + data::TimeStamp create_time_stamp_; }; CarPredictor::CarPredictor() : pimpl_(std::make_unique( - enumeration::CarIDFlag::None, CarPredictEkf {}, PredictTimeStamp {})) { } + enumeration::CarIDFlag::None, CarPredictEkf {}, data::TimeStamp {})) { } CarPredictor::CarPredictor(const enumeration::CarIDFlag& id, const CarPredictEkf& ekf, - const interfaces::ITimeStamped& create_time_stamp) + const data::TimeStamp& create_time_stamp) : pimpl_(std::make_unique(id, ekf, create_time_stamp)) { } CarPredictor::~CarPredictor() = default; @@ -52,7 +50,7 @@ CarPredictor::~CarPredictor() = default; const enumeration::ArmorIdFlag& CarPredictor::GetId() const { return pimpl_->GetId(); } std::shared_ptr CarPredictor::Predictor( - const std::time_t& time_stamp) const { + const data::TimeStamp& time_stamp) const { return pimpl_->Predictor(time_stamp); } @@ -60,7 +58,7 @@ void CarPredictor::SetId(const enumeration::CarIDFlag& id) { return pimpl_->SetI void CarPredictor::SetEkf(const CarPredictEkf& ekf) { return pimpl_->SetEkf(ekf); } -void CarPredictor::SetTimeStamp(const interfaces::ITimeStamped& time_stamp) { +void CarPredictor::SetTimeStamp(const data::TimeStamp& time_stamp) { return pimpl_->SetTimeStamp(time_stamp); }; } \ No newline at end of file diff --git a/src/v1/predictor/car/car_predictor.hpp b/src/v1/predictor/car/car_predictor.hpp index acebd0b..9289164 100644 --- a/src/v1/predictor/car/car_predictor.hpp +++ b/src/v1/predictor/car/car_predictor.hpp @@ -1,24 +1,25 @@ #pragma once #include "car_predictor_ekf.hpp" -#include "interfaces/car_state.hpp" +#include "data/time_stamped.hpp" +#include "enum/car_id.hpp" #include "interfaces/predictor.hpp" namespace world_exe::v1::predictor { -class CarPredictor : public interfaces::IPredictor { +class CarPredictor final: public interfaces::IPredictor { public: CarPredictor(); ~CarPredictor(); CarPredictor(const enumeration::CarIDFlag& id, const CarPredictEkf& ekf, - const interfaces::ITimeStamped& create_time_stamp); + const data::TimeStamp& create_time_stamp); void SetId(const enumeration::CarIDFlag& id); void SetEkf(const CarPredictEkf& ekf); - void SetTimeStamp(const interfaces::ITimeStamped& time_stamp); + void SetTimeStamp(const data::TimeStamp& time_stamp); const enumeration::ArmorIdFlag& GetId() const override; std::shared_ptr Predictor( - const std::time_t& time_stamp) const override; + const data::TimeStamp& time_stamp) const override; private: class Impl; diff --git a/src/v1/predictor/predict_armor_in_gimbal_control.cpp b/src/v1/predictor/predict_armor_in_gimbal_control.cpp index 99969ba..f8e7eb5 100644 --- a/src/v1/predictor/predict_armor_in_gimbal_control.cpp +++ b/src/v1/predictor/predict_armor_in_gimbal_control.cpp @@ -1,5 +1,6 @@ #include "predict_armor_in_gimbal_control.hpp" +#include "data/time_stamped.hpp" #include "enum/enum_tools.hpp" #include "util/index.hpp" #include @@ -9,18 +10,18 @@ class PredictArmorInGimbalControl::Impl { public: Impl() = default; Impl(const std::array, 8>& armors, - predictor::PredictTimeStamp predict_time_stamp) + data::TimeStamp predict_time_stamp) : armors_(armors) , predict_time_stamp_(std::move(predict_time_stamp)) { } void Set(const std::array, 8>& armors, - const predictor::PredictTimeStamp& predict_time_stamp) { + const data::TimeStamp& predict_time_stamp) { armors_ = armors; predict_time_stamp_ = predict_time_stamp; } void SetWithSingleId(const std::vector& armors, - const predictor::PredictTimeStamp& predict_time_stamp) { + const data::TimeStamp& predict_time_stamp) { if (armors.empty()) return; armors_[util::enumeration::GetIndex(armors[0].id)] = armors; } @@ -37,30 +38,30 @@ class PredictArmorInGimbalControl::Impl { return output_armors_; } - const interfaces::ITimeStamped& GetTimeStamped() const { return predict_time_stamp_; } + const data::TimeStamp& GetTimeStamped() const { return predict_time_stamp_; } private: std::array, 8> armors_; std::vector output_armors_; - predictor::PredictTimeStamp predict_time_stamp_; + data::TimeStamp predict_time_stamp_; }; PredictArmorInGimbalControl::PredictArmorInGimbalControl() : pimpl_(std::make_unique()) { } PredictArmorInGimbalControl::PredictArmorInGimbalControl( const std::array, 8>& armors, - const predictor::PredictTimeStamp& predict_time_stamp) + const data::TimeStamp& predict_time_stamp) : pimpl_(std::make_unique(armors, predict_time_stamp)) { } void PredictArmorInGimbalControl::Set( const std::array, 8>& armors, - const predictor::PredictTimeStamp& predict_time_stamp) { + const data::TimeStamp& predict_time_stamp) { return pimpl_->Set(armors, predict_time_stamp); } void PredictArmorInGimbalControl::SetWithSingleId( const std::vector& armors, - const predictor::PredictTimeStamp& predict_time_stamp) { + const data::TimeStamp& predict_time_stamp) { return pimpl_->SetWithSingleId(armors, predict_time_stamp); } @@ -69,7 +70,7 @@ const std::vector& PredictArmorInGimbalControl: return pimpl_->GetArmors(armor_id); } -const interfaces::ITimeStamped& PredictArmorInGimbalControl::GetTimeStamped() const { +const data::TimeStamp& PredictArmorInGimbalControl::GetTimeStamp() const { return pimpl_->GetTimeStamped(); } diff --git a/src/v1/predictor/predict_armor_in_gimbal_control.hpp b/src/v1/predictor/predict_armor_in_gimbal_control.hpp index 3139755..8cf945a 100644 --- a/src/v1/predictor/predict_armor_in_gimbal_control.hpp +++ b/src/v1/predictor/predict_armor_in_gimbal_control.hpp @@ -1,27 +1,27 @@ #pragma once +#include "data/time_stamped.hpp" #include "interfaces/armor_in_gimbal_control.hpp" -#include "predict_time_stamp.hpp" namespace world_exe::v1::predictor { -class PredictArmorInGimbalControl : public world_exe::interfaces::IArmorInGimbalControl { +class PredictArmorInGimbalControl final : public world_exe::interfaces::IArmorInGimbalControl { public: PredictArmorInGimbalControl(); PredictArmorInGimbalControl( const std::array, 8>& armors, - const predictor::PredictTimeStamp& predict_time_stamp); + const data::TimeStamp& predict_time_stamp); ~PredictArmorInGimbalControl(); void Set(const std::array, 8>& armors, - const predictor::PredictTimeStamp& predict_time_stamp); + const data::TimeStamp& predict_time_stamp); void SetWithSingleId(const std::vector& armors, - const predictor::PredictTimeStamp& predict_time_stamp); + const data::TimeStamp& predict_time_stamp); const std::vector& GetArmors( const enumeration::ArmorIdFlag& armor_id) const override; - const interfaces::ITimeStamped& GetTimeStamped() const override; + const data::TimeStamp& GetTimeStamp() const override; private: class Impl; diff --git a/src/v1/predictor/predict_time_stamp.hpp b/src/v1/predictor/predict_time_stamp.hpp deleted file mode 100644 index 03cb579..0000000 --- a/src/v1/predictor/predict_time_stamp.hpp +++ /dev/null @@ -1,19 +0,0 @@ -#pragma once - -#include "interfaces/time_stamped.hpp" - -namespace world_exe::v1::predictor { -class PredictTimeStamp : public interfaces::ITimeStamped { -public: - PredictTimeStamp() = default; - PredictTimeStamp(const std::time_t& time_stamp) - : time_stamp_(time_stamp) { } - - inline void SetTimeStamp(const time_t& time_stamp) { time_stamp_ = time_stamp; } - - const std::time_t& GetTimeStamp() const override { return time_stamp_; }; - -private: - std::time_t time_stamp_; -}; -} \ No newline at end of file diff --git a/src/v1/predictor/predictor_manager.cpp b/src/v1/predictor/predictor_manager.cpp index e964dd2..f4f2b65 100644 --- a/src/v1/predictor/predictor_manager.cpp +++ b/src/v1/predictor/predictor_manager.cpp @@ -1,9 +1,9 @@ #include "predictor_manager.hpp" #include "car/car_predictor.hpp" #include "car/car_predictor_ekf.hpp" +#include "data/time_stamped.hpp" #include "enum/enum_tools.hpp" #include "predict_armor_in_gimbal_control.hpp" -#include "predict_time_stamp.hpp" #include "util/index.hpp" #include @@ -11,11 +11,11 @@ namespace world_exe::v1::predictor { class PredictorManager::Impl { public: - inline void Update(const std::shared_ptr& data) { - const auto time_stamp = data->GetTimeStamped().GetTimeStamp(); - const auto dt = (time_stamp - last_update_time_stamp_.GetTimeStamp()); + inline void Update(const std::shared_ptr& data) { + const auto time_stamp = data->GetTimeStamp(); + const auto dt = (time_stamp - last_update_time_stamp_); - const auto transform = data->GetTransform(); + const auto transform = data->GetCameraToWorld(); const auto rotation_transform = Eigen::Quaterniond { transform.linear() }; for (int i = 0; i < 8; i++) { @@ -31,7 +31,7 @@ class PredictorManager::Impl { std::atan(tmp_armor.position.y() / tmp_armor.position.x()), -std::atan(tmp_armor.position.z() / tmp_armor.position.x()), tmp_armor.position.norm(); - predictors_[i].Update(input, {}, dt); + predictors_[i].Update(input, {}, dt.to_seconds()); } else if (armors.size() == 2) { // 当同时识别到两块装甲板时，优先更新近的那块，再更新远的 const auto armor0_yaw = util::math::get_yaw_from_quaternion(armors[0].orientation); @@ -47,7 +47,7 @@ class PredictorManager::Impl { std::atan(tmp_armor0.position.y() / tmp_armor0.position.x()), -std::atan(tmp_armor0.position.z() / tmp_armor0.position.x()), tmp_armor0.position.norm(); - predictors_[i].Update(input, {}, dt); + predictors_[i].Update(input, {}, dt.to_seconds()); // 同时识别到一辆车的两块装甲板时要调这个函数 predictors_[i].set_second_armor(); input << util::math::get_yaw_from_quaternion(tmp_armor1.orientation), @@ -60,7 +60,7 @@ class PredictorManager::Impl { std::atan(tmp_armor1.position.y() / tmp_armor1.position.x()), -std::atan(tmp_armor1.position.z() / tmp_armor1.position.x()), tmp_armor1.position.norm(); - predictors_[i].Update(input, {}, dt); + predictors_[i].Update(input, {}, dt.to_seconds()); // 同时识别到一辆车的两块装甲板时要调这个函数 predictors_[i].set_second_armor(); input << util::math::get_yaw_from_quaternion(tmp_armor0.orientation), @@ -72,12 +72,12 @@ class PredictorManager::Impl { } } - last_update_time_stamp_.SetTimeStamp(time_stamp); + last_update_time_stamp_ = time_stamp; } inline std::shared_ptr Predict( - const world_exe::enumeration::ArmorIdFlag& id, const std::time_t& time_stamp) { - const auto dt = (time_stamp - last_update_time_stamp_.GetTimeStamp()) / 1.e9; + const world_exe::enumeration::ArmorIdFlag& id, const data::TimeStamp& time_stamp) { + const auto dt = (time_stamp - last_update_time_stamp_).to_seconds(); uint32_t id_index = static_cast(enumeration::ArmorIdFlag::Hero); std::array, 8> armors; @@ -99,7 +99,7 @@ class PredictorManager::Impl { } private: - PredictTimeStamp last_update_time_stamp_ { 0 }; + data::TimeStamp last_update_time_stamp_ {}; std::array predictors_; Eigen::Affine3d transform_from_camera_to_gimbal_; @@ -112,11 +112,11 @@ PredictorManager::PredictorManager() PredictorManager::~PredictorManager() = default; std::shared_ptr PredictorManager::Predict( - const world_exe::enumeration::ArmorIdFlag& id, const std::time_t& time_stamp) { + const world_exe::enumeration::ArmorIdFlag& id, const data::TimeStamp& time_stamp) { return pimpl_->Predict(id, time_stamp); }; -void PredictorManager::Update(std::shared_ptr data) { +void PredictorManager::Update(std::shared_ptr data) { return pimpl_->Update(data); }; diff --git a/src/v1/predictor/predictor_manager.hpp b/src/v1/predictor/predictor_manager.hpp index 77b53f6..38bbb8f 100644 --- a/src/v1/predictor/predictor_manager.hpp +++ b/src/v1/predictor/predictor_manager.hpp @@ -1,6 +1,7 @@ #pragma once -#include "interfaces/predictor_update_package.hpp" +#include "data/time_stamped.hpp" +#include "data/predictor_update_package.hpp" #include "interfaces/target_predictor.hpp" namespace world_exe::v1::predictor { @@ -9,10 +10,10 @@ class PredictorManager final : public world_exe::interfaces::ITargetPredictor { PredictorManager(); ~PredictorManager(); - void Update(std::shared_ptr data); + void Update(std::shared_ptr data); virtual std::shared_ptr Predict( - const enumeration::ArmorIdFlag& id, const std::time_t& time_stamp); + const enumeration::ArmorIdFlag& id, const data::TimeStamp& time_stamp); std::shared_ptr GetPredictor(const enumeration::ArmorIdFlag& id) const; diff --git a/src/v1/sync/syncer.cpp b/src/v1/sync/syncer.cpp index 2197595..1277403 100644 --- a/src/v1/sync/syncer.cpp +++ b/src/v1/sync/syncer.cpp @@ -1,6 +1,7 @@ #include "v1/sync/syncer.hpp" #include "data/sync_data.hpp" +#include "data/time_stamped.hpp" #include #include #include @@ -14,29 +15,28 @@ struct Syncer::Impl { :time_to_hold_(time_to_hold) ,tolerable_ns_(tolerable_ns) {} - void set_data(const data::CameraGimbalMuzzleSyncData& armor_pnp) { + void set_data(const data::CameraGimbalMuzzleSyncData& data) { - data_queue_.emplace_back(armor_pnp); + data_queue_.emplace_back(data); - long now_time = - armor_pnp.camera_capture_begin_time_stamp - -std::chrono::duration_cast(time_to_hold_).count(); + auto now_time = + data.camera_capture_begin_time_stamp + - data::TimeStamp{time_to_hold_}; while(!data_queue_.empty() && data_queue_.front().camera_capture_begin_time_stamp < now_time) data_queue_.pop_front(); } - std::tuple get_data(const time_t timestamp) { - + std::tuple get_data(const data::TimeStamp& timestamp) { if (data_queue_.empty()) return {data::CameraGimbalMuzzleSyncData{}, false}; for(auto rit = data_queue_.rbegin(); rit != data_queue_.rend(); ++rit) - if (rit->camera_capture_begin_time_stamp <= timestamp) { - return {*rit, true}; - } + if (rit->camera_capture_begin_time_stamp <= timestamp) + return {*rit, true}; + return {data::CameraGimbalMuzzleSyncData{},false}; } @@ -58,7 +58,7 @@ Syncer::~Syncer() = default; void Syncer::set_data(const data::CameraGimbalMuzzleSyncData& armor_pnp) { pimpl_->set_data(armor_pnp); } -std::tuple Syncer::get_data(time_t timestamp) { +std::tuple Syncer::get_data(const data::TimeStamp& timestamp) { return pimpl_->get_data(timestamp); } diff --git a/src/v1/sync/syncer.hpp b/src/v1/sync/syncer.hpp index 330716e..189d4cb 100644 --- a/src/v1/sync/syncer.hpp +++ b/src/v1/sync/syncer.hpp @@ -1,7 +1,7 @@ #pragma once #include "data/sync_data.hpp" -#include "interfaces/predictor_update_package.hpp" +#include "data/predictor_update_package.hpp" #include #include #include @@ -15,11 +15,11 @@ class Syncer final : interfaces::ISyncBlock { void set_data(const data::CameraGimbalMuzzleSyncData& camera_data); - std::tuple get_data(time_t timestamp); + std::tuple get_data(const data::TimeStamp& timestamp); private: struct Impl; std::unique_ptr pimpl_; - std::shared_ptr last_; + std::shared_ptr last_; }; } \ No newline at end of file diff --git a/src/v1/workflow.puml b/src/v1/workflow.puml index fc67d42..5a95d64 100644 --- a/src/v1/workflow.puml +++ b/src/v1/workflow.puml @@ -46,10 +46,10 @@ car_tracing_event --> FireControl : enumeration::CarIDFlag car_tracing_event --> Tracker : enumeration::CarIDFlag Tracker --> tracker_current_armors_event : interfaces::IArmorInGimbalControl armors_in_camera_pnp_event --> SyncComponent : interfaces::IArmorInCamera -SyncComponent --> tracker_update_event : std::shared_ptr +SyncComponent --> tracker_update_event : std::shared_ptr camera_capture_transforms --> SyncComponent : data::CameraGimbalMuzzleSyncData -tracker_update_event --> FireControl : std::shared_ptr -tracker_update_event --> Tracker : std::shared_ptr +tracker_update_event --> FireControl : std::shared_ptr +tracker_update_event --> Tracker : std::shared_ptr tracker_current_armors_event --> FireControl : interfaces::IArmorInGimbalControl FireControl --> get_lastest_predictor_event : enumeration::CarIDFlag get_lastest_predictor_event --> Tracker : enumeration::CarIDFlag diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index f2b9563..004859a 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -15,7 +15,7 @@ target_compile_features(test_main PRIVATE cxx_std_20) include(FetchContent) FetchContent_Declare( googletest - URL https://github.com/google/googletest/archive/refs/heads/main.zip + SOURCE_DIR ${CMAKE_SOURCE_DIR}/thirdparty/googletest ) set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) diff --git a/tests/Pnpsolver.cpp b/tests/Pnpsolver.cpp index 2ebe8b9..74fb8d8 100644 --- a/tests/Pnpsolver.cpp +++ b/tests/Pnpsolver.cpp @@ -2,8 +2,6 @@ #include #include "mocks/MockArmor2D.hpp" #include "interfaces/pnp_solver.hpp" -#include "parameters/rm_parameters.hpp" -#include "mocks/MockHikCameraProfile.hpp" using world_exe::interfaces::IPnpSolver; using world_exe::tests::mock::MockArmorInImage; diff --git a/tests/mocks/MockArmor2D.hpp b/tests/mocks/MockArmor2D.hpp index 2cd1d63..87a8cb1 100644 --- a/tests/mocks/MockArmor2D.hpp +++ b/tests/mocks/MockArmor2D.hpp @@ -1,6 +1,6 @@ #pragma once #include "interfaces/armor_in_image.hpp" -#include "interfaces/time_stamped.hpp" +#include "data/time_stamped.hpp" #include #include #include @@ -8,10 +8,10 @@ #include namespace world_exe::tests::mock{ - class MockArmorInImage :public world_exe::interfaces::IArmorInImage,public world_exe::interfaces::ITimeStamped{ + class MockArmorInImage :public world_exe::interfaces::IArmorInImage{ public: std::vector armors; - std::time_t time_stamp = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); + data::TimeStamp time_stamp = data::TimeStamp(std::chrono::system_clock::now().time_since_epoch()); MockArmorInImage(std::vector armors):armors(armors){} ~MockArmorInImage() = default; @@ -20,10 +20,9 @@ namespace world_exe::tests::mock{ { return armors; } - const world_exe::interfaces::ITimeStamped& GetTimeStamped() const override { - return *this; + const world_exe::data::TimeStamp& GetTimeStamp() const override { + return time_stamp; } - const std::time_t& GetTimeStamp() const override { return time_stamp; } //MockArmorInImage工厂函数 static std::shared_ptr createMockArmorInImage(){ //Mock装甲板生成 diff --git a/tests/sync_test.cpp b/tests/sync_test.cpp index 7e3652f..a893976 100644 --- a/tests/sync_test.cpp +++ b/tests/sync_test.cpp @@ -1,5 +1,6 @@ #include "data/sync_data.hpp" #include "v1/sync/syncer.hpp" +#include #include #include #include @@ -14,8 +15,8 @@ using ns_t = int64_t; // 纳秒 struct FrameB { data::CameraGimbalMuzzleSyncData data; // 纳秒 - FrameB(ns_t t) : data(t) { - data.camera_capture_begin_time_stamp = t; + FrameB(ns_t t) { + data.camera_capture_begin_time_stamp = data::TimeStamp::from_nanosec(t); } }; @@ -37,7 +38,7 @@ void sync_test_main() { const ns_t period_ns = static_cast(NS_PER_SEC / frequency + 0.5); const double total_seconds = 2.0; const ns_t total_ns = static_cast(total_seconds * NS_PER_SEC + 0.5); - const ns_t min_margin_to_next_b = 500'000; // 0.0005s + const data::TimeStamp min_margin_to_next_b = data::TimeStamp::from_nanosec(500'000); // 0.0005s const unsigned seed = 12345; std::mt19937 rng(seed); @@ -49,8 +50,8 @@ void sync_test_main() { std::vector generatedAs; // 生成 B 时间点（纳秒） - std::vector bTimes; - for (ns_t t = 0; t <= total_ns + period_ns; t += period_ns) bTimes.push_back(t); + std::vector bTimes; + for (ns_t t = 0; t <= total_ns + period_ns; t += period_ns) bTimes.push_back(data::TimeStamp::from_nanosec(t)); // 第一个 B data::CameraGimbalMuzzleSyncData firstB{}; @@ -58,26 +59,28 @@ void sync_test_main() { generatedBs.push_back(firstB); syncer.set_data(firstB); for (size_t i = 0; i + 1 < bTimes.size(); ++i) { - ns_t tb = bTimes[i]; - ns_t tbNext = bTimes[i + 1]; + auto tb = bTimes[i]; + auto tbNext = bTimes[i + 1]; data::CameraGimbalMuzzleSyncData b{}; b.camera_capture_begin_time_stamp = tbNext; + ASSERT_EQ(b.camera_capture_begin_time_stamp, tbNext) << "tbNext should be equal to camera_capture_begin_time_stamp"; generatedBs.push_back(b); syncer.set_data(b); int aCountThisInterval = aCountDist(rng); for (int k = 0; k < aCountThisInterval; ++k) { - ns_t windowStart = tb + 1; - ns_t windowEnd = std::max(windowStart, tbNext - min_margin_to_next_b); + auto windowStart = tb + data::TimeStamp::from_nanosec(1); + auto windowEnd = std::max(windowStart, tbNext - min_margin_to_next_b); if (windowEnd <= windowStart) continue; double u = unitDist(rng); - ns_t tA = windowStart + static_cast((windowEnd - windowStart) * u + 0.5); + data::TimeStamp tA = windowStart + ((windowEnd - windowStart) * u); data::CameraGimbalMuzzleSyncData a{}; a.camera_capture_begin_time_stamp = tA; generatedAs.push_back(a); + ASSERT_LT(tb, tA) << "ta should be larger then tb"; auto [matched, ok] = syncer.get_data(tA); ASSERT_TRUE(ok) << "get_data should succeed for A after at least one B"; ASSERT_LE(matched.camera_capture_begin_time_stamp, tA) << "matched B timestamp <= A timestamp"; @@ -91,7 +94,7 @@ void sync_test_main() { if (matchedIndex + 1 < generatedBs.size()) { const auto& nextB = generatedBs[matchedIndex + 1]; ASSERT_LT(tA, nextB.camera_capture_begin_time_stamp) << "A must occur before next B"; - ns_t gapToNextB = nextB.camera_capture_begin_time_stamp - tA; + auto gapToNextB = nextB.camera_capture_begin_time_stamp - tA; ASSERT_GE(gapToNextB, min_margin_to_next_b) << "A too close to next B"; } } @@ -105,14 +108,11 @@ void sync_test_main() { size_t limit = std::min(generatedAs.size(), 10); for (size_t i = 0; i < limit; ++i) { const auto& a = generatedAs[i]; - time_t tA_seconds = static_cast( - std::chrono::duration_cast(std::chrono::nanoseconds(a.camera_capture_begin_time_stamp)).count() - ); - auto [matched, ok] = syncer.get_data(tA_seconds); + auto [matched, ok] = syncer.get_data(a.camera_capture_begin_time_stamp); std::cout << "A#" << i - << " t(ns)=" << a.camera_capture_begin_time_stamp << " t(s)=" << ns_to_sec(a.camera_capture_begin_time_stamp) - << " -> matched B t(ns)=" << matched.camera_capture_begin_time_stamp - << " t(s)=" << ns_to_sec(matched.camera_capture_begin_time_stamp) << "\n"; + << " t(ns)=" << a.camera_capture_begin_time_stamp.to_nanosec() << " t(s)=" << a.camera_capture_begin_time_stamp.to_seconds() + << " -> matched B t(ns)=" << matched.camera_capture_begin_time_stamp.to_nanosec() + << " t(s)=" << matched.camera_capture_begin_time_stamp.to_seconds() << "\n"; } std::cout << "All assertions passed.\n"; diff --git a/tests/test_main.cpp b/tests/test_main.cpp index 4d4d345..74996f0 100644 --- a/tests/test_main.cpp +++ b/tests/test_main.cpp @@ -1,4 +1,3 @@ -#include "Pnpsolver.cpp" #include "v1/pnpsolver/armor_pnp_solver.hpp" #include #include @@ -6,6 +5,8 @@ #include #include #include +#include +#include "Pnpsolver.cpp" #include "sync_test.cpp" using namespace world_exe::v1::pnpsolver; @@ -14,6 +15,7 @@ using world_exe::parameters::Robomaster; using ::testing::Return; using ::testing::_; + TEST_P(PnpsolverTest,AbilityTest) { ArmorIPPEPnPSolver pnp_solver_test_v1(Robomaster::NormalArmorObjectPointsOpencv,Robomaster::LargeArmorObjectPointsOpencv);