Tongji dev:add some test

src/tongji/auto_aim_system.cpp

@@ -1,19 +1,21 @@
 #include "auto_aim_system.hpp"
 
 #include <chrono>
+
 #include <cstdio>
 #include <exception>
-#include <iostream>
 #include <memory>
 #include <opencv2/core/mat.hpp>
 #include <opencv2/highgui.hpp>
-#include <print>
+#include <tuple>
 
 #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<predictor::CarPredictorManager> live_target_manager_;
     std::unique_ptr<world_exe::v1::Syncer> syncer_;
     std::unique_ptr<fire_control::FireController> fire_controller_;
+    // std::unique_ptr<tests::mock::Camera2GimbalTransformer> mock_camera_tranform_;
 };
 
 AutoAimSystem::AutoAimSystem(const bool& debug)

src/tongji/predictor/kalman_filter/extended_kalman_filter.hpp

@@ -80,9 +80,9 @@ template <typename EKFModel> 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;
 

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)

tests/mock_gimbal_test.cpp

@@ -0,0 +1,57 @@
+
+#include "mocks/MockArmorInCamera.hpp"
+#include "mocks/MockArmorInWorld.hpp"
+#include "mocks/mock_camera_tranform.hpp"
+#include <Eigen/src/Core/Matrix.h>
+#include <Eigen/src/Geometry/AngleAxis.h>
+
+#include <Eigen/src/Geometry/Quaternion.h>
+#include <memory>
+#include <opencv2/highgui.hpp>
+
+#include <utils/visualization.hpp>
+
+#include <parameters/profile.hpp>
+#include <parameters/rm_parameters.hpp>
+
+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<world_exe::tests::mock::MockArmorInCamera>(0, 0);
+    auto armor_in_gimbal = std::make_shared<world_exe::tests::mock::MockArmorInWorld>(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);
+    }
+}

tests/mocks/mock_camera_tranform.hpp

@@ -0,0 +1,96 @@
+#include <Eigen/Dense>
+
+#include <cmath>
+#include <cstdlib>
+
+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;
+    }
+};
+}