add wust typr mpc and mutipule x
This commit is contained in:
cyy_mac
360
wust_vision-main/tasks/auto_buff/auto_buff.cpp
Normal file
360
wust_vision-main/tasks/auto_buff/auto_buff.cpp
Normal file
@@ -0,0 +1,360 @@
|
||||
#include "auto_buff.hpp"
|
||||
#include "tasks/auto_buff/debug.hpp"
|
||||
#include "tasks/auto_buff/rune_control/aimer.hpp"
|
||||
#include "tasks/auto_buff/rune_detector/rune_detector.hpp"
|
||||
#include "tasks/auto_buff/rune_tracker/rune_tracker.hpp"
|
||||
#include "tasks/auto_buff/rune_where/rune_where.hpp"
|
||||
#include "tasks/type_common.hpp"
|
||||
#include "tasks/utils/utils.hpp"
|
||||
namespace wust_vision {
|
||||
namespace auto_buff {
|
||||
|
||||
struct AutoBuff::Impl {
|
||||
~Impl() {
|
||||
run_flag_ = false;
|
||||
rune_queue_->stop();
|
||||
if (processing_thread_) {
|
||||
processing_thread_->stop();
|
||||
wust_vl::common::concurrency::ThreadManager::instance().unregisterThread(
|
||||
processing_thread_->getName()
|
||||
);
|
||||
}
|
||||
}
|
||||
Impl(
|
||||
const std::string& config_path,
|
||||
TFConfig::Ptr tf_config,
|
||||
const std::pair<cv::Mat, cv::Mat>& camera_info,
|
||||
bool debug
|
||||
) {
|
||||
debug_mode_ = debug;
|
||||
auto_buff_config_parameter_ = wust_vl::common::utils::Parameter::create();
|
||||
auto_buff_config_parameter_->loadFromFile(config_path);
|
||||
auto_exposure_cfg_ = AutoExposureCfg::create();
|
||||
aimer_ = auto_buff::Aimer::create(auto_buff_config_parameter_);
|
||||
rune_tracker_ = RuneTracker::create(auto_buff_config_parameter_);
|
||||
auto_buff_config_parameter_->registerGroup(*auto_exposure_cfg_);
|
||||
auto_buff_config_parameter_->reloadFromOldPath();
|
||||
auto config = YAML::LoadFile(config_path);
|
||||
wust_vl::common::utils::ParameterManager::instance().registerParameter(
|
||||
*auto_buff_config_parameter_.get()
|
||||
);
|
||||
tf_config_ = tf_config;
|
||||
camera_info_ = camera_info;
|
||||
|
||||
rune_where_ = auto_buff::RuneWhere::create(config["rune_where"], camera_info);
|
||||
|
||||
rune_detector_ = RuneDetectorCV::make_detector(config["rune_detector"]);
|
||||
rune_detector_->setCallback(std::bind(
|
||||
&AutoBuff::Impl::runeDetectCallback,
|
||||
this,
|
||||
std::placeholders::_1,
|
||||
std::placeholders::_2,
|
||||
std::placeholders::_3
|
||||
));
|
||||
rune_queue_ =
|
||||
std::make_unique<wust_vl::common::concurrency::OrderedQueue<auto_buff::RuneFan>>(
|
||||
50,
|
||||
500
|
||||
);
|
||||
|
||||
latency_averager_ =
|
||||
std::make_unique<wust_vl::common::concurrency::Averager<double>>(100);
|
||||
}
|
||||
void start() {
|
||||
if (run_flag_) {
|
||||
return;
|
||||
}
|
||||
run_flag_ = true;
|
||||
processing_thread_ = wust_vl::common::concurrency::MonitoredThread::create(
|
||||
"AutoBuffProcessingThread",
|
||||
[this](wust_vl::common::concurrency::MonitoredThread::Ptr self) {
|
||||
this->processingLoop(self);
|
||||
}
|
||||
);
|
||||
wust_vl::common::concurrency::ThreadManager::instance().registerThread(
|
||||
processing_thread_
|
||||
);
|
||||
}
|
||||
void pushInput(CommonFrame& frame) {
|
||||
img_recv_count_++;
|
||||
auto bbox = rune_target_.expanded(
|
||||
T_camera_to_odom_,
|
||||
camera_info_.first,
|
||||
camera_info_.second,
|
||||
frame.img_frame.src_img.size()
|
||||
);
|
||||
if (bbox.area() > 100) {
|
||||
frame.expanded = bbox;
|
||||
frame.offset = cv::Point2f(bbox.x, bbox.y);
|
||||
}
|
||||
expanded_ = frame.expanded;
|
||||
rune_detector_->pushInput(frame, debug_mode_);
|
||||
}
|
||||
void runeDetectCallback(
|
||||
const auto_buff::RuneFan& fan,
|
||||
const CommonFrame& frame,
|
||||
cv::Mat& debug_img
|
||||
) {
|
||||
std::lock_guard<std::mutex> lock(callback_mutex_);
|
||||
Eigen::Vector3d v = Eigen::Vector3d::Zero();
|
||||
Eigen::Matrix3d R_gimbal2odom = Eigen::Matrix3d::Identity();
|
||||
auto ctx = std::any_cast<VisionCtx>(frame.any_ctx);
|
||||
std::pair<double, double> gimbal_py;
|
||||
if (ctx.motion_buffer) {
|
||||
const auto delay =
|
||||
std::chrono::microseconds(static_cast<int64_t>(ctx.communication_delay_μs));
|
||||
const auto t_query = fan.timestamp + delay;
|
||||
auto apply_motion = [&](const auto& att) {
|
||||
v << att.data.vx, att.data.vy, att.data.vz;
|
||||
R_gimbal2odom = Eigen::AngleAxisd(att.data.yaw, Eigen::Vector3d::UnitZ())
|
||||
* Eigen::AngleAxisd(-att.data.pitch, Eigen::Vector3d::UnitY())
|
||||
* Eigen::AngleAxisd(att.data.roll, Eigen::Vector3d::UnitX());
|
||||
gimbal_py = std::make_pair(att.data.pitch, att.data.yaw);
|
||||
};
|
||||
if (auto past_att = ctx.motion_buffer->get_interpolated(t_query)) {
|
||||
apply_motion(*past_att);
|
||||
} else if (auto last_att = ctx.motion_buffer->get_last()) {
|
||||
apply_motion(*last_att);
|
||||
}
|
||||
}
|
||||
autoExposureControl(frame.img_frame.src_img, ctx.camera);
|
||||
Eigen::Matrix4d T_camera_to_odom = utils::computeCameraToOdomTransform(
|
||||
R_gimbal2odom,
|
||||
tf_config_->R_camera2gimbal,
|
||||
tf_config_->t_camera2gimbal
|
||||
);
|
||||
T_camera_to_odom_ = T_camera_to_odom;
|
||||
auto_buff::RuneFan copy_fan = rune_where_->where(fan, T_camera_to_odom);
|
||||
copy_fan.is_big =
|
||||
InfantryMode::toAttackMode(ctx.mode) == InfantryMode::AttackMode::BIG_RUNE;
|
||||
rune_queue_->enqueue(copy_fan);
|
||||
if (debug_mode_) {
|
||||
std::lock_guard<std::mutex> lock(dbg_mutex_);
|
||||
auto_buff_debug_.img_frame = frame.img_frame;
|
||||
auto_buff_debug_.T_camera_to_odom = T_camera_to_odom_;
|
||||
auto_buff_debug_.expanded = frame.expanded;
|
||||
auto_buff_debug_.pnp_distance =
|
||||
copy_fan.fans.empty() ? 0.0 : copy_fan.fans[0].pos.norm();
|
||||
auto_buff_debug_.gimbal_py = gimbal_py;
|
||||
}
|
||||
|
||||
detect_finish_count_++;
|
||||
}
|
||||
|
||||
void runeTargetCallback(const auto_buff::RuneFan& fan) {
|
||||
if (fan.timestamp <= last_rune_target_time_) {
|
||||
WUST_WARN(logger_) << "Received out-of-order auto_buff data, discarded.";
|
||||
return;
|
||||
}
|
||||
last_rune_target_time_ = fan.timestamp;
|
||||
|
||||
auto rune_target = rune_tracker_->track(fan);
|
||||
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(target_mutex_);
|
||||
rune_target_ = rune_target;
|
||||
}
|
||||
auto now = std::chrono::steady_clock::now();
|
||||
auto latency_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
|
||||
std::chrono::steady_clock::now() - fan.timestamp
|
||||
)
|
||||
.count();
|
||||
auto latency_ms = wust_vl::common::utils::time_utils::durationMs(fan.timestamp, now);
|
||||
latency_averager_->add(latency_ms);
|
||||
auto_buff_debug_.latency_ms = latency_averager_->average();
|
||||
if (debug_mode_) {
|
||||
std::lock_guard<std::mutex> lock(dbg_mutex_);
|
||||
static double last_unwrapped_roll = 0.0;
|
||||
static double last_raw_roll = 0.0;
|
||||
const double raw_roll = rune_target.roll();
|
||||
const double raw_pred = rune_target.predictAngle(0.5);
|
||||
const double obs_angle = last_unwrapped_roll
|
||||
+ angles::shortest_angular_distance(last_raw_roll, raw_roll);
|
||||
const double pre_angle =
|
||||
obs_angle + angles::shortest_angular_distance(raw_roll, raw_pred);
|
||||
last_unwrapped_roll = obs_angle;
|
||||
last_raw_roll = raw_roll;
|
||||
auto_buff_debug_.obs_v = rune_target.v_roll();
|
||||
auto_buff_debug_.fitter_v = rune_target.getFitterSpd(
|
||||
wust_vl::common::utils::time_utils::now()
|
||||
+ std::chrono::microseconds(int(0.2 * 1e6))
|
||||
);
|
||||
auto_buff_debug_.obs_angle = obs_angle;
|
||||
auto_buff_debug_.pre_angle = pre_angle;
|
||||
auto_buff_debug_.target = rune_target;
|
||||
auto_buff_debug_.power_rune = rune_target.getPowerRune();
|
||||
}
|
||||
}
|
||||
GimbalCmd solve(double bullet_speed) {
|
||||
GimbalCmd gimbal_cmd;
|
||||
auto_buff::RuneTarget rune_target;
|
||||
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(target_mutex_);
|
||||
rune_target = rune_target_;
|
||||
}
|
||||
if (rune_target.checkTargetAppear()) {
|
||||
gimbal_cmd = aimer_->aim(rune_target, bullet_speed);
|
||||
}
|
||||
if (gimbal_cmd.fire_advice) {
|
||||
fire_count_++;
|
||||
}
|
||||
if (debug_mode_) {
|
||||
std::lock_guard<std::mutex> lock(dbg_mutex_);
|
||||
auto_buff_debug_.gimbal_cmd = gimbal_cmd;
|
||||
auto_buff_debug_.aim_target = gimbal_cmd.aim_target;
|
||||
}
|
||||
timer_cout_++;
|
||||
|
||||
return gimbal_cmd;
|
||||
}
|
||||
void processingLoop(wust_vl::common::concurrency::MonitoredThread::Ptr self) {
|
||||
while (!self->isAlive()) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||
}
|
||||
while (self->isAlive() && run_flag_) {
|
||||
if (!self->waitPoint())
|
||||
break;
|
||||
self->heartbeat();
|
||||
printStats();
|
||||
auto_buff::RuneFan auto_buff;
|
||||
// bool skip;
|
||||
// if (rune_queue_->dequeue_wait(auto_buff, skip)) {
|
||||
// runeTargetCallback(auto_buff);
|
||||
// tracker_finish_count_++;
|
||||
// if (skip) {
|
||||
// WUST_DEBUG(logger_) << "OrderQueue skip";
|
||||
// }
|
||||
// }
|
||||
if (rune_queue_->try_dequeue(auto_buff)) {
|
||||
runeTargetCallback(auto_buff);
|
||||
tracker_finish_count_++;
|
||||
} else {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||||
}
|
||||
}
|
||||
}
|
||||
void doDebug() {
|
||||
debug_mode_ = true;
|
||||
AutoBuffDebug dbg;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(dbg_mutex_);
|
||||
dbg = auto_buff_debug_;
|
||||
}
|
||||
drawDebugOverlayShm(dbg, camera_info_, false);
|
||||
debuglog(dbg);
|
||||
}
|
||||
void printStats() {
|
||||
utils::XSecOnce(
|
||||
[&] {
|
||||
WUST_INFO(logger_)
|
||||
<< "Rec: " << img_recv_count_ << ", Det: " << detect_finish_count_
|
||||
<< ", Fin: " << tracker_finish_count_ << ", Tc: " << timer_cout_
|
||||
<< ", Lat: " << auto_buff_debug_.latency_ms << "ms"
|
||||
<< ", Fire: " << fire_count_;
|
||||
img_recv_count_ = 0;
|
||||
detect_finish_count_ = 0;
|
||||
fire_count_ = 0;
|
||||
tracker_finish_count_ = 0;
|
||||
timer_cout_ = 0;
|
||||
},
|
||||
1.0
|
||||
);
|
||||
}
|
||||
void
|
||||
autoExposureControl(const cv::Mat& frame, std::shared_ptr<wust_vl::video::Camera> camera) {
|
||||
const double dt = auto_exposure_cfg_->control_interval_ms_param.get() / 1000.0;
|
||||
utils::XSecOnce(
|
||||
[&] {
|
||||
if (!auto_exposure_cfg_->enable_param.get() || frame.empty()) {
|
||||
return;
|
||||
}
|
||||
if (auto* hik = dynamic_cast<wust_vl::video::HikCamera*>(camera->getDevice())) {
|
||||
cv::Mat i_use = frame(expanded_);
|
||||
if (expanded_.area() < 100 || i_use.empty()) {
|
||||
i_use = frame;
|
||||
}
|
||||
const double brightness = utils::computeBrightness(i_use);
|
||||
|
||||
const double diff =
|
||||
brightness - auto_exposure_cfg_->target_brightness_param.get();
|
||||
const double exposure_min = auto_exposure_cfg_->exposure_min_param.get();
|
||||
const double exposure_max = auto_exposure_cfg_->exposure_max_param.get();
|
||||
double exposure_time = hik->getExposureTime();
|
||||
const double last_exposure_time = exposure_time;
|
||||
if (std::fabs(diff) > auto_exposure_cfg_->tolerance_param.get()
|
||||
&& exposure_time > 0.0) {
|
||||
exposure_time -= diff * auto_exposure_cfg_->step_gain_param.get();
|
||||
} else {
|
||||
exposure_time -= auto_exposure_cfg_->decay_step_param.get();
|
||||
}
|
||||
if (exposure_time < exposure_min)
|
||||
exposure_time = exposure_min;
|
||||
if (exposure_time > exposure_max)
|
||||
exposure_time = exposure_max;
|
||||
if (std::abs(exposure_time - last_exposure_time) > 10) {
|
||||
hik->setExposureTime(exposure_time);
|
||||
}
|
||||
}
|
||||
},
|
||||
dt
|
||||
);
|
||||
}
|
||||
std::mutex callback_mutex_;
|
||||
RuneDetectorCV::Ptr rune_detector_;
|
||||
RuneTracker::Ptr rune_tracker_;
|
||||
auto_buff::Aimer::Ptr aimer_;
|
||||
RuneWhere::Ptr rune_where_;
|
||||
std::string logger_ = "auto_buff";
|
||||
std::unique_ptr<wust_vl::common::concurrency::OrderedQueue<auto_buff::RuneFan>> rune_queue_;
|
||||
wust_vl::common::concurrency::MonitoredThread::Ptr processing_thread_;
|
||||
AutoExposureCfg::Ptr auto_exposure_cfg_;
|
||||
cv::Rect expanded_;
|
||||
auto_buff::RuneTarget rune_target_;
|
||||
bool run_flag_ = false;
|
||||
int detect_finish_count_ = 0;
|
||||
int img_recv_count_ = 0;
|
||||
int tracker_finish_count_ = 0;
|
||||
int timer_cout_ = 0;
|
||||
int fire_count_;
|
||||
std::chrono::steady_clock::time_point last_rune_target_time_;
|
||||
bool debug_mode_ = false;
|
||||
AutoBuffDebug auto_buff_debug_;
|
||||
std::unique_ptr<wust_vl::common::concurrency::Averager<double>> latency_averager_;
|
||||
TFConfig::Ptr tf_config_;
|
||||
std::pair<cv::Mat, cv::Mat> camera_info_;
|
||||
wust_vl::common::utils::Parameter::Ptr auto_buff_config_parameter_;
|
||||
Eigen::Matrix4d T_camera_to_odom_;
|
||||
|
||||
std::mutex target_mutex_;
|
||||
std::mutex dbg_mutex_;
|
||||
};
|
||||
AutoBuff::AutoBuff(
|
||||
const std::string& config_path,
|
||||
TFConfig::Ptr tf_config,
|
||||
const std::pair<cv::Mat, cv::Mat>& camera_info,
|
||||
bool debug
|
||||
):
|
||||
_impl(std::make_unique<Impl>(config_path, tf_config, camera_info, debug)) {}
|
||||
AutoBuff::~AutoBuff() {
|
||||
_impl.reset();
|
||||
}
|
||||
void AutoBuff::start() {
|
||||
_impl->start();
|
||||
}
|
||||
void AutoBuff::pushInput(CommonFrame& frame) {
|
||||
_impl->pushInput(frame);
|
||||
}
|
||||
|
||||
GimbalCmd AutoBuff::solve(double bullet_speed) {
|
||||
return _impl->solve(bullet_speed);
|
||||
}
|
||||
|
||||
wust_vl::common::concurrency::MonitoredThread::Ptr AutoBuff::getThread() {
|
||||
return _impl->processing_thread_;
|
||||
}
|
||||
void AutoBuff::doDebug() {
|
||||
_impl->doDebug();
|
||||
}
|
||||
} // namespace auto_buff
|
||||
} // namespace wust_vision
|
||||
38
wust_vision-main/tasks/auto_buff/auto_buff.hpp
Normal file
38
wust_vision-main/tasks/auto_buff/auto_buff.hpp
Normal file
@@ -0,0 +1,38 @@
|
||||
#pragma once
|
||||
#include "tasks/imodule.hpp"
|
||||
#include "tasks/type_common.hpp"
|
||||
#include "wust_vl/video/camera.hpp"
|
||||
namespace wust_vision {
|
||||
namespace auto_buff {
|
||||
|
||||
class AutoBuff: public IModule {
|
||||
public:
|
||||
using Ptr = std::shared_ptr<AutoBuff>;
|
||||
AutoBuff(
|
||||
const std::string& config_path,
|
||||
TFConfig::Ptr tf_config,
|
||||
const std::pair<cv::Mat, cv::Mat>& camera_info,
|
||||
bool debug
|
||||
);
|
||||
static Ptr create(
|
||||
const std::string& config_path,
|
||||
TFConfig::Ptr tf_config,
|
||||
const std::pair<cv::Mat, cv::Mat>& camera_info,
|
||||
bool debug
|
||||
) {
|
||||
return std::make_shared<AutoBuff>(config_path, tf_config, camera_info, debug);
|
||||
}
|
||||
~AutoBuff();
|
||||
void start() override;
|
||||
void doDebug() override;
|
||||
void pushInput(CommonFrame& frame) override;
|
||||
GimbalCmd solve(double bullet_speed) override;
|
||||
wust_vl::common::concurrency::MonitoredThread::Ptr getThread() override;
|
||||
struct Impl;
|
||||
std::unique_ptr<Impl> _impl;
|
||||
};
|
||||
inline AutoBuff::Ptr toAutoBuff(IModule::Ptr module) {
|
||||
return std::dynamic_pointer_cast<AutoBuff>(module);
|
||||
}
|
||||
} // namespace auto_buff
|
||||
} // namespace wust_vision
|
||||
241
wust_vision-main/tasks/auto_buff/debug.cpp
Normal file
241
wust_vision-main/tasks/auto_buff/debug.cpp
Normal file
@@ -0,0 +1,241 @@
|
||||
#include "debug.hpp"
|
||||
#include "tasks/auto_buff/auto_buff.hpp"
|
||||
|
||||
namespace wust_vision::auto_buff {
|
||||
void drawDebugRuneContent(
|
||||
cv::Mat& debug_img,
|
||||
const AutoBuffDebug& dbg,
|
||||
std::pair<cv::Mat, cv::Mat> camera_info
|
||||
) {
|
||||
const auto& gimbal_cmd = dbg.gimbal_cmd;
|
||||
double predict_angle = dbg.predict_angle;
|
||||
auto aim_target = dbg.aim_target;
|
||||
auto auto_buff = dbg.power_rune;
|
||||
const cv::Rect img_rect(0, 0, debug_img.cols, debug_img.rows);
|
||||
const cv::Rect roi = dbg.expanded & img_rect;
|
||||
cv::rectangle(debug_img, roi, cv::Scalar(255, 255, 255), 2);
|
||||
|
||||
const std::string latency_str = fmt::format("Latency: {:.2f}ms", dbg.latency_ms);
|
||||
cv::putText(
|
||||
debug_img,
|
||||
latency_str,
|
||||
cv::Point(10, 30),
|
||||
cv::FONT_HERSHEY_SIMPLEX,
|
||||
0.8,
|
||||
cv::Scalar(255, 255, 255),
|
||||
2
|
||||
);
|
||||
aim_target.tf(dbg.T_camera_to_odom.inverse());
|
||||
{
|
||||
const auto pts = aim_target.toPts(camera_info.first, camera_info.second);
|
||||
if (!pts.empty()) {
|
||||
cv::Scalar color = cv::Scalar(255, 255, 255);
|
||||
for (int i = 0; i < 4; i++)
|
||||
cv::line(debug_img, pts[i], pts[(i + 1) % 4], color, 2);
|
||||
|
||||
// 后表面
|
||||
for (int i = 4; i < 8; i++)
|
||||
cv::line(debug_img, pts[i], pts[4 + (i + 1) % 4], color, 2);
|
||||
|
||||
// 侧边
|
||||
for (int i = 0; i < 4; i++)
|
||||
cv::line(debug_img, pts[i], pts[i + 4], color, 2);
|
||||
cv::Point2f center(0.f, 0.f);
|
||||
for (auto pt: pts) {
|
||||
center += pt;
|
||||
}
|
||||
center *= 1.0 / pts.size();
|
||||
|
||||
if (gimbal_cmd.fire_advice) {
|
||||
int cross_len = 60;
|
||||
cv::line(
|
||||
debug_img,
|
||||
center + cv::Point2f(-cross_len, -cross_len),
|
||||
center + cv::Point2f(+cross_len, +cross_len),
|
||||
cv::Scalar(0, 0, 255),
|
||||
5
|
||||
);
|
||||
cv::line(
|
||||
debug_img,
|
||||
center + cv::Point2f(-cross_len, +cross_len),
|
||||
center + cv::Point2f(+cross_len, -cross_len),
|
||||
cv::Scalar(0, 0, 255),
|
||||
5
|
||||
);
|
||||
}
|
||||
|
||||
const double scale = 10.0;
|
||||
const double v_yaw = gimbal_cmd.v_yaw;
|
||||
const double v_pitch = gimbal_cmd.v_pitch;
|
||||
const double dx = -scale * v_yaw;
|
||||
const double dy = scale * v_pitch;
|
||||
|
||||
const cv::Point2f start_pt = center;
|
||||
const cv::Point2f end_pt = start_pt + cv::Point2f(dx, dy);
|
||||
const cv::Scalar color_x = cv::Scalar(50, 50, 255);
|
||||
cv::arrowedLine(debug_img, start_pt, end_pt, color_x, 4, cv::LINE_AA, 0, 0.2);
|
||||
}
|
||||
}
|
||||
if (gimbal_cmd.fire_advice) {
|
||||
const std::string fire_str = "Fire!";
|
||||
cv::putText(
|
||||
debug_img,
|
||||
fire_str,
|
||||
{ debug_img.cols / 2 - 100, 200 },
|
||||
cv::FONT_HERSHEY_SIMPLEX,
|
||||
2.85,
|
||||
cv::Scalar(0, 0, 255),
|
||||
2
|
||||
);
|
||||
}
|
||||
|
||||
const std::string gimbal_str = fmt::format(
|
||||
"Pitch: {:.2f}, Yaw: {:.2f}, Enable_pitch_diff: {:.2f}, Enable_yaw_diff: {:.2f}, V_yaw: {:.2f}, V_pitch: {:.2f}",
|
||||
gimbal_cmd.pitch,
|
||||
gimbal_cmd.yaw,
|
||||
gimbal_cmd.enable_pitch_diff,
|
||||
gimbal_cmd.enable_yaw_diff,
|
||||
gimbal_cmd.v_yaw,
|
||||
gimbal_cmd.v_pitch
|
||||
);
|
||||
cv::putText(
|
||||
debug_img,
|
||||
gimbal_str,
|
||||
{ 10, debug_img.rows - 30 },
|
||||
cv::FONT_HERSHEY_SIMPLEX,
|
||||
0.8,
|
||||
cv::Scalar(255, 255, 0),
|
||||
2
|
||||
);
|
||||
auto_buff.tf(dbg.T_camera_to_odom.inverse());
|
||||
auto_buff.draw(debug_img, camera_info.first, camera_info.second);
|
||||
cv::circle(
|
||||
debug_img,
|
||||
cv::Point2i(debug_img.cols / 2, debug_img.rows / 2),
|
||||
5,
|
||||
cv::Scalar(255, 255, 255),
|
||||
2
|
||||
);
|
||||
}
|
||||
void writeTargetLogToJson(const auto_buff::RuneTarget& rune_target) {
|
||||
nlohmann::json j;
|
||||
|
||||
const auto now = std::chrono::steady_clock::now();
|
||||
|
||||
nlohmann::json jr;
|
||||
jr["tracking"] = true;
|
||||
jr["id"] = static_cast<int>(rune_target.last_id);
|
||||
|
||||
const auto age_ms_r =
|
||||
std::chrono::duration_cast<std::chrono::milliseconds>(now - rune_target.timestamp_).count();
|
||||
jr["timestamp_age_ms"] = age_ms_r;
|
||||
|
||||
jr["position"] = { { "x", rune_target.centerPos().x() },
|
||||
{ "y", rune_target.centerPos().y() },
|
||||
{ "z", rune_target.centerPos().z() } };
|
||||
|
||||
jr["roll"] = rune_target.roll() * 180.0 / M_PI;
|
||||
jr["yaw"] = rune_target.yaw() * 180.0 / M_PI;
|
||||
jr["v_roll"] = rune_target.v_roll() * 180.0 / M_PI;
|
||||
|
||||
j["rune_target"] = jr;
|
||||
|
||||
// -------- 写文件 --------
|
||||
std::ofstream file("/dev/shm/target_log.json");
|
||||
if (file.is_open()) {
|
||||
file << j.dump(2);
|
||||
}
|
||||
}
|
||||
struct DebugLogs {
|
||||
#define DEBUG_LOG_LIST(X) \
|
||||
X(double, 100, time) \
|
||||
X(double, 100, raw_yaw) \
|
||||
X(double, 100, raw_pitch) \
|
||||
X(double, 100, yaw) \
|
||||
X(double, 100, pitch) \
|
||||
X(double, 100, ypd_y) \
|
||||
X(double, 100, ypd_p) \
|
||||
X(double, 100, rune_obs) \
|
||||
X(double, 100, rune_pre) \
|
||||
X(double, 100, rune_obsv) \
|
||||
X(double, 100, rune_fitv) \
|
||||
X(double, 100, gimbal_yaw) \
|
||||
X(double, 100, gimbal_pitch) \
|
||||
X(double, 100, target_v_yaw) \
|
||||
X(double, 100, control_v_yaw) \
|
||||
X(double, 100, control_v_pitch) \
|
||||
X(double, 100, yaw_diff) \
|
||||
X(double, 100, fire) \
|
||||
X(double, 100, rune_dis) \
|
||||
X(double, 100, fly_time) \
|
||||
X(double, 100, control_a_yaw) \
|
||||
X(double, 100, control_a_pitch)
|
||||
#define GEN_LOG(TYPE, SIZE, NAME) LogsStream<TYPE, SIZE> NAME##_log { #NAME };
|
||||
|
||||
#define X(TYPE, SIZE, NAME) GEN_LOG(TYPE, SIZE, NAME)
|
||||
DEBUG_LOG_LIST(X)
|
||||
#undef X
|
||||
|
||||
void clear() {
|
||||
#define X(TYPE, SIZE, NAME) NAME##_log.clear();
|
||||
DEBUG_LOG_LIST(X)
|
||||
#undef X
|
||||
}
|
||||
};
|
||||
|
||||
void debuglog(const AutoBuffDebug& dbg_rune) {
|
||||
static bool first_log = true;
|
||||
static std::chrono::steady_clock::time_point start_time;
|
||||
static DebugLogs log;
|
||||
static GimbalCmd last_cmd_;
|
||||
static double rune_dis = 0.0;
|
||||
if (first_log) {
|
||||
start_time = std::chrono::steady_clock::now();
|
||||
first_log = false;
|
||||
}
|
||||
|
||||
const auto now = std::chrono::steady_clock::now();
|
||||
const double t = std::chrono::duration<double>(now - start_time).count();
|
||||
|
||||
const auto_buff::RuneTarget& rune_target = dbg_rune.target;
|
||||
writeTargetLogToJson(rune_target);
|
||||
|
||||
double armor_yaw = 0.0, ypd_y = 0.0, ypd_p = 0.0, armor_distance = 0.0;
|
||||
if (dbg_rune.pnp_distance > 1.0) {
|
||||
rune_dis = dbg_rune.pnp_distance;
|
||||
}
|
||||
|
||||
GimbalCmd i_use;
|
||||
if (dbg_rune.gimbal_cmd.appear) {
|
||||
i_use = dbg_rune.gimbal_cmd;
|
||||
} else {
|
||||
i_use = last_cmd_;
|
||||
}
|
||||
last_cmd_ = i_use;
|
||||
nlohmann::json j;
|
||||
log.time_log.handleOnce(t, j);
|
||||
log.raw_yaw_log.handleOnce(i_use.target_yaw, j);
|
||||
log.raw_pitch_log.handleOnce(i_use.target_pitch, j);
|
||||
log.yaw_log.handleOnce(i_use.yaw, j);
|
||||
log.pitch_log.handleOnce(i_use.pitch, j);
|
||||
log.rune_obs_log.handleOnce(dbg_rune.obs_angle, j);
|
||||
log.rune_pre_log.handleOnce(dbg_rune.pre_angle, j);
|
||||
log.rune_fitv_log.handleOnce(dbg_rune.fitter_v * 180.0 / M_PI, j);
|
||||
log.rune_obsv_log.handleOnce(dbg_rune.obs_v * 180.0 / M_PI, j);
|
||||
log.gimbal_pitch_log.handleOnce(dbg_rune.gimbal_py.first * 180.0 / M_PI, j);
|
||||
log.gimbal_yaw_log.handleOnce(dbg_rune.gimbal_py.second * 180.0 / M_PI, j);
|
||||
log.control_v_pitch_log.handleOnce(i_use.v_pitch, j);
|
||||
log.control_v_yaw_log.handleOnce(i_use.v_yaw, j);
|
||||
log.fire_log.handleOnce(i_use.fire_advice, j);
|
||||
log.rune_dis_log.handleOnce(rune_dis, j);
|
||||
log.fly_time_log.handleOnce(i_use.fly_time, j);
|
||||
log.control_a_yaw_log.handleOnce(i_use.a_yaw / 180.0 * M_PI, j);
|
||||
log.control_a_pitch_log.handleOnce(i_use.a_pitch / 180.0 * M_PI, j);
|
||||
|
||||
std::ofstream file("/dev/shm/cmd_log.json");
|
||||
if (file.is_open()) {
|
||||
file << j.dump();
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace wust_vision::auto_buff
|
||||
39
wust_vision-main/tasks/auto_buff/debug.hpp
Normal file
39
wust_vision-main/tasks/auto_buff/debug.hpp
Normal file
@@ -0,0 +1,39 @@
|
||||
#pragma once
|
||||
#include "tasks/auto_buff/auto_buff.hpp"
|
||||
#include "tasks/auto_buff/rune_tracker/rune_target.hpp"
|
||||
#include "tasks/utils/debug_utils.hpp"
|
||||
#include <wust_vl/video/icamera.hpp>
|
||||
namespace wust_vision::auto_buff {
|
||||
struct AutoBuffDebug {
|
||||
wust_vl::video::ImageFrame img_frame;
|
||||
auto_buff::RuneTarget target;
|
||||
AimTarget aim_target;
|
||||
auto_buff::PowerRune power_rune;
|
||||
double predict_angle;
|
||||
GimbalCmd gimbal_cmd;
|
||||
Eigen::Matrix4d T_camera_to_odom;
|
||||
double latency_ms;
|
||||
double obs_angle;
|
||||
double pre_angle;
|
||||
double fitter_v;
|
||||
double obs_v;
|
||||
cv::Rect expanded;
|
||||
double pnp_distance;
|
||||
std::pair<double, double> gimbal_py;
|
||||
};
|
||||
void drawDebugRuneContent(
|
||||
cv::Mat& debug_img,
|
||||
const AutoBuffDebug& dbg,
|
||||
std::pair<cv::Mat, cv::Mat> camera_info
|
||||
);
|
||||
void writeTargetLogToJson(const auto_buff::RuneTarget& rune_target);
|
||||
inline void drawDebugOverlayShm(
|
||||
const AutoBuffDebug& dbg,
|
||||
std::pair<cv::Mat, cv::Mat> camera_info,
|
||||
bool auto_fps
|
||||
) {
|
||||
static ShmWriter shm { "/debug_frame" };
|
||||
drawDebugOverlayImpl(dbg, camera_info, auto_fps, drawDebugRuneContent, shm);
|
||||
}
|
||||
void debuglog(const AutoBuffDebug& dbg);
|
||||
} // namespace wust_vision::auto_buff
|
||||
237
wust_vision-main/tasks/auto_buff/rune_control/aimer.cpp
Normal file
237
wust_vision-main/tasks/auto_buff/rune_control/aimer.cpp
Normal file
@@ -0,0 +1,237 @@
|
||||
#include "aimer.hpp"
|
||||
#include "tasks/auto_buff/rune_tracker/rune_target.hpp"
|
||||
#include "wust_vl/common/utils/manual_compensator.hpp"
|
||||
namespace wust_vision {
|
||||
|
||||
namespace auto_buff {
|
||||
|
||||
struct Aimer::Impl {
|
||||
public:
|
||||
struct AimerConfig: wust_vl::common::utils::ParamGroup {
|
||||
static constexpr const char* Logger = "Config: auto_buff::aimer";
|
||||
static constexpr const char* kKey = "aimer";
|
||||
const char* key() const override {
|
||||
return kKey;
|
||||
}
|
||||
using Ptr = std::shared_ptr<AimerConfig>;
|
||||
AimerConfig() {}
|
||||
static Ptr create() {
|
||||
return std::make_shared<AimerConfig>();
|
||||
}
|
||||
std::shared_ptr<wust_vl::common::utils::ManualCompensator> manual_compensator;
|
||||
GEN_PARAM(double, prediction_delay);
|
||||
GEN_PARAM(double, shooting_range_h);
|
||||
GEN_PARAM(double, shooting_range_w);
|
||||
GEN_PARAM(double, min_enable_pitch_deg);
|
||||
GEN_PARAM(double, min_enable_yaw_deg);
|
||||
bool first_load = false;
|
||||
using OffsetEntry = wust_vl::common::utils::OffsetEntry;
|
||||
|
||||
static std::vector<OffsetEntry>
|
||||
parseTrajectoryOffset(const YAML::Node& node, double& base_pitch, double& base_yaw) {
|
||||
std::vector<OffsetEntry> entries;
|
||||
|
||||
if (!node || !node["trajectory_offset"]) {
|
||||
return entries;
|
||||
}
|
||||
|
||||
for (const auto& n: node["trajectory_offset"]) {
|
||||
entries.push_back(OffsetEntry { .d_min = n["d_min"].as<double>(),
|
||||
.d_max = n["d_max"].as<double>(),
|
||||
.h_min = n["h_min"].as<double>(),
|
||||
.h_max = n["h_max"].as<double>(),
|
||||
.pitch_off = n["pitch_off"].as<double>(),
|
||||
.yaw_off = n["yaw_off"].as<double>() });
|
||||
}
|
||||
|
||||
base_pitch = node["base_offset"]["pitch"].as<double>();
|
||||
base_yaw = node["base_offset"]["yaw"].as<double>();
|
||||
|
||||
return entries;
|
||||
}
|
||||
std::vector<OffsetEntry> last_entries_;
|
||||
double last_base_pitch_ = 0.0;
|
||||
double last_base_yaw_ = 0.0;
|
||||
bool first_load_ = true;
|
||||
void loadSelf(const YAML::Node& node) override {
|
||||
double base_pitch = 0.0;
|
||||
double base_yaw = 0.0;
|
||||
|
||||
auto entries = parseTrajectoryOffset(node, base_pitch, base_yaw);
|
||||
|
||||
const bool trajectory_changed = first_load_ || entries != last_entries_
|
||||
|| base_pitch != last_base_pitch_ || base_yaw != last_base_yaw_;
|
||||
if (trajectory_changed) {
|
||||
manual_compensator =
|
||||
std::make_shared<wust_vl::common::utils::ManualCompensator>();
|
||||
|
||||
manual_compensator->setBasePitch(base_pitch);
|
||||
manual_compensator->setBaseYaw(base_yaw);
|
||||
|
||||
if (!manual_compensator->updateMapFlow(entries) || entries.empty()) {
|
||||
std::cout << "manual_compensator init failed" << std::endl;
|
||||
}
|
||||
|
||||
last_entries_ = entries;
|
||||
last_base_pitch_ = base_pitch;
|
||||
last_base_yaw_ = base_yaw;
|
||||
first_load_ = false;
|
||||
}
|
||||
shooting_range_h_param.load(node);
|
||||
shooting_range_w_param.load(node);
|
||||
min_enable_pitch_deg_param.load(node);
|
||||
min_enable_yaw_deg_param.load(node);
|
||||
prediction_delay_param.load(node);
|
||||
}
|
||||
};
|
||||
Impl(wust_vl::common::utils::Parameter::Ptr auto_buff_config_parameter) {
|
||||
aimer_config_ = AimerConfig::create();
|
||||
trajectory_compensator_config_ = TrajectoryCompensatorConfig::create();
|
||||
auto_buff_config_parameter->registerGroup(*aimer_config_);
|
||||
auto_buff_config_parameter->registerGroup(*trajectory_compensator_config_);
|
||||
auto_buff_config_parameter->reloadFromOldPath();
|
||||
}
|
||||
std::tuple<double, double> calEnableDiff(Eigen::Vector3d aim_target_pos) const noexcept {
|
||||
const double distance = aim_target_pos.norm();
|
||||
double shooting_range_yaw =
|
||||
std::abs(atan2(aimer_config_->shooting_range_w_param.get() / 2, distance));
|
||||
double shooting_range_pitch =
|
||||
std::abs(atan2(aimer_config_->shooting_range_h_param.get() / 2, distance));
|
||||
constexpr double yaw_factor = 1.0;
|
||||
constexpr double pitch_factor = 1.0;
|
||||
shooting_range_yaw = std::max(
|
||||
shooting_range_yaw,
|
||||
aimer_config_->min_enable_yaw_deg_param.get() * M_PI / 180
|
||||
);
|
||||
shooting_range_pitch = std::max(
|
||||
shooting_range_pitch,
|
||||
aimer_config_->min_enable_pitch_deg_param.get() * M_PI / 180
|
||||
);
|
||||
shooting_range_yaw *= yaw_factor;
|
||||
shooting_range_pitch *= pitch_factor;
|
||||
|
||||
return std::make_tuple(std::abs(shooting_range_yaw), std::abs(shooting_range_pitch));
|
||||
}
|
||||
struct ControlPoint {
|
||||
double yaw;
|
||||
double pitch;
|
||||
Eigen::Vector3d aim_pos;
|
||||
};
|
||||
ControlPoint getControlPoint(RuneTarget target, double bullet_speed) const noexcept {
|
||||
auto [aim_target_pos, _] = target.getHitPoint();
|
||||
ControlPoint cp;
|
||||
double control_yaw = std::atan2(aim_target_pos.y(), aim_target_pos.x());
|
||||
double raw_pitch = std::atan2(
|
||||
aim_target_pos.z(),
|
||||
std::sqrt(
|
||||
aim_target_pos.x() * aim_target_pos.x()
|
||||
+ aim_target_pos.y() * aim_target_pos.y()
|
||||
)
|
||||
);
|
||||
try {
|
||||
trajectory_compensator_config_->trajectory_compensator
|
||||
->compensate(aim_target_pos, raw_pitch, bullet_speed);
|
||||
} catch (std::exception& e) {
|
||||
std::cout << "compensate error: " << e.what() << std::endl;
|
||||
}
|
||||
|
||||
double control_pitch = raw_pitch;
|
||||
const auto offs = aimer_config_->manual_compensator->angleHardCorrect(
|
||||
aim_target_pos.head(2).norm(),
|
||||
aim_target_pos.z()
|
||||
);
|
||||
control_yaw = angles::normalize_angle((control_yaw + offs[1] * M_PI / 180.0));
|
||||
control_pitch = (control_pitch + offs[0] * M_PI / 180.0);
|
||||
cp.pitch = control_pitch;
|
||||
cp.yaw = control_yaw;
|
||||
cp.aim_pos = aim_target_pos;
|
||||
return cp;
|
||||
}
|
||||
GimbalCmd aim(RuneTarget target, double bullet_speed) {
|
||||
GimbalCmd cmd;
|
||||
|
||||
// 当前时间
|
||||
const auto now = wust_vl::common::utils::time_utils::now();
|
||||
|
||||
// 首次预测目标位置
|
||||
target.predictWithFitter(now);
|
||||
auto [p0, q0] = target.getHitPoint();
|
||||
|
||||
// 迭代计算飞行时间
|
||||
bool converged = false;
|
||||
double prev_fly_time = trajectory_compensator_config_->trajectory_compensator
|
||||
->getFlyingTime(p0, bullet_speed);
|
||||
|
||||
std::vector<RuneTarget> iteration_target(10, target);
|
||||
for (int iter = 0; iter < 10; ++iter) {
|
||||
iteration_target[iter].predictWithFitter(prev_fly_time);
|
||||
auto [pb, qb] = iteration_target[iter].getHitPoint();
|
||||
double iter_fly_time = trajectory_compensator_config_->trajectory_compensator
|
||||
->getFlyingTime(pb, bullet_speed);
|
||||
if (std::abs(iter_fly_time - prev_fly_time) < 0.001) {
|
||||
converged = true;
|
||||
break;
|
||||
}
|
||||
prev_fly_time = iter_fly_time;
|
||||
}
|
||||
|
||||
const double predict_time = prev_fly_time + aimer_config_->prediction_delay_param.get();
|
||||
target.predictWithFitter(predict_time);
|
||||
const auto cp = getControlPoint(target, bullet_speed);
|
||||
// RuneTarget target_prev, target_next = target;
|
||||
RuneTarget target_prev = target;
|
||||
RuneTarget target_next = target;
|
||||
|
||||
const double dt = 0.01;
|
||||
target_prev.predictWithFitter(-dt);
|
||||
auto cp_prev = getControlPoint(target_prev, bullet_speed);
|
||||
|
||||
target_next.predictWithFitter(dt);
|
||||
auto cp_next = getControlPoint(target_next, bullet_speed);
|
||||
|
||||
double yaw_speed = (cp_next.yaw - cp_prev.yaw) / (2.0 * dt);
|
||||
double pitch_speed = (cp_next.pitch - cp_prev.pitch) / (2.0 * dt);
|
||||
double yaw_acc = (cp_next.yaw - 2.0 * cp.yaw + cp_prev.yaw) / (dt * dt);
|
||||
double pitch_acc = (cp_next.pitch - 2.0 * cp.pitch + cp_prev.pitch) / (dt * dt);
|
||||
|
||||
AimTarget aim_target;
|
||||
aim_target.pos = cp.aim_pos;
|
||||
|
||||
// 填充 GimbalCmd
|
||||
cmd.distance = cp.aim_pos.norm();
|
||||
cmd.aim_target = aim_target;
|
||||
cmd.yaw = cp.yaw * 180.0 / M_PI;
|
||||
cmd.pitch = cp.pitch * 180.0 / M_PI;
|
||||
// cmd.v_yaw = yaw_speed * 180.0 / M_PI; // 转为度/秒
|
||||
// cmd.v_pitch = pitch_speed * 180.0 / M_PI;
|
||||
// cmd.a_yaw = yaw_acc * 180.0 / M_PI; // 转为度/秒²
|
||||
// cmd.a_pitch = pitch_acc * 180.0 / M_PI;
|
||||
cmd.v_yaw = 0.0; // 转为度/秒
|
||||
cmd.v_pitch = 0.0;
|
||||
cmd.a_yaw = 0.0; // 转为度/秒²
|
||||
cmd.a_pitch = 0.0;
|
||||
const auto [enable_yaw, enable_pitch] = calEnableDiff(cp.aim_pos);
|
||||
cmd.fire_advice = true;
|
||||
cmd.enable_yaw_diff = enable_yaw;
|
||||
cmd.enable_pitch_diff = enable_pitch;
|
||||
cmd.target_yaw = cp.yaw * 180.0 / M_PI;
|
||||
cmd.target_pitch = cp.pitch * 180.0 / M_PI;
|
||||
cmd.fly_time = prev_fly_time;
|
||||
cmd.appear = true;
|
||||
|
||||
return cmd;
|
||||
}
|
||||
TrajectoryCompensatorConfig::Ptr trajectory_compensator_config_;
|
||||
AimerConfig::Ptr aimer_config_;
|
||||
};
|
||||
Aimer::Aimer(wust_vl::common::utils::Parameter::Ptr auto_buff_config_parameter) {
|
||||
_impl = std::make_unique<Impl>(auto_buff_config_parameter);
|
||||
}
|
||||
Aimer::~Aimer() {
|
||||
_impl.reset();
|
||||
}
|
||||
GimbalCmd Aimer::aim(const auto_buff::RuneTarget& target, double bullet_speed) {
|
||||
return _impl->aim(target, bullet_speed);
|
||||
}
|
||||
} // namespace auto_buff
|
||||
} // namespace wust_vision
|
||||
22
wust_vision-main/tasks/auto_buff/rune_control/aimer.hpp
Normal file
22
wust_vision-main/tasks/auto_buff/rune_control/aimer.hpp
Normal file
@@ -0,0 +1,22 @@
|
||||
#pragma once
|
||||
#include "tasks/auto_buff/rune_tracker/rune_target.hpp"
|
||||
#include "tasks/type_common.hpp"
|
||||
namespace wust_vision {
|
||||
namespace auto_buff {
|
||||
class Aimer {
|
||||
public:
|
||||
using Ptr = std::unique_ptr<Aimer>;
|
||||
Aimer(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<Aimer>(auto_buff_config_parameter);
|
||||
}
|
||||
~Aimer();
|
||||
GimbalCmd aim(const auto_buff::RuneTarget& target, double bullet_speed);
|
||||
|
||||
private:
|
||||
struct Impl;
|
||||
std::unique_ptr<Impl> _impl;
|
||||
};
|
||||
|
||||
} // namespace auto_buff
|
||||
} // namespace wust_vision
|
||||
497
wust_vision-main/tasks/auto_buff/rune_detector/rune_detector.cpp
Normal file
497
wust_vision-main/tasks/auto_buff/rune_detector/rune_detector.cpp
Normal file
@@ -0,0 +1,497 @@
|
||||
#include "rune_detector.hpp"
|
||||
#include "tasks/utils/utils.hpp"
|
||||
#include <opencv2/highgui.hpp>
|
||||
namespace wust_vision {
|
||||
namespace auto_buff {
|
||||
struct RuneDetectorCV::Impl {
|
||||
public:
|
||||
Impl(const YAML::Node& node) {
|
||||
params_.load(node);
|
||||
}
|
||||
|
||||
void setCallback(DetectorCallback callback) {
|
||||
callback_ = callback;
|
||||
}
|
||||
cv::Mat preProcess(const cv::Mat& src, bool use_red = false) {
|
||||
// cv::Mat bin;
|
||||
// cv::cvtColor(src, bin, cv::COLOR_RGB2GRAY);
|
||||
// cv::threshold(bin, bin, params_.bin_threshold, 255, cv::THRESH_BINARY);
|
||||
std::vector<cv::Mat> channels;
|
||||
cv::split(src, channels); // BGR
|
||||
|
||||
cv::Mat diff;
|
||||
if (use_red) {
|
||||
cv::subtract(channels[2], channels[0], diff); // R - B
|
||||
} else {
|
||||
cv::subtract(channels[0], channels[2], diff); // B - R
|
||||
}
|
||||
|
||||
cv::Mat bin;
|
||||
cv::threshold(diff, bin, params_.bin_threshold, 255, cv::THRESH_BINARY);
|
||||
// cv::imshow("bin", bin);
|
||||
// cv::waitKey(1);
|
||||
return bin;
|
||||
}
|
||||
inline auto_buff::RuneCenter getRuneCenter(
|
||||
const std::vector<std::vector<cv::Point>>& contours,
|
||||
const std::vector<cv::Vec4i>& hierarchy,
|
||||
cv::Size image_size,
|
||||
cv::Point2f offset,
|
||||
cv::Mat& debug_img,
|
||||
std::vector<bool>& used_flags
|
||||
) {
|
||||
auto_buff::RuneCenter result;
|
||||
struct Node {
|
||||
cv::Point2f center;
|
||||
int idx;
|
||||
cv::RotatedRect rr;
|
||||
};
|
||||
|
||||
std::vector<Node> nodes;
|
||||
|
||||
for (int i = 0; i < contours.size(); i++) {
|
||||
if (used_flags[i])
|
||||
continue;
|
||||
if (hierarchy[i][3] != -1)
|
||||
continue;
|
||||
|
||||
double area = cv::contourArea(contours[i]);
|
||||
if (area < params_.rune_center_min_area || area > params_.rune_center_max_area)
|
||||
continue;
|
||||
|
||||
cv::RotatedRect rr = cv::minAreaRect(contours[i]);
|
||||
float w = rr.size.width;
|
||||
float h = rr.size.height;
|
||||
|
||||
if (w < 5 || h < 5)
|
||||
continue;
|
||||
|
||||
double ratio = (w > h ? w / h : h / w);
|
||||
if (ratio - 1.0 > params_.rune_center_1x1ratio_tol)
|
||||
continue;
|
||||
|
||||
double rect_area = w * h;
|
||||
if (rect_area <= 1e-5)
|
||||
continue;
|
||||
|
||||
double fill_ratio = area / rect_area;
|
||||
if (fill_ratio < params_.rune_center_fill_ratio_min)
|
||||
continue;
|
||||
|
||||
nodes.push_back({ rr.center, i, rr });
|
||||
|
||||
if (!debug_img.empty()) {
|
||||
cv::Point2f pts[4];
|
||||
rr.points(pts);
|
||||
for (size_t k = 0; k < 4; k++) {
|
||||
pts[k] += offset;
|
||||
}
|
||||
for (int k = 0; k < 4; k++) {
|
||||
cv::line(debug_img, pts[k], pts[(k + 1) % 4], cv::Scalar(0, 255, 0), 2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (nodes.empty())
|
||||
return result;
|
||||
|
||||
cv::Point2f img_center(image_size.width * 0.5f, image_size.height * 0.5f);
|
||||
|
||||
double best_dist = 1e18;
|
||||
int best_idx = -1;
|
||||
cv::RotatedRect best_rr;
|
||||
|
||||
for (auto& n: nodes) {
|
||||
double dx = n.center.x - img_center.x;
|
||||
double dy = n.center.y - img_center.y;
|
||||
double dist2 = dx * dx + dy * dy;
|
||||
|
||||
if (dist2 < best_dist) {
|
||||
best_dist = dist2;
|
||||
best_idx = n.idx;
|
||||
best_rr = n.rr;
|
||||
}
|
||||
}
|
||||
|
||||
if (!debug_img.empty()) {
|
||||
cv::circle(
|
||||
debug_img,
|
||||
img_center + offset,
|
||||
5,
|
||||
cv::Scalar(0, 255, 255),
|
||||
-1
|
||||
); // 图像中心
|
||||
|
||||
cv::Point2f pts[4];
|
||||
best_rr.points(pts);
|
||||
for (size_t k = 0; k < 4; k++) {
|
||||
pts[k] += offset;
|
||||
}
|
||||
for (int k = 0; k < 4; k++) {
|
||||
cv::line(debug_img, pts[k], pts[(k + 1) % 4], cv::Scalar(0, 0, 255), 2);
|
||||
}
|
||||
}
|
||||
|
||||
return auto_buff::RuneCenter(best_rr);
|
||||
}
|
||||
|
||||
inline int findTopParent(int idx, const std::vector<cv::Vec4i>& hierarchy) {
|
||||
int p = hierarchy[idx][3]; // parent
|
||||
while (p != -1 && hierarchy[p][3] != -1) {
|
||||
p = hierarchy[p][3]; // 一直追溯到最顶层 parent
|
||||
}
|
||||
return p; // 若 p == -1 表示 contour 本身就是顶层轮廓
|
||||
}
|
||||
|
||||
inline std::vector<auto_buff::RunePan> markRuneTarget(
|
||||
const std::vector<std::vector<cv::Point>>& contours,
|
||||
const std::vector<cv::Vec4i>& hierarchy,
|
||||
std::vector<bool>& used_flags
|
||||
) {
|
||||
std::vector<auto_buff::RunePan> results;
|
||||
if (hierarchy.empty())
|
||||
return results;
|
||||
|
||||
struct Node {
|
||||
int idx;
|
||||
cv::Point2f center;
|
||||
int parent_top_id;
|
||||
};
|
||||
|
||||
std::vector<Node> candidates;
|
||||
|
||||
for (int i = 0; i < contours.size(); i++) {
|
||||
if (used_flags[i])
|
||||
continue;
|
||||
|
||||
const auto& cnt = contours[i];
|
||||
|
||||
double contour_area = cv::contourArea(cnt);
|
||||
if (contour_area < params_.rune_target_min_area
|
||||
|| contour_area > params_.rune_target_max_area)
|
||||
continue;
|
||||
|
||||
cv::Moments m = cv::moments(cnt);
|
||||
if (m.m00 == 0)
|
||||
continue;
|
||||
|
||||
cv::Point2f center(m.m10 / m.m00, m.m01 / m.m00);
|
||||
int top_parent = findTopParent(i, hierarchy);
|
||||
candidates.push_back({ i, center, top_parent });
|
||||
}
|
||||
|
||||
if (candidates.size() < 3)
|
||||
return results;
|
||||
|
||||
std::unordered_map<int, std::vector<int>> groups;
|
||||
for (int i = 0; i < candidates.size(); i++) {
|
||||
groups[candidates[i].parent_top_id].push_back(i);
|
||||
}
|
||||
|
||||
for (auto& [parent_top_id, idx_list]: groups) {
|
||||
int M = idx_list.size();
|
||||
if (M < 3 || M > 7)
|
||||
continue;
|
||||
|
||||
std::vector<int> cluster_id(M, -1);
|
||||
int cluster_count = 0;
|
||||
|
||||
for (int i = 0; i < M; i++) {
|
||||
if (cluster_id[i] != -1)
|
||||
continue;
|
||||
|
||||
cluster_id[i] = cluster_count;
|
||||
|
||||
std::queue<int> q;
|
||||
q.push(i);
|
||||
|
||||
while (!q.empty()) {
|
||||
int u = q.front();
|
||||
q.pop();
|
||||
|
||||
for (int v = 0; v < M; v++) {
|
||||
if (cluster_id[v] != -1)
|
||||
continue;
|
||||
|
||||
auto& cu = candidates[idx_list[u]].center;
|
||||
auto& cv = candidates[idx_list[v]].center;
|
||||
|
||||
double dx = cu.x - cv.x;
|
||||
double dy = cu.y - cv.y;
|
||||
double dist = std::sqrt(dx * dx + dy * dy);
|
||||
|
||||
if (dist <= params_.rune_target_cluster_radius) {
|
||||
cluster_id[v] = cluster_count;
|
||||
q.push(v);
|
||||
}
|
||||
}
|
||||
}
|
||||
cluster_count++;
|
||||
}
|
||||
|
||||
std::vector<int> cluster_size(cluster_count, 0);
|
||||
for (int id: cluster_id)
|
||||
cluster_size[id]++;
|
||||
|
||||
std::vector<std::vector<cv::Point2f>> cluster_points(cluster_count);
|
||||
|
||||
for (int i = 0; i < M; i++) {
|
||||
int cid = cluster_id[i];
|
||||
|
||||
if (cluster_size[cid] >= 3) {
|
||||
int contour_index = candidates[idx_list[i]].idx;
|
||||
used_flags[contour_index] = true;
|
||||
cluster_points[cid].push_back(candidates[idx_list[i]].center);
|
||||
}
|
||||
}
|
||||
|
||||
for (int cid = 0; cid < cluster_count; cid++) {
|
||||
if (cluster_points[cid].size() < 3)
|
||||
continue;
|
||||
|
||||
cv::RotatedRect rr = cv::minAreaRect(cluster_points[cid]);
|
||||
double w = rr.size.width;
|
||||
double h = rr.size.height;
|
||||
|
||||
if (w < 1 || h < 1)
|
||||
continue;
|
||||
|
||||
double ratio = (w > h ? w / h : h / w);
|
||||
if (ratio > params_.rune_target_max_square_ratio)
|
||||
continue;
|
||||
|
||||
std::vector<std::pair<double, cv::Point2f>> dist_list;
|
||||
dist_list.reserve(cluster_points[cid].size());
|
||||
|
||||
for (auto& p: cluster_points[cid]) {
|
||||
double dx = p.x - rr.center.x;
|
||||
double dy = p.y - rr.center.y;
|
||||
double dist = dx * dx + dy * dy;
|
||||
dist_list.emplace_back(dist, p);
|
||||
}
|
||||
|
||||
std::sort(dist_list.begin(), dist_list.end(), [](auto& a, auto& b) {
|
||||
return a.first > b.first;
|
||||
});
|
||||
|
||||
std::vector<cv::Point2f> corner_points;
|
||||
for (int i = 0; i < 4 && i < dist_list.size(); i++)
|
||||
corner_points.push_back(dist_list[i].second);
|
||||
|
||||
auto_buff::RunePan pan;
|
||||
pan.center = rr.center;
|
||||
pan.corners = corner_points;
|
||||
if (corner_points.size() > 3)
|
||||
pan.is_valid = true;
|
||||
|
||||
results.push_back(pan);
|
||||
}
|
||||
}
|
||||
|
||||
return results;
|
||||
}
|
||||
|
||||
inline void markInvalidContours(
|
||||
cv::Mat& color,
|
||||
cv::Mat& debug_img,
|
||||
const std::vector<std::vector<cv::Point>>& contours,
|
||||
std::vector<bool>& used_flags,
|
||||
const cv::Rect& valid_rect,
|
||||
bool filter_red,
|
||||
double diff_thresh
|
||||
) {
|
||||
used_flags.assign(contours.size(), false);
|
||||
|
||||
for (int i = 0; i < contours.size(); i++) {
|
||||
cv::Rect r = cv::boundingRect(contours[i]);
|
||||
if (r.width < 5 || r.height < 5)
|
||||
continue;
|
||||
|
||||
cv::Rect rr = r & cv::Rect(0, 0, color.cols, color.rows);
|
||||
if (rr.width < 2 || rr.height < 2)
|
||||
continue;
|
||||
|
||||
const cv::Mat roi = color(rr);
|
||||
const cv::Scalar avg = cv::mean(roi);
|
||||
|
||||
const double B = avg[0], G = avg[1], R = avg[2];
|
||||
|
||||
const double diff_RB = R - B;
|
||||
const double diff_BR = B - R;
|
||||
|
||||
const bool is_red = (diff_RB > diff_thresh);
|
||||
const bool is_blue = (diff_BR > diff_thresh);
|
||||
|
||||
bool invalid = false;
|
||||
|
||||
if (filter_red) {
|
||||
if (is_red)
|
||||
invalid = true;
|
||||
} else {
|
||||
if (is_blue)
|
||||
invalid = true;
|
||||
}
|
||||
cv::Rect inter = r & valid_rect;
|
||||
bool inside_region = (inter.area() > 0);
|
||||
|
||||
used_flags[i] = !invalid || !inside_region;
|
||||
|
||||
if (!used_flags[i]) {
|
||||
if (!debug_img.empty())
|
||||
cv::drawContours(debug_img, contours, i, cv::Scalar(255, 0, 0), 2);
|
||||
}
|
||||
}
|
||||
}
|
||||
static bool isUpscaled(const cv::Rect& roi, int model_w, int model_h) {
|
||||
float scale = std::min(model_w / float(roi.width), model_h / float(roi.height));
|
||||
|
||||
return scale > 1.0f;
|
||||
}
|
||||
void pushInput(CommonFrame& frame, bool debug) {
|
||||
frame.id = current_id_++;
|
||||
auto_buff::RuneFan fan {
|
||||
.is_valid = false,
|
||||
.id = frame.id,
|
||||
.timestamp = frame.img_frame.timestamp,
|
||||
|
||||
};
|
||||
cv::Mat debug_img;
|
||||
if (debug) {
|
||||
debug_img = frame.img_frame.src_img.clone();
|
||||
}
|
||||
cv::Mat roi = frame.img_frame.src_img(frame.expanded);
|
||||
|
||||
cv::Mat processed_img = preProcess(roi, frame.detect_color);
|
||||
|
||||
std::vector<std::vector<cv::Point>> contours;
|
||||
std::vector<cv::Vec4i> hierarchy;
|
||||
|
||||
cv::findContours(
|
||||
processed_img,
|
||||
contours,
|
||||
hierarchy,
|
||||
cv::RETR_TREE,
|
||||
cv::CHAIN_APPROX_SIMPLE
|
||||
);
|
||||
std::vector<bool> used_flags;
|
||||
used_flags.assign(contours.size(), false);
|
||||
markInvalidContours(
|
||||
roi,
|
||||
debug_img,
|
||||
contours,
|
||||
used_flags,
|
||||
cv::Rect(0, 0, roi.cols, roi.rows),
|
||||
frame.detect_color,
|
||||
params_.color_diff_threshold
|
||||
);
|
||||
auto rune_center =
|
||||
getRuneCenter(contours, hierarchy, roi.size(), frame.offset, debug_img, used_flags);
|
||||
std::vector<auto_buff::RunePan> rune_pans =
|
||||
markRuneTarget(contours, hierarchy, used_flags);
|
||||
for (auto& rune_pan: rune_pans) {
|
||||
if (rune_center.is_valid) {
|
||||
rune_pan.addReferRuneCenter(rune_center);
|
||||
}
|
||||
if (rune_pan.is_valid && rune_pan.has_refer) {
|
||||
auto_buff::RuneFan::Simple simple;
|
||||
simple.points2d.push_back(rune_center.center);
|
||||
for (auto& pt: rune_pan.corners) {
|
||||
simple.points2d.push_back(pt);
|
||||
}
|
||||
simple.points2d.push_back(rune_pan.center);
|
||||
fan.fans.push_back(simple);
|
||||
}
|
||||
if (!debug_img.empty())
|
||||
rune_pan.draw(debug_img, frame.offset);
|
||||
}
|
||||
auto_buff::RuneFan tmp = fan;
|
||||
for (int i = 0; i < tmp.fans.size(); i++) {
|
||||
for (int j = 0; j < tmp.fans.size(); j++) {
|
||||
if (i == j)
|
||||
continue;
|
||||
|
||||
fan.fans[i].addOther(tmp.fans[j]);
|
||||
}
|
||||
}
|
||||
fan.addOffset(frame.offset);
|
||||
if (callback_) {
|
||||
callback_(fan, frame, debug_img);
|
||||
}
|
||||
}
|
||||
|
||||
DetectorCallback callback_;
|
||||
cv::Mat tmp_R_;
|
||||
int current_id_ = 0;
|
||||
struct Params {
|
||||
double rune_center_min_area = 100.0;
|
||||
double rune_center_max_area = 2000.0;
|
||||
double rune_center_1x1ratio_tol = 0.7;
|
||||
double rune_center_fill_ratio_min = 0.7;
|
||||
|
||||
double rune_target_min_area = 100.0;
|
||||
double rune_target_max_area = 3000.0;
|
||||
double rune_target_max_square_ratio = 1.3;
|
||||
double rune_target_cluster_radius = 70.0;
|
||||
|
||||
double bin_threshold = 150.0;
|
||||
double color_diff_threshold = 40.0;
|
||||
|
||||
int target_width = 416;
|
||||
int target_height = 416;
|
||||
|
||||
void load(const YAML::Node& node) {
|
||||
// center params
|
||||
rune_center_min_area = node["rune_center_min_area"]
|
||||
? node["rune_center_min_area"].as<double>()
|
||||
: rune_center_min_area;
|
||||
rune_center_max_area = node["rune_center_max_area"]
|
||||
? node["rune_center_max_area"].as<double>()
|
||||
: rune_center_max_area;
|
||||
rune_center_1x1ratio_tol = node["rune_center_1x1ratio_tol"]
|
||||
? node["rune_center_1x1ratio_tol"].as<double>()
|
||||
: rune_center_1x1ratio_tol;
|
||||
rune_center_fill_ratio_min = node["rune_center_fill_ratio_min"]
|
||||
? node["rune_center_fill_ratio_min"].as<double>()
|
||||
: rune_center_fill_ratio_min;
|
||||
|
||||
// target params
|
||||
rune_target_min_area = node["rune_target_min_area"]
|
||||
? node["rune_target_min_area"].as<double>()
|
||||
: rune_target_min_area;
|
||||
rune_target_max_area = node["rune_target_max_area"]
|
||||
? node["rune_target_max_area"].as<double>()
|
||||
: rune_target_max_area;
|
||||
rune_target_max_square_ratio = node["rune_target_max_square_ratio"]
|
||||
? node["rune_target_max_square_ratio"].as<double>()
|
||||
: rune_target_max_square_ratio;
|
||||
rune_target_cluster_radius = node["rune_target_cluster_radius"]
|
||||
? node["rune_target_cluster_radius"].as<double>()
|
||||
: rune_target_cluster_radius;
|
||||
|
||||
bin_threshold =
|
||||
node["bin_threshold"] ? node["bin_threshold"].as<double>() : bin_threshold;
|
||||
|
||||
color_diff_threshold = node["color_diff_threshold"]
|
||||
? node["color_diff_threshold"].as<double>()
|
||||
: color_diff_threshold;
|
||||
|
||||
target_width = node["target_width"] ? node["target_width"].as<int>() : target_width;
|
||||
target_height =
|
||||
node["target_height"] ? node["target_height"].as<int>() : target_height;
|
||||
}
|
||||
} params_;
|
||||
};
|
||||
RuneDetectorCV::RuneDetectorCV(const YAML::Node& node) {
|
||||
_impl = std::make_unique<Impl>(node);
|
||||
}
|
||||
RuneDetectorCV::~RuneDetectorCV() {
|
||||
_impl.reset();
|
||||
}
|
||||
void RuneDetectorCV::pushInput(CommonFrame& frame, bool debug) {
|
||||
_impl->pushInput(frame, debug);
|
||||
}
|
||||
void RuneDetectorCV::setCallback(DetectorCallback callback) {
|
||||
_impl->setCallback(callback);
|
||||
}
|
||||
} // namespace auto_buff
|
||||
} // namespace wust_vision
|
||||
@@ -0,0 +1,24 @@
|
||||
#pragma once
|
||||
|
||||
#include "tasks/auto_buff/type.hpp"
|
||||
namespace wust_vision {
|
||||
namespace auto_buff {
|
||||
class RuneDetectorCV {
|
||||
public:
|
||||
using DetectorCallback =
|
||||
std::function<void(const auto_buff::RuneFan&, const CommonFrame&, cv::Mat&)>;
|
||||
using Ptr = std::unique_ptr<RuneDetectorCV>;
|
||||
RuneDetectorCV(const YAML::Node& node);
|
||||
static inline std::unique_ptr<RuneDetectorCV> make_detector(const YAML::Node& node) {
|
||||
return std::make_unique<RuneDetectorCV>(node);
|
||||
}
|
||||
~RuneDetectorCV();
|
||||
void pushInput(CommonFrame& frame, bool debug = false);
|
||||
void setCallback(DetectorCallback callback);
|
||||
|
||||
private:
|
||||
struct Impl;
|
||||
std::unique_ptr<Impl> _impl;
|
||||
};
|
||||
} // namespace auto_buff
|
||||
} // namespace wust_vision
|
||||
@@ -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
336
wust_vision-main/tasks/auto_buff/rune_tracker/rune_target.cpp
Normal 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
230
wust_vision-main/tasks/auto_buff/rune_tracker/rune_target.hpp
Normal 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
121
wust_vision-main/tasks/auto_buff/rune_tracker/rune_tracker.cpp
Normal 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
|
||||
@@ -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
265
wust_vision-main/tasks/auto_buff/rune_tracker/spd_fitter.hpp
Normal 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, ¶ms_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
|
||||
213
wust_vision-main/tasks/auto_buff/rune_where/rune_where.cpp
Normal file
213
wust_vision-main/tasks/auto_buff/rune_where/rune_where.cpp
Normal file
@@ -0,0 +1,213 @@
|
||||
#include "rune_where.hpp"
|
||||
#include "tasks/utils/utils.hpp"
|
||||
#include <Eigen/Dense>
|
||||
#include <opencv2/core/eigen.hpp>
|
||||
|
||||
namespace wust_vision {
|
||||
namespace auto_buff {
|
||||
struct RuneWhere::Impl {
|
||||
public:
|
||||
Impl(const YAML::Node& config, const std::pair<cv::Mat, cv::Mat>& camera_info) {
|
||||
camera_info_ = camera_info;
|
||||
}
|
||||
|
||||
struct Params {
|
||||
enum class OptMode : int { GOLDEN = 0, CERES = 1, NONE = 2 } opt_mode;
|
||||
OptMode fromString(const std::string& mode) {
|
||||
if (mode == "golden" || mode == "GOLDEN") {
|
||||
return OptMode::GOLDEN;
|
||||
} else if (mode == "none" || mode == "NONE") {
|
||||
return OptMode::NONE;
|
||||
} else {
|
||||
return OptMode::NONE;
|
||||
}
|
||||
}
|
||||
|
||||
int golden_search_side_deg = 60;
|
||||
void load(const YAML::Node& node) {
|
||||
opt_mode = fromString(node["roll_opt"]["mode"].as<std::string>());
|
||||
golden_search_side_deg = node["roll_opt"]["golden_search_side_deg"].as<int>();
|
||||
}
|
||||
} params_;
|
||||
auto_buff::RuneFan
|
||||
where(auto_buff::RuneFan f, Eigen::Matrix4d T_camera_to_odom) const noexcept {
|
||||
const Eigen::Matrix3d R_imu_cam = T_camera_to_odom.block<3, 3>(0, 0);
|
||||
for (auto& fan: f.fans) {
|
||||
cv::Mat rvec, tvec;
|
||||
cv::solvePnP(
|
||||
fan.getObjs(),
|
||||
fan.landmarks(),
|
||||
camera_info_.first,
|
||||
camera_info_.second,
|
||||
rvec,
|
||||
tvec,
|
||||
false,
|
||||
cv::SOLVEPNP_IPPE //平移更稳定，（旋转这里纯靠后面优化）
|
||||
);
|
||||
cv::Mat R_cv;
|
||||
cv::Rodrigues(rvec, R_cv);
|
||||
Eigen::Matrix3d R = utils::cvToEigen(R_cv);
|
||||
Eigen::Vector3d t = utils::cvToEigen(tvec);
|
||||
if (params_.opt_mode != Params::OptMode::NONE) {
|
||||
R = solveBa_R(fan, t, R, R_imu_cam);
|
||||
}
|
||||
|
||||
fan.ori = Eigen::Quaterniond(R);
|
||||
fan.pos = t;
|
||||
Eigen::Vector3d pos_camera = fan.pos;
|
||||
fan.target_pos = utils::transformPosition(pos_camera, T_camera_to_odom);
|
||||
|
||||
const Eigen::Quaterniond
|
||||
q_camera(fan.ori.w(), fan.ori.x(), fan.ori.y(), fan.ori.z());
|
||||
const Eigen::Quaterniond q_odom =
|
||||
utils::transformOrientation(q_camera, T_camera_to_odom);
|
||||
fan.target_ori = q_odom;
|
||||
|
||||
f.is_valid = true;
|
||||
}
|
||||
return f;
|
||||
}
|
||||
std::vector<Eigen::Vector2d> reprojection(
|
||||
double roll,
|
||||
const std::vector<cv::Point3f>& object_points,
|
||||
const std::vector<cv::Point2f>& landmarks,
|
||||
const Eigen::Matrix3d& Rci,
|
||||
double pitch,
|
||||
double yaw,
|
||||
const Eigen::Vector3d& t
|
||||
) const noexcept {
|
||||
const Eigen::AngleAxisd ay(yaw, Eigen::Vector3d::UnitZ());
|
||||
const Eigen::AngleAxisd ap(pitch, Eigen::Vector3d::UnitY());
|
||||
const Eigen::AngleAxisd ar(roll, Eigen::Vector3d::UnitX());
|
||||
const Eigen::Matrix3d R = Rci * (ay * ap * ar).toRotationMatrix();
|
||||
|
||||
cv::Mat rvec, R_cv;
|
||||
cv::eigen2cv(R, R_cv);
|
||||
cv::Rodrigues(R_cv, rvec);
|
||||
|
||||
const cv::Mat tvec = (cv::Mat_<double>(3, 1) << t.x(), t.y(), t.z());
|
||||
|
||||
std::vector<cv::Point2f> pts_2d;
|
||||
pts_2d.reserve(object_points.size());
|
||||
cv::projectPoints(
|
||||
object_points,
|
||||
rvec,
|
||||
tvec,
|
||||
camera_info_.first,
|
||||
camera_info_.second,
|
||||
pts_2d
|
||||
);
|
||||
|
||||
std::vector<Eigen::Vector2d> image_points;
|
||||
image_points.reserve(pts_2d.size());
|
||||
|
||||
for (const auto& p: pts_2d) {
|
||||
image_points.emplace_back(p.x, p.y);
|
||||
}
|
||||
|
||||
return image_points;
|
||||
}
|
||||
double reprojectionErrorRoll(
|
||||
double roll,
|
||||
const std::vector<cv::Point3f>& obj,
|
||||
const std::vector<cv::Point2f>& lm,
|
||||
const Eigen::Matrix3d& Rci,
|
||||
double pitch,
|
||||
double yaw,
|
||||
const Eigen::Vector3d& t
|
||||
) const noexcept {
|
||||
const auto image_points = reprojection(roll, obj, lm, Rci, pitch, yaw, t);
|
||||
double cost = 0.0;
|
||||
|
||||
for (size_t i = 0; i < image_points.size(); ++i) {
|
||||
Eigen::Vector2d obs(lm[i].x, lm[i].y);
|
||||
cost += (image_points[i] - obs).squaredNorm();
|
||||
}
|
||||
return cost;
|
||||
}
|
||||
|
||||
double goldenRoll(
|
||||
double init,
|
||||
const std::vector<cv::Point3f>& obj,
|
||||
const std::vector<cv::Point2f>& lm,
|
||||
const Eigen::Matrix3d& Rci,
|
||||
double pitch,
|
||||
double yaw,
|
||||
const Eigen::Vector3d& t
|
||||
) const noexcept {
|
||||
constexpr double phi = 1.618033988749894848; // golden ratio
|
||||
double l = init - params_.golden_search_side_deg * M_PI / 180.0;
|
||||
double r = init + params_.golden_search_side_deg * M_PI / 180.0;
|
||||
|
||||
double r1 = r - (r - l) / phi;
|
||||
double r2 = l + (r - l) / phi;
|
||||
|
||||
double f1 = reprojectionErrorRoll(r1, obj, lm, Rci, pitch, yaw, t);
|
||||
double f2 = reprojectionErrorRoll(r2, obj, lm, Rci, pitch, yaw, t);
|
||||
|
||||
while (r - l > 0.0001) { // 约 0.0057 度
|
||||
if (f1 < f2) {
|
||||
r = r2;
|
||||
r2 = r1;
|
||||
f2 = f1;
|
||||
r1 = r - (r - l) / phi;
|
||||
f1 = reprojectionErrorRoll(r1, obj, lm, Rci, pitch, yaw, t);
|
||||
} else {
|
||||
l = r1;
|
||||
r1 = r2;
|
||||
f1 = f2;
|
||||
r2 = l + (r - l) / phi;
|
||||
f2 = reprojectionErrorRoll(r2, obj, lm, Rci, pitch, yaw, t);
|
||||
}
|
||||
}
|
||||
|
||||
return 0.5 * (l + r); // final best roll
|
||||
}
|
||||
|
||||
Eigen::Matrix3d solveBa_R(
|
||||
const auto_buff::RuneFan::Simple& rune_fan,
|
||||
const Eigen::Vector3d& t_camera_armor,
|
||||
const Eigen::Matrix3d& R_camera_armor,
|
||||
const Eigen::Matrix3d& R_imu_camera
|
||||
) const noexcept {
|
||||
Eigen::Matrix3d R_imu_armor = R_imu_camera * R_camera_armor;
|
||||
Eigen::Matrix3d R_camera_imu = R_imu_camera.transpose();
|
||||
double initial_roll = std::atan2(R_imu_armor(2, 1), R_imu_armor(2, 2));
|
||||
double roll = initial_roll;
|
||||
Eigen::Vector3d t_imu_armor = R_imu_camera * t_camera_armor;
|
||||
double yaw = std::atan2(t_imu_armor.y(), t_imu_armor.x());
|
||||
double pitch = 0;
|
||||
auto cv_points = rune_fan.getObjs();
|
||||
const auto& landmarks = rune_fan.landmarks();
|
||||
if (params_.opt_mode == Params::OptMode::GOLDEN) {
|
||||
roll = goldenRoll(
|
||||
roll,
|
||||
cv_points,
|
||||
landmarks,
|
||||
R_camera_imu,
|
||||
pitch,
|
||||
yaw,
|
||||
t_camera_armor
|
||||
);
|
||||
}
|
||||
const Eigen::AngleAxisd ay(yaw, Eigen::Vector3d::UnitZ());
|
||||
const Eigen::AngleAxisd ap(pitch, Eigen::Vector3d::UnitY());
|
||||
const Eigen::AngleAxisd ar(roll, Eigen::Vector3d::UnitX());
|
||||
|
||||
const Eigen::Matrix3d R_result = R_camera_imu * (ay * ap * ar).toRotationMatrix();
|
||||
return R_result;
|
||||
}
|
||||
|
||||
std::pair<cv::Mat, cv::Mat> camera_info_;
|
||||
};
|
||||
RuneWhere::RuneWhere(const YAML::Node& config, const std::pair<cv::Mat, cv::Mat>& camera_info) {
|
||||
_impl = std::make_unique<Impl>(config, camera_info);
|
||||
}
|
||||
RuneWhere::~RuneWhere() {
|
||||
_impl.reset();
|
||||
}
|
||||
auto_buff::RuneFan RuneWhere::where(auto_buff::RuneFan f, Eigen::Matrix4d T_camera_to_odom) {
|
||||
return _impl->where(f, T_camera_to_odom);
|
||||
}
|
||||
} // namespace auto_buff
|
||||
} // namespace wust_vision
|
||||
39
wust_vision-main/tasks/auto_buff/rune_where/rune_where.hpp
Normal file
39
wust_vision-main/tasks/auto_buff/rune_where/rune_where.hpp
Normal file
@@ -0,0 +1,39 @@
|
||||
// Created by Labor 2023.8.25
|
||||
// Maintained by Labor, Chengfu Zou
|
||||
// Copyright (C) FYT Vision Group. All rights reserved.
|
||||
// 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 "tasks/auto_buff/type.hpp"
|
||||
namespace wust_vision {
|
||||
namespace auto_buff {
|
||||
class RuneWhere {
|
||||
public:
|
||||
using Ptr = std::unique_ptr<RuneWhere>;
|
||||
RuneWhere(const YAML::Node& config, const std::pair<cv::Mat, cv::Mat>& camera_info);
|
||||
static Ptr
|
||||
create(const YAML::Node& config, const std::pair<cv::Mat, cv::Mat>& camera_info) {
|
||||
return std::make_unique<RuneWhere>(config, camera_info);
|
||||
}
|
||||
~RuneWhere();
|
||||
auto_buff::RuneFan where(auto_buff::RuneFan f, Eigen::Matrix4d T_camera_to_odom);
|
||||
|
||||
private:
|
||||
struct Impl;
|
||||
std::unique_ptr<Impl> _impl;
|
||||
};
|
||||
|
||||
} // namespace auto_buff
|
||||
} // namespace wust_vision
|
||||
339
wust_vision-main/tasks/auto_buff/type.cpp
Normal file
339
wust_vision-main/tasks/auto_buff/type.cpp
Normal file
@@ -0,0 +1,339 @@
|
||||
#include "type.hpp"
|
||||
#include "tasks/utils/utils.hpp"
|
||||
namespace wust_vision {
|
||||
namespace auto_buff {
|
||||
void RunePan::draw(cv::Mat& img, const cv::Point2f& offset) const {
|
||||
if (!is_valid || corners.size() < 3)
|
||||
return;
|
||||
|
||||
std::vector<cv::Point2f> sorted_corners = corners;
|
||||
for (auto& pt: sorted_corners) {
|
||||
pt += offset;
|
||||
}
|
||||
// 画边
|
||||
for (size_t i = 0; i < sorted_corners.size(); ++i) {
|
||||
cv::line(
|
||||
img,
|
||||
sorted_corners[i],
|
||||
sorted_corners[(i + 1) % sorted_corners.size()],
|
||||
cv::Scalar(0, 255, 255),
|
||||
2
|
||||
);
|
||||
}
|
||||
|
||||
// 画中心点
|
||||
cv::circle(img, center, 3, cv::Scalar(255, 0, 255), -1);
|
||||
if (has_refer) {
|
||||
// 画角点编号
|
||||
for (size_t i = 0; i < sorted_corners.size(); ++i) {
|
||||
cv::Point2f p = sorted_corners[i];
|
||||
|
||||
// 绘制角点位置
|
||||
cv::circle(img, p, 3, cv::Scalar(0, 0, 255), -1);
|
||||
|
||||
// 让数字稍微往右下偏移，避免盖到角点
|
||||
cv::Point2f text_pos = p + cv::Point2f(5, -5);
|
||||
|
||||
cv::putText(
|
||||
img,
|
||||
std::to_string(i),
|
||||
text_pos,
|
||||
cv::FONT_HERSHEY_SIMPLEX,
|
||||
0.5,
|
||||
cv::Scalar(0, 255, 0),
|
||||
1
|
||||
);
|
||||
}
|
||||
}
|
||||
}
|
||||
double RunePan::getArea() const {
|
||||
if (corners.size() < 3)
|
||||
return 0.0;
|
||||
std::vector<cv::Point2f> sorted_corners = corners;
|
||||
std::sort(
|
||||
sorted_corners.begin(),
|
||||
sorted_corners.end(),
|
||||
[this](const cv::Point2f& a, const cv::Point2f& b) {
|
||||
double angA = std::atan2(a.y - center.y, a.x - center.x);
|
||||
double angB = std::atan2(b.y - center.y, b.x - center.x);
|
||||
return angA < angB;
|
||||
}
|
||||
);
|
||||
return cv::contourArea(sorted_corners);
|
||||
}
|
||||
|
||||
void RunePan::addReferRuneCenter(const RuneCenter& rc) {
|
||||
if (!rc.is_valid || !is_valid)
|
||||
return;
|
||||
if (corners.size() != 4)
|
||||
return;
|
||||
|
||||
cv::Point2f down_vec = rc.center - center;
|
||||
float norm = std::sqrt(down_vec.x * down_vec.x + down_vec.y * down_vec.y);
|
||||
if (norm < 1e-6f)
|
||||
return;
|
||||
has_refer = true;
|
||||
float angle_ref = std::atan2(down_vec.y, down_vec.x);
|
||||
|
||||
// 获取4个点在旋转后的角度
|
||||
struct Node {
|
||||
float ang;
|
||||
cv::Point2f p;
|
||||
};
|
||||
std::vector<Node> arr;
|
||||
arr.reserve(4);
|
||||
|
||||
for (auto& p: corners) {
|
||||
cv::Point2f v = p - center;
|
||||
|
||||
// 旋转坐标，使 down_vec 对齐 angle=0
|
||||
float ang = std::atan2(v.y, v.x) - angle_ref;
|
||||
|
||||
// 归一化到 (-π, π]
|
||||
while (ang <= -CV_PI)
|
||||
ang += 2 * CV_PI;
|
||||
while (ang > CV_PI)
|
||||
ang -= 2 * CV_PI;
|
||||
|
||||
arr.push_back({ ang, p });
|
||||
}
|
||||
|
||||
// 按角度排序（从 -π 到 π）
|
||||
std::sort(arr.begin(), arr.end(), [](const Node& a, const Node& b) {
|
||||
return a.ang < b.ang;
|
||||
});
|
||||
|
||||
// 准备象限变量并标记
|
||||
cv::Point2f lu(0, 0), ru(0, 0), rd(0, 0), ld(0, 0);
|
||||
bool has_lu = false, has_ru = false, has_rd = false, has_ld = false;
|
||||
|
||||
for (const auto& n: arr) {
|
||||
float a = n.ang;
|
||||
|
||||
if (a > CV_PI / 2 && a <= CV_PI) {
|
||||
lu = n.p;
|
||||
has_lu = true;
|
||||
} else if (a > 0 && a <= CV_PI / 2) {
|
||||
ru = n.p;
|
||||
has_ru = true;
|
||||
} else if (a > -CV_PI / 2 && a <= 0) {
|
||||
rd = n.p;
|
||||
has_rd = true;
|
||||
} else { // a > -CV_PI && a <= -CV_PI/2
|
||||
ld = n.p;
|
||||
has_ld = true;
|
||||
}
|
||||
}
|
||||
|
||||
std::array<cv::Point2f, 4> ordered;
|
||||
|
||||
if (has_lu && has_ru && has_rd && has_ld) {
|
||||
ordered[0] = lu;
|
||||
ordered[1] = ru;
|
||||
ordered[2] = rd;
|
||||
ordered[3] = ld;
|
||||
corners.assign(ordered.begin(), ordered.end());
|
||||
return;
|
||||
}
|
||||
|
||||
float target = 3.0f * CV_PI / 4.0f; // 135°
|
||||
int best_idx = 0;
|
||||
float best_diff = std::numeric_limits<float>::max();
|
||||
for (int i = 0; i < (int)arr.size(); ++i) {
|
||||
float d = std::fabs(angles::shortest_angular_distance(target, arr[i].ang)
|
||||
); // 如果没有 angles::shortest_angular_distance，可以用下面替代
|
||||
if (d < best_diff) {
|
||||
best_diff = d;
|
||||
best_idx = i;
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = 0; i < 4; ++i) {
|
||||
int idx = (best_idx + i) % 4;
|
||||
ordered[i] = arr[idx].p;
|
||||
}
|
||||
|
||||
corners.assign(ordered.begin(), ordered.end());
|
||||
}
|
||||
|
||||
void RuneFan::Simple::addOther(const Simple& other) {
|
||||
auto l1 = points2d[0] - points2d[5];
|
||||
auto l2 = other.points2d[0] - other.points2d[5];
|
||||
float a1 = std::atan2(l1.y, l1.x);
|
||||
float a2 = std::atan2(l2.y, l2.x);
|
||||
|
||||
float d = a1 - a2;
|
||||
d = normalizeAngle0to2pi(d);
|
||||
|
||||
int id = 0;
|
||||
double min_err = 1e9;
|
||||
for (int i = 0; i < angle_diffs.size(); i++) {
|
||||
double err = std::abs(angle_diffs[i] - d);
|
||||
if (err < min_err) {
|
||||
min_err = err;
|
||||
id = i;
|
||||
}
|
||||
}
|
||||
|
||||
if (id < 1) {
|
||||
return;
|
||||
}
|
||||
has_other++;
|
||||
points2d.push_back(other.points2d[1]);
|
||||
points2d.push_back(other.points2d[2]);
|
||||
points2d.push_back(other.points2d[3]);
|
||||
points2d.push_back(other.points2d[4]);
|
||||
|
||||
double roll = -angle_diffs[id];
|
||||
|
||||
points3d.push_back(rotateX(points3d[1], roll));
|
||||
points3d.push_back(rotateX(points3d[2], roll));
|
||||
points3d.push_back(rotateX(points3d[3], roll));
|
||||
points3d.push_back(rotateX(points3d[4], roll));
|
||||
}
|
||||
|
||||
void RuneFan::Simple::drawLandmarks(cv::Mat& image) const {
|
||||
std::vector<cv::Point2f> lm = landmarks();
|
||||
for (size_t i = 0; i < lm.size(); i++) {
|
||||
cv::circle(image, lm[i], 3, cv::Scalar(255, 255, 255), -1);
|
||||
if (i == 0) {
|
||||
cv::putText(
|
||||
image,
|
||||
"R",
|
||||
lm[i],
|
||||
cv::FONT_HERSHEY_SIMPLEX,
|
||||
1.5,
|
||||
cv::Scalar(40, 255, 40),
|
||||
2
|
||||
);
|
||||
} else {
|
||||
cv::putText(
|
||||
image,
|
||||
std::to_string(i),
|
||||
lm[i],
|
||||
cv::FONT_HERSHEY_SIMPLEX,
|
||||
0.5,
|
||||
cv::Scalar(255, 255, 255),
|
||||
2
|
||||
);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void RuneFan::addOffset(const cv::Point2f& offset) {
|
||||
for (auto& fan: fans) {
|
||||
for (auto& point: fan.points2d) {
|
||||
point += offset;
|
||||
}
|
||||
}
|
||||
}
|
||||
void RuneFan::transform(const Eigen::Matrix<float, 3, 3>& transform_matrix) {
|
||||
for (auto& fan: fans) {
|
||||
for (auto& pt: fan.points2d) {
|
||||
pt = utils::transformPoint2D(transform_matrix, pt);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void PowerRune::Pose::tf(Eigen::Matrix4d T_camera_to_odom) {
|
||||
Eigen::Vector4d pos_camera(pos.x(), pos.y(), pos.z(), 1.0);
|
||||
Eigen::Vector4d pos_odom = T_camera_to_odom * pos_camera;
|
||||
|
||||
pos.x() = pos_odom.x();
|
||||
pos.y() = pos_odom.y();
|
||||
pos.z() = pos_odom.z();
|
||||
Eigen::Matrix3d R_camera_to_odom = T_camera_to_odom.block<3, 3>(0, 0);
|
||||
Eigen::Quaterniond q_camera(ori.w(), ori.x(), ori.y(), ori.z());
|
||||
Eigen::Matrix3d R_ori_camera = q_camera.normalized().toRotationMatrix();
|
||||
|
||||
Eigen::Matrix3d R_ori_odom = R_camera_to_odom * R_ori_camera;
|
||||
Eigen::Quaterniond q_odom(R_ori_odom);
|
||||
|
||||
ori.w() = q_odom.w();
|
||||
ori.x() = q_odom.x();
|
||||
ori.y() = q_odom.y();
|
||||
ori.z() = q_odom.z();
|
||||
}
|
||||
|
||||
std::vector<cv::Point2f> PowerRune::Pose::toPts(
|
||||
const cv::Mat& camera_intrinsic,
|
||||
const cv::Mat& camera_distortion,
|
||||
const std::vector<cv::Point3f>& obj_points
|
||||
) const {
|
||||
std::vector<cv::Point2f> pts;
|
||||
if (pos.norm() < 0.5) {
|
||||
return pts;
|
||||
}
|
||||
|
||||
cv::Mat tvec = (cv::Mat_<double>(3, 1) << pos.x(), pos.y(), pos.z());
|
||||
Eigen::Matrix3d tf_rot = 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);
|
||||
|
||||
cv::projectPoints(obj_points, rvec, tvec, camera_intrinsic, camera_distortion, pts);
|
||||
|
||||
return pts;
|
||||
}
|
||||
void PowerRune::Pose::draw(
|
||||
cv::Mat& img,
|
||||
const cv::Mat& camera_intrinsic,
|
||||
const cv::Mat& camera_distortion,
|
||||
const std::vector<cv::Point3f>& obj_points,
|
||||
cv::Scalar color
|
||||
) const {
|
||||
auto pts = toPts(camera_intrinsic, camera_distortion, obj_points);
|
||||
if (!pts.empty()) {
|
||||
for (int i = 0; i < 4; i++)
|
||||
cv::line(img, pts[i], pts[(i + 1) % 4], color, 2);
|
||||
|
||||
// 后表面
|
||||
for (int i = 4; i < 8; i++)
|
||||
cv::line(img, pts[i], pts[4 + (i + 1) % 4], color, 2);
|
||||
|
||||
// 侧边
|
||||
for (int i = 0; i < 4; i++)
|
||||
cv::line(img, pts[i], pts[i + 4], color, 2);
|
||||
cv::Point2f center(0.f, 0.f);
|
||||
for (auto pt: pts) {
|
||||
center += pt;
|
||||
}
|
||||
center *= 1.0 / pts.size();
|
||||
}
|
||||
}
|
||||
|
||||
void PowerRune::tf(Eigen::Matrix4d T_camera_to_odom) {
|
||||
center.tf(T_camera_to_odom);
|
||||
for (auto& fan: fans)
|
||||
fan.tf(T_camera_to_odom);
|
||||
}
|
||||
void
|
||||
PowerRune::draw(cv::Mat& img, const cv::Mat& camera_intrinsic, const cv::Mat& camera_distortion)
|
||||
const {
|
||||
center.draw(img, camera_intrinsic, camera_distortion);
|
||||
for (int i = 0; i < fans.size(); i++) {
|
||||
if (i == hit_id)
|
||||
fans[i].draw(
|
||||
img,
|
||||
camera_intrinsic,
|
||||
camera_distortion,
|
||||
FAN_BLOCK,
|
||||
cv::Scalar(40, 255, 40)
|
||||
);
|
||||
else
|
||||
fans[i].draw(img, camera_intrinsic, camera_distortion, FAN_BLOCK);
|
||||
}
|
||||
}
|
||||
} // namespace auto_buff
|
||||
} // namespace wust_vision
|
||||
124
wust_vision-main/tasks/auto_buff/type.hpp
Normal file
124
wust_vision-main/tasks/auto_buff/type.hpp
Normal file
@@ -0,0 +1,124 @@
|
||||
#pragma once
|
||||
#include "tasks/type_common.hpp"
|
||||
|
||||
namespace wust_vision {
|
||||
namespace auto_buff {
|
||||
constexpr double RUNE_PAN_BOX_DIS = 0.16;
|
||||
constexpr double RUNE_R2PANCENTER = 0.75;
|
||||
struct RuneCenter {
|
||||
cv::Point2f center;
|
||||
cv::RotatedRect rr;
|
||||
bool is_valid = false;
|
||||
RuneCenter() = default;
|
||||
RuneCenter(cv::RotatedRect rect): rr(rect) {
|
||||
center = rect.center;
|
||||
is_valid = rr.size.area() > 0;
|
||||
}
|
||||
};
|
||||
|
||||
class RunePan {
|
||||
public:
|
||||
cv::Point2f center;
|
||||
std::vector<cv::Point2f> corners;
|
||||
bool is_valid = false;
|
||||
bool has_refer = false;
|
||||
|
||||
void draw(cv::Mat& img, const cv::Point2f& offset) const;
|
||||
double getArea() const;
|
||||
void addReferRuneCenter(const RuneCenter& rc);
|
||||
};
|
||||
|
||||
struct RuneFan {
|
||||
public:
|
||||
bool is_valid = false;
|
||||
int id;
|
||||
bool is_big = false;
|
||||
std::chrono::steady_clock::time_point timestamp;
|
||||
struct Simple {
|
||||
int has_other = 0;
|
||||
std::vector<double> angle_diffs = { 0,
|
||||
2 * M_PI / 5,
|
||||
2 * M_PI / 5 * 2,
|
||||
2 * M_PI / 5 * 3,
|
||||
2 * M_PI / 5 * 4 };
|
||||
std::vector<cv::Point2f> points2d;
|
||||
std::vector<cv::Point3f> points3d = {
|
||||
{ 0.0f, 0.0f, 0.0f }, // P0
|
||||
{ 0.0f, RUNE_PAN_BOX_DIS / 2.0f, RUNE_R2PANCENTER + RUNE_PAN_BOX_DIS / 2.0f }, // P1
|
||||
{ 0.0f, RUNE_PAN_BOX_DIS / 2.0f, RUNE_R2PANCENTER - RUNE_PAN_BOX_DIS / 2.0f }, // P2
|
||||
{ 0.0f,
|
||||
-RUNE_PAN_BOX_DIS / 2.0f,
|
||||
RUNE_R2PANCENTER - RUNE_PAN_BOX_DIS / 2.0f }, // P3
|
||||
{ 0.0f,
|
||||
-RUNE_PAN_BOX_DIS / 2.0f,
|
||||
RUNE_R2PANCENTER + RUNE_PAN_BOX_DIS / 2.0f }, // P4
|
||||
{ 0.0f, 0.0f, RUNE_R2PANCENTER } // P5
|
||||
};
|
||||
inline cv::Point3f rotateX(const cv::Point3f& p, double roll) {
|
||||
double c = std::cos(roll);
|
||||
double s = std::sin(roll);
|
||||
return { p.x, float(p.y * c - p.z * s), float(p.y * s + p.z * c) };
|
||||
}
|
||||
inline double normalizeAngle0to2pi(double a) {
|
||||
a = std::fmod(a, 2 * M_PI);
|
||||
if (a < 0)
|
||||
a += 2 * M_PI;
|
||||
return a;
|
||||
}
|
||||
|
||||
Eigen::Vector3d pos;
|
||||
Eigen::Quaterniond ori;
|
||||
Eigen::Vector3d target_pos;
|
||||
Eigen::Quaterniond target_ori;
|
||||
void addOther(const Simple& other);
|
||||
std::vector<cv::Point2f> landmarks() const {
|
||||
return points2d;
|
||||
}
|
||||
void drawLandmarks(cv::Mat& image) const;
|
||||
std::vector<cv::Point3f> getObjs() const {
|
||||
return points3d;
|
||||
}
|
||||
};
|
||||
std::vector<Simple> fans;
|
||||
void addOffset(const cv::Point2f& offset);
|
||||
void transform(const Eigen::Matrix<float, 3, 3>& transform_matrix);
|
||||
};
|
||||
static std::vector<cv::Point3f> FAN_BLOCK = {
|
||||
{ -0.05f, -0.20f, -0.15f }, // 0: 左下前
|
||||
{ 0.05f, -0.20f, -0.15f }, // 1: 右下前
|
||||
{ 0.05f, 0.20f, -0.15f }, // 2: 右上前
|
||||
{ -0.05f, 0.20f, -0.15f }, // 3: 左上前
|
||||
{ -0.05f, -0.20f, 0.15f }, // 4: 左下后
|
||||
{ 0.05f, -0.20f, 0.15f }, // 5: 右下后
|
||||
{ 0.05f, 0.20f, 0.15f }, // 6: 右上后
|
||||
{ -0.05f, 0.20f, 0.15f } // 7: 左上后
|
||||
};
|
||||
|
||||
struct PowerRune {
|
||||
bool is_valid = false;
|
||||
struct Pose {
|
||||
Eigen::Vector3d pos;
|
||||
Eigen::Quaterniond ori;
|
||||
void tf(Eigen::Matrix4d T_camera_to_odom);
|
||||
std::vector<cv::Point2f> toPts(
|
||||
const cv::Mat& camera_intrinsic,
|
||||
const cv::Mat& camera_distortion,
|
||||
const std::vector<cv::Point3f>& obj_points = AIM_TARGET_BLOCK
|
||||
) const;
|
||||
void draw(
|
||||
cv::Mat& img,
|
||||
const cv::Mat& camera_intrinsic,
|
||||
const cv::Mat& camera_distortion,
|
||||
const std::vector<cv::Point3f>& obj_points = AIM_TARGET_BLOCK,
|
||||
cv::Scalar color = cv::Scalar(255, 255, 255)
|
||||
) const;
|
||||
};
|
||||
Pose center;
|
||||
std::vector<Pose> fans;
|
||||
int hit_id;
|
||||
void tf(Eigen::Matrix4d T_camera_to_odom);
|
||||
void
|
||||
draw(cv::Mat& img, const cv::Mat& camera_intrinsic, const cv::Mat& camera_distortion) const;
|
||||
};
|
||||
} // namespace auto_buff
|
||||
} // namespace wust_vision
|
||||
Reference in New Issue
Block a user