add wust typr mpc and mutipule x
This commit is contained in:
cyy_mac
@@ -0,0 +1,143 @@
|
||||
#include "guidance_target.hpp"
|
||||
namespace wust_vision {
|
||||
namespace auto_guidance {
|
||||
GuidanceTarget::GuidanceTarget() {
|
||||
target_state_ = Eigen::VectorXd::Zero(imgbox_model::X_N);
|
||||
}
|
||||
GuidanceTarget::GuidanceTarget(const GreenLight& light, TargetConfig target_config) {
|
||||
target_config_ = target_config;
|
||||
auto yfv2 = imgbox_model::Predict(0.01);
|
||||
auto yhv2 = imgbox_model::Measure();
|
||||
auto yu_qv2 = [this]() { return computeProcessNoise(0.01); };
|
||||
|
||||
auto yu_rv2 = [this](const Eigen::Matrix<double, imgbox_model::Z_N, 1>& z) {
|
||||
return this->computeMeasurementCovariance(z);
|
||||
};
|
||||
Eigen::DiagonalMatrix<double, imgbox_model::X_N> p0;
|
||||
p0.diagonal() << 1000, 1000, 1000, 1000, 64000, 64000, 64000, 64000;
|
||||
esekf_ = imgbox_model::BBox8ESEKF(yfv2, yhv2, yu_qv2, yu_rv2, p0);
|
||||
|
||||
esekf_.setResidualFunc([this](
|
||||
const Eigen::Matrix<double, imgbox_model::Z_N, 1>& z_pred,
|
||||
const Eigen::Matrix<double, imgbox_model::Z_N, 1>& z
|
||||
) {
|
||||
Eigen::Matrix<double, imgbox_model::Z_N, 1> r = z - z_pred;
|
||||
return r;
|
||||
});
|
||||
esekf_.setIterationNum(target_config_.iter_num);
|
||||
esekf_.setInjectFunc([this](
|
||||
const Eigen::Matrix<double, imgbox_model::X_N, 1>& delta,
|
||||
Eigen::Matrix<double, imgbox_model::X_N, 1>& nominal
|
||||
) {
|
||||
for (int i = 0; i < imgbox_model::X_N; i++) {
|
||||
nominal[i] += delta[i];
|
||||
}
|
||||
});
|
||||
|
||||
double cx = light.center_point.x;
|
||||
double cy = light.center_point.y;
|
||||
double w = light.box.width;
|
||||
double h = light.box.height;
|
||||
target_state_ << cx, 0, cy, 0, w, 0, h, 0;
|
||||
esekf_.setState(target_state_);
|
||||
last_t_ = light.timestamp;
|
||||
position_ = light.position;
|
||||
timestamp_ = light.timestamp;
|
||||
image_size_ = light.image_size;
|
||||
is_inited_ = true;
|
||||
}
|
||||
Eigen::Matrix<double, imgbox_model::Z_N, imgbox_model::Z_N>
|
||||
GuidanceTarget::computeMeasurementCovariance(
|
||||
const Eigen::Matrix<double, imgbox_model::Z_N, 1>& z
|
||||
) const {
|
||||
Eigen::Matrix<double, imgbox_model::Z_N, imgbox_model::Z_N> r;
|
||||
// clang-format off
|
||||
r <<target_config_.xy_r, 0, 0, 0,
|
||||
0, target_config_.xy_r , 0, 0,
|
||||
0, 0, target_config_.wh_r, 0,
|
||||
0, 0, 0,target_config_.wh_r;
|
||||
// clang-format on
|
||||
return r;
|
||||
}
|
||||
Eigen::Matrix<double, imgbox_model::X_N, imgbox_model::X_N>
|
||||
GuidanceTarget::computeProcessNoise(double dt) const {
|
||||
Eigen::Matrix<double, imgbox_model::X_N, imgbox_model::X_N> q;
|
||||
|
||||
double t = dt;
|
||||
double q_pp = pow(t, 4) / 4.0 * target_config_.q_xy;
|
||||
double q_pv = pow(t, 3) / 2.0 * target_config_.q_xy;
|
||||
double q_vv = pow(t, 2) * target_config_.q_xy;
|
||||
|
||||
double q_ss = pow(t, 4) / 4.0 * target_config_.q_wh;
|
||||
double q_sv = pow(t, 3) / 2.0 * target_config_.q_wh;
|
||||
double q_vvs = pow(t, 2) * target_config_.q_wh;
|
||||
|
||||
// clang-format off
|
||||
// cx vx cy vy w vw h vh
|
||||
q << q_pp, q_pv, 0, 0, 0, 0, 0, 0,
|
||||
q_pv, q_vv, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, q_pp, q_pv, 0, 0, 0, 0,
|
||||
0, 0, q_pv, q_vv, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, q_ss, q_sv, 0, 0,
|
||||
0, 0, 0, 0, q_sv, q_vvs, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, q_ss, q_sv,
|
||||
0, 0, 0, 0, 0, 0, q_sv, q_vvs;
|
||||
|
||||
// clang-format on
|
||||
return q;
|
||||
}
|
||||
void GuidanceTarget::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 GuidanceTarget::predict(double dt) {
|
||||
dt_ = dt;
|
||||
|
||||
esekf_.setPredictFunc(imgbox_model::Predict { dt });
|
||||
auto yu_qv2 = [dt, this]() { return computeProcessNoise(dt); };
|
||||
|
||||
esekf_.setUpdateQ(yu_qv2);
|
||||
|
||||
target_state_ = esekf_.predict();
|
||||
}
|
||||
|
||||
bool GuidanceTarget::update(const GreenLights& lights) {
|
||||
auto ls = lights.lights;
|
||||
timestamp_ = lights.timestamp;
|
||||
|
||||
auto yu_rv2 = [this](const Eigen::Matrix<double, imgbox_model::Z_N, 1>& z) {
|
||||
return this->computeMeasurementCovariance(z);
|
||||
};
|
||||
esekf_.setUpdateR(yu_rv2);
|
||||
int best_id = -1;
|
||||
double min_error = std::numeric_limits<double>::max();
|
||||
for (int i = 0; i < ls.size(); i++) {
|
||||
double centor_error = cv::norm(ls[i].center_point - center());
|
||||
double pos_error = (ls[i].position - position_).norm();
|
||||
if (centor_error < min_error && pos_error < target_config_.max_dis_diff) {
|
||||
min_error = centor_error;
|
||||
best_id = i;
|
||||
}
|
||||
}
|
||||
if (best_id == -1) {
|
||||
return false;
|
||||
}
|
||||
measurement_ = Eigen::Vector4d(
|
||||
ls[best_id].center_point.x,
|
||||
ls[best_id].center_point.y,
|
||||
ls[best_id].box.width,
|
||||
ls[best_id].box.height
|
||||
);
|
||||
esekf_.setMeasureFunc(imgbox_model::Measure());
|
||||
target_state_ = esekf_.update(measurement_);
|
||||
position_ = ls[best_id].position;
|
||||
image_size_ = ls[best_id].image_size;
|
||||
last_t_ = timestamp_;
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace auto_guidance
|
||||
} // namespace wust_vision
|
||||
@@ -0,0 +1,89 @@
|
||||
#pragma once
|
||||
#include "motion_models/imgbox_model.hpp"
|
||||
#include "tasks/auto_guidance/type.hpp"
|
||||
#include <wust_vl/common/utils/timer.hpp>
|
||||
#include <yaml-cpp/yaml.h>
|
||||
namespace wust_vision {
|
||||
namespace auto_guidance {
|
||||
struct TargetConfig {
|
||||
void load(const YAML::Node& config) {
|
||||
xy_r = config["xy_r"].as<double>();
|
||||
wh_r = config["wh_r"].as<double>();
|
||||
q_xy = config["q_xy"].as<double>();
|
||||
q_wh = config["q_wh"].as<double>();
|
||||
iter_num = config["iter_num"].as<int>();
|
||||
max_dis_diff = config["max_dis_diff"].as<double>();
|
||||
}
|
||||
double xy_r = 0.05;
|
||||
double wh_r = 0.05;
|
||||
double q_xy = 10;
|
||||
double q_wh = 10;
|
||||
int iter_num = 2;
|
||||
double max_dis_diff = 2.0;
|
||||
};
|
||||
class GuidanceTarget {
|
||||
public:
|
||||
GuidanceTarget();
|
||||
GuidanceTarget(const GreenLight& light, TargetConfig target_config);
|
||||
GuidanceTarget& operator=(const GuidanceTarget&) = default;
|
||||
bool update(const GreenLights& lights);
|
||||
|
||||
void predict(std::chrono::steady_clock::time_point t);
|
||||
void predict(double dt);
|
||||
Eigen::Matrix<double, imgbox_model::Z_N, imgbox_model::Z_N>
|
||||
computeMeasurementCovariance(const Eigen::Matrix<double, imgbox_model::Z_N, 1>& z) const;
|
||||
Eigen::Matrix<double, imgbox_model::X_N, imgbox_model::X_N> computeProcessNoise(double dt
|
||||
) const;
|
||||
std::chrono::steady_clock::time_point last_t_;
|
||||
std::chrono::steady_clock::time_point timestamp_;
|
||||
double dt_;
|
||||
cv::Size2d image_size_;
|
||||
imgbox_model::BBox8ESEKF esekf_;
|
||||
Eigen::Matrix<double, imgbox_model::Z_N, 1> measurement_ =
|
||||
Eigen::Matrix<double, imgbox_model::Z_N, 1>::Zero();
|
||||
Eigen::Matrix<double, imgbox_model::X_N, 1> target_state_ =
|
||||
Eigen::Matrix<double, imgbox_model::X_N, 1>::Zero();
|
||||
Eigen::Vector3d position_;
|
||||
TargetConfig target_config_;
|
||||
bool is_inited_ = false;
|
||||
bool is_tracking_ = false;
|
||||
bool checkappear() {
|
||||
return is_tracking_
|
||||
&& wust_vl::common::utils::time_utils::durationSec(
|
||||
timestamp_,
|
||||
wust_vl::common::utils::time_utils::now()
|
||||
)
|
||||
< 3.0;
|
||||
}
|
||||
cv::Point2d center() const {
|
||||
return cv::Point2d(target_state_(0), target_state_(2));
|
||||
}
|
||||
cv::Rect2d box() const {
|
||||
return cv::Rect2d(
|
||||
target_state_(0) - target_state_(4) / 2,
|
||||
target_state_(2) - target_state_(6) / 2,
|
||||
target_state_(4),
|
||||
target_state_(6)
|
||||
);
|
||||
}
|
||||
void draw(cv::Mat& img) const {
|
||||
cv::rectangle(img, box(), cv::Scalar(255, 50, 0), 2);
|
||||
cv::circle(img, center(), 3, cv::Scalar(255, 255, 255), -1);
|
||||
cv::line(
|
||||
img,
|
||||
cv::Point(center().x, center().y),
|
||||
cv::Point(img.cols / 2.0, center().y),
|
||||
cv::Scalar(0, 0, 255),
|
||||
2
|
||||
);
|
||||
cv::line(
|
||||
img,
|
||||
cv::Point2f(img.cols / 2.0, 0),
|
||||
cv::Point2f(img.cols / 2.0, img.rows),
|
||||
cv::Scalar(255, 255, 255),
|
||||
2
|
||||
);
|
||||
}
|
||||
};
|
||||
} // namespace auto_guidance
|
||||
} // namespace wust_vision
|
||||
@@ -0,0 +1,109 @@
|
||||
#include "guidance_tracker.hpp"
|
||||
namespace wust_vision {
|
||||
namespace auto_guidance {
|
||||
struct GuidanceTracker::Impl {
|
||||
public:
|
||||
Impl(const YAML::Node& config) {
|
||||
target_config_.load(config["target"]);
|
||||
tracking_thres_ = config["tracking_thres"].as<int>(5);
|
||||
lost_dt_ = config["lost_time_thres"].as<double>();
|
||||
}
|
||||
|
||||
GuidanceTarget track(const GreenLights& lights) {
|
||||
double dt = std::chrono::duration<double>(lights.timestamp - last_time_).count();
|
||||
last_time_ = lights.timestamp;
|
||||
lost_thres_ = std::abs(static_cast<int>(lost_dt_ / dt));
|
||||
bool found;
|
||||
if (tracker_state == LOST) {
|
||||
found = initTarget(lights);
|
||||
} else {
|
||||
found = updateTarget(lights);
|
||||
}
|
||||
updateFsm(found);
|
||||
|
||||
return guidance_target_;
|
||||
}
|
||||
void updateFsm(bool found) {
|
||||
if (tracker_state == DETECTING) {
|
||||
if (found) {
|
||||
detect_count_++;
|
||||
if (detect_count_ > tracking_thres_) {
|
||||
detect_count_ = 0;
|
||||
tracker_state = TRACKING;
|
||||
}
|
||||
} else {
|
||||
detect_count_ = 0;
|
||||
tracker_state = LOST;
|
||||
}
|
||||
} else if (tracker_state == TRACKING) {
|
||||
if (!found) {
|
||||
tracker_state = TEMP_LOST;
|
||||
lost_count_++;
|
||||
}
|
||||
} else if (tracker_state == TEMP_LOST) {
|
||||
if (!found) {
|
||||
lost_count_++;
|
||||
if (lost_count_ > lost_thres_) {
|
||||
lost_count_ = 0;
|
||||
tracker_state = LOST;
|
||||
}
|
||||
} else {
|
||||
tracker_state = TRACKING;
|
||||
lost_count_ = 0;
|
||||
}
|
||||
}
|
||||
if (tracker_state == LOST || tracker_state == DETECTING) {
|
||||
guidance_target_.is_tracking_ = false;
|
||||
} else {
|
||||
guidance_target_.is_tracking_ = true;
|
||||
}
|
||||
}
|
||||
bool initTarget(const GreenLights& lights) {
|
||||
int best_id = -1;
|
||||
double max_score = -1e9;
|
||||
for (int i = 0; i < lights.lights.size(); i++) {
|
||||
if (lights.lights[i].score > max_score) {
|
||||
max_score = lights.lights[i].score;
|
||||
best_id = i;
|
||||
}
|
||||
}
|
||||
if (best_id == -1) {
|
||||
return false;
|
||||
}
|
||||
tracker_state = DETECTING;
|
||||
guidance_target_ = GuidanceTarget(lights.lights[best_id], target_config_);
|
||||
return true;
|
||||
}
|
||||
bool updateTarget(const GreenLights& lights) {
|
||||
guidance_target_.predict(lights.timestamp);
|
||||
return guidance_target_.update(lights);
|
||||
}
|
||||
enum State {
|
||||
LOST,
|
||||
DETECTING,
|
||||
TRACKING,
|
||||
TEMP_LOST,
|
||||
} tracker_state = LOST;
|
||||
GuidanceTarget guidance_target_;
|
||||
int tracking_thres_;
|
||||
int lost_thres_;
|
||||
int detect_count_ = 0;
|
||||
int lost_count_ = 0;
|
||||
double lost_dt_;
|
||||
std::chrono::steady_clock::time_point last_time_;
|
||||
TargetConfig target_config_;
|
||||
};
|
||||
GuidanceTracker::GuidanceTracker(const YAML::Node& config) {
|
||||
_impl = std::make_unique<Impl>(config);
|
||||
}
|
||||
GuidanceTracker::~GuidanceTracker() {
|
||||
_impl.reset();
|
||||
}
|
||||
GuidanceTarget GuidanceTracker::track(const GreenLights& lights) {
|
||||
return _impl->track(lights);
|
||||
}
|
||||
std::chrono::steady_clock::time_point GuidanceTracker::getLastTime() const {
|
||||
return _impl->last_time_;
|
||||
}
|
||||
} // namespace auto_guidance
|
||||
} // namespace wust_vision
|
||||
@@ -0,0 +1,21 @@
|
||||
#pragma once
|
||||
#include "tasks/auto_guidance/guidance_tracker/guidance_target.hpp"
|
||||
namespace wust_vision {
|
||||
namespace auto_guidance {
|
||||
class GuidanceTracker {
|
||||
public:
|
||||
using Ptr = std::unique_ptr<GuidanceTracker>;
|
||||
GuidanceTracker(const YAML::Node& config);
|
||||
~GuidanceTracker();
|
||||
static inline Ptr create(const YAML::Node& config) {
|
||||
return std::make_unique<GuidanceTracker>(config);
|
||||
}
|
||||
GuidanceTarget track(const GreenLights& lights);
|
||||
std::chrono::steady_clock::time_point getLastTime() const;
|
||||
|
||||
private:
|
||||
struct Impl;
|
||||
std::unique_ptr<Impl> _impl;
|
||||
};
|
||||
} // namespace auto_guidance
|
||||
} // namespace wust_vision
|
||||
@@ -0,0 +1,45 @@
|
||||
#pragma once
|
||||
|
||||
#include "KalmanHyLib/kalman_hybird_lib.hpp"
|
||||
#include <ceres/ceres.h>
|
||||
|
||||
namespace imgbox_model {
|
||||
|
||||
static constexpr int X_N = 8; // cx vx cy vy w vw h vh
|
||||
static constexpr int Z_N = 4; // measured cx cy w h
|
||||
|
||||
// ========================== Predict Model ==========================
|
||||
struct Predict {
|
||||
Predict() = default;
|
||||
explicit Predict(double dt): dt_(dt) {}
|
||||
|
||||
template<typename T>
|
||||
void operator()(const T x0[X_N], T x1[X_N]) {
|
||||
for (int i = 0; i < X_N; i++) {
|
||||
x1[i] = x0[i];
|
||||
}
|
||||
x1[0] += x0[1] * dt_; // cx
|
||||
x1[2] += x0[3] * dt_; // cy
|
||||
x1[4] += x0[5] * dt_; // w
|
||||
x1[6] += x0[7] * dt_; // h
|
||||
}
|
||||
|
||||
double dt_;
|
||||
};
|
||||
|
||||
// ========================== Measurement Model ==========================
|
||||
struct Measure {
|
||||
Measure() = default;
|
||||
template<typename T>
|
||||
void operator()(const T x[X_N], T z[Z_N]) const {
|
||||
z[0] = x[0]; // cx
|
||||
z[1] = x[2]; // cy
|
||||
z[2] = x[4]; // w
|
||||
z[3] = x[6]; // h
|
||||
}
|
||||
};
|
||||
|
||||
using BBox8EKF = kalman_hybird_lib::ExtendedKalmanFilter<X_N, Z_N, Predict, Measure>;
|
||||
using BBox8ESEKF = kalman_hybird_lib::ErrorStateEKF<X_N, Z_N, Predict, Measure>;
|
||||
|
||||
} // namespace imgbox_model
|
||||
Reference in New Issue
Block a user