Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

为 tracker 模块设计新的 update 接口，并修改 KalmanFilter 的错误用法 #68

Merged
merged 1 commit into from
Mar 20, 2024
Merged

为 tracker 模块设计新的 update 接口，并修改 KalmanFilter 的错误用法 #68

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 4 additions & 4 deletions extra/decider/src/translation_decider/translation_decider.cpp
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Loading