From e7069fc14e3f64aa19b4293f50dd8bf4b564f58c Mon Sep 17 00:00:00 2001 From: zhaoxi <535394140@qq.com> Date: Wed, 20 Mar 2024 16:34:40 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=20KalmanFilter=20=E7=9A=84?= =?UTF-8?q?=E4=BD=BF=E7=94=A8=E6=96=B9=E5=BC=8F=201.=20=E5=88=A0=E9=99=A4?= =?UTF-8?q?=E6=97=A0=E6=B3=95=E7=9B=B4=E6=8E=A5=E8=A2=AB=E8=A7=82=E6=B5=8B?= =?UTF-8?q?=E7=9A=84=E8=A7=82=E6=B5=8B=E9=87=8F=202.=20=E4=B8=BA=20tracker?= =?UTF-8?q?=20=E6=A8=A1=E5=9D=97=E6=8F=90=E4=BE=9B=E6=96=B0=E7=9A=84=20upd?= =?UTF-8?q?ate=20=E6=8E=A5=E5=8F=A3=203.=20=E5=90=8C=E6=AD=A5=E6=89=80?= =?UTF-8?q?=E6=9C=89=E5=8A=9F=E8=83=BD=E6=A8=A1=E5=9D=97=E5=AF=B9=E4=BA=8E?= =?UTF-8?q?=20tracker::update()=20=E7=9A=84=E8=B0=83=E7=94=A8=E6=96=B9?= =?UTF-8?q?=E5=BC=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../translation_decider.cpp | 8 +- extra/detector/src/armor_detector/match.cpp | 10 +- extra/detector/src/gyro_detector/match.cpp | 4 +- extra/detector/src/rune_detector/match.cpp | 23 +++-- extra/detector/src/tag_detector/detect.cpp | 40 ++++---- extra/group/include/rmvl/group/gyro_group.h | 5 +- extra/group/param/gyro_group.para | 4 +- extra/group/src/gyro_group/filter_update.cpp | 27 ++---- extra/group/src/gyro_group/init.cpp | 5 +- extra/group/src/gyro_group/sync.cpp | 6 +- .../src/armor_predictor/armor_predictor.cpp | 14 ++- .../src/rune_predictor/rune_predictor.cpp | 6 +- .../src/spi_rune_predictor/predict.cpp | 11 +-- .../test/test_spi_rune_predictor.cpp | 2 +- .../include/rmvl/tracker/gyro_tracker.h | 16 ++-- .../include/rmvl/tracker/planar_tracker.h | 18 ++-- .../include/rmvl/tracker/rune_tracker.h | 17 ++-- extra/tracker/include/rmvl/tracker/tracker.h | 40 ++++---- extra/tracker/param/gyro_tracker.para | 12 +-- extra/tracker/param/planar_tracker.para | 7 +- extra/tracker/param/rune_tracker.para | 2 +- extra/tracker/src/default_tracker.cpp | 3 +- .../src/gyro_tracker/filter_update.cpp | 92 +++++-------------- .../tracker/src/gyro_tracker/gyro_tracker.cpp | 2 +- .../src/planar_tracker/filter_update.cpp | 67 ++++---------- .../src/planar_tracker/planar_tracker.cpp | 33 +++---- .../src/planar_tracker/vanish_process.cpp | 8 +- .../src/rune_tracker/filter_update.cpp | 27 ++---- .../tracker/src/rune_tracker/rune_tracker.cpp | 12 +-- .../src/rune_tracker/vanish_process.cpp | 13 +-- extra/tracker/test/gyro_tracker_test.cpp | 2 +- extra/tracker/test/planar_tracker_test.cpp | 7 +- 32 files changed, 226 insertions(+), 317 deletions(-) diff --git a/extra/decider/src/translation_decider/translation_decider.cpp b/extra/decider/src/translation_decider/translation_decider.cpp index beb25fa0..b99e612c 100755 --- a/extra/decider/src/translation_decider/translation_decider.cpp +++ b/extra/decider/src/translation_decider/translation_decider.cpp @@ -37,18 +37,18 @@ DecideInfo TranslationDecider::decide(const std::vector &groups, RMS for (const auto &p_tracker : trackers) if (p_tracker == last_target) info.target = p_tracker; - // 未右键时，每帧更新，实时选择离图像中心最近的追踪器 + // 未找到目标，实时选择离图像中心最近的追踪器 if (info.target == nullptr) info.target = getClosestTracker(detect_info.src, trackers); // 判断是否正在追踪 - if (info.target) + if (info.target != nullptr) { const auto &dKt = predict_info.dynamic_prediction.at(info.target); const auto &dB = predict_info.static_prediction.at(info.target); const auto &comp = compensate_info.compensation.at(info.target); - auto angle_dKt = cv::Point2f(dKt(YAW), dKt(PITCH)); - auto angle_dB = cv::Point2f(dB(YAW), dB(PITCH)); + cv::Point2f angle_dKt(dKt(YAW), dKt(PITCH)); + cv::Point2f angle_dB(dB(YAW), dB(PITCH)); info.exp_angle = info.target->getRelativeAngle(); info.exp_angle += angle_dKt + angle_dB; // 加入预测 diff --git a/extra/detector/src/armor_detector/match.cpp b/extra/detector/src/armor_detector/match.cpp index 1a7d3924..c7daa627 100644 --- a/extra/detector/src/armor_detector/match.cpp +++ b/extra/detector/src/armor_detector/match.cpp @@ -67,7 +67,7 @@ void ArmorDetector::matchArmors(std::vector &trackers, const std:: return getDistance(lhs->getCenter(), p_tracker->front()->getCenter()) < getDistance(rhs->getCenter(), p_tracker->front()->getCenter()); }); - p_tracker->update(*min_it, _tick, _gyro_data); + p_tracker->update(*min_it); armor_set.erase(*min_it); } // 没有匹配到的装甲板作为新的序列 @@ -86,12 +86,12 @@ void ArmorDetector::matchArmors(std::vector &trackers, const std:: return getDistance(p_combo->getCenter(), lhs->front()->getCenter()) < getDistance(p_combo->getCenter(), rhs->front()->getCenter()); }); - min_dis_tracker->get()->update(p_combo, _tick, _gyro_data); + min_dis_tracker->get()->update(p_combo); tracker_set.erase(*min_dis_tracker); } // 没有匹配到的序列传入 nullptr for (const auto &p_tracker : tracker_set) - p_tracker->update(nullptr, _tick, _gyro_data); + p_tracker->update(_tick, _gyro_data); } // 如果当前帧识别到的装甲板数量 = 序列数量 else @@ -113,11 +113,11 @@ void ArmorDetector::matchArmors(std::vector &trackers, const std:: if (isChange(trackers[i]->front(), *min_it, min_dis)) { // 创建新序列，原来的序列打入 nullptr - trackers[i]->update(nullptr, _tick, _gyro_data); + trackers[i]->update(_tick, _gyro_data); trackers.emplace_back(PlanarTracker::make_tracker(*min_it)); } else - trackers[i]->update(*min_it, _tick, _gyro_data); + trackers[i]->update(*min_it); armor_set.erase(*min_it); } } diff --git a/extra/detector/src/gyro_detector/match.cpp b/extra/detector/src/gyro_detector/match.cpp index 3f980143..c3bd61e8 100755 --- a/extra/detector/src/gyro_detector/match.cpp +++ b/extra/detector/src/gyro_detector/match.cpp @@ -286,8 +286,8 @@ void GyroDetector::matchOneGroup(group::ptr group, const std::vector (getDistance(cur_p[0], rhs_p[0]) + getDistance(cur_p[1], rhs_p[1]) + getDistance(cur_p[2], rhs_p[2]) + getDistance(cur_p[3], rhs_p[3])); }); - min_tracker->update(p_combo, _tick, _gyro_data); // 更新追踪器 - tracker_set.erase(min_tracker); // 移出待匹配追踪器序列 + min_tracker->update(p_combo); // 更新追踪器 + tracker_set.erase(min_tracker); // 移出待匹配追踪器序列 auto p_gyro_tracker = GyroTracker::cast(min_tracker); p_gyro_tracker->updateVanishState(GyroTracker::APPEAR); // 设置为可见 } diff --git a/extra/detector/src/rune_detector/match.cpp b/extra/detector/src/rune_detector/match.cpp index 13b197bb..46dbda8d 100644 --- a/extra/detector/src/rune_detector/match.cpp +++ b/extra/detector/src/rune_detector/match.cpp @@ -52,7 +52,7 @@ void RuneDetector::matchRunes(std::vector &trackers, const std::ve return getDeltaAngle(lhs->getAngle(), p_tracker->front()->getAngle()) < getDeltaAngle(rhs->getAngle(), p_tracker->front()->getAngle()); }); - p_tracker->update(closest_rune, _tick, _gyro_data); + p_tracker->update(closest_rune); combo_set.erase(closest_rune); } // 没有匹配到的神符作为新的序列，创建新的 tracker @@ -67,17 +67,16 @@ void RuneDetector::matchRunes(std::vector &trackers, const std::ve for (auto p_combo : combos) { // 离 active_rune 最近的 tracker - tracker::ptr closest_tracker = *min_element(trackers.begin(), trackers.end(), - [&p_combo](tracker::const_ptr lhs, tracker::const_ptr rhs) { - return getDeltaAngle(p_combo->getAngle(), lhs->front()->getAngle()) < - getDeltaAngle(p_combo->getAngle(), rhs->front()->getAngle()); - }); - closest_tracker->update(p_combo, _tick, _gyro_data); + tracker::ptr closest_tracker = *min_element(trackers.begin(), trackers.end(), [&](tracker::const_ptr lhs, tracker::const_ptr rhs) { + return getDeltaAngle(p_combo->getAngle(), lhs->front()->getAngle()) < + getDeltaAngle(p_combo->getAngle(), rhs->front()->getAngle()); + }); + closest_tracker->update(p_combo); tracker_set.erase(closest_tracker); } - // 没有匹配到的序列传入 nullptr - for (auto &p_tracker : tracker_set) - p_tracker->update(nullptr, _tick, _gyro_data); + // 没有匹配到的序列则执行丢帧操作 + for (auto p_tracker : tracker_set) + p_tracker->update(_tick, _gyro_data); } // 如果当前帧识别到的装甲板数量 = 序列数量 else @@ -97,10 +96,10 @@ void RuneDetector::matchRunes(std::vector &trackers, const std::ve float min_delta_angle = getDeltaAngle(closest_combo->getAngle(), trackers[i]->front()->getAngle()); // 判断是否角度差过大 if (min_delta_angle < 50.f) - trackers[i]->update(closest_combo, _tick, _gyro_data); + trackers[i]->update(closest_combo); else { - trackers[i]->update(nullptr, _tick, _gyro_data); + trackers[i]->update(_tick, _gyro_data); trackers.emplace_back(RuneTracker::make_tracker(closest_combo)); } combo_set.erase(closest_combo); diff --git a/extra/detector/src/tag_detector/detect.cpp b/extra/detector/src/tag_detector/detect.cpp index 6ec53913..55301f11 100644 --- a/extra/detector/src/tag_detector/detect.cpp +++ b/extra/detector/src/tag_detector/detect.cpp @@ -125,12 +125,12 @@ void TagDetector::match(std::vector &trackers, const std::vector 序列数量 + // 如果当前帧识别到的视觉标签 `rm::Tag` 数量 > 序列数量 if (combos.size() > trackers.size()) { - // 初始化装甲板集合 + // 初始化视觉标签 `rm::Tag` 集合 std::unordered_set tag_set(combos.begin(), combos.end()); - // 距离最近的装甲板匹配到相应的序列中, 并 update + // 距离最近的视觉标签 `rm::Tag` 匹配到相应的序列中, 并 update for (auto p_tracker : trackers) { // 离 p_tracker 最近的 combo 及其距离 @@ -138,59 +138,59 @@ void TagDetector::match(std::vector &trackers, const std::vectorgetCenter(), p_tracker->front()->getCenter()) < getDistance(rhs->getCenter(), p_tracker->front()->getCenter()); }); - p_tracker->update(*min_it, _tick, _gyro_data); + p_tracker->update(*min_it); tag_set.erase(*min_it); } - // 没有匹配到的装甲板作为新的序列 + // 没有匹配到的视觉标签 `rm::Tag` 作为新的序列 for (const auto &p_combo : tag_set) trackers.emplace_back(PlanarTracker::make_tracker(p_combo)); } - // 如果当前帧识别到的装甲板数量 < 序列数量 + // 如果当前帧识别到的视觉标签 `rm::Tag` 数量 < 序列数量 else if (combos.size() < trackers.size()) { // 初始化追踪器集合 std::unordered_set tracker_set(trackers.begin(), trackers.end()); for (const auto &p_combo : combos) { - // 离 tag 最近的 tracker 及其距离 - auto min_dis_tracker = min_element(trackers.begin(), trackers.end(), [&](tracker::const_ptr lhs, tracker::const_ptr rhs) { + // 离视觉标签最近的 tracker 及其距离 + auto min_dis_tracker = *min_element(trackers.begin(), trackers.end(), [&](tracker::const_ptr lhs, tracker::const_ptr rhs) { return getDistance(p_combo->getCenter(), lhs->front()->getCenter()) < getDistance(p_combo->getCenter(), rhs->front()->getCenter()); }); - min_dis_tracker->get()->update(p_combo, _tick, _gyro_data); - tracker_set.erase(*min_dis_tracker); + min_dis_tracker->update(p_combo); + tracker_set.erase(min_dis_tracker); } // 没有匹配到的序列传入 nullptr - for (const auto &p_tracker : tracker_set) - p_tracker->update(nullptr, _tick, _gyro_data); + for (auto p_tracker : tracker_set) + p_tracker->update(_tick, _gyro_data); } - // 如果当前帧识别到的装甲板数量 = 序列数量 + // 如果当前帧识别到的视觉标签 `rm::Tag` 数量 = 序列数量 else { - // 初始化装甲板集合 + // 初始化视觉标签 `rm::Tag` 集合 std::unordered_set tag_set(combos.begin(), combos.end()); // 防止出现迭代器非法化的情况，此处使用下标访问 size_t before_size = trackers.size(); // 存储原始 trackers 大小 for (size_t i = 0; i < before_size; i++) { // 离 tracker 最近的 combo - auto min_it = min_element(tag_set.begin(), tag_set.end(), [&](combo::const_ptr combo_1, combo::const_ptr combo_2) { + auto min_it = *min_element(tag_set.begin(), tag_set.end(), [&](combo::const_ptr combo_1, combo::const_ptr combo_2) { return getDistance(combo_1->getCenter(), trackers[i]->front()->getCenter()) < getDistance(combo_2->getCenter(), trackers[i]->front()->getCenter()); }); // 最短距离 - float min_dis = getDistance(min_it->get()->getCenter(), trackers[i]->front()->getCenter()); + float min_dis = getDistance(min_it->getCenter(), trackers[i]->front()->getCenter()); // 判断是否突变 //! @todo 这段掉帧处理需要增加其他信息，保证 tracker 的匹配正确 if (isChange(min_dis)) { // 创建新序列，原来的序列打入 nullptr - trackers[i]->update(nullptr, _tick, _gyro_data); - trackers.emplace_back(PlanarTracker::make_tracker(*min_it)); + trackers[i]->update(_tick, _gyro_data); + trackers.emplace_back(PlanarTracker::make_tracker(min_it)); } else - trackers[i]->update(*min_it, _tick, _gyro_data); - tag_set.erase(*min_it); + trackers[i]->update(min_it); + tag_set.erase(min_it); } } } diff --git a/extra/group/include/rmvl/group/gyro_group.h b/extra/group/include/rmvl/group/gyro_group.h index a233690e..cc1166aa 100644 --- a/extra/group/include/rmvl/group/gyro_group.h +++ b/extra/group/include/rmvl/group/gyro_group.h @@ -35,10 +35,9 @@ class GyroGroup final : public group //! 追踪器状态哈希表 [追踪器 : 追踪器状态] std::unordered_map _tracker_state; - KF66f _center3d_filter; //!< 旋转中心点位置滤波器 + KF63f _center3d_filter; //!< 旋转中心点位置滤波器 - std::deque _center3d_deq; //!< 旋转中心点坐标时间队列（原始数据） - std::deque _rotspeed_deq; //!< 旋转速度时间队列（原始数据） + std::deque _rotspeed_deq; //!< 旋转速度时间队列（原始数据） cv::Vec3f _center3d; //!< 陀螺仪坐标系下旋转中心点坐标（滤波数据） cv::Vec3f _speed3d; //!< 旋转中心点平移速度（滤波数据） diff --git a/extra/group/param/gyro_group.para b/extra/group/param/gyro_group.para index 0d7d7c4e..026da3cc 100644 --- a/extra/group/param/gyro_group.para +++ b/extra/group/param/gyro_group.para @@ -5,5 +5,5 @@ float ROTSPEED_SIZE = 50 # 旋转速度队列长度 float MIN_HIGH_ROT_SPEED = 4.5 # 判定为高速状态的最低转速 float MAX_LOW_ROT_SPEED = 1.5 # 判定为低速状态的最高转速 -Matx66f CENTER3D_Q = Matx66f::diag({1, 1, 1, 0.1, 0.1, 0.1}) # 旋转中心位置滤波器过程噪声协方差矩阵 -Matx66f CENTER3D_R = Matx66f::diag({20, 20, 20, 50, 50, 50}) # 旋转中心位置滤波器测量噪声协方差矩阵 +Matx66f CENTER3D_Q = Matx66f::eye() # 旋转中心位置滤波器过程噪声协方差矩阵 +Matx33f CENTER3D_R = Matx33f::diag({20, 20, 20}) # 旋转中心位置滤波器测量噪声协方差矩阵 diff --git a/extra/group/src/gyro_group/filter_update.cpp b/extra/group/src/gyro_group/filter_update.cpp index 277600f6..8a6213a0 100644 --- a/extra/group/src/gyro_group/filter_update.cpp +++ b/extra/group/src/gyro_group/filter_update.cpp @@ -18,30 +18,17 @@ namespace rm void GyroGroup::updateCenter3DFilter(cv::Vec3f center, float t, cv::Vec3f &update_center, cv::Vec3f &update_speed) { - // 更新位置信息 - _center3d_deq.push_front(center); - if (_center3d_deq.size() > 16) - _center3d_deq.pop_back(); // 设置状态转移矩阵 - _center3d_filter.setA(cv::Matx66f{1, 0, 0, t, 0, 0, - 0, 1, 0, 0, t, 0, - 0, 0, 1, 0, 0, t, - 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 1}); + _center3d_filter.setA({1, 0, 0, t, 0, 0, + 0, 1, 0, 0, t, 0, + 0, 0, 1, 0, 0, t, + 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 1}); // -------------------- 预测 -------------------- _center3d_filter.predict(); // -------------------- 更新 -------------------- - // 获取速度 - if (_center3d_deq.empty()) - RMVL_Error(RMVL_StsBadSize, "\"_center3d_deq\" is empty"); - size_t center3d_num = _center3d_deq.size(); - cv::Vec3f speed = center3d_num >= 4U - ? (_center3d_deq[0] + _center3d_deq[1] - _center3d_deq[2] - _center3d_deq[3]) / (4.f * t) - // 队列长度不够长，则在获取速度时选择首尾取平均值 - : (_center3d_deq.front() - _center3d_deq.back()) / static_cast(center3d_num - 1) / t; - cv::Matx61f correct_val = _center3d_filter.correct({center(0), center(1), center(2), - speed(0), speed(1), speed(2)}); + cv::Matx61f correct_val = _center3d_filter.correct(center); update_center = {correct_val(0), correct_val(1), correct_val(2)}; update_speed = {correct_val(3), correct_val(4), correct_val(5)}; } diff --git a/extra/group/src/gyro_group/init.cpp b/extra/group/src/gyro_group/init.cpp index 67f47d21..3a85c821 100644 --- a/extra/group/src/gyro_group/init.cpp +++ b/extra/group/src/gyro_group/init.cpp @@ -58,7 +58,6 @@ void GyroGroup::initGroup(const std::unordered_set &visible_combo_se for (int i = 0; i < _armor_num; i++) _tracker_state[tracker_vec[i]] = state_vec[i]; // 旋转中心点坐标初始化 - _center3d_deq.emplace_front(group_center3d); _center3d = group_center3d; _center = group_center2d; // 序列可见性初始化 @@ -76,9 +75,7 @@ void GyroGroup::initFilter() // 初始化旋转中心点位置滤波器 _center3d_filter.setQ(para::gyro_group_param.CENTER3D_Q); _center3d_filter.setR(para::gyro_group_param.CENTER3D_R); - _center3d_filter.init(cv::Matx61f(_center3d(0), _center3d(1), _center3d(2), - _speed3d(0), _speed3d(1), _speed3d(2)), - 1e-2); + _center3d_filter.init(cv::Matx61f(_center3d(0), _center3d(1), _center3d(2), 0, 0, 0), 1e-2); } } // namespace rm diff --git a/extra/group/src/gyro_group/sync.cpp b/extra/group/src/gyro_group/sync.cpp index 6d9d3be4..5aeea58f 100644 --- a/extra/group/src/gyro_group/sync.cpp +++ b/extra/group/src/gyro_group/sync.cpp @@ -106,7 +106,7 @@ void GyroGroup::sync(const GyroData &gyro_data, double tick) cv::Matx33f new_rmat = rot * p_tracker->getExtrinsics().R(); // 新的旋转矩阵 cv::Vec3f new_tvec = _center3d + rot * center2combo + cv::Vec3f(0, current_state.delta_y(), 0); // 新的平移向量 auto p_armor = constructComboForced(p_tracker->front(), _gyro_data, new_rmat, new_tvec, _tick); - std::const_pointer_cast(p_tracker)->update(p_armor, _tick, _gyro_data); + p_tracker->update(p_armor); } } // 利用可见追踪器信息，完成强制构造与不可见追踪器的更新 @@ -128,7 +128,7 @@ void GyroGroup::sync(const GyroData &gyro_data, double tick) cv::Matx33f new_rmat = rot * visible_tracker->getExtrinsics().R(); // 新的旋转矩阵 cv::Vec3f new_tvec = _center3d + rot * center2combo + cv::Vec3f(0, current_state.delta_y(), 0); // 新的平移向量 auto p_armor = constructComboForced(visible_tracker->front(), _gyro_data, new_rmat, new_tvec, _tick); - std::const_pointer_cast(p_tracker)->update(p_armor, _tick, _gyro_data); + p_tracker->update(p_armor); } } else // visible_num == 2 @@ -162,7 +162,7 @@ void GyroGroup::sync(const GyroData &gyro_data, double tick) auto p_armor = constructComboForced(visible_trackers[i]->front(), _gyro_data, new_rmat, new_tvec, _tick); // 同步高度差 current_state.delta_y(_tracker_state[visible_trackers[i]].delta_y()); - std::const_pointer_cast(p_tracker)->update(p_armor, _tick, _gyro_data); + p_tracker->update(p_armor); } } // ----------------------【更新 RobotType】---------------------- diff --git a/extra/predictor/src/armor_predictor/armor_predictor.cpp b/extra/predictor/src/armor_predictor/armor_predictor.cpp index 520c628c..a832e5fe 100755 --- a/extra/predictor/src/armor_predictor/armor_predictor.cpp +++ b/extra/predictor/src/armor_predictor/armor_predictor.cpp @@ -26,11 +26,15 @@ PredictInfo ArmorPredictor::predict(const std::vector &groups, const for (auto p_tracker : p_group->data()) { double tf = (tof.find(p_tracker) == tof.end()) ? 0. : tof.at(p_tracker); - - double dB_yaw = p_tracker->getSpeed().x * para::armor_predictor_param.YAW_B; - double dB_pitch = p_tracker->getSpeed().y * para::armor_predictor_param.PITCH_B; - double dKt_yaw = p_tracker->getSpeed().x * para::armor_predictor_param.YAW_K * tf; - double dKt_pitch = p_tracker->getSpeed().y * para::armor_predictor_param.PITCH_K * tf; + // 获取陀螺仪角速度 + cv::Point2f gyro_speed = {p_tracker->front()->getGyroData().rotation.yaw_speed, + p_tracker->front()->getGyroData().rotation.pitch_speed}; + // 计算用于预测的合成角速度 + cv::Point2f speed = p_tracker->getSpeed() + gyro_speed; + double dB_yaw = speed.x * para::armor_predictor_param.YAW_B; + double dB_pitch = speed.y * para::armor_predictor_param.PITCH_B; + double dKt_yaw = speed.x * para::armor_predictor_param.YAW_K * tf; + double dKt_pitch = speed.y * para::armor_predictor_param.PITCH_K * tf; // 更新预测信息 info.static_prediction[p_tracker][YAW] = dB_yaw; diff --git a/extra/predictor/src/rune_predictor/rune_predictor.cpp b/extra/predictor/src/rune_predictor/rune_predictor.cpp index a2f8af16..9b6a5491 100755 --- a/extra/predictor/src/rune_predictor/rune_predictor.cpp +++ b/extra/predictor/src/rune_predictor/rune_predictor.cpp @@ -37,10 +37,8 @@ PredictInfo RunePredictor::predict(const std::vector &groups, const double dKt = p_rune_tracker->getRotatedSpeed() * para::rune_predictor_param.PREDICT_K * tf; double dB = p_rune_tracker->getRotatedSpeed() * para::rune_predictor_param.PREDICT_B; // 更新预测量 - auto &dynamic_vec = info.dynamic_prediction[p_tracker]; - dynamic_vec[ANG_Z] = dKt; - auto &static_vec = info.static_prediction[p_tracker]; - static_vec[ANG_Z] = dB; + info.dynamic_prediction.at(p_tracker)[ANG_Z] = dKt; + info.static_prediction.at(p_tracker)[ANG_Z] = dB; } return info; } diff --git a/extra/predictor/src/spi_rune_predictor/predict.cpp b/extra/predictor/src/spi_rune_predictor/predict.cpp index eccea564..d3450fb1 100644 --- a/extra/predictor/src/spi_rune_predictor/predict.cpp +++ b/extra/predictor/src/spi_rune_predictor/predict.cpp @@ -70,19 +70,18 @@ PredictInfo SpiRunePredictor::predict(const std::vector &groups, con // ------------------- 系统参数辨识过程 ------------------- auto p_rune_group = RuneGroup::cast(groups.front()); const auto &trackers = p_rune_group->data(); - size_t trackers_num = trackers.size(); const auto &raw_datas = p_rune_group->getRawDatas(); identifier(raw_datas); // ---------------------- 预测量计算 ---------------------- - for (size_t i = 0; i < trackers_num; ++i) + for (auto p_tracker : trackers) { // 静态预测角度增量 - auto dB = staticPredict(trackers[i]); + auto dB = staticPredict(p_tracker); // 动态预测角度增量 - double tf = (tof.find(trackers[i]) == tof.end()) ? 0. : tof.at(trackers[i]); + double tf = (tof.find(p_tracker) == tof.end()) ? 0. : tof.at(p_tracker); auto dKt = anglePredict(raw_datas, tf); - info.static_prediction[trackers[i]](ANG_Z) = dB; - info.dynamic_prediction[trackers[i]](ANG_Z) = dKt; + info.static_prediction[p_tracker](ANG_Z) = dB; + info.dynamic_prediction[p_tracker](ANG_Z) = dKt; } return info; } diff --git a/extra/predictor/test/test_spi_rune_predictor.cpp b/extra/predictor/test/test_spi_rune_predictor.cpp index eb7d9585..71a8511f 100644 --- a/extra/predictor/test/test_spi_rune_predictor.cpp +++ b/extra/predictor/test/test_spi_rune_predictor.cpp @@ -52,7 +52,7 @@ TEST(Run_Accuracy, data_from_0_300) double tick = Timer::now(); auto new_rune = Rune::make_combo(new_target, p_center, gyro_data, tick, true); - p_tracker->update(new_rune, tick, gyro_data); + p_tracker->update(new_rune); p_group->sync(gyro_data, tick); // predict diff --git a/extra/tracker/include/rmvl/tracker/gyro_tracker.h b/extra/tracker/include/rmvl/tracker/gyro_tracker.h index bfd47296..cb731a02 100644 --- a/extra/tracker/include/rmvl/tracker/gyro_tracker.h +++ b/extra/tracker/include/rmvl/tracker/gyro_tracker.h @@ -38,9 +38,9 @@ class GyroTracker final : public tracker cv::Vec2f _pose; //!< 修正后的装甲板姿态法向量 float _rotspeed{}; //!< 绕 y 轴自转角速度（俯视顺时针为正，滤波数据，弧度） - KF44f _motion_filter; //!< 目标转角滤波器 - KF66f _center3d_filter; //!< 位置滤波器 - KF44f _pose_filter; //!< 姿态滤波器 + KF42f _motion_filter; //!< 目标转角滤波器 + KF63f _center3d_filter; //!< 位置滤波器 + KF42f _pose_filter; //!< 姿态滤波器 std::deque _type_deque; //!< 装甲板状态队列（数字） @@ -76,14 +76,14 @@ class GyroTracker final : public tracker */ static inline GyroTracker::const_ptr cast(tracker::const_ptr p_tracker) { return std::dynamic_pointer_cast(p_tracker); } + [[deprecated]] void update(double, const GyroData &) override{}; + /** - * @brief 更新时间序列 + * @brief 使用捕获的 `combo` 更新平面目标追踪器 * - * @param[in] p_armor 传入 tracker 的组合体 - * @param[in] tick 时间点 - * @param[in] gyro_data 云台数据 + * @param[in] p_combo 待传入 tracker 的平面目标，必须严格保证不为空 */ - void update(combo::ptr p_armor, double tick, const GyroData &gyro_data) override; + void update(combo::ptr p_combo) override; /** * @brief 更新消失状态 diff --git a/extra/tracker/include/rmvl/tracker/planar_tracker.h b/extra/tracker/include/rmvl/tracker/planar_tracker.h index 6f3ac1cf..dc0a45c5 100755 --- a/extra/tracker/include/rmvl/tracker/planar_tracker.h +++ b/extra/tracker/include/rmvl/tracker/planar_tracker.h @@ -26,8 +26,8 @@ namespace rm class PlanarTracker final : public tracker { private: - KF22f _distance_filter; //!< 距离滤波器 - KF44f _motion_filter; //!< 运动滤波器 + KF21f _distance_filter; //!< 距离滤波器 + KF42f _motion_filter; //!< 运动滤波器 float _last_distance = 0; //!< 目标上一帧的距离 std::deque _relative_speeds; //!< 图像速度的容器 std::deque _type_deque; //!< 状态队列 @@ -51,13 +51,19 @@ class PlanarTracker final : public tracker static inline PlanarTracker::ptr make_tracker(combo::ptr p_combo) { return std::make_shared(p_combo); } /** - * @brief 更新时间序列 + * @brief 丢失目标时，使用时间点和陀螺仪数据更新平面目标追踪器 * - * @param[in] p_combo 传入 tracker 的平面目标 * @param[in] tick 时间点 - * @param[in] gyro_data 云台数据 + * @param[in] gyro_data 陀螺仪数据 */ - void update(combo::ptr p_combo, double tick, const GyroData &gyro_data) override; + void update(double tick, const GyroData &gyro_data) override; + + /** + * @brief 使用捕获的 `combo` 更新平面目标追踪器 + * + * @param[in] p_combo 待传入 tracker 的平面目标，必须严格保证不为空 + */ + void update(combo::ptr p_combo) override; private: /** diff --git a/extra/tracker/include/rmvl/tracker/rune_tracker.h b/extra/tracker/include/rmvl/tracker/rune_tracker.h index 6d957a4a..b247e794 100755 --- a/extra/tracker/include/rmvl/tracker/rune_tracker.h +++ b/extra/tracker/include/rmvl/tracker/rune_tracker.h @@ -31,10 +31,9 @@ namespace rm */ class RuneTracker final : public tracker { - int _round{}; //!< 圈数 - float _rotated_speed; //!< 神符旋转角速度 - std::deque _angles; //!< 角度容器 - KF22f _filter; //!< 神符的角度滤波器 + int _round{}; //!< 圈数 + float _rotated_speed; //!< 神符旋转角速度 + KF21f _filter; //!< 神符的角度滤波器 public: using ptr = std::shared_ptr; @@ -73,13 +72,19 @@ class RuneTracker final : public tracker static inline RuneTracker::const_ptr cast(tracker::const_ptr p_tracker) { return std::dynamic_pointer_cast(p_tracker); } /** - * @brief 更新时间序列 + * @brief 使用捕获的 `rm::Rune` 组合体更新追踪器 * * @param[in] p_rune 神符共享指针 + */ + void update(combo::ptr p_rune) override; + + /** + * @brief `rm::Rune` 目标丢失，使用时间点和陀螺仪数据更新追踪器 + * * @param[in] tick 当前时间点 * @param[in] gyro_data 云台数据 */ - void update(combo::ptr p_rune, double tick, const GyroData &gyro_data) override; + void update(double tick, const GyroData &gyro_data) override; /** * @brief 滤波器初始化 diff --git a/extra/tracker/include/rmvl/tracker/tracker.h b/extra/tracker/include/rmvl/tracker/tracker.h index d2fea325..60d880ca 100755 --- a/extra/tracker/include/rmvl/tracker/tracker.h +++ b/extra/tracker/include/rmvl/tracker/tracker.h @@ -28,15 +28,15 @@ class tracker std::deque _combo_deque; //!< 组合体时间队列 uint32_t _vanish_num{}; //!< 消失帧数 - RMStatus _type{}; //!< 追踪器类型 - float _height{}; //!< 追踪器高度（可表示修正后） - float _width{}; //!< 追踪器宽度（可表示修正后） - float _angle{}; //!< 追踪器角度（可表示修正后） - cv::Point2f _center; //!< 追踪器中心点（可表示修正后） - cv::Point2f _relative_angle; //!< 相对目标转角（可表示修正后） - std::vector _corners; //!< 追踪器角点（可表示修正后） - CameraExtrinsics _extrinsic; //!< 相机外参（可表示修正后） - cv::Point2f _speed; //!< 相对目标转角速度 + RMStatus _type{}; //!< 追踪器类型 + float _height{}; //!< 追踪器高度（可表示修正后） + float _width{}; //!< 追踪器宽度（可表示修正后） + float _angle{}; //!< 追踪器角度（可表示修正后） + cv::Point2f _center; //!< 追踪器中心点（可表示修正后） + cv::Point2f _relative_angle; //!< 相对目标转角（可表示修正后） + std::vector _corners; //!< 追踪器角点（可表示修正后） + CameraExtrinsics _extrinsic; //!< 相机外参（可表示修正后） + cv::Point2f _speed; //!< 相对目标转角速度 public: using ptr = std::shared_ptr; @@ -47,13 +47,20 @@ class tracker virtual ~tracker() = default; /** - * @brief 更新追踪器 + * @brief 使用已捕获的 `combo` 更新追踪器 + * + * @param[in] tick 当前时间点 + * @param[in] gyro_data 当前陀螺仪信息 + */ + virtual void update(combo::ptr p_combo) = 0; + + /** + * @brief 未捕获 `combo`，但使用其余数据更新追踪器（即目标丢失时的操作） * - * @param[in] p_combo 更新的组合体 * @param[in] tick 当前时间点 * @param[in] gyro_data 当前陀螺仪信息 */ - virtual void update(combo::ptr p_combo, double tick, const GyroData &gyro_data) = 0; + virtual void update(double tick, const GyroData &gyro_data) = 0; //! 获取时间队列中最新的组合体 inline combo::ptr front() const { return _combo_deque.front(); } @@ -108,13 +115,14 @@ class DefaultTracker final : public tracker static inline DefaultTracker::ptr make_tracker(combo::ptr p_combo) { return std::make_shared(p_combo); } /** - * @brief 更新追踪器 + * @brief 使用已捕获的 `combo` 更新追踪器 * * @param[in] p_combo 更新的组合体 - * @param[in] tick 当前时间点 - * @param[in] gyro_data 当前陀螺仪信息 */ - void update(combo::ptr p_combo, double tick, const GyroData &gyro_data) override; + void update(combo::ptr p_combo) override; + + //! 未捕获 `combo`，仅更新消失帧数 + inline void update(double, const GyroData &) override { _vanish_num++; } private: /** diff --git a/extra/tracker/param/gyro_tracker.para b/extra/tracker/param/gyro_tracker.para index c13363bf..18e882b9 100644 --- a/extra/tracker/param/gyro_tracker.para +++ b/extra/tracker/param/gyro_tracker.para @@ -2,9 +2,9 @@ double SAMPLE_INTERVAL = 15 # 采样时间（单位: ms） float MAX_ROTSPEED = 20 # 模型旋转速度上限（单位: rad/s） float MIN_ROTSPEED = 0 # 模型旋转速度下限（单位: rad/s） -Matx44f MOTION_Q = Matx44f::eye() # 目标转角过程噪声协方差矩阵 -Matx44f MOTION_R = Matx44f::diag({0.5, 0.5, 5, 5}) # 目标转角测量噪声协方差矩阵 -Matx66f POSITION_Q = Matx66f::eye() # 位置过程噪声协方差矩阵 -Matx66f POSITION_R = Matx66f::diag({2, 2, 2, 8, 8, 8}) # 位置测量噪声协方差矩阵 -Matx44f POSE_Q = Matx44f::eye() # 姿态过程噪声协方差矩阵 -Matx44f POSE_R = Matx44f::diag({5, 5, 20, 20}) # 姿态测量噪声协方差矩阵 +Matx44f MOTION_Q = Matx44f::eye() # 目标转角过程噪声协方差矩阵 +Matx22f MOTION_R = Matx22f::diag({0.1, 0.1}) # 目标转角测量噪声协方差矩阵 +Matx66f POSITION_Q = Matx66f::eye() # 位置过程噪声协方差矩阵 +Matx33f POSITION_R = Matx33f::diag({2, 2, 2}) # 位置测量噪声协方差矩阵 +Matx44f POSE_Q = Matx44f::eye() # 姿态过程噪声协方差矩阵 +Matx22f POSE_R = Matx22f::diag({5, 5 20}) # 姿态测量噪声协方差矩阵 diff --git a/extra/tracker/param/planar_tracker.para b/extra/tracker/param/planar_tracker.para index 535eaed0..fd775a17 100644 --- a/extra/tracker/param/planar_tracker.para +++ b/extra/tracker/param/planar_tracker.para @@ -1,5 +1,8 @@ uint32_t TRACK_FRAMES = 4 # 追踪帧数 double SAMPLE_INTERVAL = 15 # 采样时间 -Matx44f Q = Matx44f::eye() # 过程噪声协方差矩阵 -Matx44f R = Matx44f::diag({0.5, 0.5, 4, 4}) # 测量噪声协方差矩阵 +Matx22f DIS_Q = Matx22f::diag({0.1, 0.5}) # 距离状态空间方程，过程噪声协方差 +float DIS_R = 0.1 # 距离状态空间方程，测量噪声协方差 + +Matx44f MOTION_Q = Matx44f::eye() # 运动状态空间方程，过程噪声协方差矩阵 +Matx22f MOTION_R = Matx22f::diag({0.1, 0.1}) # 运动状态空间方程，测量噪声协方差矩阵 diff --git a/extra/tracker/param/rune_tracker.para b/extra/tracker/param/rune_tracker.para index ac384cfd..667ce18b 100644 --- a/extra/tracker/param/rune_tracker.para +++ b/extra/tracker/param/rune_tracker.para @@ -2,6 +2,6 @@ uint32_t TRACK_FRAMES = 4 # 追踪帧数 double SAMPLE_INTERVAL = 10 # 采样时间（单位: ms） Matx22f ROTATE_Q = Matx22f::eye() # 角度滤波器过程噪声协方差矩阵 -Matx22f ROTATE_R = Matx22f::diag({1, 4}) # 角度滤波器测量噪声协方差矩阵 +float ROTATE_R = 1 # 角度滤波器测量噪声协方差矩阵 Matx44f MOTION_Q = Matx44f::eye() # 运动滤波器过程噪声协方差矩阵 Matx44f MOTION_R = Matx44f::diag({1, 1, 4, 4}) # 运动滤波器测量噪声协方差矩阵 diff --git a/extra/tracker/src/default_tracker.cpp b/extra/tracker/src/default_tracker.cpp index 21c95d27..ebc2ebe5 100644 --- a/extra/tracker/src/default_tracker.cpp +++ b/extra/tracker/src/default_tracker.cpp @@ -29,10 +29,11 @@ rm::DefaultTracker::DefaultTracker(rm::combo::ptr p_combo) : rm::tracker() updateData(p_combo); } -void rm::DefaultTracker::update(rm::combo::ptr p_combo, double, const rm::GyroData &) +void rm::DefaultTracker::update(rm::combo::ptr p_combo) { updateData(p_combo); _combo_deque.push_front(p_combo); if (_combo_deque.size() > 32U) _combo_deque.pop_back(); + _vanish_num = 0; } diff --git a/extra/tracker/src/gyro_tracker/filter_update.cpp b/extra/tracker/src/gyro_tracker/filter_update.cpp index d7813e22..28cdf7fc 100644 --- a/extra/tracker/src/gyro_tracker/filter_update.cpp +++ b/extra/tracker/src/gyro_tracker/filter_update.cpp @@ -9,8 +9,6 @@ * */ -#include - #include "rmvl/tracker/gyro_tracker.h" #include "rmvlpara/combo/armor.h" @@ -23,8 +21,8 @@ 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); // 距离的过程噪声仔细调整 + _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, 1e-2); @@ -45,92 +43,50 @@ void GyroTracker::initFilter() void GyroTracker::updateMotionFilter() { float t = _duration; - // Set the state transition matrix: A - _motion_filter.setA(cv::Matx44f{ - 1, 0, t, 0, - 0, 1, 0, t, - 0, 0, 1, 0, - 0, 0, 0, 1}); - // Kalman predict + // 设置状态转移矩阵 + _motion_filter.setA({1, 0, t, 0, + 0, 1, 0, t, + 0, 0, 1, 0, + 0, 0, 0, 1}); + // 预测 _motion_filter.predict(); - // 获取陀螺仪角速度 - cv::Point2f gyro_speed = {at(0)->getGyroData().rotation.yaw_speed, - at(0)->getGyroData().rotation.pitch_speed}; - // 相对角速度 - cv::Point2f relative_speed; - if (_combo_deque.size() >= 4U) - { - relative_speed = (_combo_deque[0]->getRelativeAngle() + - _combo_deque[1]->getRelativeAngle() - - _combo_deque[2]->getRelativeAngle() - - _combo_deque[3]->getRelativeAngle()) / - (4.f * t); - } - // 队列长度不够长，则在获取速度时选择首尾取平均值 - else if (size() > 1 && size() < 4) - { - relative_speed = (front()->getRelativeAngle() - - back()->getRelativeAngle()) / - static_cast(size() - 1) / t; - } - else - relative_speed = -gyro_speed; // 确保获取的绝对速度为 0 - // 绝对速度 - _speed = relative_speed + gyro_speed; - - cv::Matx41f result = _motion_filter.correct({front()->getRelativeAngle().x, // 位置_x - front()->getRelativeAngle().y, // 位置_y - _speed.x, // 速度_x - _speed.y}); // 速度_y - + // 更新 + auto result = _motion_filter.correct(front()->getRelativeAngle()); _relative_angle = {result(0), result(1)}; // 相对角度滤波数值 - _speed = {result(2), result(3)}; // 绝对速度滤波数值 + _speed = {result(2), result(3)}; // 相对速度滤波数值 } void GyroTracker::updatePositionFilter() { float t = _duration; // 设置状态转移矩阵 - _center3d_filter.setA(cv::Matx66f{1, 0, 0, t, 0, 0, - 0, 1, 0, 0, t, 0, - 0, 0, 1, 0, 0, t, - 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 1}); - // 距离变化速度 - cv::Vec3f delta_tvec = 0.f; - if (_combo_deque.size() >= 4) - delta_tvec = (at(0)->getExtrinsics().tvec() + at(1)->getExtrinsics().tvec() - - at(2)->getExtrinsics().tvec() - at(3)->getExtrinsics().tvec()) / - (4.f * t); - else if (_combo_deque.size() > 1 && _combo_deque.size() < 4) - delta_tvec = (front()->getExtrinsics().tvec() - back()->getExtrinsics().tvec()) / - (static_cast(_combo_deque.size() - 1) * t); + _center3d_filter.setA({1, 0, 0, t, 0, 0, + 0, 1, 0, 0, t, 0, + 0, 0, 1, 0, 0, t, + 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 1}); // 预测 _center3d_filter.predict(); // 更新 cv::Vec3f tvec = at(0)->getExtrinsics().tvec(); - cv::Matx61f correct_position = _center3d_filter.correct({tvec[0], tvec[1], tvec[2], - delta_tvec[0], delta_tvec[1], delta_tvec[2]}); - cv::Vec3f correct_tvec = {correct_position(0), correct_position(1), correct_position(2)}; - _extrinsic.tvec(correct_tvec); + cv::Matx61f correct_position = _center3d_filter.correct(tvec); + _extrinsic.tvec({correct_position(0), correct_position(1), correct_position(2)}); } void GyroTracker::updatePoseFilter() { float t = _duration; // 设置状态转移矩阵 - _pose_filter.setA(cv::Matx44f{1, 0, t, 0, - 0, 1, 0, t, - 0, 0, 1, 0, - 0, 0, 0, 1}); - // 姿态变化速度 - cv::Vec2f dpose = (Armor::cast(at(0))->getPose() - Armor::cast(at(1))->getPose()) / t; + _pose_filter.setA({1, 0, t, 0, + 0, 1, 0, t, + 0, 0, 1, 0, + 0, 0, 0, 1}); // 预测 _pose_filter.predict(); // 更新 cv::Vec2f pose = Armor::cast(front())->getPose(); - cv::Matx41f correct_pose = _pose_filter.correct({pose(0), pose(1), dpose(0), dpose(1)}); + cv::Matx41f correct_pose = _pose_filter.correct(pose); _pose = {correct_pose(0), correct_pose(1)}; } diff --git a/extra/tracker/src/gyro_tracker/gyro_tracker.cpp b/extra/tracker/src/gyro_tracker/gyro_tracker.cpp index 272a89aa..ac62ac08 100755 --- a/extra/tracker/src/gyro_tracker/gyro_tracker.cpp +++ b/extra/tracker/src/gyro_tracker/gyro_tracker.cpp @@ -41,7 +41,7 @@ GyroTracker::GyroTracker(combo::ptr p_armor) initFilter(); } -void GyroTracker::update(combo::ptr p_armor, double, const GyroData &) +void GyroTracker::update(combo::ptr p_armor) { if (p_armor == nullptr) RMVL_Error(RMVL_StsBadArg, "Input argument \"p_armor\" is nullptr."); diff --git a/extra/tracker/src/planar_tracker/filter_update.cpp b/extra/tracker/src/planar_tracker/filter_update.cpp index 022290f6..a13c0637 100644 --- a/extra/tracker/src/planar_tracker/filter_update.cpp +++ b/extra/tracker/src/planar_tracker/filter_update.cpp @@ -20,13 +20,13 @@ void PlanarTracker::initFilter() { combo::ptr first_combo = _combo_deque.front(); // 初始化距离滤波器 - _distance_filter.setR(cv::Matx22f::diag({0.01, 0.02})); // 距离为两帧差，误差为两倍 - _distance_filter.setQ(cv::Matx22f::diag({0.01, 0.01})); // 距离的过程噪声随意调整 + _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->getExtrinsics().distance(), 0}; _distance_filter.init(init_dis_vec, 1e-2); - // 初始化运动滤波器，相对角度和角速度 - _motion_filter.setR(para::planar_tracker_param.R); // 距离为两帧差，误差为两倍 - _motion_filter.setQ(para::planar_tracker_param.Q); // 距离的过程噪声仔细调整 + // 初始化运动滤波器 + _motion_filter.setR(para::planar_tracker_param.MOTION_R); + _motion_filter.setQ(para::planar_tracker_param.MOTION_Q); cv::Matx41f init_move_vec = {first_combo->getRelativeAngle().x, first_combo->getRelativeAngle().y, 0, 0}; @@ -36,9 +36,9 @@ void PlanarTracker::initFilter() void PlanarTracker::updateDistanceFilter() { // 设置状态转移矩阵 - _distance_filter.setA(cv::Matx22f{1, 1, - 0, 1}); - // 距离变化速度 / 差分 + _distance_filter.setA({1, 1, + 0, 1}); + // 距离差分 float current_distance = _combo_deque.front()->getExtrinsics().distance(); float delta_distance = current_distance - _last_distance; // 预测 @@ -52,52 +52,23 @@ void PlanarTracker::updateDistanceFilter() void PlanarTracker::updateMotionFilter() { - // 帧差时间 + // 采样时间 float t = 0.f; if (_combo_deque.size() >= 2) t = (_combo_deque.front()->getTick() - _combo_deque.back()->getTick()) / static_cast(_combo_deque.size() - 1); else t = para::planar_tracker_param.SAMPLE_INTERVAL / 1000.; - // Set the state transition matrix: A - _motion_filter.setA(cv::Matx44f{ - 1, 0, t, 0, - 0, 1, 0, t, - 0, 0, 1, 0, - 0, 0, 0, 1}); - // Kalman predict + // 设置状态转移矩阵 + _motion_filter.setA({1, 0, t, 0, + 0, 1, 0, t, + 0, 0, 1, 0, + 0, 0, 0, 1}); + // 预测 _motion_filter.predict(); - // 获取陀螺仪角速度 - cv::Point2f gyro_speed = {at(0)->getGyroData().rotation.yaw_speed, - at(0)->getGyroData().rotation.pitch_speed}; - // 相对角速度 - cv::Point2f relative_speed; - if (_combo_deque.size() >= 4U) - { - relative_speed = (_combo_deque[0]->getRelativeAngle() + - _combo_deque[1]->getRelativeAngle() - - _combo_deque[2]->getRelativeAngle() - - _combo_deque[3]->getRelativeAngle()) / - (4.f * t); - } - // 队列长度不够长，则在获取速度时选择首尾取平均值 - else if (size() > 1 && size() < 4) - { - relative_speed = (front()->getRelativeAngle() - - back()->getRelativeAngle()) / - static_cast(size() - 1) / t; - } - else - relative_speed = -gyro_speed; // 确保获取的绝对速度为 0 - // 绝对速度 - _speed = relative_speed + gyro_speed; - - cv::Matx41f result = _motion_filter.correct({front()->getRelativeAngle().x, // 位置_x - front()->getRelativeAngle().y, // 位置_y - _speed.x, // 速度_x - _speed.y}); // 速度_y - - _relative_angle = {result(0), result(1)}; // 相对角度滤波数值 - _speed = {result(2), result(3)}; // 绝对速度滤波数值 + // 更新 + cv::Matx41f xk = _motion_filter.correct(front()->getRelativeAngle()); + _relative_angle = {xk(0), xk(1)}; // 相对角度滤波数值 + _speed = {xk(2), xk(3)}; // 绝对速度滤波数值 } } // namespace rm diff --git a/extra/tracker/src/planar_tracker/planar_tracker.cpp b/extra/tracker/src/planar_tracker/planar_tracker.cpp index d38455ea..02e52064 100755 --- a/extra/tracker/src/planar_tracker/planar_tracker.cpp +++ b/extra/tracker/src/planar_tracker/planar_tracker.cpp @@ -38,32 +38,27 @@ PlanarTracker::PlanarTracker(combo::ptr p_combo) updateData(p_combo); } -void PlanarTracker::update(combo::ptr p_combo, double tick, const GyroData &gyro_data) +void PlanarTracker::update(combo::ptr p_combo) { if (p_combo == nullptr) - { - ++_vanish_num; - vanishProcess(tick, gyro_data); - } - else - { - updateData(p_combo); - _combo_deque.emplace_front(p_combo); - // 更新状态 - updateType(p_combo->getType()); - // 重置丢失帧数 - _vanish_num = 0; + RMVL_Error(RMVL_StsBadArg, "Pointer of the input argument combo::ptr is nullptr"); + + updateData(p_combo); + _combo_deque.emplace_front(p_combo); + // 更新状态 + updateType(p_combo->getType()); + // 重置丢失帧数 + _vanish_num = 0; + + // 更新距离 KF + updateDistanceFilter(); + // 更新平面运动轨迹 KF + updateMotionFilter(); - // 更新距离 KF - updateDistanceFilter(); - // 更新平面运动轨迹 KF - updateMotionFilter(); - } if (_combo_deque.size() >= 12U) _combo_deque.pop_back(); } - void PlanarTracker::updateType(RMStatus stat) { _type = stat; diff --git a/extra/tracker/src/planar_tracker/vanish_process.cpp b/extra/tracker/src/planar_tracker/vanish_process.cpp index 8d503897..32afeb23 100644 --- a/extra/tracker/src/planar_tracker/vanish_process.cpp +++ b/extra/tracker/src/planar_tracker/vanish_process.cpp @@ -11,16 +11,14 @@ #include "rmvl/tracker/planar_tracker.h" -#include "rmvlpara/camera/camera.h" -#include "rmvlpara/tracker/planar_tracker.h" - namespace rm { -void PlanarTracker::vanishProcess([[maybe_unused]] double tick, [[maybe_unused]] const GyroData &gyro_data) +void PlanarTracker::update([[maybe_unused]] double tick, [[maybe_unused]] const GyroData &gyro_data) { - if (_combo_deque.empty() || _vanish_num == 0) + if (_combo_deque.empty()) return; + _vanish_num++; //! @note 后续完成 #10 后再进行完善 combo::ptr combo = _combo_deque.front(); diff --git a/extra/tracker/src/rune_tracker/filter_update.cpp b/extra/tracker/src/rune_tracker/filter_update.cpp index f2065216..2d54d32c 100644 --- a/extra/tracker/src/rune_tracker/filter_update.cpp +++ b/extra/tracker/src/rune_tracker/filter_update.cpp @@ -22,30 +22,19 @@ void RuneTracker::initFilter(float init_angle, float init_speed) auto first_combo = _combo_deque.front(); // 初始化旋转滤波器 - _filter.setR(para::rune_tracker_param.ROTATE_R); + _filter.setR({para::rune_tracker_param.ROTATE_R}); _filter.setQ(para::rune_tracker_param.ROTATE_Q); - cv::Matx21f init_rotate_vec = {init_angle, init_speed}; - _filter.init(init_rotate_vec, 1e-2); + _filter.init({init_angle, init_speed}, 1e-2); } void RuneTracker::updateRotateFilter(float t) { - // 匀速运动模型 - _filter.setA(cv::Matx22f{1, t, 0, 1}); - auto pre_vec = _filter.predict(); - // 神符速度默认为卡尔曼上次根据过往数据的预测结果 - float raw_speed = pre_vec(1); - // 更新速度 - if (_angles.size() > 1) - { - if (_angles.size() >= 4) - raw_speed = (_angles[0] + _angles[1] - _angles[2] - _angles[3]) / (4 * t); - else - raw_speed = (_angles.front() - _angles.back()) / (static_cast(_angles.size() - 1) * t); - } - - cv::Matx21f result = _filter.correct({_angle, raw_speed}); - // 更新速度 + _filter.setA({1, t, + 0, 1}); + // 预测 + _filter.predict(); + // 更新 + cv::Matx21f result = _filter.correct({_angle}); _rotated_speed = result(1); } diff --git a/extra/tracker/src/rune_tracker/rune_tracker.cpp b/extra/tracker/src/rune_tracker/rune_tracker.cpp index 9b592730..fffb867b 100755 --- a/extra/tracker/src/rune_tracker/rune_tracker.cpp +++ b/extra/tracker/src/rune_tracker/rune_tracker.cpp @@ -37,13 +37,10 @@ RuneTracker::RuneTracker(combo::ptr p_rune) updateFromRune(p_rune); } -void RuneTracker::update(combo::ptr p_rune, double tick, const GyroData &gyro_data) +void RuneTracker::update(combo::ptr p_rune) { if (p_rune == nullptr) - { - ++_vanish_num; - vanishProcess(tick, gyro_data); - } + RMVL_Error(RMVL_StsBadArg, "Pointer of the input argument combo::ptr is nullptr"); else { _combo_deque.push_front(p_rune); @@ -51,7 +48,6 @@ void RuneTracker::update(combo::ptr p_rune, double tick, const GyroData &gyro_da updateFromRune(p_rune); // 更新神符转动的圈数，并计算在考虑圈数时的完全值 _angle = calculateTotalAngle(); - _angles.push_front(_angle); // 更新滤波器 float t = 0.f; if (_combo_deque.size() >= 2) @@ -62,10 +58,8 @@ void RuneTracker::update(combo::ptr p_rune, double tick, const GyroData &gyro_da // 重置消失帧数 _vanish_num = 0; } - if (_combo_deque.size() >= 32U) + if (_combo_deque.size() >= 8U) _combo_deque.pop_back(); - if (_angles.size() >= 32U) - _angles.pop_back(); } float RuneTracker::calculateTotalAngle() diff --git a/extra/tracker/src/rune_tracker/vanish_process.cpp b/extra/tracker/src/rune_tracker/vanish_process.cpp index c9d74acb..9014d3ac 100644 --- a/extra/tracker/src/rune_tracker/vanish_process.cpp +++ b/extra/tracker/src/rune_tracker/vanish_process.cpp @@ -27,7 +27,7 @@ namespace rm * @param[in] gyro_data 最新陀螺仪数据 * @return 构造出的神符 */ -Rune::ptr runeConstructForced(Rune::ptr ref_rune, float delta_angle, double tick, const GyroData &gyro_data) +static Rune::ptr runeConstructForced(Rune::ptr ref_rune, float delta_angle, double tick, const GyroData &gyro_data) { auto p_rune_target = RuneTarget::cast(ref_rune->at(0)); auto p_rune_center = RuneCenter::cast(ref_rune->at(1)); @@ -56,19 +56,20 @@ Rune::ptr runeConstructForced(Rune::ptr ref_rune, float delta_angle, double tick return Rune::make_combo(p_new_rune_target, p_new_rune_center, gyro_data, tick, true); } -void RuneTracker::vanishProcess(double tick, const GyroData &gyro_data) +void RuneTracker::update(double tick, const GyroData &gyro_data) { // 判空 - if (_combo_deque.empty() || _vanish_num == 0) + if (_combo_deque.empty()) return; + _vanish_num++; // 获取帧差时间 float t = 0.f; if (_combo_deque.size() >= 2) t = (_combo_deque.front()->getTick() - _combo_deque.back()->getTick()) / static_cast(_combo_deque.size() - 1); else t = para::rune_tracker_param.SAMPLE_INTERVAL / 1000.; - _filter.setA(cv::Matx22f{1, t, - 0, 1}); + _filter.setA({1, t, + 0, 1}); // 旋转状态先验估计 auto rotate_pre = _filter.predict(); @@ -83,7 +84,7 @@ void RuneTracker::vanishProcess(double tick, const GyroData &gyro_data) _relative_angle = calculateRelativeAngle(para::camera_param.cameraMatrix, relative_center); // 直接更新后验估计 - _filter.correct(rotate_pre); + _filter.correct({rotate_pre(0)}); _combo_deque.emplace_front(p_rune); } diff --git a/extra/tracker/test/gyro_tracker_test.cpp b/extra/tracker/test/gyro_tracker_test.cpp index ee204e61..e5663917 100644 --- a/extra/tracker/test/gyro_tracker_test.cpp +++ b/extra/tracker/test/gyro_tracker_test.cpp @@ -94,7 +94,7 @@ TEST_F(GyroTrackerTest, tracker_update_with_1_armor) Armor::ptr armor = buildArmor(cv::Point(500, 300), 8); tracker::ptr p_tracker = GyroTracker::make_tracker(armor); Armor::ptr armor2 = buildArmor(cv::Point(505, 300), 8); - p_tracker->update(armor2, tick, gyro_data); + p_tracker->update(armor2); EXPECT_EQ(p_tracker->size(), 2); EXPECT_NE(p_tracker->getRelativeAngle(), armor2->getRelativeAngle()); } diff --git a/extra/tracker/test/planar_tracker_test.cpp b/extra/tracker/test/planar_tracker_test.cpp index 8739e67a..410ce3b0 100644 --- a/extra/tracker/test/planar_tracker_test.cpp +++ b/extra/tracker/test/planar_tracker_test.cpp @@ -92,19 +92,18 @@ TEST_F(PlanarTrackerTest, tracker_update_with_1_armor) rm::Armor::ptr armor = buildArmor(cv::Point(500, 300), 8); rm::tracker::ptr p_tracker = rm::PlanarTracker::make_tracker(armor); rm::Armor::ptr armor2 = buildArmor(cv::Point(505, 300), 8); - p_tracker->update(armor2, tick, gyro_data); + p_tracker->update(armor2); EXPECT_EQ(p_tracker->size(), 2); EXPECT_NE(p_tracker->getRelativeAngle(), armor2->getRelativeAngle()); } -// 追踪器传入空掉帧处理功能验证 +// 追踪器掉帧处理功能验证 TEST_F(PlanarTrackerTest, tracker_update_with_none) { // 传入装甲板后传入空 rm::Armor::ptr armor = buildArmor(cv::Point(500, 300), 8); rm::tracker::ptr p_tracker = rm::PlanarTracker::make_tracker(armor); - rm::Armor::ptr armor2 = nullptr; - p_tracker->update(armor2, tick, gyro_data); + p_tracker->update(tick, gyro_data); EXPECT_EQ(p_tracker->size(), 2); EXPECT_EQ(p_tracker->front()->getCenter(), armor->getCenter()); EXPECT_EQ(p_tracker->getVanishNumber(), 1);