移除 GyroTracker 不必要的滤波

extra/combo/include/rmvl/combo/armor.h

@@ -70,15 +70,15 @@ class Armor : public combo
     /**
      * @brief 动态类型转换
      *
-     * @param[in] p_combo combo::ptr 抽象指针
+     * @param[in] p_combo `combo::ptr` 指针
      * @return 派生对象指针
      */
     static inline Armor::ptr cast(combo::ptr p_combo) { return std::dynamic_pointer_cast<Armor>(p_combo); }
 
     /**
      * @brief 动态类型转换
      *
-     * @param[in] p_combo combo::const_ptr 抽象指针
+     * @param[in] p_combo `combo::const_ptr` 指针
      * @return 派生对象指针
      */
     static inline Armor::const_ptr cast(combo::const_ptr p_combo) { return std::dynamic_pointer_cast<const Armor>(p_combo); }

extra/group/src/gyro_group/sync.cpp

@@ -68,8 +68,8 @@ void GyroGroup::sync(const GyroData &gyro_data, double tick)
             _center3d(1) -= _tracker_state[visible_trackers.front()].delta_y();
     }
     // 获取旋转角速度与平均采样时间
-    float rotation_speed = 0.f;
-    float sample_time = 0.f;
+    float rotation_speed{};
+    float sample_time{};
     for (const auto &p_tracker : _trackers)
     {
         auto p_gyro_tracker = GyroTracker::cast(p_tracker);

extra/tracker/include/rmvl/tracker/gyro_tracker.h

@@ -34,11 +34,10 @@ class GyroTracker final : public tracker
     };
 
 private:
-    double _duration{}; //!< 采样帧差时间
-    cv::Vec2f _pose;    //!< 修正后的装甲板姿态法向量
-    float _rotspeed{};  //!< 绕 y 轴自转角速度（俯视顺时针为正，滤波数据，弧度）
+    float _duration{}; //!< 采样帧差时间
+    cv::Vec2f _pose;   //!< 修正后的装甲板姿态法向量
+    float _rotspeed{}; //!< 绕 y 轴自转角速度（俯视顺时针为正，滤波数据，弧度）
 
-    KF42f _motion_filter;   //!< 目标转角滤波器
     KF63f _center3d_filter; //!< 位置滤波器
     KF42f _pose_filter;     //!< 姿态滤波器
 
@@ -76,7 +75,7 @@ class GyroTracker final : public tracker
      */
     static inline GyroTracker::const_ptr cast(tracker::const_ptr p_tracker) { return std::dynamic_pointer_cast<const GyroTracker>(p_tracker); }
 
-    [[deprecated]] void update(double, const GyroData &) override{};
+    [[deprecated]] void update(double, const GyroData &) override {};
 
     /**
      * @brief 使用捕获的 `combo` 更新平面目标追踪器
@@ -93,7 +92,7 @@ class GyroTracker final : public tracker
     inline void updateVanishState(VanishState state) { state == VANISH ? _vanish_num++ : _vanish_num = 0; }
 
     //! 获取帧差时间
-    inline double getDuration() const { return _duration; }
+    inline float getDuration() const { return _duration; }
     //! 获取修正后的装甲板姿态法向量
     inline const cv::Vec2f &getPose() const { return _pose; }
     //! 获取绕 y 轴的自转角速度（俯视顺时针为正，滤波数据，弧度）
@@ -117,13 +116,6 @@ class GyroTracker final : public tracker
      */
     void updateType(RMStatus stat);
 
-    /**
-     * @brief 更新运动滤波器
-     * @note 将图像相对速度和陀螺仪速度融合后再做滤波的好处是，
-     *       可以一定程度上减少时序不精准的问题
-     */
-    void updateMotionFilter();
-
     //! 更新位置滤波器
     void updatePositionFilter();
 

extra/tracker/include/rmvl/tracker/planar_tracker.h

@@ -89,8 +89,6 @@ class PlanarTracker final : public tracker
 
     /**
      * @brief 更新运动滤波器
-     * @note 将图像相对速度和陀螺仪速度融合后再做滤波的好处是，
-     *       可以一定程度上减少时序不精准的问题
      * @note 帧差时间 t: (若只有一帧则取默认采样时间，否则取平均数值)
      */
     void updateMotionFilter();

extra/tracker/param/gyro_tracker.para

@@ -2,8 +2,6 @@ double SAMPLE_INTERVAL = 15 # 采样时间（单位: ms）
 float MAX_ROTSPEED = 20     # 模型旋转速度上限（单位: rad/s）
 float MIN_ROTSPEED = 0      # 模型旋转速度下限（单位: rad/s）
 
-Matx44f MOTION_Q = Matx44f::eye()                   # 目标转角过程噪声协方差矩阵
-Matx22f MOTION_R = Matx22f::diag({0.01, 0.01})      # 目标转角测量噪声协方差矩阵
 Matx66f POSITION_Q = Matx66f::eye()                 # 位置过程噪声协方差矩阵
 Matx33f POSITION_R = Matx33f::diag({0.1, 0.1, 0.5}) # 位置测量噪声协方差矩阵
 Matx44f POSE_Q = Matx44f::eye()                     # 姿态过程噪声协方差矩阵

extra/tracker/src/gyro_tracker/filter_update.cpp

@@ -20,12 +20,6 @@ namespace rm
 void GyroTracker::initFilter()
 {
     auto first_combo = _combo_deque.front();
-    // 初始化运动滤波器 相对角度和角速度
-    _motion_filter.setR(para::gyro_tracker_param.MOTION_R);
-    _motion_filter.setQ(para::gyro_tracker_param.MOTION_Q);
-    const auto &relative_angle = first_combo->getRelativeAngle();
-    cv::Matx41f init_move_vec = {relative_angle.x, relative_angle.y, 0, 0};
-    _motion_filter.init(init_move_vec, 1e5f);
     // 初始化位置滤波器
     _center3d_filter.setR(para::gyro_tracker_param.POSITION_R);
     _center3d_filter.setQ(para::gyro_tracker_param.POSITION_Q);
@@ -40,25 +34,9 @@ void GyroTracker::initFilter()
     _pose_filter.init(init_pose_vec, 1e5f);
 }
 
-void GyroTracker::updateMotionFilter()
-{
-    float t = _duration;
-    // 设置状态转移矩阵
-    _motion_filter.setA({1, 0, t, 0,
-                         0, 1, 0, t,
-                         0, 0, 1, 0,
-                         0, 0, 0, 1});
-    // 预测
-    _motion_filter.predict();
-    // 更新
-    auto result = _motion_filter.correct(front()->getRelativeAngle());
-    _relative_angle = {result(0), result(1)}; // 相对角度滤波数值
-    _speed = {result(2), result(3)};          // 相对速度滤波数值
-}
-
 void GyroTracker::updatePositionFilter()
 {
-    float t = _duration;
+    float t{_duration};
     // 设置状态转移矩阵
     _center3d_filter.setA({1, 0, 0, t, 0, 0,
                            0, 1, 0, 0, t, 0,
@@ -76,7 +54,7 @@ void GyroTracker::updatePositionFilter()
 
 void GyroTracker::updatePoseFilter()
 {
-    float t = _duration;
+    float t{_duration};
     // 设置状态转移矩阵
     _pose_filter.setA({1, 0, t, 0,
                        0, 1, 0, t,

extra/tracker/src/gyro_tracker/gyro_tracker.cpp

@@ -53,15 +53,12 @@ void GyroTracker::update(combo::ptr p_armor)
     // 帧差时间计算
     if (_combo_deque.empty())
         RMVL_Error(RMVL_StsBadSize, "\"_combo_deque\" is empty");
-    _duration = 0.f;
-    if (_combo_deque.size() >= 2)
-        _duration = (_combo_deque.front()->getTick() - _combo_deque.back()->getTick()) / static_cast<double>(_combo_deque.size() - 1);
-    else
-        _duration = para::gyro_tracker_param.SAMPLE_INTERVAL / 1000.;
+    _duration = (_combo_deque.size() >= 2)
+                    ? (_combo_deque.front()->getTick() - _combo_deque.back()->getTick()) / static_cast<double>(_combo_deque.size() - 1)
+                    : para::gyro_tracker_param.SAMPLE_INTERVAL / 1000.;
     if (std::isnan(_duration))
         RMVL_Error(RMVL_StsDivByZero, "\"t\" is nan");
     // 更新滤波器
-    updateMotionFilter();
     updatePositionFilter();
     updatePoseFilter();
     // 计算旋转角速度

extra/tracker/src/rune_tracker/rune_tracker.cpp

@@ -80,8 +80,7 @@ float RuneTracker::calculateTotalAngle()
         current_angle += (current_angle > 0.f) ? -360.f : 360.f;
     // 更新角度
     auto p_rune = Rune::cast(_combo_deque.front());
-    if (p_rune == nullptr)
-        RMVL_Error(RMVL_BadDynamicType, "Dynamic type of the combo::ptr is not equal to Rune::ptr ");
+    RMVL_DbgAssert(p_rune != nullptr);
     return current_angle + 360.f * _round;
 }