Skip to content

Commit

Permalink
修改 KalmanFilter 的使用方式
Browse files Browse the repository at this point in the history 
1. 删除无法直接被观测的观测量
2. 为 tracker 模块提供新的 update 接口
3. 同步所有功能模块对于 tracker::update() 的调用方式
  • Loading branch information
@zhaoxi-scut
zhaoxi-scut committed Mar 21, 2024
1 parent 212a798 commit 363143a
Show file tree
Hide file tree
Showing 32 changed files with 226 additions and 317 deletions.
8 changes: 4 additions & 4 deletions extra/decider/src/translation_decider/translation_decider.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,18 +37,18 @@ DecideInfo TranslationDecider::decide(const std::vector<group::ptr> &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; // 加入预测
Expand Down
10 changes: 5 additions & 5 deletions extra/detector/src/armor_detector/match.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,7 +67,7 @@ void ArmorDetector::matchArmors(std::vector<tracker::ptr> &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);
}
// 没有匹配到的装甲板作为新的序列
Expand All @@ -86,12 +86,12 @@ void ArmorDetector::matchArmors(std::vector<tracker::ptr> &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
Expand All @@ -113,11 +113,11 @@ void ArmorDetector::matchArmors(std::vector<tracker::ptr> &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);
}
}
Expand Down
4 changes: 2 additions & 2 deletions extra/detector/src/gyro_detector/match.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -286,8 +286,8 @@ void GyroDetector::matchOneGroup(group::ptr group, const std::vector<combo::ptr>
(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); // 设置为可见
}
Expand Down
23 changes: 11 additions & 12 deletions extra/detector/src/rune_detector/match.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,7 @@ void RuneDetector::matchRunes(std::vector<tracker::ptr> &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
Expand All @@ -67,17 +67,16 @@ void RuneDetector::matchRunes(std::vector<tracker::ptr> &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
Expand All @@ -97,10 +96,10 @@ void RuneDetector::matchRunes(std::vector<tracker::ptr> &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);
Expand Down
40 changes: 20 additions & 20 deletions extra/detector/src/tag_detector/detect.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -125,72 +125,72 @@ void TagDetector::match(std::vector<tracker::ptr> &trackers, const std::vector<c
return;
}

// 如果当前帧识别到的装甲板数量 > 序列数量
// 如果当前帧识别到的视觉标签 `rm::Tag` 数量 > 序列数量
if (combos.size() > trackers.size())
{
// 初始化装甲板集合
// 初始化视觉标签 `rm::Tag` 集合
std::unordered_set<combo::ptr> tag_set(combos.begin(), combos.end());
// 距离最近的装甲板匹配到相应的序列中, 并 update
// 距离最近的视觉标签 `rm::Tag` 匹配到相应的序列中, 并 update
for (auto p_tracker : trackers)
{
// 离 p_tracker 最近的 combo 及其距离
auto min_it = min_element(combos.begin(), combos.end(), [&](combo::const_ptr lhs, combo::const_ptr rhs) {
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);
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::ptr> 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<combo::ptr> 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);
}
}
}
Expand Down
5 changes: 2 additions & 3 deletions extra/group/include/rmvl/group/gyro_group.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,10 +35,9 @@ class GyroGroup final : public group
//! 追踪器状态哈希表 [追踪器 : 追踪器状态]
std::unordered_map<tracker::ptr, TrackerState> _tracker_state;

KF66f _center3d_filter; //!< 旋转中心点位置滤波器
KF63f _center3d_filter; //!< 旋转中心点位置滤波器

std::deque<cv::Vec3f> _center3d_deq; //!< 旋转中心点坐标时间队列（原始数据）
std::deque<float> _rotspeed_deq; //!< 旋转速度时间队列（原始数据）
std::deque<float> _rotspeed_deq; //!< 旋转速度时间队列（原始数据）

cv::Vec3f _center3d; //!< 陀螺仪坐标系下旋转中心点坐标（滤波数据）
cv::Vec3f _speed3d; //!< 旋转中心点平移速度（滤波数据）
Expand Down
4 changes: 2 additions & 2 deletions extra/group/param/gyro_group.para
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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}) # 旋转中心位置滤波器测量噪声协方差矩阵
27 changes: 7 additions & 20 deletions extra/group/src/gyro_group/filter_update.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<float>(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)};
}
Expand Down
5 changes: 1 addition & 4 deletions extra/group/src/gyro_group/init.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,6 @@ void GyroGroup::initGroup(const std::unordered_set<combo::ptr> &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;
// 序列可见性初始化
Expand All @@ -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
6 changes: 3 additions & 3 deletions extra/group/src/gyro_group/sync.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<tracker>(p_tracker)->update(p_armor, _tick, _gyro_data);
p_tracker->update(p_armor);
}
}
// 利用可见追踪器信息，完成强制构造与不可见追踪器的更新
Expand All @@ -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<tracker>(p_tracker)->update(p_armor, _tick, _gyro_data);
p_tracker->update(p_armor);
}
}
else // visible_num == 2
Expand Down Expand Up @@ -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<tracker>(p_tracker)->update(p_armor, _tick, _gyro_data);
p_tracker->update(p_armor);
}
}
// ----------------------【更新 RobotType】----------------------
Expand Down
14 changes: 9 additions & 5 deletions extra/predictor/src/armor_predictor/armor_predictor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,11 +26,15 @@ PredictInfo ArmorPredictor::predict(const std::vector<group::ptr> &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;
Expand Down
6 changes: 2 additions & 4 deletions extra/predictor/src/rune_predictor/rune_predictor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,10 +37,8 @@ PredictInfo RunePredictor::predict(const std::vector<group::ptr> &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;
}
Expand Down
Loading

0 comments on commit 363143a

Please sign in to comment.