Skip to content

Commit

Permalink
Merge pull request #77 from Alex-Beng/dev
Browse files Browse the repository at this point in the history
【事不过三】修复Kalman Filter in an ugly way
  • Loading branch information
GengGode authored Dec 3, 2023
2 parents 6db3272 + ed835a1 commit 2c3a49b
Show file tree
Hide file tree
Showing 13 changed files with 853 additions and 53 deletions.
646 changes: 646 additions & 0 deletions doc/KF/测试文档.md

Large diffs are not rendered by default.

8 changes: 8 additions & 0 deletions source/src/algorithms/algorithms.include.h
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
#pragma once
#include <opencv2/opencv.hpp>
#include "utils/Utils.h"

typedef std::pair<int, std::string> error_info;

Expand All @@ -20,3 +21,10 @@ struct position_calculation_config
bool error = false;
error_info err = {0, ""};
};

struct odometer_config
{
bool error = false;
error_info err = {0, ""};
double scale = 1.0;
};
100 changes: 100 additions & 0 deletions source/src/algorithms/algorithms.visual.odometer.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,100 @@
#pragma once
#include "algorithms.include.h"

// 视觉里程计
// 每次小地图更新时调用
// init:传入小地图与大地图的像素缩放比例
// when mini map update:尝试与上一张小地图进行匹配,匹配成功,则返回控制量dx dy

// TODO:把这个缩放比例使用仿射/自己组Ax=B求解,并测试性能。
// TODO:封装一下,此版本测试filter可行性

cv::Mat last_mini_map;
bool inited = false;

bool orb_match(cv::Mat &img1, cv::Mat &img2, cv::Point2f &offset);

void set_mini_map(const cv::Mat &giMiniMapRef)
{
last_mini_map = giMiniMapRef.clone();
inited = true;
}

bool control_odometer_calculation(const cv::Mat &giMiniMapRef, cv::Point2d &control, odometer_config &config)
{
if (!inited) {
// JUST INIT IT
last_mini_map = giMiniMapRef.clone();
inited = true;
control = cv::Point2d(0, 0);
return false;
}
else {
auto curr_mini_map = giMiniMapRef.clone();
// use orb match to get the u, aka offset
cv::Point2f offset;
if (orb_match(last_mini_map, curr_mini_map, offset)) {
control = cv::Point2d(offset.x * config.scale, offset.y * config.scale);
last_mini_map = curr_mini_map.clone();
return true;
}
else {
control = cv::Point2d(0, 0);
return false;
}
}
}

// 草,surfmatch跟定位功能耦合有、严重
// 这里再造一个orb match + 最优比次优
// 几乎必然是同比例的,返回偏移量
bool orb_match(cv::Mat &img1, cv::Mat &img2, cv::Point2f &offset)
{
// 不crop了,因为上一张会被crop两次。
// img1 = TianLi::Utils::crop_border(img1, 0.15);
// img2 = TianLi::Utils::crop_border(img2, 0.15);
// resize 是等比例缩放,就不超了

// 分别计算orb特征点
cv::Ptr<cv::ORB> orb = cv::ORB::create(5000);
std::vector<cv::KeyPoint> kp1, kp2;
cv::Mat desp1, desp2;
orb->detectAndCompute(img1, cv::Mat(), kp1, desp1);
orb->detectAndCompute(img2, cv::Mat(), kp2, desp2);

if (desp1.empty() || desp2.empty()) {
return false;
}

// 首先采用knnMatch剔除最近匹配点距离与次近匹配点距离比率大于0.6的舍去
cv::BFMatcher matcher(cv::NORM_HAMMING);
std::vector<std::vector<cv::DMatch>> matches;
matcher.knnMatch(desp1, desp2, matches, 2);
// 解决没有匹配点的情况
if (matches.size() == 0) {
return false;
}
std::vector<cv::DMatch> good_matches;
for (int i = 0; i < matches.size(); i++) {
if (matches[i][0].distance < 0.6 * matches[i][1].distance) {
good_matches.push_back(matches[i][0]);
}
}

if (good_matches.size() == 0) {
return false;
}

auto img2_copy = img2.clone();
// 画出good matches,然后保存
// cv::drawMatches(img1, kp1, img2, kp2, good_matches, img2_copy);
// cv::imwrite("good_matches.jpg", img2_copy);

// 计算偏移量,直接取平均
cv::Point2f sum_offset(0, 0);
for (int i = 0; i < good_matches.size(); i++) {
sum_offset += kp2[good_matches[i].trainIdx].pt - kp1[good_matches[i].queryIdx].pt;
}
offset = cv::Point2f(sum_offset.x / good_matches.size(), sum_offset.y / good_matches.size());
return true;
}
2 changes: 1 addition & 1 deletion source/src/filter/Filter.h
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@ class Filter
Filter();
virtual ~Filter();
public:
virtual cv::Point2d filterting(const cv::Point2d& pos) = 0;
virtual cv::Point2d filterting(const cv::Point2d &pos, const cv::Point2f &u_k) = 0;
virtual cv::Point2d re_init_filterting(const cv::Point2d& pos) = 0;
public:
enum FilterType
Expand Down
72 changes: 35 additions & 37 deletions source/src/filter/kalman/Kalman.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -5,46 +5,44 @@ Kalman::Kalman()
{
type = FilterType::Kalman;

KF = cv::KalmanFilter(stateNum, measureNum, 0);
state = cv::Mat(stateNum, 1, CV_32F); //state(x,y,detaX,detaY)
KF = cv::KalmanFilter(stateNum, measureNum, controlNum);
state = cv::Mat(stateNum, 1, CV_32F);
processNoise = cv::Mat(stateNum, 1, CV_32F);
measurement = cv::Mat::zeros(measureNum, 1, CV_32F); //measurement(x,y)

randn(state, cv::Scalar::all(0), cv::Scalar::all(0.1)); //随机生成一个矩阵,期望是0,标准差为0.1;
KF.transitionMatrix = (cv::Mat_<float>(4, 4) <<
1, 0, 1, 0,
0, 1, 0, 1,
0, 0, 1, 0,
0, 0, 0, 1);//元素导入矩阵,按行;

//setIdentity: 缩放的单位对角矩阵;
//!< measurement matrix (H) 观测模型
setIdentity(KF.measurementMatrix);

//!< process noise covariance matrix (Q)
// wk 是过程噪声,并假定其符合均值为零,协方差矩阵为Qk(Q)的多元正态分布;
// set A
KF.transitionMatrix = (cv::Mat_<float>(2, 2) <<
1, 0,
0, 1);
// set B
KF.controlMatrix = (cv::Mat_<float>(2, 2) <<
1, 0,
0, 1);
// set Q
setIdentity(KF.processNoiseCov, cv::Scalar::all(1e-5));
// set H
KF.measurementMatrix = (cv::Mat_<float>(2, 2) <<
1, 0,
0, 1);
// set R
setIdentity(KF.measurementNoiseCov, cv::Scalar::all(1e-5));

//!< measurement noise covariance matrix (R)
//vk 是观测噪声,其均值为零,协方差矩阵为Rk,且服从正态分布;
setIdentity(KF.measurementNoiseCov, cv::Scalar::all(1e-1));

//!< priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)*/ A代表F: transitionMatrix
//预测估计协方差矩阵;
setIdentity(KF.errorCovPost, cv::Scalar::all(1));

//!< corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))
//initialize post state of kalman filter at random
randn(KF.statePost, cv::Scalar::all(0), cv::Scalar::all(0.1));
}

Kalman::~Kalman()
{
}

cv::Point2d Kalman::filterting(const cv::Point2d& pos)
cv::Point2d Kalman::filterting(const cv::Point2d& pos, const cv::Point2f& u_k)
{
cv::Mat prediction = KF.predict();
// use u_k to predict
// make u_k to cv::Mat
cv::Mat u_k_mat = cv::Mat::zeros(2, 1, CV_32F);
u_k_mat.at<float>(0, 0) = u_k.x;
u_k_mat.at<float>(1, 0) = u_k.y;
cv::Mat prediction = KF.predict(u_k_mat);
cv::Point2d predictPt = cv::Point2d(prediction.at<float>(0), prediction.at<float>(1));

//3.update measurement
Expand All @@ -60,23 +58,22 @@ cv::Point2d Kalman::filterting(const cv::Point2d& pos)

cv::Point2d Kalman::re_init_filterting(const cv::Point2d& pos)
{
KF.init(stateNum, measureNum, 0);
KF.init(stateNum, measureNum, controlNum);

state = cv::Mat(stateNum, 1, CV_32F); //state(x,y,detaX,detaY)
processNoise = cv::Mat(stateNum, 1, CV_32F);
measurement = cv::Mat::zeros(measureNum, 1, CV_32F); //measurement(x,y)

randn(state, cv::Scalar::all(0), cv::Scalar::all(0.1)); //随机生成一个矩阵,期望是0,标准差为0.1;
KF.transitionMatrix = (cv::Mat_<float>(4, 4) <<
1, 0, 1, 0,
0, 1, 0, 1,
0, 0, 1, 0,
0, 0, 0, 1);//元素导入矩阵,按行;

//setIdentity: 缩放的单位对角矩阵;
//!< measurement matrix (H) 观测模型
setIdentity(KF.measurementMatrix);

// set A
KF.transitionMatrix = (cv::Mat_<float>(2, 2) <<
1, 0,
0, 1);
// set B
KF.controlMatrix = (cv::Mat_<float>(2, 2) <<
1, 0,
0, 1);
// set Q
//!< process noise covariance matrix (Q)
// wk 是过程噪声,并假定其符合均值为零,协方差矩阵为Qk(Q)的多元正态分布;
setIdentity(KF.processNoiseCov, cv::Scalar::all(1e-5));
Expand All @@ -87,6 +84,7 @@ cv::Point2d Kalman::re_init_filterting(const cv::Point2d& pos)

//!< priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)*/ A代表F: transitionMatrix
//预测估计协方差矩阵;
// 这啥啊,这不计算中的误差协方差
setIdentity(KF.errorCovPost, cv::Scalar::all(1));

//!< corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))
Expand Down
24 changes: 21 additions & 3 deletions source/src/filter/kalman/Kalman.h
Original file line number Diff line number Diff line change
Expand Up @@ -6,14 +6,32 @@ class Kalman : public Filter
Kalman();
~Kalman();
public:
virtual cv::Point2d filterting(const cv::Point2d& pos) override;
virtual cv::Point2d filterting(const cv::Point2d &pos, const cv::Point2f &u_k) override;
virtual cv::Point2d re_init_filterting(const cv::Point2d& pos) override;
private:
int stateNum = 4;
int stateNum = 2;
int measureNum = 2;
int controlNum = 2;

// motion model
// x(k) = A*x(k-1) + B*u(k) + w(k)
// w(k) ~ N(0, Q)

// update model
// z(k) = H*x(k) + v(k)
// v(k) ~ N(0, R)

// Where:
// x = [x y]'
// z = [x y]'
// u = [dx dy]'
// A = [1 0; 0 1]
// B = [1 0; 0 1]
// H = [1 0; 0 1]
// Q = [1 0; 0 1] * 1e-5
// R = [1 0; 0 1] * 1e-5
cv::KalmanFilter KF;
cv::Mat state; /* (phi, delta_phi) */
cv::Mat state;
cv::Mat processNoise;
cv::Mat measurement;
};
4 changes: 2 additions & 2 deletions source/src/filter/smooth/Smooth.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@ Smooth::~Smooth()
{
}

cv::Point2d Smooth::filterting(const cv::Point2d& pos)
cv::Point2d Smooth::filterting(const cv::Point2d &pos, const cv::Point2f &u_k)
{
mean_pos = pos * 0.1 + mean_pos * 0.9;
return mean_pos;
Expand All @@ -19,5 +19,5 @@ cv::Point2d Smooth::filterting(const cv::Point2d& pos)
cv::Point2d Smooth::re_init_filterting(const cv::Point2d& pos)
{
mean_pos = pos;
return filterting(pos);
return filterting(pos, cv::Point2f(0, 0));
}
2 changes: 1 addition & 1 deletion source/src/filter/smooth/Smooth.h
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@ class Smooth : public Filter
Smooth();
~Smooth();
public:
virtual cv::Point2d filterting(const cv::Point2d& pos) override;
virtual cv::Point2d filterting(const cv::Point2d &pos, const cv::Point2f &u_k) override;
virtual cv::Point2d re_init_filterting(const cv::Point2d& pos) override;
private:
cv::Point2d mean_pos;
Expand Down
2 changes: 1 addition & 1 deletion source/src/filter/untouched/Untouched.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@ Untouched::~Untouched()
{
}

cv::Point2d Untouched::filterting(const cv::Point2d& pos)
cv::Point2d Untouched::filterting(const cv::Point2d &pos, const cv::Point2f &u_k)
{
return pos;
}
Expand Down
2 changes: 1 addition & 1 deletion source/src/filter/untouched/Untouched.h
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,6 @@ class Untouched : public Filter
Untouched();
~Untouched();
public:
virtual cv::Point2d filterting(const cv::Point2d& pos) override;
virtual cv::Point2d filterting(const cv::Point2d &pos, const cv::Point2f &u_k) override;
virtual cv::Point2d re_init_filterting(const cv::Point2d& pos) override;
};
37 changes: 32 additions & 5 deletions source/src/genshin/match/position/genshin.match.position.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
#include "resources/Resources.h"
#include "Match/surf/SurfMatch.h"
#include "filter/kalman/Kalman.h"
#include "algorithms/algorithms.visual.odometer.h"

cv::Mat to_color(cv::Mat& img_object)
{
Expand Down Expand Up @@ -176,14 +177,40 @@ void TianLi::Genshin::Match::get_avatar_position(const GenshinMinimap& genshin_m
{
cv::Point2d pos = out_genshin_position.position;
cv::Point2d filt_pos;
if (out_genshin_position.config.is_coveying || out_genshin_position.config.is_continuity == false)
{
// 查看od初始化了没
auto u_k = cv::Point2d(0, 0);
auto od_valid = control_odometer_calculation(genshin_minimap.img_minimap, u_k, odometer_config());
if (!od_valid) {
// TODO:no u_k update
filt_pos = out_genshin_position.config.pos_filter->re_init_filterting(pos);
set_mini_map(genshin_minimap.img_minimap);
}
else
{
filt_pos = out_genshin_position.config.pos_filter->filterting(pos);
else {
cout << "u_k: " << u_k << endl;
filt_pos = out_genshin_position.config.pos_filter->filterting(pos, u_k);
}


// if (out_genshin_position.config.is_coveying || out_genshin_position.config.is_continuity == false)
// {
// filt_pos = out_genshin_position.config.pos_filter->re_init_filterting(pos);
// // TODO:封装
// set_mini_map(genshin_minimap.img_minimap);
// }
// else
// {
// auto u_k = cv::Point2d(0, 0);
// auto od_valid = control_odometer_calculation(genshin_minimap.img_minimap, u_k, odometer_config());
// if (od_valid)
// {
// filt_pos = out_genshin_position.config.pos_filter->filterting(pos, u_k);
// }
// else
// {
// // TODO:no u_k update
// filt_pos = out_genshin_position.config.pos_filter->re_init_filterting(pos);
// }
// }
out_genshin_position.position = filt_pos;
}

Expand Down
2 changes: 1 addition & 1 deletion source/src/version/version.ver
Original file line number Diff line number Diff line change
@@ -1 +1 @@
8.0.4
8.0.4
Loading

0 comments on commit 2c3a49b

Please sign in to comment.