cyy/yq_2026_duo
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add wust typr mpc and mutipule x

Browse Source
This commit is contained in:
cyy_mac
2026-03-27 03:41:42 +08:00
parent 2c64655fae
commit 7dcb53bb77
192 changed files with 29571 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

71
wust_vision-main/tasks/auto_buff/rune_tracker/motion_models/motion_modelrypd.hpp Normal file
View File

@@ -0,0 +1,71 @@
// Copyright 2025 Xiaojian Wu
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#pragma once
#include <ceres/ceres.h>
#include "KalmanHyLib/kalman_hybird_lib.hpp"
namespace ypdrune_motion_model {
constexpr int X_N = 6, Z_N = 5;
using VecZ = Eigen::Matrix<double, Z_N, 1>;
using VecX = Eigen::Matrix<double, X_N, 1>;
enum class Meas : uint8_t { YPD_Y = 0, YPD_P = 1, YPD_D = 2, ORI_YAW = 3, ORI_ROLL = 4, Z_N = 5 };
enum class State : uint8_t { CX = 0, CY = 1, CZ = 2, YAW = 3, ROLL = 4, VROLL = 5, X_N = 6 };
struct Predict {
Predict() = default;
explicit Predict(double dt): dt(dt) {}
template<typename T>
void operator()(const T x0[X_N], T x1[X_N]) const {
for (int i = 0; i < X_N; ++i) {
x1[i] = x0[i];
}
x1[(int)State::ROLL] += x0[(int)State::VROLL] * dt;
}
double dt;
};
template<typename T>
T normalize_angle_t(T angle) {
T two_pi = T(2.0 * M_PI);
return angle - two_pi * floor((angle + T(M_PI)) / two_pi);
}
struct Measure {
Measure() = default;
explicit Measure(int id): id(id) {}
template<typename T>
void operator()(const T x[X_N], T z[Z_N]) const {
T xy_dist = ceres::sqrt(
x[(int)State::CX] * x[(int)State::CX] + x[(int)State::CY] * x[(int)State::CY]
);
T dist = ceres::sqrt(xy_dist * xy_dist + x[(int)State::CZ] * x[(int)State::CZ]);
// Observation model
z[(int)Meas::YPD_Y] = ceres::atan2(x[1], x[0]); // yaw
z[(int)Meas::YPD_P] = ceres::atan2(x[2], xy_dist); // pitch
z[(int)Meas::YPD_D] = dist; // distance
z[(int)Meas::ORI_YAW] = x[(int)State::YAW]; // orientation_yaw
z[(int)Meas::ORI_ROLL] = normalize_angle_t(x[(int)State::ROLL] + id * 2 * M_PI / 5); // roll
}
void h(const VecX& x, VecZ& z) const {
assert(x.size() == X_N);
assert(z.size() == Z_N);
operator()(x.data(), z.data());
}
int id = 0;
};
using RuneESKF = kalman_hybird_lib::ErrorStateEKF<X_N, Z_N, Predict, Measure>;
} // namespace ypdrune_motion_model

336
wust_vision-main/tasks/auto_buff/rune_tracker/rune_target.cpp Normal file
View File

@@ -0,0 +1,336 @@
#include "rune_target.hpp"
#include <iostream>
namespace wust_vision {
namespace auto_buff {
RuneTarget::RuneTarget(
const auto_buff::RuneFan& fan,
RuneTargetConfig::Ptr target_config,
double pre_v_roll
) {
is_big_ = false;
start_time_ = fan.timestamp;
target_config_ = target_config;
fitter_.setWindow(target_config_->big_window_sec_param.get());
auto f = MModel::Predict(0.005);
auto h = MModel::Measure(0);
auto u_q = [this]() {
Eigen::Matrix<double, MModel::X_N, MModel::X_N> q;
return q;
};
auto u_r = [this](const Eigen::Matrix<double, MModel::Z_N, 1>& z) {
Eigen::Matrix<double, MModel::Z_N, MModel::Z_N> r;
return r;
};
Eigen::DiagonalMatrix<double, MModel::X_N> p0;
p0.setIdentity();
esekf_ypd_ = MModel::RuneESKF(f, h, u_q, u_r, p0);
esekf_ypd_.setResidualFunc([](const Eigen::VectorXd& z_pred, const Eigen::VectorXd& z) {
Eigen::VectorXd r = z - z_pred;
r[(int)MModel::Meas::YPD_Y] = angles::shortest_angular_distance(
z_pred[(int)MModel::Meas::YPD_Y],
z[(int)MModel::Meas::YPD_Y]
); // yaw
r[(int)MModel::Meas::ORI_YAW] = angles::shortest_angular_distance(
z_pred[(int)MModel::Meas::ORI_YAW],
z[(int)MModel::Meas::ORI_YAW]
); // ori_yaw
r[(int)MModel::Meas::ORI_ROLL] = angles::shortest_angular_distance(
z_pred[(int)MModel::Meas::ORI_ROLL],
z[(int)MModel::Meas::ORI_ROLL]
); // ori_roll
return r;
});
esekf_ypd_.setIterationNum(target_config_->esekf_iter_num_param.get());
esekf_ypd_.setInjectFunc([](const Eigen::Matrix<double, MModel::X_N, 1>& delta,
Eigen::Matrix<double, MModel::X_N, 1>& nominal) {
for (int i = 0; i < MModel::X_N; i++) {
if (i == (int)MModel::Meas::ORI_YAW || i == (int)MModel::Meas::ORI_ROLL)
continue;
nominal[i] += delta[i];
}
nominal[(int)MModel::Meas::ORI_YAW] = angles::normalize_angle(
nominal[(int)MModel::Meas::ORI_YAW] + delta[(int)MModel::Meas::ORI_YAW]
);
nominal[(int)MModel::Meas::ORI_ROLL] = angles::normalize_angle(
nominal[(int)MModel::Meas::ORI_ROLL] + delta[(int)MModel::Meas::ORI_ROLL]
);
});
double xc = fan.fans.front().target_pos.x();
double yc = fan.fans.front().target_pos.y();
double zc = fan.fans.front().target_pos.z();
double yaw = utils::orientationToYaw<RuneTarget>(fan.fans.front().target_ori);
double roll = utils::orientationToRoll<RuneTarget>(fan.fans.front().target_ori);
target_state_ = Eigen::VectorXd::Zero(MModel::X_N);
target_state_ << xc, yc, zc, yaw, roll, pre_v_roll;
esekf_ypd_.setState(target_state_);
fitter_.update(0, 0);
last_time_ = 0;
is_inited = true;
last_t_ = fan.timestamp;
timestamp_ = fan.timestamp;
}
Eigen::Matrix<double, MModel::Z_N, MModel::Z_N>
RuneTarget::computeMeasurementCovariance(const Eigen::Matrix<double, MModel::Z_N, 1>& z) const {
Eigen::Matrix<double, MModel::Z_N, MModel::Z_N> r;
// clang-format off
r << target_config_->yp_r_param.get() , 0 , 0 , 0 , 0,
0 , target_config_->yp_r_param.get() , 0 , 0 , 0,
0 , 0 , target_config_->dis_r_param.get() , 0 , 0,
0 , 0 , 0 , target_config_->yaw_r_param.get() , 0,
0 , 0 , 0 , 0 , target_config_->roll_r_param.get();
// clang-format on
return r;
}
Eigen::Matrix<double, MModel::X_N, MModel::X_N> RuneTarget::computeProcessNoise(double dt
) const {
Eigen::Matrix<double, MModel::X_N, MModel::X_N> q;
double t = dt;
double v1 = target_config_->q_roll_param.get();
double q_roll_roll = pow(t, 4) / 4 * v1, q_roll_vroll = pow(t, 3) / 2 * v1,
q_vroll_vroll = pow(t, 2) * v1;
double q_xyz = target_config_->q_xyz_param.get();
double q_yaw = target_config_->q_yaw_param.get();
// clang-format off
// xc yc zc yaw roll v_roll
q << q_xyz, 0, 0, 0, 0, 0,
0, q_xyz, 0, 0, 0, 0,
0, 0, q_xyz, 0, 0, 0,
0, 0, 0, q_yaw, 0, 0,
0, 0, 0, 0, q_roll_roll, q_roll_vroll,
0, 0, 0, 0, q_roll_vroll, q_vroll_vroll;
// clang-format on
return q;
}
void RuneTarget::predict(std::chrono::steady_clock::time_point t) {
double dt = wust_vl::common::utils::time_utils::durationSec(last_t_, t);
predict(dt);
last_t_ = t;
}
void RuneTarget::predict(double dt) {
dt_ = dt;
esekf_ypd_.setPredictFunc(MModel::Predict { dt });
auto u_q = [dt, this]() { return computeProcessNoise(dt); };
esekf_ypd_.setUpdateQ(u_q);
target_state_ = esekf_ypd_.predict();
}
bool RuneTarget::update(const auto_buff::RuneFan& fans) {
timestamp_ = fans.timestamp;
if (fans.fans.empty()) {
return false;
}
update_ids.clear();
auto matched = match(fans.fans);
bool has_match = false;
for (auto [id, fan]: matched) {
measurement_ = getMeasure(fan);
update_ids.push_back(id);
auto yu_rv2 = [this](const Eigen::Matrix<double, MModel::Z_N, 1>& z) {
return this->computeMeasurementCovariance(z);
};
esekf_ypd_.setUpdateR(yu_rv2);
esekf_ypd_.setMeasureFunc(MModel::Measure { id });
esekf_ypd_.update(measurement_);
if (!is_big_)
last_id = id;
has_match = true;
}
bool no_change = true;
for (auto id: update_ids) {
if (id != last_id)
no_change = false;
}
if (!no_change && update_ids.size() > 1)
last_id = update_ids[0];
// if (update_ids.size() > 1)
// is_big_ = true;
double tostart =
wust_vl::common::utils::time_utils::durationSec(start_time_, fans.timestamp);
fitter_.update(tostart, v_roll());
fitter_.setAngleRef(tostart, roll());
fitter_.fitAsync();
last_time_ = tostart;
return has_match;
}
cv::Rect RuneTarget::expanded(
Eigen::Matrix4d T_camera_to_odom,
const cv::Mat& camera_intrinsic,
const cv::Mat& camera_distortion,
const cv::Size& image_size
) {
double dt = wust_vl::common::utils::time_utils::durationSec(
timestamp_,
wust_vl::common::utils::time_utils::now()
);
if (!is_inited || dt > target_config_->lost_time_thres_param.get()) {
return cv::Rect(0, 0, 0, 0);
}
const float car_box_half = 1.0;
static std::vector<cv::Point3f> CAR_BOX;
CAR_BOX = { { 0, car_box_half, -car_box_half },
{ 0, -car_box_half, -car_box_half },
{ 0, -car_box_half, car_box_half },
{ 0, car_box_half, car_box_half } };
Eigen::Matrix4d T_odom_to_camera = T_camera_to_odom.inverse();
Eigen::Vector4d pos_odom(centerPos().x(), centerPos().y(), centerPos().z(), 1.0);
Eigen::Vector4d pos_cam = T_odom_to_camera * pos_odom;
if (pos_cam.z() <= 0.2) {
return cv::Rect(0, 0, 0, 0);
}
cv::Mat tvec = (cv::Mat_<double>(3, 1) << pos_cam.x(), pos_cam.y(), pos_cam.z());
Eigen::Vector3d euler;
euler.z() = M_PI / 2.0;
euler.y() = 0;
euler.x() = std::atan2(pos_odom.y(), pos_odom.x());
Eigen::Quaterniond ori = utils::eulerToQuat(euler, utils::EulerOrder::ZYX);
auto target_ori = utils::transformOrientation(ori, T_odom_to_camera);
Eigen::Matrix3d tf_rot = target_ori.toRotationMatrix();
cv::Mat rot_mat =
(cv::Mat_<double>(3, 3) << tf_rot(0, 0),
tf_rot(0, 1),
tf_rot(0, 2),
tf_rot(1, 0),
tf_rot(1, 1),
tf_rot(1, 2),
tf_rot(2, 0),
tf_rot(2, 1),
tf_rot(2, 2));
cv::Mat rvec;
cv::Rodrigues(rot_mat, rvec);
std::vector<cv::Point2f> pts_2d;
cv::projectPoints(CAR_BOX, rvec, tvec, camera_intrinsic, camera_distortion, pts_2d);
cv::Rect rect = cv::boundingRect(pts_2d);
cv::Rect img_rect(0, 0, image_size.width, image_size.height);
if ((rect & img_rect).area() <= 0) {
return cv::Rect(0, 0, 0, 0);
}
int base_side = std::max(rect.width, rect.height);
int max_side = std::max(image_size.width, image_size.height);
double lost_dt = target_config_->lost_time_thres_param.get();
double dt_clamped = std::max(0.0, std::min(dt, lost_dt));
int side = static_cast<int>(base_side + (max_side - base_side) * (dt_clamped / lost_dt));
if (dt >= lost_dt) {
side = max_side;
}
int cx = rect.x + rect.width / 2;
int cy = rect.y + rect.height / 2;
cv::Rect square(cx - side / 2, cy - side / 2, side, side);
square &= img_rect;
return square;
}
std::vector<std::pair<int, auto_buff::RuneFan::Simple>>
RuneTarget::match(const std::vector<auto_buff::RuneFan::Simple>& fans) {
std::vector<std::pair<int, auto_buff::RuneFan::Simple>> result;
const int n_obs = (int)(fans.size());
const int armors_num = 5;
const double GATE = target_config_->match_gate_param.get();
const double max_cost = 1e9;
std::vector<std::vector<double>> cost(n_obs, std::vector<double>(armors_num, max_cost + 1));
std::vector<MModel::VecZ> meas_list(n_obs);
for (int j = 0; j < n_obs; ++j) {
meas_list[j] = getMeasure(fans[j]);
}
for (int j = 0; j < n_obs; ++j) {
for (int id = 0; id < armors_num; ++id) {
MModel::Measure measure(id);
MModel::VecZ z_pred;
measure.h(target_state_, z_pred);
MModel::VecZ nu = meas_list[j] - z_pred;
nu[(int)MModel::Meas::YPD_Y] =
angles::normalize_angle(nu[(int)MModel::Meas::YPD_Y]);
nu[(int)MModel::Meas::YPD_P] =
angles::normalize_angle(nu[(int)MModel::Meas::YPD_P]);
nu[(int)MModel::Meas::ORI_YAW] =
angles::normalize_angle(nu[(int)MModel::Meas::ORI_YAW]);
nu[(int)MModel::Meas::ORI_ROLL] =
angles::normalize_angle(nu[(int)MModel::Meas::ORI_ROLL]);
auto R = computeMeasurementCovariance(z_pred);
double d2 = nu.transpose() * R.ldlt().solve(nu);
// 门控
if (std::isfinite(d2) && d2 < GATE) {
cost[j][id] = d2;
}
}
}
std::vector<bool> used_obs(n_obs, false);
std::vector<bool> used_id(armors_num, false);
while (true) {
double best = max_cost;
int best_j = -1;
int best_id = -1;
for (int j = 0; j < n_obs; ++j) {
if (used_obs[j])
continue;
for (int id = 0; id < armors_num; ++id) {
if (used_id[id])
continue;
if (cost[j][id] < best) {
best = cost[j][id];
best_j = j;
best_id = id;
}
}
}
if (best_j < 0 || best_id < 0) {
break;
}
used_obs[best_j] = true;
used_id[best_id] = true;
result.push_back(std::make_pair(best_id, fans[best_j]));
}
// for (auto fan: fans) {
// int id;
// auto min_angle_error = 1e10;
// const auto angles = getAngles();
// for (int i = 0; i < angles.size(); i++) {
// auto angle_error = std::abs(angles::normalize_angle(
// angles::normalize_angle(orientationToRoll(fan.target_ori)) - angles[i]
// ));
// if (angle_error < min_angle_error) {
// min_angle_error = angle_error;
// id = i;
// }
// }
// result.push_back(std::make_pair(id, fan));
// }
return result;
}
} // namespace auto_buff
} // namespace wust_vision

230
wust_vision-main/tasks/auto_buff/rune_tracker/rune_target.hpp Normal file
View File

@@ -0,0 +1,230 @@
#pragma once
#include "spd_fitter.hpp"
#include "tasks/auto_buff/rune_tracker/motion_models/motion_modelrypd.hpp"
#include "tasks/auto_buff/type.hpp"
#include "tasks/utils/utils.hpp"
#include <wust_vl/common/utils/timer.hpp>
namespace wust_vision {
namespace auto_buff {
namespace MModel = ypdrune_motion_model;
struct RuneTargetConfig: wust_vl::common::utils::ParamGroup {
static constexpr const char* kKey = "rune_tracker";
const char* key() const override {
return kKey;
}
GEN_PARAM(int, esekf_iter_num);
GEN_PARAM(double, lost_time_thres);
GEN_PARAM(int, tracking_thres);
GEN_PARAM(double, max_dis_diff);
GEN_PARAM(double, match_gate);
GEN_PARAM(double, q_roll);
GEN_PARAM(double, qyaw_output);
GEN_PARAM(double, q_xyz);
GEN_PARAM(double, q_yaw);
GEN_PARAM(double, yp_r);
GEN_PARAM(double, dis_r);
GEN_PARAM(double, yaw_r);
GEN_PARAM(double, roll_r);
GEN_PARAM(double, big_window_sec);
using Ptr = std::shared_ptr<RuneTargetConfig>;
RuneTargetConfig() {}
static Ptr create() {
return std::make_shared<RuneTargetConfig>();
}
void loadSelf(const YAML::Node& node) override {
esekf_iter_num_param.load(node);
lost_time_thres_param.load(node);
tracking_thres_param.load(node);
max_dis_diff_param.load(node);
match_gate_param.load(node);
q_roll_param.load(node);
q_xyz_param.load(node);
q_yaw_param.load(node);
yp_r_param.load(node);
dis_r_param.load(node);
roll_r_param.load(node);
big_window_sec_param.load(node);
}
};
class RuneTarget {
public:
RuneTarget() = default;
RuneTarget(
const auto_buff::RuneFan& fan,
RuneTargetConfig::Ptr target_config,
double pre_v_roll
);
bool is_big_ = false;
double last_ypd_y = 0;
bool is_inited = false;
int last_id;
std::vector<int> update_ids;
RuneTargetConfig::Ptr target_config_;
std::chrono::steady_clock::time_point last_t_;
std::chrono::steady_clock::time_point timestamp_;
std::chrono::steady_clock::time_point start_time_;
double dt_;
double last_time_ = 0;
SinSpeedFitter fitter_;
MModel::RuneESKF esekf_ypd_;
Eigen::Matrix<double, MModel::Z_N, 1> measurement_ =
Eigen::Matrix<double, MModel::Z_N, 1>::Zero();
Eigen::Matrix<double, MModel::X_N, 1> target_state_ =
Eigen::Matrix<double, MModel::X_N, 1>::Zero();
cv::Rect expanded(
Eigen::Matrix4d T_camera_to_odom,
const cv::Mat& camera_intrinsic,
const cv::Mat& camera_distortion,
const cv::Size& image_size
);
bool update(const auto_buff::RuneFan& fan);
void predict(std::chrono::steady_clock::time_point t);
void predict(double dt);
Eigen::Matrix<double, MModel::Z_N, MModel::Z_N>
computeMeasurementCovariance(const Eigen::Matrix<double, MModel::Z_N, 1>& z) const;
Eigen::Matrix<double, MModel::X_N, MModel::X_N> computeProcessNoise(double dt) const;
inline bool checkTargetAppear() {
bool appear = is_inited
&& wust_vl::common::utils::time_utils::durationSec(
timestamp_,
wust_vl::common::utils::time_utils::now()
) < target_config_->lost_time_thres_param.get();
return appear;
}
double predictAngle(std::chrono::steady_clock::time_point t) const {
double to_start = wust_vl::common::utils::time_utils::durationSec(start_time_, t);
return fitter_.predictAngle(to_start);
}
double predictAngle(double dt) const {
return fitter_.predictAngle(last_time_ + dt);
}
void predictWithFitter(double dt) {
if (is_big_) {
double to_start = last_time_ + dt;
double angle = fitter_.predictAngle(to_start);
double speed = fitter_.predictSpeed(to_start);
auto state = esekf_ypd_.getState();
state[(int)MModel::State::ROLL] = angles::normalize_angle(angle);
state[(int)MModel::State::VROLL] = speed;
esekf_ypd_.setState(state);
} else {
predict(dt);
}
}
void predictWithFitter(std::chrono::steady_clock::time_point t) {
double dt = wust_vl::common::utils::time_utils::durationSec(last_t_, t);
predictWithFitter(dt);
last_t_ = t;
}
double getFitterSpd(std::chrono::steady_clock::time_point t) {
double to_start = wust_vl::common::utils::time_utils::durationSec(start_time_, t);
return fitter_.predictSpeed(to_start);
}
Eigen::Vector3d centerPos() const {
return { target_state_((int)MModel::State::CX),
target_state_((int)MModel::State::CY),
target_state_((int)MModel::State::CZ) };
}
std::vector<double> getAngles() {
std::vector<double> angles;
for (int i = 0; i < 5; i++) {
auto angle = angles::normalize_angle(
target_state_[(int)MModel::State::ROLL] + i * 2 * M_PI / 5
);
angles.push_back(angle);
}
return angles;
}
bool diverged() const {
return diverged(target_state_);
}
bool diverged(Eigen::VectorXd target_state) const {
return false;
}
double roll() const {
return target_state_[(int)MModel::State::ROLL];
}
double curr_roll() const {
return roll() + last_id * 2 * M_PI / 5;
}
double real_roll(int id) const {
return roll() + id * 2 * M_PI / 5;
}
double yaw() const {
return target_state_[(int)MModel::State::YAW];
}
double v_roll() const {
return target_state_[(int)MModel::State::VROLL];
}
std::vector<std::pair<int, auto_buff::RuneFan::Simple>>
match(const std::vector<auto_buff::RuneFan::Simple>& fans);
std::vector<std::pair<Eigen::Vector3d, Eigen::Quaterniond>> getAllPose() const {
std::vector<std::pair<Eigen::Vector3d, Eigen::Quaterniond>> poses;
for (int i = 0; i < 5; i++) {
poses.emplace_back(getPose(i));
}
return poses;
}
std::pair<Eigen::Vector3d, Eigen::Quaterniond> getPose(int id) const {
Eigen::Vector3d euler = Eigen::Vector3d(yaw(), 0.0, real_roll(id));
auto q = utils::eulerToQuat(euler, utils::EulerOrder::ZYX);
return computeBladeTipPose(centerPos(), q, id);
}
std::pair<Eigen::Vector3d, Eigen::Quaterniond>
computeBladeTipPose(const Eigen::Vector3d& center_pos, const Eigen::Quaterniond& q, int id)
const {
// tip 的局部坐标（沿 local X 方向）
Eigen::Vector3d local_tip(0.0, 0.0, RUNE_R2PANCENTER);
Eigen::Vector3d tip_pos = center_pos + q * local_tip;
Eigen::Vector3d euler = Eigen::Vector3d(yaw(), 0.0, real_roll(id));
return { tip_pos, utils::eulerToQuat(euler, utils::EulerOrder::ZYX) };
}
std::pair<Eigen::Vector3d, Eigen::Quaterniond> getHitPoint() const {
return getPose(last_id);
}
auto_buff::PowerRune getPowerRune() const {
auto_buff::PowerRune power_rune;
if (!is_inited) {
return power_rune;
}
power_rune.center.pos = centerPos();
Eigen::Vector3d euler = Eigen::Vector3d(yaw(), 0.0, real_roll(last_id));
auto q = Eigen::Quaterniond();
power_rune.center.ori = q;
auto all_pose = getAllPose();
for (int i = 0; i < all_pose.size(); i++) {
auto_buff::PowerRune::Pose pose;
pose.pos = all_pose[i].first;
pose.ori = all_pose[i].second;
power_rune.fans.push_back(pose);
}
power_rune.hit_id = last_id;
return power_rune;
}
Eigen::Matrix<double, MModel::Z_N, 1> getMeasure(const auto_buff::RuneFan::Simple& fan) {
auto p = fan.target_pos;
double measured_yaw = utils::orientationToYaw<RuneTarget>(fan.target_ori);
double measured_roll = utils::orientationToRoll<RuneTarget>(fan.target_ori);
double ypd_y = std::atan2(p.y(), p.x());
ypd_y = this->last_ypd_y + angles::shortest_angular_distance(this->last_ypd_y, ypd_y);
this->last_ypd_y = ypd_y;
double ypd_p = std::atan2(p.z(), std::sqrt(p.x() * p.x() + p.y() * p.y()));
double ypd_d = std::sqrt(p.x() * p.x() + p.y() * p.y() + p.z() * p.z());
Eigen::Matrix<double, MModel::Z_N, 1> measure;
measure << ypd_y, ypd_p, ypd_d, measured_yaw, measured_roll;
return measure;
}
};
} // namespace auto_buff
} // namespace wust_vision

121
wust_vision-main/tasks/auto_buff/rune_tracker/rune_tracker.cpp Normal file
View File

@@ -0,0 +1,121 @@
#include "rune_tracker.hpp"
namespace wust_vision {
namespace auto_buff {
struct RuneTracker::Impl {
public:
Impl(wust_vl::common::utils::Parameter::Ptr auto_buff_config_parameter) {
tracker_state = LOST;
target_ = auto_buff::RuneTarget();
target_config_ = RuneTargetConfig::create();
auto_buff_config_parameter->registerGroup(*target_config_);
auto_buff_config_parameter->reloadFromOldPath();
}
auto_buff::RuneTarget track(const auto_buff::RuneFan& fan) {
double dt = std::chrono::duration<double>(fan.timestamp - last_time_).count();
last_time_ = fan.timestamp;
lost_thres_ =
std::abs(static_cast<int>(target_config_->lost_time_thres_param.get() / dt));
bool found;
if (tracker_state == LOST) {
found = initTarget(fan);
} else {
found = updateTarget(fan);
}
updateFsm(found);
return target_;
}
void updateFsm(bool found) {
switch (tracker_state) {
case DETECTING:
if (found) {
if (++detect_count_ > target_config_->tracking_thres_param.get()) {
detect_count_ = 0;
tracker_state = TRACKING;
}
} else {
detect_count_ = 0;
tracker_state = LOST;
}
break;
case TRACKING:
if (!found) {
tracker_state = TEMP_LOST;
lost_count_ = 1;
}
break;
case TEMP_LOST:
if (!found) {
if (++lost_count_ > lost_thres_) {
lost_count_ = 0;
tracker_state = LOST;
}
} else {
lost_count_ = 0;
tracker_state = TRACKING;
}
break;
default:
break;
}
// target_.is_tracking = (tracker_state == TRACKING || tracker_state == TEMP_LOST);
if (found)
++found_count_;
// if (target_.is_tracking) {
// pre_v_roll_ = target_.v_roll();
// }
}
bool initTarget(const auto_buff::RuneFan& fan) {
if (!fan.is_valid || fan.fans.empty()) {
return false;
}
target_ = auto_buff::RuneTarget(fan, target_config_, pre_v_roll_);
tracker_state = DETECTING;
return true;
}
bool updateTarget(const auto_buff::RuneFan& fan) {
if (!fan.is_valid || fan.fans.empty()) {
return false;
}
auto fan_copy = fan;
std::erase_if(fan_copy.fans, [this](const auto_buff::RuneFan::Simple& f) {
bool pose_check = std::abs((f.target_pos - target_.centerPos()).norm())
< target_config_->max_dis_diff_param.get()
&& f.target_pos.norm() > 1.0;
return !pose_check;
});
target_.predict(fan_copy.timestamp);
return target_.update(fan_copy);
}
enum State {
LOST,
DETECTING,
TRACKING,
TEMP_LOST,
} tracker_state = LOST;
auto_buff::RuneTarget target_;
int detect_count_ = 0;
int lost_count_ = 0;
int found_count_ = 0;
double pre_v_roll_ = 0;
int lost_thres_ = 0;
std::chrono::steady_clock::time_point last_time_;
RuneTargetConfig::Ptr target_config_;
};
RuneTracker::RuneTracker(wust_vl::common::utils::Parameter::Ptr auto_buff_config_parameter) {
_impl = std::make_unique<Impl>(auto_buff_config_parameter);
}
RuneTracker::~RuneTracker() {
_impl.reset();
}
auto_buff::RuneTarget RuneTracker::track(const auto_buff::RuneFan& fan) {
return _impl->track(fan);
}
} // namespace auto_buff
} // namespace wust_vision

20
wust_vision-main/tasks/auto_buff/rune_tracker/rune_tracker.hpp Normal file
View File

@@ -0,0 +1,20 @@
#pragma once
#include "rune_target.hpp"
namespace wust_vision {
namespace auto_buff {
class RuneTracker {
public:
using Ptr = std::unique_ptr<RuneTracker>;
RuneTracker(wust_vl::common::utils::Parameter::Ptr auto_buff_config_parameter);
static Ptr create(wust_vl::common::utils::Parameter::Ptr auto_buff_config_parameter) {
return std::make_unique<RuneTracker>(auto_buff_config_parameter);
}
~RuneTracker();
auto_buff::RuneTarget track(const auto_buff::RuneFan& fan);
private:
struct Impl;
std::unique_ptr<Impl> _impl;
};
} // namespace auto_buff
} // namespace wust_vision

265
wust_vision-main/tasks/auto_buff/rune_tracker/spd_fitter.hpp Normal file
View File

@@ -0,0 +1,265 @@
#pragma once
#include <Eigen/Dense>
#include <algorithm>
#include <atomic>
#include <ceres/ceres.h>
#include <cmath>
#include <iostream>
#include <limits>
#include <mutex>
#include <thread>
#include <vector>
namespace wust_vision {
namespace auto_buff {
class SinSpeedFitter {
public:
struct P {
double a, w, t0;
};
static constexpr double a_min_ = 0.780;
static constexpr double a_max_ = 1.045;
static constexpr double w_min_ = 1.884;
static constexpr double w_max_ = 2.000;
SinSpeedFitter() {}
void setWindow(double w) {
window_sec_ = w;
}
SinSpeedFitter(const SinSpeedFitter& other) {
std::scoped_lock lock(other.mtx_);
params_ = other.params_;
times_ = other.times_;
speeds_ = other.speeds_;
has_angle_ref_ = other.has_angle_ref_;
angle_ref_time_ = other.angle_ref_time_;
angle_ref_value_ = other.angle_ref_value_;
sign_ = other.sign_;
fitting_ = false;
}
SinSpeedFitter& operator=(const SinSpeedFitter& other) {
if (this != &other) {
std::scoped_lock lock(mtx_, other.mtx_);
params_ = other.params_;
times_ = other.times_;
speeds_ = other.speeds_;
has_angle_ref_ = other.has_angle_ref_;
angle_ref_time_ = other.angle_ref_time_;
angle_ref_value_ = other.angle_ref_value_;
sign_ = other.sign_;
fitting_ = false;
}
return *this;
}
void update(double time_s, double speed_rads) {
std::scoped_lock lock(mtx_);
auto it = std::lower_bound(times_.begin(), times_.end(), time_s);
size_t idx = std::distance(times_.begin(), it);
times_.insert(it, time_s);
speeds_.insert(speeds_.begin() + idx, speed_rads);
const double window_sec = 5.0;
if (!times_.empty()) {
double latest = times_.back();
while (!times_.empty() && latest - times_.front() > window_sec) {
times_.erase(times_.begin());
speeds_.erase(speeds_.begin());
}
}
}
void fit(bool verbose = false) {
std::scoped_lock lock(mtx_);
fitImpl(verbose);
}
void fitAsync(bool verbose = false) {
if (fitting_.exchange(true)) {
if (verbose)
std::cout << "[SinSpeedFitter] Previous async fit still running, skip.\n";
return;
}
std::vector<double> t_copy, s_copy;
P params_snapshot;
{
std::scoped_lock lock(mtx_);
t_copy = times_;
s_copy = speeds_;
params_snapshot = params_;
}
std::thread([this, t_copy, s_copy, params_snapshot, verbose]() {
fitImpl(verbose, &t_copy, &s_copy, &params_snapshot);
fitting_ = false;
}).detach();
}
double predictSpeed(double t) const {
std::scoped_lock lock(mtx_);
double a = params_.a;
double w = params_.w;
double b = 2.090 - a;
return sign_ * (a * std::sin(w * (t - params_.t0)) + b);
}
double predictAngle(double t) const {
std::scoped_lock lock(mtx_);
if (!has_angle_ref_)
return 0.0;
double a = params_.a;
double w = params_.w;
double b = 2.090 - a;
double theta = sign_ * (-a / w * std::cos(w * (t - params_.t0)) + b * (t - params_.t0));
double theta_ref = sign_
* (-a / w * std::cos(w * (angle_ref_time_ - params_.t0))
+ b * (angle_ref_time_ - params_.t0));
return angle_ref_value_ + (theta - theta_ref);
}
void setAngleRef(double time_s, double angle_rad) {
std::scoped_lock lock(mtx_);
angle_ref_time_ = time_s;
angle_ref_value_ = angle_rad;
has_angle_ref_ = true;
}
const P& params() const {
return params_;
}
int sign() const {
return sign_;
}
bool isFitting() const {
return fitting_.load();
}
private:
struct SinResidual {
SinResidual(double t, double s, int sign): t_(t), s_(s), sign_(sign) {}
template<typename T>
bool operator()(const T* const p, T* residual) const {
const T& a = p[0];
const T& w = p[1];
const T& t0 = p[2];
T b = T(2.090) - a;
T pred = T(sign_) * (a * sin(w * (T(t_) - t0)) + b);
residual[0] = T(s_) - pred;
return true;
}
double t_, s_;
int sign_;
};
bool fitImpl(
bool verbose,
const std::vector<double>* t_ptr = nullptr,
const std::vector<double>* s_ptr = nullptr,
const P* params_snapshot = nullptr
) {
const auto& t_in = t_ptr ? *t_ptr : times_;
const auto& s_in = s_ptr ? *s_ptr : speeds_;
if (t_in.size() < 3) {
if (verbose)
std::cerr << "[SinSpeedFitter] need >=3 samples\n";
return false;
}
std::vector<std::pair<double, double>> tmp;
tmp.reserve(t_in.size());
for (size_t i = 0; i < t_in.size(); ++i)
tmp.emplace_back(t_in[i], s_in[i]);
std::sort(tmp.begin(), tmp.end());
std::vector<double> t_unique, s_unique;
t_unique.reserve(tmp.size());
s_unique.reserve(tmp.size());
double last_t = std::numeric_limits<double>::quiet_NaN();
for (auto& [t, s]: tmp) {
if (t_unique.empty() || std::abs(t - last_t) > 1e-9) {
t_unique.push_back(t);
s_unique.push_back(s);
last_t = t;
}
}
P params_initial = params_snapshot ? *params_snapshot : params_;
double err_pos = fit_with_sign(+1, t_unique, s_unique, params_initial, verbose);
double err_neg = fit_with_sign(-1, t_unique, s_unique, params_initial, verbose);
std::scoped_lock lock(mtx_);
sign_ = (err_pos <= err_neg) ? +1 : -1;
return true;
}
double fit_with_sign(
int sgn,
const std::vector<double>& t_unique,
const std::vector<double>& s_unique,
P params_initial,
bool verbose
) {
ceres::Problem problem;
double params[3] = { params_initial.a, params_initial.w, params_initial.t0 };
for (size_t i = 0; i < t_unique.size(); ++i) {
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<SinResidual, 1, 3>(
new SinResidual(t_unique[i], s_unique[i], sgn)
),
nullptr,
params
);
}
problem.SetParameterLowerBound(params, 0, a_min_);
problem.SetParameterUpperBound(params, 0, a_max_);
problem.SetParameterLowerBound(params, 1, w_min_);
problem.SetParameterUpperBound(params, 1, w_max_);
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
options.max_num_iterations = 100;
options.minimizer_progress_to_stdout = verbose;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
double err_sum = 0.0;
for (size_t i = 0; i < t_unique.size(); ++i) {
double pred = sgn
* (params[0] * std::sin(params[1] * (t_unique[i] - params[2]))
+ (2.090 - params[0]));
double e = s_unique[i] - pred;
err_sum += e * e;
}
if (verbose)
std::cout << (sgn > 0 ? "[+] " : "[-] ") << summary.BriefReport()
<< " error=" << err_sum << std::endl;
std::scoped_lock lock(mtx_);
params_.a = params[0];
params_.w = params[1];
params_.t0 = params[2];
return err_sum;
}
private:
mutable std::mutex mtx_;
P params_ { 1.0, 1.9, 0.0 };
std::vector<double> times_;
std::vector<double> speeds_;
bool has_angle_ref_ = false;
double angle_ref_time_ = 0.0;
double angle_ref_value_ = 0.0;
int sign_ = 1;
std::atomic<bool> fitting_ { false };
double window_sec_ = 1.0;
};
} // namespace auto_buff
} // namespace wust_vision