🔧 修改 combo 模块成员函数名

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

@@ -62,16 +62,24 @@ class combo
     inline cv::Point2f center() const { return _center; }
     //! 获取组合体角点
     inline const std::vector<cv::Point2f> &corners() const { return _corners; }
+
+    /**
+     * @brief 获取指定角点
+     * 
+     * @param[in] idx 下标
+     * @return 指定角点 
+     */
+    inline cv::Point2f corner(int idx) const { return _corners[idx]; }
     //! 获取组合体相机外参
-    inline const CameraExtrinsics &extrinsics() const { return _extrinsic; }
+    inline const CameraExtrinsics &extrinsic() const { return _extrinsic; }
     //! 获取组合体类型
     inline RMStatus type() const { return _type; }
     //! 获取捕获该组合体的时间点
     inline double tick() const { return _tick; }
     //! 获取组合体的相对目标转角
     inline cv::Point2f getRelativeAngle() const { return _relative_angle; }
     //! 获取组合体当前的 IMU 数据
-    inline const ImuData &getImuData() const { return _imu_data; }
+    inline const ImuData &imu() const { return _imu_data; }
 
     /**
      * @brief 获取指定特征

extra/compensator/src/gravity_compensator/gravity_compensator.cpp

@@ -123,10 +123,10 @@ CompensateInfo GravityCompensator::Impl::compensate(const std::vector<group::ptr
         for (auto &p_tracker : p_group->data())
         {
             // 单位换算
-            double dis = p_tracker->extrinsics().distance() / 1000.;
+            double dis = p_tracker->extrinsic().distance() / 1000.;
             // 提取当前 IMU 角度
-            auto gyro_angle = cv::Point2f(p_tracker->front()->getImuData().rotation.yaw,
-                                          p_tracker->front()->getImuData().rotation.pitch);
+            auto gyro_angle = cv::Point2f(p_tracker->front()->imu().rotation.yaw,
+                                          p_tracker->front()->imu().rotation.pitch);
             // 目标与云台转轴的连线与水平方向的夹角
             double angle = gyro_angle.y + p_tracker->getRelativeAngle().y;
             // 计算补偿角度和对应的子弹飞行时间（模型中角度要求向上为正，这里需取反）

extra/compensator/src/gyro_compensator/gyro_compensator.cpp

@@ -111,8 +111,8 @@ CompensateInfo GyroCompensator::compensate(const std::vector<group::ptr> &groups
         // 目标转角
         auto relative_angle = calculateRelativeAngle(para::camera_param.cameraMatrix, p_group->center());
         // 提取当前 IMU 角度
-        auto gyro_angle = cv::Point2f(p_gyro_group->getImuData().rotation.yaw,
-                                      p_gyro_group->getImuData().rotation.pitch);
+        auto gyro_angle = cv::Point2f(p_gyro_group->imu().rotation.yaw,
+                                      p_gyro_group->imu().rotation.pitch);
         // 目标与云台转轴的连线与水平方向的夹角
         double angle = gyro_angle.y + relative_angle.y;
         double gp = rad2deg(-getPitch(dis * cos(deg2rad(-angle)), // 模型中角度要求向上为正，这里需取反

extra/decider/src/gyro_decider/gyro_decider.cpp

@@ -55,7 +55,7 @@ static void calcPrediction(group::ptr p_group, tracker::const_ptr p_tracker, cv:
         RMVL_Error(RMVL_BadDynamicType, "Fail to cast the type of \"p_group\" to \"GyroGroup::ptr\"");
     // 旋转预测增量的分量计算
     float c = cos(rotangle), s = sin(rotangle);
-    const auto &tvec = p_tracker->extrinsics().tvec();
+    const auto &tvec = p_tracker->extrinsic().tvec();
     cv::Vec3f center2combo_pose = tvec - p_gyro_group->getCenter3D(); // 旋转中心到 combo 的向量
     cv::Matx33f rot = {c, 0, s,
                        0, 1, 0,
@@ -66,7 +66,7 @@ static void calcPrediction(group::ptr p_group, tracker::const_ptr p_tracker, cv:
     cv::Vec3f motion_p = tvec + motion_dp;
     // IMU 坐标系转相机坐标系
     cv::Matx33f I = cv::Matx33f::eye();
-    Armor::imuConvertToCamera(I, motion_p, p_gyro_group->getImuData(), I, motion_p);
+    Armor::imuConvertToCamera(I, motion_p, p_gyro_group->imu(), I, motion_p);
     // 解算 3D 预测值
     point_p3d = motion_p;
     // 解算 2D 预测值
@@ -94,7 +94,7 @@ static cv::Point2f calculateHighSpeedBasicResponse(group::ptr target_group, trac
     cv::Point3f min_pitch3d;
     min_pitch3d.x = GyroGroup::cast(target_group)->getCenter3D()(0);
     for (const auto &p_target : target_group->data())
-        min_pitch3d.y += p_target->extrinsics().tvec()(1);
+        min_pitch3d.y += p_target->extrinsic().tvec()(1);
     min_pitch3d.y /= static_cast<float>(target_group->data().size());
     min_pitch3d.z = predict_center3d.z;
     auto min_pitch_target2d = cameraConvertToPixel(para::camera_param.cameraMatrix, para::camera_param.distCoeffs, min_pitch3d);

extra/decider/src/rune_decider/rune_decider.cpp

@@ -41,7 +41,7 @@ static void getPredictMsgFromAngleZ(float d_predict, tracker::ptr ref_tracker, c
     predict_center.y = rune_center.y - feature_distance * sin(deg2rad(ref_tracker->angle() + d_predict));
     // 平面修正
     auto correct_vec = Rune::verticalConvertToCamera(predict_center - rune_center,
-                                                     ref_tracker->front()->getImuData().rotation.pitch);
+                                                     ref_tracker->front()->imu().rotation.pitch);
     d_center = rune_center + cv::Point2f(correct_vec);
     d_angle = calculateRelativeAngle(para::camera_param.cameraMatrix, rune_center + cv::Point2f(correct_vec)) -
               ref_tracker->getRelativeAngle();

extra/detector/src/gyro_detector/match.cpp

@@ -126,7 +126,7 @@ void GyroDetector::match(std::vector<group::ptr> &groups, std::vector<combo::ptr
             for (size_t i = 0; i < combo_size; i++)
             {
                 combo::const_ptr p_combo = combo_vec[i];
-                combos_t[i] = p_combo->extrinsics().tvec();
+                combos_t[i] = p_combo->extrinsic().tvec();
                 combos_P[i] = Armor::cast(p_combo)->getPose();
                 combos_r[i] = para::gyro_group_param.INIT_RADIUS;
             }
@@ -260,7 +260,7 @@ void GyroDetector::matchOneGroup(group::ptr group, const std::vector<combo::ptr>
     std::unordered_set<tracker::ptr> tracker_set(trackers.begin(), trackers.end());
     // 根据距离 distance 排序，选出待匹配的 trackers
     sort(trackers.begin(), trackers.end(), [](tracker::const_ptr lhs, tracker::const_ptr rhs) {
-        return lhs->extrinsics().distance() < rhs->extrinsics().distance();
+        return lhs->extrinsic().distance() < rhs->extrinsic().distance();
     });
     // 待匹配参考 tracker 的大小
     size_t ref_size = trackers.size();

extra/group/include/rmvl/group/gyro_group.h

@@ -137,7 +137,7 @@ class GyroGroup final : public group
     //! 获取相机坐标系自转角速度（俯视顺时针为正，滤波数据，弧度）
     inline float getRotatedSpeed() const { return _rotspeed; }
     //! 获取 IMU 数据
-    inline ImuData getImuData() const { return _imu_data; }
+    inline ImuData imu() const { return _imu_data; }
     //! 获取旋转状态
     inline RotStatus getRotStatus() const { return _rot_status; }
 

extra/group/src/gyro_group/calc.cpp

@@ -72,7 +72,7 @@ combo::ptr GyroGroup::constructComboForced(combo::ptr p_combo, const ImuData &im
     // IMU 坐标系转化为相机坐标系
     cv::Matx33f cam_rmat;
     cv::Vec3f cam_tvec;
-    Armor::imuConvertToCamera(gyro_rmat, gyro_tvec, p_combo->getImuData(), cam_rmat, cam_tvec);
+    Armor::imuConvertToCamera(gyro_rmat, gyro_tvec, p_combo->imu(), cam_rmat, cam_tvec);
 
     if (cam_tvec(2) <= 0)
         RMVL_Error_(RMVL_StsError, "Bad value of \"cam_tvec\", cam_tvec(2) = %.3f", cam_tvec(2));
@@ -110,9 +110,9 @@ void GyroGroup::getGroupInfo(const std::vector<combo::ptr> &visible_combos, std:
     });
     for (size_t i = 0; i < visible_num; ++i)
     {
-        Rs[i] = operate_combos[i]->extrinsics().R();
+        Rs[i] = operate_combos[i]->extrinsic().R();
         Ps[i] = Armor::cast(operate_combos[i])->getPose();
-        ts[i] = operate_combos[i]->extrinsics().tvec();
+        ts[i] = operate_combos[i]->extrinsic().tvec();
         rs[i] = para::gyro_group_param.INIT_RADIUS;
     }
 

extra/group/src/gyro_group/init.cpp

@@ -28,7 +28,7 @@ GyroGroup::GyroGroup(const std::vector<combo::ptr> &first_combos, int armor_num)
     std::vector<combo::ptr> combo_vec;   // 整车模型中的组合体列表
     cv::Vec3f group_center3d;            // 旋转中心点相机坐标
     cv::Point2f group_center2d;          // 旋转中心点像素坐标
-    _imu_data = first_combos.front()->getImuData();
+    _imu_data = first_combos.front()->imu();
     _tick = first_combos.front()->tick();
     // 获取 RobotType
     std::vector<RobotType> robot_type_vec;

extra/group/src/gyro_group/sync.cpp

@@ -55,9 +55,9 @@ void GyroGroup::sync(const ImuData &imu_data, double tick)
     std::vector<float> rs(visible_num);       // 旋转半径
     for (size_t i = 0; i < visible_num; ++i)
     {
-        Rs[i] = visible_trackers[i]->extrinsics().R();
+        Rs[i] = visible_trackers[i]->extrinsic().R();
         Ps[i] = GyroTracker::cast(visible_trackers[i])->getPose();
-        ts[i] = visible_trackers[i]->extrinsics().tvec();
+        ts[i] = visible_trackers[i]->extrinsic().tvec();
         rs[i] = _tracker_state[visible_trackers[i]].radius();
     }
     if (visible_num != 0)
@@ -103,7 +103,7 @@ void GyroGroup::sync(const ImuData &imu_data, double tick)
             // 旋转中心到组合体的线段向量
             cv::Vec2f tmp = -p_gyro_tracker->getPose() * current_state.radius();
             cv::Vec3f center2combo(tmp(0), 0, tmp(1));
-            cv::Matx33f new_rmat = rot * p_tracker->extrinsics().R();                                    // 新的旋转矩阵
+            cv::Matx33f new_rmat = rot * p_tracker->extrinsic().R();                                    // 新的旋转矩阵
             cv::Vec3f new_tvec = _center3d + rot * center2combo + cv::Vec3f(0, current_state.delta_y(), 0); // 新的平移向量
             auto p_armor = constructComboForced(p_tracker->front(), _imu_data, new_rmat, new_tvec, _tick);
             p_tracker->update(p_armor);
@@ -125,7 +125,7 @@ void GyroGroup::sync(const ImuData &imu_data, double tick)
             auto &current_state = _tracker_state[p_tracker];
             // 绕 y 轴旋转
             auto rot = euler2Mat(static_cast<float>(2_PI / _armor_num * static_cast<double>((i + 1))), Y);
-            cv::Matx33f new_rmat = rot * visible_tracker->extrinsics().R();                              // 新的旋转矩阵
+            cv::Matx33f new_rmat = rot * visible_tracker->extrinsic().R();                              // 新的旋转矩阵
             cv::Vec3f new_tvec = _center3d + rot * center2combo + cv::Vec3f(0, current_state.delta_y(), 0); // 新的平移向量
             auto p_armor = constructComboForced(visible_tracker->front(), _imu_data, new_rmat, new_tvec, _tick);
             p_tracker->update(p_armor);
@@ -157,7 +157,7 @@ void GyroGroup::sync(const ImuData &imu_data, double tick)
             // 绕 y 轴旋转
             auto rot = euler2Mat(static_cast<float>(PI), Y);
             // 平移向量的旋转增量
-            cv::Matx33f new_rmat = rot * visible_trackers[i]->extrinsics().R();                             // 新旋转矩阵
+            cv::Matx33f new_rmat = rot * visible_trackers[i]->extrinsic().R();                             // 新旋转矩阵
             cv::Vec3f new_tvec = _center3d + rot * center2combo[i] + cv::Vec3f(0, current_state.delta_y(), 0); // 新平移向量
             auto p_armor = constructComboForced(visible_trackers[i]->front(), _imu_data, new_rmat, new_tvec, _tick);
             // 同步高度差

extra/predictor/src/armor_predictor/armor_predictor.cpp

@@ -27,8 +27,8 @@ PredictInfo ArmorPredictor::predict(const std::vector<group::ptr> &groups, const
         {
             double tf = (tof.find(p_tracker) == tof.end()) ? 0. : tof.at(p_tracker);
             // 获取 IMU 角速度
-            cv::Point2f gyro_speed = {p_tracker->front()->getImuData().rotation.yaw_speed,
-                                      p_tracker->front()->getImuData().rotation.pitch_speed};
+            cv::Point2f gyro_speed = {p_tracker->front()->imu().rotation.yaw_speed,
+                                      p_tracker->front()->imu().rotation.pitch_speed};
             // 计算用于预测的合成角速度
             cv::Point2f speed = p_tracker->speed() + gyro_speed;
             double dB_yaw = speed.x * para::armor_predictor_param.YAW_B;

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

@@ -97,7 +97,7 @@ class tracker
     //! 修正后的相对角度（角度制）
     inline const cv::Point2f &getRelativeAngle() const { return _relative_angle; }
     //! 修正后的相机外参
-    inline const CameraExtrinsics &extrinsics() const { return _extrinsic; }
+    inline const CameraExtrinsics &extrinsic() const { return _extrinsic; }
     //! 获取追踪器修正后的目标转角速度（角度制）
     inline const cv::Point2f &speed() const { return _speed; }
 };

extra/tracker/src/default_tracker.cpp

@@ -21,7 +21,7 @@ void DefaultTracker::updateData(combo::ptr p_combo)
     _angle = p_combo->height();
     _corners = p_combo->corners();
     _center = p_combo->center();
-    _extrinsic = p_combo->extrinsics();
+    _extrinsic = p_combo->extrinsic();
 }
 
 DefaultTracker::DefaultTracker(combo::ptr p_combo) : tracker()

extra/tracker/src/gyro_tracker/filter_update.cpp

@@ -23,7 +23,7 @@ void GyroTracker::initFilter()
     // 初始化位置滤波器
     _center3d_filter.setR(para::gyro_tracker_param.POSITION_R);
     _center3d_filter.setQ(para::gyro_tracker_param.POSITION_Q);
-    const auto &tvec = first_combo->extrinsics().tvec();
+    const auto &tvec = first_combo->extrinsic().tvec();
     cv::Matx61f init_position_vec = {tvec(0), tvec(1), tvec(2), 0, 0, 0};
     _center3d_filter.init(init_position_vec, 1e5f);
     // 初始化姿态滤波器
@@ -47,7 +47,7 @@ void GyroTracker::updatePositionFilter()
     // 预测
     _center3d_filter.predict();
     // 更新
-    cv::Vec3f tvec = at(0)->extrinsics().tvec();
+    cv::Vec3f tvec = at(0)->extrinsic().tvec();
     cv::Matx61f correct_position = _center3d_filter.correct(tvec);
     _extrinsic.tvec({correct_position(0), correct_position(1), correct_position(2)});
 }

extra/tracker/src/gyro_tracker/gyro_tracker.cpp

@@ -23,7 +23,7 @@ void GyroTracker::updateFromCombo(combo::ptr p_combo)
     _center = p_combo->center();
     _relative_angle = p_combo->getRelativeAngle();
     _corners = p_combo->corners();
-    _extrinsic = p_combo->extrinsics();
+    _extrinsic = p_combo->extrinsic();
     _pose = Armor::cast(p_combo)->getPose();
 }
 
@@ -33,7 +33,7 @@ GyroTracker::GyroTracker(combo::ptr p_armor)
         RMVL_Error(RMVL_StsBadArg, "Input argument \"p_armor\" is nullptr.");
     updateFromCombo(p_armor);
 
-    _extrinsic = p_armor->extrinsics();
+    _extrinsic = p_armor->extrinsic();
     _type = p_armor->type();
     _combo_deque.push_back(p_armor);
     _type_deque.push_back(_type.RobotTypeID);

extra/tracker/src/planar_tracker/filter_update.cpp

@@ -22,7 +22,7 @@ void PlanarTracker::initFilter()
     // 初始化距离滤波器
     _distance_filter.setR({para::planar_tracker_param.DIS_R});
     _distance_filter.setQ(para::planar_tracker_param.DIS_Q);
-    cv::Matx21f init_dis_vec = {first_combo->extrinsics().distance(), 0};
+    cv::Matx21f init_dis_vec = {first_combo->extrinsic().distance(), 0};
     _distance_filter.init(init_dis_vec, 1e5f);
     // 初始化运动滤波器
     _motion_filter.setR(para::planar_tracker_param.MOTION_R);
@@ -38,7 +38,7 @@ void PlanarTracker::updateDistanceFilter()
     // 设置状态转移矩阵
     _distance_filter.setA({1, 1,
                            0, 1});
-    float current_distance = _combo_deque.front()->extrinsics().distance();
+    float current_distance = _combo_deque.front()->extrinsic().distance();
     // 预测
     _distance_filter.predict();
     // 更新

extra/tracker/src/planar_tracker/planar_tracker.cpp

@@ -22,7 +22,7 @@ void PlanarTracker::updateData(combo::ptr p_combo)
     _angle = p_combo->height();
     _corners = p_combo->corners();
     _center = p_combo->center();
-    _extrinsic = p_combo->extrinsics();
+    _extrinsic = p_combo->extrinsic();
 }
 
 PlanarTracker::PlanarTracker(combo::ptr p_combo)

extra/tracker/src/rune_tracker/rune_tracker.cpp

@@ -20,7 +20,7 @@ void RuneTracker::updateFromRune(combo::ptr p_combo)
     _width = p_combo->width();
     _height = p_combo->height();
     _corners = p_combo->corners();
-    _extrinsic = p_combo->extrinsics();
+    _extrinsic = p_combo->extrinsic();
     _center = p_combo->center();
     _type = p_combo->type();
     _relative_angle = p_combo->getRelativeAngle();

extra/tracker/src/rune_tracker/vanish_process.cpp

@@ -41,8 +41,8 @@ static Rune::ptr runeConstructForced(Rune::ptr ref_rune, float delta_angle, doub
                        -std::sin(deg2rad(delta_angle)), std::cos(deg2rad(delta_angle))};
     target_vec = rot * target_vec;
     // 获取新的神符中心图像中心点
-    auto old_gyro_angle = cv::Point2f(ref_rune->getImuData().rotation.yaw,
-                                      ref_rune->getImuData().rotation.pitch);
+    auto old_gyro_angle = cv::Point2f(ref_rune->imu().rotation.yaw,
+                                      ref_rune->imu().rotation.pitch);
     auto new_gyro_angle = cv::Point2f(imu_data.rotation.yaw, imu_data.rotation.pitch);
     // -(new_gyro_angle - old_gyro_angle)
     auto dpoint = calculateRelativeCenter(para::camera_param.cameraMatrix, old_gyro_angle - new_gyro_angle) -