diff --git a/src/tongji/auto_aim_system.cpp b/src/tongji/auto_aim_system.cpp index 3c1986a..48ecfc0 100644 --- a/src/tongji/auto_aim_system.cpp +++ b/src/tongji/auto_aim_system.cpp @@ -1,19 +1,21 @@ #include "auto_aim_system.hpp" #include + #include #include -#include #include #include #include -#include +#include #include "../v1/sync/syncer.hpp" #include "core/event_bus.hpp" #include "data/mat_stamped.hpp" #include "data/predictor_update_package.hpp" +#include "data/sync_data.hpp" #include "data/time_stamped.hpp" +#include "enum/car_id.hpp" #include "parameters/params_system_v1.hpp" #include "parameters/profile.hpp" #include "parameters/rm_parameters.hpp" @@ -23,7 +25,6 @@ #include "tongji/solver/solver.hpp" #include "tongji/state_machine/state_machine.hpp" #include "utils/fps_counter.hpp" -#include "utils/visualization.hpp" #include "v1/identifier/identifier.hpp" namespace world_exe::tongji { @@ -73,8 +74,6 @@ class AutoAimSystem::Impl { // if (fps_.count()) std::cout << fps_.fps() << std::endl; // return; - // std::cout << "here" << std::endl; - if (flag == enumeration::ArmorIdFlag::None) return; // std::cout << "here" << std::endl; state_machine_->Update(armors_in_image, @@ -167,6 +166,7 @@ class AutoAimSystem::Impl { std::shared_ptr live_target_manager_; std::unique_ptr syncer_; std::unique_ptr fire_controller_; + // std::unique_ptr mock_camera_tranform_; }; AutoAimSystem::AutoAimSystem(const bool& debug) diff --git a/src/tongji/predictor/kalman_filter/extended_kalman_filter.hpp b/src/tongji/predictor/kalman_filter/extended_kalman_filter.hpp index b595114..bd95c1c 100644 --- a/src/tongji/predictor/kalman_filter/extended_kalman_filter.hpp +++ b/src/tongji/predictor/kalman_filter/extended_kalman_filter.hpp @@ -80,9 +80,9 @@ template class ExtendedKalmanFilter { // Stable Compution of the Posterior Covariance // https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/07-Kalman-Filter-Math.ipynb - // FIXME: P -> P^- - P << (I - K * H) * P_prior; + // FIXME: P -> P^- // P << (I - K * H) * P_prior * (I - K * H).transpose() + K * R * K.transpose(); + P << (I - K * H) * P_prior; const auto P_inverse = P_prior.inverse(); const auto error = x - x_prior; diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index eb1aed9..0c5665a 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -11,6 +11,9 @@ add_executable(test_main test_main.cpp) add_executable(runable runable_main.cpp) +add_executable(gimbal_mock mock_gimbal_test.cpp) + + # 确保测试使用 C++20 target_compile_features(test_main PRIVATE cxx_std_20) target_compile_features(runable PRIVATE cxx_std_20) @@ -30,10 +33,15 @@ target_link_libraries(test_main GTest::gmock_main alliance_auto_aim ) + target_link_libraries(runable alliance_auto_aim ) +target_link_libraries(gimbal_mock + alliance_auto_aim +) + include(GoogleTest) gtest_discover_tests(test_main) \ No newline at end of file diff --git a/tests/mock_gimbal_test.cpp b/tests/mock_gimbal_test.cpp new file mode 100644 index 0000000..c9090f9 --- /dev/null +++ b/tests/mock_gimbal_test.cpp @@ -0,0 +1,57 @@ + +#include "mocks/MockArmorInCamera.hpp" +#include "mocks/MockArmorInWorld.hpp" +#include "mocks/mock_camera_tranform.hpp" +#include +#include + +#include +#include +#include + +#include + +#include +#include + +int main() { + using namespace world_exe::util::visualization; + + Eigen::Vector3d V_input(1., 0., 0.); + double Omega_Yaw = 0.1; + double Omega_Pitch = 0.05; + double Max_Pitch = M_PI / 48.0; + auto transformer = world_exe::tests::mock::Camera2GimbalTransformer( + V_input, Omega_Yaw, Omega_Pitch, Max_Pitch); + + double dt = 0.01; + + cv::Mat image = cv::Mat { 1080, 1440, CV_8UC3, { 255, 255, 255 } }; + auto tmp = image.clone(); + + auto armor_in_camera = std::make_shared(0, 0); + auto armor_in_gimbal = std::make_shared(0, 0); + + auto armors_in_camera_3 = armor_in_camera->GetArmors(armor_in_camera->armorid); + auto armors_in_gimbal_3 = armor_in_gimbal->GetArmors(armor_in_gimbal->armorid); + + auto num = armors_in_camera_3.size(); + while (true) { + image.copyTo(tmp); + + draw_armor_in_camera(*armor_in_camera, + world_exe::parameters::HikCameraProfile::get_intrinsic_parameters(), + world_exe::parameters::HikCameraProfile::get_distortion_parameters(), + world_exe::parameters::Robomaster::NormalArmorObjectPointsRos, tmp); + + auto transform = transformer.updateAndGetTransform(dt); + + draw_armor_in_gimbal(*armor_in_gimbal, + world_exe::parameters::HikCameraProfile::get_intrinsic_parameters(), + world_exe::parameters::HikCameraProfile::get_distortion_parameters(), + world_exe::parameters::Robomaster::NormalArmorObjectPointsRos, transform.inverse(), + tmp); + cv::imshow("imshow", tmp); + cv::waitKey(1); + } +} \ No newline at end of file diff --git a/tests/mocks/mock_camera_tranform.hpp b/tests/mocks/mock_camera_tranform.hpp new file mode 100644 index 0000000..a130dad --- /dev/null +++ b/tests/mocks/mock_camera_tranform.hpp @@ -0,0 +1,96 @@ +#include + +#include +#include + +using namespace Eigen; +using AffineTransform = Affine3d; // 仿射变换矩阵 (4x4) +using Vector3D = Vector3d; +using QuaternionType = Quaterniond; + +namespace world_exe::tests::mock { + +class Camera2GimbalTransformer { +private: + // --- 状态变量 --- + // 当前的仿射变换 (包含位置和姿态) + AffineTransform T_current; + + // 当前时间 + double current_time; + + // --- 输入参数 --- + // 1. 水平平移速度 (在世界坐标系下) + const Vector3D V_horizontal; + // 2. 绕竖直轴 (Z轴) 的角速度 (在世界坐标系下) + const double Omega_Yaw; + // 3. 绕 Pitch 轴的角速度 (在世界坐标系下) + double Omega_Pitch; + // 4. 最大 Pitch 角度 (用于约束) + const double Max_Pitch_Angle; + + // 用于追踪当前 Pitch 角度，以实现角度约束 (假设Pitch轴是Y轴) + double current_pitch_angle; + +public: + /** + * @brief 构造函数 + * @param v_horiz 水平速度向量 (例如: 1.0, 0.0, 0.0) + * @param omega_yaw 绕 Z 轴角速度 (rad/s) + * @param omega_pitch 绕 Pitch 轴角速度 (rad/s) + * @param max_pitch_angle 绕 Pitch 轴的最大角度限制 (rad) + */ + Camera2GimbalTransformer( + const Vector3D& v_horiz, double omega_yaw, double omega_pitch, double max_pitch_angle) + : V_horizontal(v_horiz) + , Omega_Yaw(omega_yaw) + , Omega_Pitch(omega_pitch) + , Max_Pitch_Angle(std::abs(max_pitch_angle)) + , // 确保最大角度是正值 + T_current(AffineTransform::Identity()) + , current_time(0.0) + , current_pitch_angle(0.0) { } + + /** + * @brief 按照给定的时间步长进行一次状态积分，并返回更新后的 Affine 变换 + * @param dt 时间步长 (秒) + * @return 更新后的 Affine 变换矩阵 + */ + AffineTransform updateAndGetTransform(double dt) { + Vector3D delta_t = V_horizontal * dt; + + double delta_yaw = Omega_Yaw * dt; + AngleAxisd R_yaw(delta_yaw, Vector3D::UnitZ()); + + double delta_pitch = Omega_Pitch * dt; + double next_pitch_angle = current_pitch_angle + delta_pitch; + if (next_pitch_angle > Max_Pitch_Angle || next_pitch_angle < -Max_Pitch_Angle) { + + Omega_Pitch = -Omega_Pitch; + delta_pitch = Omega_Pitch * dt; + } + + current_pitch_angle = current_pitch_angle + delta_pitch; + + AngleAxisd R_pitch(delta_pitch, Vector3D::UnitY()); + + // 2c. 组合旋转增量：R_total = R_yaw * R_pitch + // 假设 Pitch 和 Yaw 是在世界坐标系下连续作用的 (即增量也是在世界坐标系下) + // 结果是一个 AngleAxisd 或 Quaternion，需要转换为 Affine + AffineTransform T_rot_increment = AffineTransform(R_yaw * R_pitch); + + // --- 3. 组合总增量 T_delta --- + + // T_delta = T_平移 * T_旋转 + AffineTransform T_delta = Translation3d(delta_t) * T_rot_increment; + + // --- 4. 状态更新 --- + + // T_new = T_delta * T_old (因为 T_delta 是相对于世界坐标系W的增量) + T_current = T_delta * T_current; + current_time += dt; + + return T_current; + } +}; +}