能量机关状态估计测试
This commit is contained in:
commit
c32f47ff8b
|
|
@ -0,0 +1,2 @@
|
||||||
|
/cmake-build-debug/
|
||||||
|
/.idea
|
||||||
|
|
@ -0,0 +1,29 @@
|
||||||
|
cmake_minimum_required(VERSION 3.0)
|
||||||
|
|
||||||
|
project(rm2023_power_rune)
|
||||||
|
|
||||||
|
set(CMAKE_CXX_STANDARD 11)
|
||||||
|
|
||||||
|
find_package(Ceres REQUIRED)
|
||||||
|
include_directories(${CERES_INCLUDE_DIRS})
|
||||||
|
find_package( OpenCV REQUIRED )
|
||||||
|
include_directories( ${OpenCV_INCLUDE_DIRS} )
|
||||||
|
find_package(Eigen3 REQUIRED)
|
||||||
|
include_directories(${EIGEN3_INCLUDE_DIRS})
|
||||||
|
|
||||||
|
#matplotlib cpp
|
||||||
|
include_directories( ${PYTHON_INCLUDE_DIRS} )
|
||||||
|
#find_package(PythonLibs REQUIRED)
|
||||||
|
|
||||||
|
find_package(Python COMPONENTS Development)
|
||||||
|
#include_directories(/usr/local/lib/python2.7/dist-packages/numpy/core/include)
|
||||||
|
|
||||||
|
|
||||||
|
include_directories( include )
|
||||||
|
add_executable(rm2023_power_rune src/main.cpp src/bufferDetector.cpp src/parameter.cpp src/PnP.cpp src/preprocessing.cpp src/tuneParameter.cpp src/rune_fit.cpp include/ExtendedKalmanFilter.h src/ExtendedKalmanFilter.cpp src/rune_tracker.cpp)
|
||||||
|
target_link_libraries(rm2023_power_rune ${OpenCV_LIBS})
|
||||||
|
target_link_libraries(rm2023_power_rune ${EIGEN3_LIBRARIES})
|
||||||
|
target_link_libraries(rm2023_power_rune ${CERES_LIBRARIES})
|
||||||
|
target_link_libraries(rm2023_power_rune ${PYTHON_LIBRARIES})
|
||||||
|
#target_include_directories(power_rune PRIVATE ${PYTHON_INCLUDE_DIRS})
|
||||||
|
target_link_libraries(rm2023_power_rune Python::Python)
|
||||||
|
|
@ -0,0 +1,11 @@
|
||||||
|
Final a: 0.838939 w: 1.78194 b: 1.25106 phi: 2.39606
|
||||||
|
|
||||||
|
Final a: 0.843604 w: 1.76684 b: 1.2464 phi: 2.3677
|
||||||
|
|
||||||
|
Final a: 0.755944 w: 1.98226 b: 1.33406 phi: -3.48658
|
||||||
|
|
||||||
|
Final a: 0.626155 w: 2.80235 b: 1.46385 phi: -5.04199
|
||||||
|
|
||||||
|
Final a: 0.577276 w: 1.05471 b: 1.51272 phi: 3.47982
|
||||||
|
|
||||||
|
Final a: 0.516106 w: 1.52706 b: 1.57389 phi: 0
|
||||||
|
|
@ -0,0 +1,75 @@
|
||||||
|
//
|
||||||
|
// Copyright 2022 Chen Jun
|
||||||
|
//
|
||||||
|
|
||||||
|
#include <Eigen/Dense>
|
||||||
|
#include <functional>
|
||||||
|
|
||||||
|
#ifndef POWER_RUNE_EXTENDEDKALMANFILTER_H
|
||||||
|
#define POWER_RUNE_EXTENDEDKALMANFILTER_H
|
||||||
|
|
||||||
|
namespace rm_power_rune {
|
||||||
|
|
||||||
|
class ExtendedKalmanFilter
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
ExtendedKalmanFilter() = default;
|
||||||
|
|
||||||
|
using VecVecFunc = std::function<Eigen::VectorXd(const Eigen::VectorXd &)>;
|
||||||
|
using VecMatFunc = std::function<Eigen::MatrixXd(const Eigen::VectorXd &)>;
|
||||||
|
using VoidMatFunc = std::function<Eigen::MatrixXd()>;
|
||||||
|
|
||||||
|
explicit ExtendedKalmanFilter(
|
||||||
|
const VecVecFunc & f, const VecVecFunc & h, const VecMatFunc & j_f, const VecMatFunc & j_h,
|
||||||
|
const VoidMatFunc & u_q, const VecMatFunc & u_r, const Eigen::MatrixXd & P0);
|
||||||
|
|
||||||
|
// Set the initial state
|
||||||
|
void setState(const Eigen::VectorXd & x0);
|
||||||
|
|
||||||
|
// Compute a predicted state
|
||||||
|
Eigen::MatrixXd predict();
|
||||||
|
|
||||||
|
// Update the estimated state based on measurement
|
||||||
|
Eigen::MatrixXd update(const Eigen::VectorXd & z);
|
||||||
|
|
||||||
|
private:
|
||||||
|
// Process nonlinear vector function
|
||||||
|
VecVecFunc f;
|
||||||
|
// Observation nonlinear vector function
|
||||||
|
VecVecFunc h;
|
||||||
|
// Jacobian of f()
|
||||||
|
VecMatFunc jacobian_f;
|
||||||
|
Eigen::MatrixXd F;
|
||||||
|
// Jacobian of h()
|
||||||
|
VecMatFunc jacobian_h;
|
||||||
|
Eigen::MatrixXd H;
|
||||||
|
// Process noise covariance matrix
|
||||||
|
VoidMatFunc update_Q;
|
||||||
|
Eigen::MatrixXd Q;
|
||||||
|
// Measurement noise covariance matrix
|
||||||
|
VecMatFunc update_R;
|
||||||
|
Eigen::MatrixXd R;
|
||||||
|
|
||||||
|
// Priori error estimate covariance matrix
|
||||||
|
Eigen::MatrixXd P_pri;
|
||||||
|
// Posteriori error estimate covariance matrix
|
||||||
|
Eigen::MatrixXd P_post;
|
||||||
|
|
||||||
|
// Kalman gain
|
||||||
|
Eigen::MatrixXd K;
|
||||||
|
|
||||||
|
// System dimensions
|
||||||
|
int n;
|
||||||
|
|
||||||
|
// N-size identity
|
||||||
|
Eigen::MatrixXd I;
|
||||||
|
|
||||||
|
// Priori state
|
||||||
|
Eigen::VectorXd x_pri;
|
||||||
|
// Posteriori state
|
||||||
|
Eigen::VectorXd x_post;
|
||||||
|
};
|
||||||
|
|
||||||
|
} // rm_power_rune
|
||||||
|
|
||||||
|
#endif //POWER_RUNE_EXTENDEDKALMANFILTER_H
|
||||||
|
|
@ -0,0 +1,10 @@
|
||||||
|
#pragma once
|
||||||
|
#include <opencv2/opencv.hpp>
|
||||||
|
#include <opencv2/highgui/highgui_c.h>
|
||||||
|
#include <iostream>
|
||||||
|
#include <Eigen/Core>
|
||||||
|
using std::cout;
|
||||||
|
using std::endl;
|
||||||
|
|
||||||
|
|
||||||
|
void PNP(const cv::Mat& img, std::vector<cv::Point2f> points, cv::Mat cam, cv::Mat dis);
|
||||||
|
|
@ -0,0 +1,39 @@
|
||||||
|
#pragma once
|
||||||
|
#include <opencv2/opencv.hpp>
|
||||||
|
#include <opencv2/highgui/highgui_c.h>
|
||||||
|
#include <iostream>
|
||||||
|
|
||||||
|
using std::cout;
|
||||||
|
using std::endl;
|
||||||
|
using std::vector;
|
||||||
|
|
||||||
|
//击打板点类
|
||||||
|
class armorPoint {
|
||||||
|
public:
|
||||||
|
cv::Point2f p;
|
||||||
|
double distance;
|
||||||
|
};
|
||||||
|
|
||||||
|
//上一帧的信息类
|
||||||
|
class PreviousFrameInfo {
|
||||||
|
public:
|
||||||
|
vector<cv::Point2f> previousFramePoints;
|
||||||
|
std::chrono::duration<double> elapsedTime;
|
||||||
|
bool firstFrame = true;
|
||||||
|
double dt;
|
||||||
|
};
|
||||||
|
|
||||||
|
class TargetInfo{
|
||||||
|
public:
|
||||||
|
vector<cv::Point2f> target_points_;
|
||||||
|
double target_speed_;
|
||||||
|
double radius_;
|
||||||
|
cv::Point2f centor_;
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
void drawRotatedRect(cv::Mat image, const cv::RotatedRect& rect, const cv::Scalar& color, int thickness);
|
||||||
|
|
||||||
|
double distance(cv::Point a, cv::Point b);
|
||||||
|
|
||||||
|
TargetInfo findTarget(cv::Mat rawImages, PreviousFrameInfo & previousFrameInfo, cv::Mat mask);
|
||||||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,11 @@
|
||||||
|
#pragma once
|
||||||
|
#include <opencv2/opencv.hpp>
|
||||||
|
#include <opencv2/highgui/highgui_c.h>
|
||||||
|
#include <iostream>
|
||||||
|
|
||||||
|
using std::cout;
|
||||||
|
using std::endl;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 用于将调参后的所有参数数据记录
|
||||||
|
/// </summary>
|
||||||
|
|
@ -0,0 +1,10 @@
|
||||||
|
#pragma once
|
||||||
|
#include <opencv2/opencv.hpp>
|
||||||
|
#include <opencv2/highgui/highgui_c.h>
|
||||||
|
#include <iostream>
|
||||||
|
|
||||||
|
using std::cout;
|
||||||
|
using std::endl;
|
||||||
|
|
||||||
|
|
||||||
|
cv::Mat preprocessing(cv::Mat rawImages);
|
||||||
|
|
@ -0,0 +1,57 @@
|
||||||
|
#ifndef RUNE_FIT_HPP_
|
||||||
|
#define RUNE_FIT_HPP_
|
||||||
|
|
||||||
|
// OpenCV
|
||||||
|
#include <opencv2/core/types.hpp>
|
||||||
|
#include <opencv2/opencv.hpp>
|
||||||
|
|
||||||
|
#include <ceres/ceres.h>
|
||||||
|
|
||||||
|
// STD
|
||||||
|
#include <utility>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
namespace rm_power_rune
|
||||||
|
{
|
||||||
|
struct SpeedfitResidual
|
||||||
|
{
|
||||||
|
SpeedfitResidual(double x, double y)
|
||||||
|
: t_(x), speed_(y) {}
|
||||||
|
|
||||||
|
template <typename T>
|
||||||
|
bool operator()(const T *const param, T *residual) const
|
||||||
|
{
|
||||||
|
//param[0]->a 0.78-1.045
|
||||||
|
//param[1]->w 1.884-2
|
||||||
|
//b= 2.090-a
|
||||||
|
residual[0] = T(speed_) - (param[0] * ceres::sin(param[1] * T(t_)+param[2]) + (2.090 - param[0]));
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
private:
|
||||||
|
// Observations for a sample.
|
||||||
|
const double t_;
|
||||||
|
const double speed_;
|
||||||
|
};
|
||||||
|
|
||||||
|
class rune_fitter
|
||||||
|
{
|
||||||
|
private:
|
||||||
|
std::vector<double> t_data_,speed_data_;
|
||||||
|
double curve_param_[3]={0.78,1.884,0};
|
||||||
|
int fitdataCount_; //拟合数据个数
|
||||||
|
bool fit_flag;
|
||||||
|
public:
|
||||||
|
rune_fitter(int data_count);
|
||||||
|
~rune_fitter();
|
||||||
|
bool add_data(double t,double speed);
|
||||||
|
bool fit_speed();
|
||||||
|
bool is_data_enough();
|
||||||
|
bool is_fitted();
|
||||||
|
void draw_result();
|
||||||
|
void draw_result_matplot();
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
} // namespace rm_power_rune
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,27 @@
|
||||||
|
//
|
||||||
|
// Created by hshine on 23-10-10.
|
||||||
|
//
|
||||||
|
|
||||||
|
#ifndef RM2023_POWER_RUNE_RUNE_TRACKER_H
|
||||||
|
#define RM2023_POWER_RUNE_RUNE_TRACKER_H
|
||||||
|
|
||||||
|
#include "ExtendedKalmanFilter.h"
|
||||||
|
|
||||||
|
namespace rm_power_rune {
|
||||||
|
|
||||||
|
class rune_tracker {
|
||||||
|
private:
|
||||||
|
ExtendedKalmanFilter rune_ekf_;
|
||||||
|
|
||||||
|
Eigen::VectorXd observe_;
|
||||||
|
double dt_;
|
||||||
|
public:
|
||||||
|
Eigen::VectorXd state_;
|
||||||
|
rune_tracker(double a0,double w0,double phi0,double r0);
|
||||||
|
void track(double r,double v,double dt);
|
||||||
|
double get_velocity();
|
||||||
|
};
|
||||||
|
|
||||||
|
} // rm_rune
|
||||||
|
|
||||||
|
#endif //RM2023_POWER_RUNE_RUNE_TRACKER_H
|
||||||
|
|
@ -0,0 +1,23 @@
|
||||||
|
#pragma once
|
||||||
|
#include <opencv2/opencv.hpp>
|
||||||
|
#include <opencv2/highgui/highgui_c.h>
|
||||||
|
#include <iostream>
|
||||||
|
|
||||||
|
using std::cout;
|
||||||
|
using std::endl;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 用于对传入的视频资源进行trackbar调参
|
||||||
|
/// </summary>
|
||||||
|
|
||||||
|
|
||||||
|
void CallBack_threshold(int pos, void* userdata); //声明二值化的回调函数
|
||||||
|
void CallBack_dilate(int pos, void* userdata); //声明膨胀的回调函数
|
||||||
|
//void CallBack_erode(int pos, void* userdata); //声明腐蚀的回调函数
|
||||||
|
//void CallBack_insideCircle(int element_size, void* strElement);//声明内圈遮罩的回调函数
|
||||||
|
|
||||||
|
|
||||||
|
void tuneParameter(cv::Mat src); //声明图像预处理的函数
|
||||||
|
|
||||||
|
|
@ -0,0 +1,31 @@
|
||||||
|
<?xml version="1.0"?>
|
||||||
|
<opencv_storage>
|
||||||
|
<calibration_time>"2023年十月六号下午四点"</calibration_time>
|
||||||
|
<nr_of_frames>26</nr_of_frames>
|
||||||
|
<image_width>1920</image_width>
|
||||||
|
<image_height>1080</image_height>
|
||||||
|
<board_width>9</board_width>
|
||||||
|
<board_height>6</board_height>
|
||||||
|
<square_size>7.7440002441406250e+01</square_size>
|
||||||
|
<!-- flags: +fix_k3 +fix_k4 +fix_k5 -->
|
||||||
|
<flags>6272</flags>
|
||||||
|
<fisheye_model>0</fisheye_model>
|
||||||
|
<camera_matrix type_id="opencv-matrix">
|
||||||
|
<rows>3</rows>
|
||||||
|
<cols>3</cols>
|
||||||
|
<dt>d</dt>
|
||||||
|
<data>
|
||||||
|
1286.05409 0. 652.70282
|
||||||
|
0. 1289.09266 526.6223
|
||||||
|
0. 0. 1.
|
||||||
|
</data></camera_matrix>
|
||||||
|
<distortion_coefficients type_id="opencv-matrix">
|
||||||
|
<rows>1</rows>
|
||||||
|
<cols>5</cols>
|
||||||
|
<dt>d</dt>
|
||||||
|
<data>
|
||||||
|
-0.198084 0.065966 -0.001208 -0.000127 0.000000
|
||||||
|
|
||||||
|
</data></distortion_coefficients>
|
||||||
|
<avg_reprojection_error>2.7648134620933651e-01</avg_reprojection_error>
|
||||||
|
</opencv_storage>
|
||||||
|
|
@ -0,0 +1,51 @@
|
||||||
|
//
|
||||||
|
// Copyright 2022 Chen Jun
|
||||||
|
//
|
||||||
|
|
||||||
|
#include "ExtendedKalmanFilter.h"
|
||||||
|
|
||||||
|
namespace rm_power_rune {
|
||||||
|
ExtendedKalmanFilter::ExtendedKalmanFilter(
|
||||||
|
const VecVecFunc & f, const VecVecFunc & h, const VecMatFunc & j_f, const VecMatFunc & j_h,
|
||||||
|
const VoidMatFunc & u_q, const VecMatFunc & u_r, const Eigen::MatrixXd & P0)
|
||||||
|
: f(f),
|
||||||
|
h(h),
|
||||||
|
jacobian_f(j_f),
|
||||||
|
jacobian_h(j_h),
|
||||||
|
update_Q(u_q),
|
||||||
|
update_R(u_r),
|
||||||
|
P_post(P0),
|
||||||
|
n(P0.rows()),
|
||||||
|
I(Eigen::MatrixXd::Identity(n, n)),
|
||||||
|
x_pri(n),
|
||||||
|
x_post(n)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
void ExtendedKalmanFilter::setState(const Eigen::VectorXd & x0) { x_post = x0; }
|
||||||
|
|
||||||
|
Eigen::MatrixXd ExtendedKalmanFilter::predict()
|
||||||
|
{
|
||||||
|
F = jacobian_f(x_post), Q = update_Q();
|
||||||
|
|
||||||
|
x_pri = f(x_post);
|
||||||
|
P_pri = F * P_post * F.transpose() + Q;
|
||||||
|
|
||||||
|
// handle the case when there will be no measurement before the next predict
|
||||||
|
x_post = x_pri;
|
||||||
|
P_post = P_pri;
|
||||||
|
|
||||||
|
return x_pri;
|
||||||
|
}
|
||||||
|
|
||||||
|
Eigen::MatrixXd ExtendedKalmanFilter::update(const Eigen::VectorXd & z)
|
||||||
|
{
|
||||||
|
H = jacobian_h(x_pri), R = update_R(z);
|
||||||
|
|
||||||
|
K = P_pri * H.transpose() * (H * P_pri * H.transpose() + R).inverse();
|
||||||
|
x_post = x_pri + K * (z - h(x_pri));
|
||||||
|
P_post = (I - K * H) * P_pri;
|
||||||
|
|
||||||
|
return x_post;
|
||||||
|
}
|
||||||
|
} // rm_power_rune
|
||||||
|
|
@ -0,0 +1,58 @@
|
||||||
|
#include "PnP.h"
|
||||||
|
|
||||||
|
|
||||||
|
class PnPInfo {
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
//Point2f reproject(Vector3d& xyz)
|
||||||
|
//{
|
||||||
|
// Matrix3d mat_intrinsic;
|
||||||
|
// cv2eigen(intrinsic, mat_intrinsic);
|
||||||
|
// //(u,v,1)^T = (1/Z) * K * (X,Y,Z)^T
|
||||||
|
// auto result = (1.f / xyz[2]) * mat_intrinsic * (xyz);//解算前进行单位转换
|
||||||
|
// return Point2f(result[0], result[1]);
|
||||||
|
//}
|
||||||
|
|
||||||
|
//pnp位姿解算
|
||||||
|
void PNP(const cv::Mat & img, std::vector<cv::Point2f> points , cv::Mat cam, cv::Mat dis){
|
||||||
|
|
||||||
|
//给PNP参考的世界坐标点
|
||||||
|
std::vector<cv::Point3f> points_world;
|
||||||
|
|
||||||
|
//顺时针的世界坐标
|
||||||
|
points_world = {
|
||||||
|
{-161.5, 52.0, 0.0},
|
||||||
|
{161.5, 52.0, 0.0},
|
||||||
|
{167.5, -45.0, 0.0},
|
||||||
|
{-167.5, -45.0, 0.0},
|
||||||
|
{0.0, 0.0, 0.0} };
|
||||||
|
|
||||||
|
cv::Mat rVec = cv::Mat::zeros(3, 1, CV_64FC1);
|
||||||
|
cv::Mat tVec = cv::Mat::zeros(3, 1, CV_64FC1);
|
||||||
|
|
||||||
|
if(points.size() > 0)
|
||||||
|
solvePnP(points_world, points, cam, dis, rVec, tVec, false, cv::SOLVEPNP_IPPE);
|
||||||
|
|
||||||
|
|
||||||
|
// 提取旋转向量的分量
|
||||||
|
double xr = rVec.at<double>(0);
|
||||||
|
double yr = rVec.at<double>(1);
|
||||||
|
double zr = rVec.at<double>(2);
|
||||||
|
|
||||||
|
// 提取平移向量的分量
|
||||||
|
double xt = tVec.at<double>(0);
|
||||||
|
double yt = tVec.at<double>(1);
|
||||||
|
double zt = tVec.at<double>(2);
|
||||||
|
|
||||||
|
// 构造旋转向量和平移向量的字符串表示
|
||||||
|
std::string rvec_str = "rotation_vector: (" + std::to_string(xr) + ", " + std::to_string(yr) + ", " + std::to_string(zr) + ")";
|
||||||
|
std::string tvec_str = "translation_vector: (" + std::to_string(xt) + ", " + std::to_string(yt) + ", " + std::to_string(zt) + ")";
|
||||||
|
|
||||||
|
putText(img, rvec_str, cv::Point2f(5, 25), cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255,255,255), 1);
|
||||||
|
putText(img, tvec_str, cv::Point2f(5, 50), cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255, 255, 255), 1);
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
@ -0,0 +1,370 @@
|
||||||
|
#include "bufferDetector.h"
|
||||||
|
|
||||||
|
|
||||||
|
#define ENEMY BLUE
|
||||||
|
#define RED 0
|
||||||
|
#define BLUE 1
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
//画出旋转矩形
|
||||||
|
void drawRotatedRect(cv::Mat image, const cv::RotatedRect& rect, const cv::Scalar& color, int thickness) {
|
||||||
|
cv::Point2f vertex[4];
|
||||||
|
rect.points(vertex);
|
||||||
|
for (int i = 0; i < 4; i++)
|
||||||
|
{
|
||||||
|
line(image, vertex[i], vertex[(i + 1) % 4], color, thickness);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 寻找轮廓、找到击打点位
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="mask"></param>
|
||||||
|
|
||||||
|
TargetInfo findTarget(cv::Mat rawImages, PreviousFrameInfo& previousFrameInfo, cv::Mat mask){
|
||||||
|
|
||||||
|
TargetInfo target_info;//返回目标信息 目标点和角速度
|
||||||
|
cv::Point2f target;
|
||||||
|
|
||||||
|
vector<vector<cv::Point>> contours;
|
||||||
|
//轮廓的层次结构 检测待击打板
|
||||||
|
vector<cv::Vec4i> hierarchy;
|
||||||
|
findContours(mask, contours, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_SIMPLE);
|
||||||
|
//drawContours(rawImages, contours, -1, Scalar(0,255,0), 1);
|
||||||
|
|
||||||
|
cv::Point2f R_center;
|
||||||
|
vector<cv::RotatedRect> contours_minRect_in;
|
||||||
|
vector<cv::RotatedRect> contours_minRect_out;
|
||||||
|
vector<cv::RotatedRect> contours_minRect_Target;
|
||||||
|
|
||||||
|
for (unsigned int contour_index = 0; contour_index < contours.size(); contour_index++) {
|
||||||
|
|
||||||
|
//进行多边形逼近
|
||||||
|
vector<cv::Point> approxCurve;
|
||||||
|
double epsilon = 0.01 * arcLength(contours[contour_index], true);
|
||||||
|
bool closed = true; // 指定逼近多边形是否闭合
|
||||||
|
approxPolyDP(contours[contour_index], approxCurve, epsilon, closed);
|
||||||
|
|
||||||
|
//drawContours(rawImages, vector<vector<Point>>{approxCurve}, -1, Scalar(0, 0, 255), 2);
|
||||||
|
|
||||||
|
//找到每个轮廓的最小外接矩形
|
||||||
|
cv::RotatedRect minRect = minAreaRect(contours[contour_index]);
|
||||||
|
cv::Rect rect = boundingRect(contours[contour_index]);
|
||||||
|
|
||||||
|
int area = minRect.size.area();
|
||||||
|
//输出轮廓的大小
|
||||||
|
//putText(rawImages, to_string(area), minRect.center , FONT_HERSHEY_SIMPLEX, 1, Scalar(255, 255, 255), 6);
|
||||||
|
|
||||||
|
//输出最小矩形的大小
|
||||||
|
//putText(rawImages, to_string(area), minRect.center, FONT_HERSHEY_SIMPLEX, 0.7, Scalar(255, 255, 255), 2);
|
||||||
|
|
||||||
|
////用于确定中心R标的范围区间
|
||||||
|
//if (area > 50 && area < 400) {
|
||||||
|
// if (minRect.size.width > minRect.size.height) {
|
||||||
|
// if (minRect.size.height / minRect.size.width > 0.9 && minRect.size.height / minRect.size.width < 1.3) {
|
||||||
|
// cout << to_string(double(minRect.size.width / minRect.size.height)) << endl;
|
||||||
|
// putText(rawImages, to_string(double(minRect.size.width / minRect.size.height)), minRect.center + Point2f(20, 20), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(255, 255, 255), 2);
|
||||||
|
// circle(rawImages, minRect.center, 2, Scalar(0, 255, 255), 3);
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
// else if(minRect.size.height / minRect.size.width > 0.9 && minRect.size.height / minRect.size.width < 1.3) {
|
||||||
|
// cout << to_string(double(minRect.size.height / minRect.size.width)) << endl;
|
||||||
|
// putText(rawImages, to_string(double(minRect.size.height / minRect.size.width)), minRect.center + Point2f(20, 20), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(255, 255, 255), 2);
|
||||||
|
// circle(rawImages, minRect.center, 2, Scalar(0, 255, 255), 3);
|
||||||
|
// }
|
||||||
|
|
||||||
|
//}
|
||||||
|
|
||||||
|
|
||||||
|
//找到配对的两块
|
||||||
|
//只取外层轮廓
|
||||||
|
if (hierarchy[contour_index][3] == -1) {
|
||||||
|
|
||||||
|
//putText(rawImages, to_string(hierarchy[contour_index][3]) , minRect.center, FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 0, 255), 2);
|
||||||
|
|
||||||
|
// 最小矩形4800-5700 轮廓面积 1000-1100 1.2-1.35
|
||||||
|
if (area > 4000 && area < 6500) {
|
||||||
|
|
||||||
|
if (minRect.size.width > minRect.size.height) {
|
||||||
|
if (minRect.size.width / minRect.size.height > 0.9 && minRect.size.width / minRect.size.height < 1.6) {
|
||||||
|
contours_minRect_in.push_back(minRect);
|
||||||
|
drawRotatedRect(rawImages, minRect, cv::Scalar(0, 255, 0), 2);
|
||||||
|
//circle(rawImages, minRect.center, 1, Scalar(0, 0, 255), 3);
|
||||||
|
|
||||||
|
//筛选出没有子轮廓的内层矩形
|
||||||
|
if (hierarchy[contour_index][2] == -1) {
|
||||||
|
contours_minRect_Target.push_back(minRect);
|
||||||
|
//putText(rawImages, "IN", minRect.center, FONT_HERSHEY_SIMPLEX, 0.5, Scalar(255, 255, 255), 2);
|
||||||
|
circle(rawImages, minRect.center, 1, cv::Scalar(0, 255, 255), 5);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if (minRect.size.height / minRect.size.width > 0.9 && minRect.size.height / minRect.size.width < 1.6) {
|
||||||
|
contours_minRect_in.push_back(minRect);
|
||||||
|
drawRotatedRect(rawImages, minRect, cv::Scalar(0, 255, 0), 2);
|
||||||
|
//circle(rawImages, minRect.center, 1, Scalar(0, 0, 255), 3);
|
||||||
|
|
||||||
|
//筛选出没有子轮廓的内层矩形
|
||||||
|
if (hierarchy[contour_index][2] == -1) {
|
||||||
|
contours_minRect_Target.push_back(minRect);
|
||||||
|
//putText(rawImages, "IN", minRect.center, FONT_HERSHEY_SIMPLEX, 0.5, Scalar(255, 255, 255), 2);
|
||||||
|
//drawRotatedRect(rawImages, minRect, Scalar(0, 0, 255), 2);
|
||||||
|
circle(rawImages, minRect.center, 1, cv::Scalar(0, 255, 255), 5);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
// 最小矩形 1200-1400 轮廓面积 450-500 2.3-2.7
|
||||||
|
if (area > 1100 && area < 1500) {
|
||||||
|
|
||||||
|
if (minRect.size.width > minRect.size.height) {
|
||||||
|
if (minRect.size.width / minRect.size.height > 2.1 && minRect.size.width / minRect.size.height < 2.9) {
|
||||||
|
contours_minRect_out.push_back(minRect);
|
||||||
|
//putText(rawImages, "OUT", minRect.center, FONT_HERSHEY_SIMPLEX, 0.5, Scalar(255, 255, 255), 2);
|
||||||
|
drawRotatedRect(rawImages, minRect, cv::Scalar(0, 255, 0), 2);
|
||||||
|
//circle(rawImages, minRect.center, 1, Scalar(0, 0, 255), 3);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if (minRect.size.height / minRect.size.width > 2.1 && minRect.size.height / minRect.size.width < 2.9) {
|
||||||
|
contours_minRect_out.push_back(minRect);
|
||||||
|
//putText(rawImages, "OUT", minRect.center, FONT_HERSHEY_SIMPLEX, 0.5, Scalar(255, 255, 255), 2);
|
||||||
|
drawRotatedRect(rawImages, minRect, cv::Scalar(0, 255, 0), 2);
|
||||||
|
//circle(rawImages, minRect.center, 1, Scalar(0, 0, 255), 3);
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
//找到中心点R标
|
||||||
|
//最小矩形 150-200 轮廓面积 0.9-1.1
|
||||||
|
if (area > 100 && area < 200) {
|
||||||
|
|
||||||
|
//cout << "area:" << area << "ratio:" << min(minRect.size.height / minRect.size.width, minRect.size.width / minRect.size.height);
|
||||||
|
if (minRect.size.width > minRect.size.height) {
|
||||||
|
if (minRect.size.width / minRect.size.height > 0.7 && minRect.size.width / minRect.size.height < 1.5) {
|
||||||
|
putText(rawImages, "R", minRect.center + cv::Point2f(20, 20), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255), 2);
|
||||||
|
circle(rawImages, minRect.center, 18, cv::Scalar(0, 255, 0), 2);
|
||||||
|
R_center = minRect.center;
|
||||||
|
//drawRotatedRect(rawImages, minRect, Scalar(0, 255, 0), 2);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if (minRect.size.height / minRect.size.width > 0.7 && minRect.size.height / minRect.size.width < 1.5) {
|
||||||
|
putText(rawImages, "R", minRect.center + cv::Point2f(20, 20), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255), 2);
|
||||||
|
circle(rawImages, minRect.center, 18, cv::Scalar(0, 255, 0), 2);
|
||||||
|
R_center = minRect.center;
|
||||||
|
//drawRotatedRect(rawImages, minRect, Scalar(0, 255, 0), 2);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
//存入目标点
|
||||||
|
vector<armorPoint> armorPoints;
|
||||||
|
|
||||||
|
//配对击打板 判断两个中心点的距离 存在已击打的内矩形的子轮廓识别为外矩形 distance 68-69
|
||||||
|
for (const auto minRect : contours_minRect_out) {
|
||||||
|
//存入内外矩形的点
|
||||||
|
cv::Point2f rectPointsIn[4];
|
||||||
|
cv::Point2f rectPointsOut[4];
|
||||||
|
|
||||||
|
|
||||||
|
if (contours_minRect_Target.size() == 1) {
|
||||||
|
cv::Point2f pTarget = contours_minRect_Target[0].center;
|
||||||
|
double dist = cv::norm (pTarget - minRect.center);
|
||||||
|
//cout << "dist:" << dist << " ";
|
||||||
|
if (dist < 80 && dist > 55) {
|
||||||
|
minRect.points(rectPointsIn);
|
||||||
|
//外
|
||||||
|
for (const auto point : rectPointsIn) {
|
||||||
|
armorPoint armorPoint;
|
||||||
|
armorPoint.p = point;
|
||||||
|
armorPoint.distance = cv::norm(point - R_center);
|
||||||
|
armorPoints.push_back(armorPoint);
|
||||||
|
}
|
||||||
|
//距离排序 取离R标最近的两个点
|
||||||
|
sort(armorPoints.begin(), armorPoints.end(), [](armorPoint a, armorPoint b) {return a.distance < b.distance; });
|
||||||
|
armorPoints.pop_back();
|
||||||
|
armorPoints.pop_back();
|
||||||
|
|
||||||
|
contours_minRect_Target[0].points(rectPointsOut);
|
||||||
|
//内
|
||||||
|
for (const auto point : rectPointsOut) {
|
||||||
|
armorPoint armorPoint;
|
||||||
|
armorPoint.p = point;
|
||||||
|
armorPoint.distance = cv::norm(point - R_center);
|
||||||
|
armorPoints.push_back(armorPoint);
|
||||||
|
}
|
||||||
|
//距离排序 取离R标最远的两个点
|
||||||
|
sort(armorPoints.begin() + 2, armorPoints.begin() + 6, [](armorPoint a, armorPoint b) {return a.distance > b.distance; });
|
||||||
|
|
||||||
|
for (int i = 0; i < 4; i++) {
|
||||||
|
circle(rawImages, armorPoints[i].p, 1, cv::Scalar(0, 255, 255), 5);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
//cout << dist << endl;
|
||||||
|
//putText(rawImages, to_string(dist), minRect.center + Point2f(20, 20), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(255, 255, 255), 3);
|
||||||
|
//line(rawImages, p, minRect.center, Scalar(0, 0, 255), 2);
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
putText(rawImages, "Error: multiple targets ...", cv::Point2f(20, 20), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255), 1);
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
//找到击打点位
|
||||||
|
float sum_x = 0, sum_y = 0;
|
||||||
|
for (unsigned int i = 0; i < 4; i++) {
|
||||||
|
if (i < armorPoints.size()) {
|
||||||
|
sum_x += armorPoints[i].p.x;
|
||||||
|
sum_y += armorPoints[i].p.y;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
target.x = sum_x / 4;
|
||||||
|
target.y = sum_y / 4;
|
||||||
|
|
||||||
|
//识别的点与目标击打点
|
||||||
|
vector<cv::Point2f> points;
|
||||||
|
if (armorPoints.size() > 4) {
|
||||||
|
armorPoints.erase(armorPoints.begin() + 4, armorPoints.end());
|
||||||
|
}
|
||||||
|
|
||||||
|
for (const auto armorpoint : armorPoints) {
|
||||||
|
points.push_back(armorpoint.p);
|
||||||
|
}
|
||||||
|
//如果找到了目标
|
||||||
|
if(!points.empty())
|
||||||
|
points.push_back(target);
|
||||||
|
|
||||||
|
//画出击打的目标点
|
||||||
|
circle(rawImages, target, 1, cv::Scalar(0, 0, 255), 5);
|
||||||
|
//画出预测的圆周运动轨迹
|
||||||
|
double R = cv::norm(target - R_center);
|
||||||
|
circle(rawImages, R_center, R, cv::Scalar(0, 0, 255), 2);
|
||||||
|
|
||||||
|
//获得顺时针的角点排序
|
||||||
|
double x = target.x - R_center.x;
|
||||||
|
double y = R_center.y - target.y;
|
||||||
|
|
||||||
|
|
||||||
|
//解决扇叶未激活的时候位置跳变的问题 1-2
|
||||||
|
if (previousFrameInfo.previousFramePoints.size() == 5 && points.size() >= 4) {
|
||||||
|
if (norm(previousFrameInfo.previousFramePoints[4] - points[4]) > 4 ) {
|
||||||
|
previousFrameInfo.previousFramePoints.clear();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
//加上根据上一帧判断的信息
|
||||||
|
if (points.size() >= 4 && previousFrameInfo.previousFramePoints.empty()) {
|
||||||
|
//分不同的象限进行识别
|
||||||
|
if (x <= 0 && y > 0) {
|
||||||
|
if (points[0].x > points[1].x) {
|
||||||
|
swap(points[0], points[1]);
|
||||||
|
}
|
||||||
|
if (points[2].x < points[3].x) {
|
||||||
|
swap(points[2], points[3]);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if (x > 0 && y >= 0) {
|
||||||
|
if (points[0].y > points[1].y) {
|
||||||
|
swap(points[0], points[1]);
|
||||||
|
}
|
||||||
|
if (points[2].y < points[3].y) {
|
||||||
|
swap(points[2], points[3]);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if (x >= 0 && y < 0) {
|
||||||
|
if (points[0].x < points[1].x) {
|
||||||
|
swap(points[0], points[1]);
|
||||||
|
}
|
||||||
|
if (points[2].x > points[3].x) {
|
||||||
|
swap(points[2], points[3]);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if (x < 0 && y <= 0) {
|
||||||
|
if (points[0].y < points[1].y) {
|
||||||
|
swap(points[0], points[1]);
|
||||||
|
}
|
||||||
|
if (points[2].y > points[3].y) {
|
||||||
|
swap(points[2], points[3]);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
//根据上一帧与这一帧的点的距离判断 保证点的顺序不变
|
||||||
|
else if (points.size() >= 4 && previousFrameInfo.previousFramePoints.size() == 5) {
|
||||||
|
if (cv::norm(previousFrameInfo.previousFramePoints[0] - points[0]) > cv::norm(previousFrameInfo.previousFramePoints[0] - points[1])) {
|
||||||
|
swap(points[0], points[1]);
|
||||||
|
}
|
||||||
|
if (cv::norm(previousFrameInfo.previousFramePoints[2] - points[2]) > cv::norm(previousFrameInfo.previousFramePoints[2] - points[3])) {
|
||||||
|
swap(points[2], points[3]);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
//标记四个点的位置
|
||||||
|
if (points.size() >= 4) {
|
||||||
|
|
||||||
|
for (unsigned int i = 0; i < 4; i++) {
|
||||||
|
putText(rawImages, std::to_string(i), points[i], cv::FONT_HERSHEY_SIMPLEX, 0.7, cv::Scalar(0, 0, 255), 2);
|
||||||
|
}
|
||||||
|
//输出一下角度的变化转化为弧度制 求出角速度
|
||||||
|
if (previousFrameInfo.previousFramePoints.size() >= 4) {
|
||||||
|
cv::Point2f targetR_center = target - R_center;
|
||||||
|
cv::Point2f previousPointR_center = previousFrameInfo.previousFramePoints[4] - R_center;
|
||||||
|
|
||||||
|
double angle = abs(cv::fastAtan2(targetR_center.y, targetR_center.x) - cv::fastAtan2(previousPointR_center.y, previousPointR_center.x));
|
||||||
|
|
||||||
|
double angleRadians = (angle * CV_PI) / 180.0;
|
||||||
|
//double angularVelocity = angleRadians / previousFrameInfo.elapsedTime.count();
|
||||||
|
double angularVelocity = angleRadians / previousFrameInfo.dt;
|
||||||
|
|
||||||
|
target_info.radius_ = cv::norm(targetR_center);
|
||||||
|
target_info.target_speed_ = angularVelocity;
|
||||||
|
target_info.centor_ = R_center;
|
||||||
|
//cout<< angleRadians << " rads / " << previousFrameInfo.elapsedTime.count() << " s = angularVelocity:" << angularVelocity << "rads/s" << endl;
|
||||||
|
cout << "angularVelocity:" << angularVelocity << "rads / s" << endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
//清空上一帧的点位信息
|
||||||
|
previousFrameInfo.previousFramePoints.clear();
|
||||||
|
for (const auto point : points) {
|
||||||
|
previousFrameInfo.previousFramePoints.push_back(point);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
//检测上跳变的原因 原因：图像预处、视频问题 发现不符合逻辑
|
||||||
|
/*cout << "armorPoint[0]:" << armorPoints[0].p << " armorPoints[1] " << armorPoints[1].p<<" ";
|
||||||
|
cout << "target:" << target << " R_center : " << R_center;
|
||||||
|
cout << "\tx : " << x << " y : " << y << endl;*/
|
||||||
|
|
||||||
|
|
||||||
|
//cout << armorPoints.size() << endl;
|
||||||
|
//cout << "contours_minRect_in.size:" << contours_minRect_in.size() << " contours_minRect_out.size: " << contours_minRect_out.size() << endl;
|
||||||
|
//cout << "contours_minRect_Target.size"<<contours_minRect_Target.size() << endl;
|
||||||
|
|
||||||
|
target_info.target_points_ = points;
|
||||||
|
return target_info;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
@ -0,0 +1,235 @@
|
||||||
|
#include<opencv2/opencv.hpp>
|
||||||
|
#include <opencv2/highgui/highgui_c.h>
|
||||||
|
#include<iostream>
|
||||||
|
#include "tuneParameter.h"
|
||||||
|
#include "preprocessing.h"
|
||||||
|
#include "bufferDetector.h"
|
||||||
|
#include "PnP.h"
|
||||||
|
|
||||||
|
#include "rune_fit.hpp"
|
||||||
|
#include "rune_tracker.h"
|
||||||
|
#include "matplotlibcpp.h"
|
||||||
|
using std::cout;
|
||||||
|
using std::endl;
|
||||||
|
|
||||||
|
|
||||||
|
////暂停 获取ROI处理图像
|
||||||
|
//cv::Mat rawImages;
|
||||||
|
//cv::Rect roiRect;
|
||||||
|
//cv::Mat ROI;
|
||||||
|
//cv::Point startPoint;
|
||||||
|
//cv::Point endPoint;
|
||||||
|
//bool downFlag = false;
|
||||||
|
//bool upFlag = false;
|
||||||
|
//
|
||||||
|
//void MouseEvent(int event, int x, int y, int flags, void* data)
|
||||||
|
//{
|
||||||
|
//
|
||||||
|
// if (event == cv::EVENT_LBUTTONDOWN)
|
||||||
|
// {
|
||||||
|
// downFlag = true;
|
||||||
|
// startPoint.x = x;
|
||||||
|
// startPoint.y = y;
|
||||||
|
// }
|
||||||
|
//
|
||||||
|
// if (event == cv::EVENT_LBUTTONUP)
|
||||||
|
// {
|
||||||
|
// upFlag = true;
|
||||||
|
// endPoint.x = x;
|
||||||
|
// endPoint.y = y;
|
||||||
|
// }
|
||||||
|
//
|
||||||
|
// if (downFlag == true && upFlag == false)
|
||||||
|
// {
|
||||||
|
// cv::Point tempPoint;
|
||||||
|
// tempPoint.x = x;
|
||||||
|
// tempPoint.y = y;
|
||||||
|
//
|
||||||
|
// cv::Mat tempImage = rawImages.clone();
|
||||||
|
// rectangle(tempImage, startPoint, tempPoint, cv::Scalar(255, 0, 0), 2, 3, 0);
|
||||||
|
// imshow("ROIing", tempImage);
|
||||||
|
// }
|
||||||
|
//
|
||||||
|
// if (downFlag == true && upFlag == true)
|
||||||
|
// {
|
||||||
|
//
|
||||||
|
// roiRect.width = abs(startPoint.x - endPoint.x);
|
||||||
|
// roiRect.height = abs(startPoint.y - endPoint.y);
|
||||||
|
// roiRect.x = min(startPoint.x, endPoint.x);
|
||||||
|
// roiRect.y = min(startPoint.y, endPoint.y);
|
||||||
|
// cout << roiRect.width << " " << roiRect.height << " " << roiRect.x << " " << roiRect.y << endl;
|
||||||
|
// cv::Mat ROI(rawImages, roiRect);
|
||||||
|
// imshow("ROI", ROI);
|
||||||
|
// downFlag = false;
|
||||||
|
// upFlag = false;
|
||||||
|
// }
|
||||||
|
// cv::waitKey(0);
|
||||||
|
//
|
||||||
|
//}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 主函数
|
||||||
|
/// </summary>
|
||||||
|
/// <returns></returns>
|
||||||
|
int main() {
|
||||||
|
std::basic_string<char> path = "../vedio/blue.mp4";
|
||||||
|
cv::VideoCapture cap(path);
|
||||||
|
bool paused = 0;
|
||||||
|
|
||||||
|
//相机参数
|
||||||
|
cv::Mat cameraMatrix;
|
||||||
|
cv::Mat distCoeffs;
|
||||||
|
|
||||||
|
cv::FileStorage fs("../out_camera_data.xml", cv::FileStorage::READ);
|
||||||
|
fs["camera_matrix"] >> cameraMatrix;
|
||||||
|
fs["distortion_coefficients"] >> distCoeffs;
|
||||||
|
|
||||||
|
rm_power_rune::rune_fitter fitter(900);
|
||||||
|
std::vector<double> state_v,measure_v;
|
||||||
|
if (cap.isOpened()) {
|
||||||
|
cv::Rect roiRect;
|
||||||
|
cv::Mat rawImages, ROI, mask;
|
||||||
|
PreviousFrameInfo previousFrameInfo;
|
||||||
|
previousFrameInfo.dt=0.01;
|
||||||
|
std::chrono::high_resolution_clock::time_point previousFrameEndTime;
|
||||||
|
std::chrono::high_resolution_clock::time_point start_time;
|
||||||
|
//Final a: 0.838939 w: 1.78194 b: 1.25106 phi: 2.39606
|
||||||
|
rm_power_rune::rune_tracker runeTracker(0.66,2.16,1.89,110);
|
||||||
|
|
||||||
|
while (true) {
|
||||||
|
|
||||||
|
//判断上一帧的相关信息
|
||||||
|
if (previousFrameInfo.firstFrame) {
|
||||||
|
previousFrameEndTime = std::chrono::high_resolution_clock::now();
|
||||||
|
start_time = previousFrameEndTime;
|
||||||
|
previousFrameInfo.firstFrame = false;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
std::chrono::duration<double,std::ratio<1,1>> duration_s(previousFrameEndTime-start_time);
|
||||||
|
|
||||||
|
TargetInfo targetInfo;
|
||||||
|
vector<cv::Point2f> targetPoints;
|
||||||
|
|
||||||
|
//进行视频资源的读取
|
||||||
|
if (!paused) {
|
||||||
|
cap >> rawImages;
|
||||||
|
//暂停视频
|
||||||
|
if (rawImages.empty())
|
||||||
|
break;
|
||||||
|
//截取部分区域进行处理
|
||||||
|
roiRect.width = 605;
|
||||||
|
roiRect.height = 500;
|
||||||
|
/*roiRect.x = 154;
|
||||||
|
roiRect.y = 176;*/
|
||||||
|
roiRect.x = 125;
|
||||||
|
roiRect.y = 80;
|
||||||
|
cv::Mat ROI(rawImages, roiRect);
|
||||||
|
|
||||||
|
//图像预处理
|
||||||
|
mask = preprocessing(ROI);
|
||||||
|
//寻找轮廓、确定击打点
|
||||||
|
targetInfo = findTarget(ROI, previousFrameInfo, mask);
|
||||||
|
auto t = std::chrono::high_resolution_clock::now();
|
||||||
|
previousFrameInfo.elapsedTime = std::chrono::high_resolution_clock::now() - previousFrameEndTime;
|
||||||
|
previousFrameEndTime = t;
|
||||||
|
//预测 等待时机...啊...耐心
|
||||||
|
if(targetInfo.target_speed_<=5)
|
||||||
|
{
|
||||||
|
measure_v.push_back(targetInfo.target_speed_);
|
||||||
|
runeTracker.track(targetInfo.radius_,targetInfo.target_speed_,previousFrameInfo.dt);
|
||||||
|
}
|
||||||
|
|
||||||
|
double a2 = runeTracker.state_(0),w2 = runeTracker.state_(1),phi2 = runeTracker.state_(2),t2 = runeTracker.state_(5)+0.05;
|
||||||
|
double target_theta = -(a2/w2)*cos(w2*t2)+(2.090-a2)*t2+(a2/w2);
|
||||||
|
|
||||||
|
cv::Point center= targetInfo.centor_;
|
||||||
|
int radius = runeTracker.state_(3);
|
||||||
|
|
||||||
|
// 绘制圆
|
||||||
|
cv::circle(ROI, center, radius, cv::Scalar(255, 0, 0), 2);
|
||||||
|
|
||||||
|
// 指定角度（以rad为单位）
|
||||||
|
// double angle = target_theta;
|
||||||
|
//
|
||||||
|
// // 根据指定角度计算半径的终点坐标
|
||||||
|
// cv::Point endpoint;
|
||||||
|
// endpoint.x = center.x + static_cast<int>(radius * cos(angle ));
|
||||||
|
// endpoint.y = center.y - static_cast<int>(radius * sin(angle ));
|
||||||
|
// cv::line(ROI, center, endpoint, cv::Scalar(0, 0, 255), 2);
|
||||||
|
|
||||||
|
|
||||||
|
double v = runeTracker.state_(4),statet = runeTracker.state_(5);
|
||||||
|
|
||||||
|
|
||||||
|
state_v.push_back(v);
|
||||||
|
|
||||||
|
matplotlibcpp::clf();
|
||||||
|
|
||||||
|
matplotlibcpp::plot(measure_v);matplotlibcpp::plot(state_v);
|
||||||
|
matplotlibcpp::pause(0.0001);
|
||||||
|
|
||||||
|
// static double t_totle=0;
|
||||||
|
//
|
||||||
|
// if(targetInfo.target_speed_<=5)
|
||||||
|
// {
|
||||||
|
// fitter.add_data(t_totle,targetInfo.target_speed_);
|
||||||
|
// }t_totle+=previousFrameInfo.dt;
|
||||||
|
// if(fitter.is_data_enough()&&(!fitter.is_fitted()))
|
||||||
|
// {
|
||||||
|
// fitter.fit_speed();
|
||||||
|
// fitter.draw_result_matplot();
|
||||||
|
// }
|
||||||
|
|
||||||
|
//位姿解算
|
||||||
|
PNP(ROI, targetInfo.target_points_, cameraMatrix, distCoeffs);
|
||||||
|
|
||||||
|
cv::namedWindow("rawImages", 0);
|
||||||
|
//resizeWindow("rawImages", 741, 620);
|
||||||
|
imshow("rawImages", ROI);
|
||||||
|
// cv::waitKey(1);
|
||||||
|
}
|
||||||
|
// 按q退出循环、按空格暂停视频进行调节
|
||||||
|
|
||||||
|
char key = cv::waitKey(2);
|
||||||
|
if (key == 'q')
|
||||||
|
break;
|
||||||
|
else if (key == ' ') {
|
||||||
|
paused = !paused;
|
||||||
|
////截取感兴趣的区域
|
||||||
|
//namedWindow("ROIing");
|
||||||
|
//resizeWindow("ROIing", 741, 620);
|
||||||
|
//imshow("ROIing", rawImages);
|
||||||
|
//setMouseCallback("ROIing", MouseEvent, 0);
|
||||||
|
//waitKey(0);
|
||||||
|
|
||||||
|
////截取部分区域进行处理
|
||||||
|
//roiRect.width = 605;
|
||||||
|
//roiRect.height = 500;
|
||||||
|
///*roiRect.x = 154;
|
||||||
|
//roiRect.y = 176;*/
|
||||||
|
//roiRect.x = 125;
|
||||||
|
//roiRect.y = 80;
|
||||||
|
//Mat ROI(rawImages, roiRect);
|
||||||
|
////调参
|
||||||
|
//tuneParameter(ROI);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
matplotlibcpp::show();
|
||||||
|
cout << "视频图像处理完成" << endl;
|
||||||
|
//释放视频资源
|
||||||
|
cap.release();
|
||||||
|
}
|
||||||
|
else
|
||||||
|
cout << "视频图像读取不成功" << endl;
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,3 @@
|
||||||
|
#include "parameter.h"
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -0,0 +1,46 @@
|
||||||
|
#include "preprocessing.h"
|
||||||
|
|
||||||
|
|
||||||
|
#define ENEMY BLUE
|
||||||
|
#define RED 0
|
||||||
|
#define BLUE 1
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 进行图像预处理
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="rawImages"></param>
|
||||||
|
|
||||||
|
cv::Mat preprocessing(cv::Mat rawImages) {
|
||||||
|
cv::Mat subtracted, imageBGR, imageGray, thresholdImg, mask;
|
||||||
|
|
||||||
|
cvtColor(rawImages, imageGray, cv::COLOR_BGR2GRAY);
|
||||||
|
threshold(imageGray, thresholdImg, 176, 255, cv::THRESH_BINARY);
|
||||||
|
//通道相减 突出识别颜色
|
||||||
|
std::vector<cv::Mat> channels;
|
||||||
|
split(rawImages, channels);
|
||||||
|
if (ENEMY == RED)
|
||||||
|
subtract(channels[2], channels[0], subtracted);
|
||||||
|
else if (ENEMY == BLUE)
|
||||||
|
subtract(channels[0], channels[2], subtracted);
|
||||||
|
|
||||||
|
//图像预处理参数 二值化、膨胀、形态学闭操作
|
||||||
|
int s = 3, th = 89;
|
||||||
|
threshold(subtracted, subtracted, th, 255, cv::THRESH_BINARY);
|
||||||
|
mask = subtracted & thresholdImg;
|
||||||
|
|
||||||
|
cv::Mat structureelement = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(s, s), cv::Point(-1, -1));
|
||||||
|
dilate(mask, mask, structureelement, cv::Point(-1, -1), 1);
|
||||||
|
|
||||||
|
cv::Mat kernel = cv::getStructuringElement(0, cv::Size(5, 5));
|
||||||
|
morphologyEx(mask, mask, cv::MORPH_CLOSE, kernel);
|
||||||
|
|
||||||
|
|
||||||
|
cv::namedWindow("mask", 0);
|
||||||
|
cv::resizeWindow("mask", 741, 620);
|
||||||
|
imshow("mask", mask);
|
||||||
|
cv::waitKey(1);
|
||||||
|
|
||||||
|
return mask;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,128 @@
|
||||||
|
#include <rune_fit.hpp>
|
||||||
|
#include <matplotlibcpp.h>
|
||||||
|
namespace rm_power_rune
|
||||||
|
{
|
||||||
|
rune_fitter::rune_fitter(int data_count)
|
||||||
|
{
|
||||||
|
fitdataCount_ = data_count;
|
||||||
|
fit_flag = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
rune_fitter::~rune_fitter()
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
bool rune_fitter::add_data(double t, double speed)
|
||||||
|
{
|
||||||
|
if (this->speed_data_.size() >= fitdataCount_)
|
||||||
|
return false;
|
||||||
|
else
|
||||||
|
{
|
||||||
|
speed_data_.push_back(speed);
|
||||||
|
t_data_.push_back(t);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool rune_fitter::fit_speed()
|
||||||
|
{
|
||||||
|
ceres::Problem problem;
|
||||||
|
for (int i = 0; i < fitdataCount_; i++)
|
||||||
|
{
|
||||||
|
problem.AddResidualBlock(
|
||||||
|
new ceres::AutoDiffCostFunction<SpeedfitResidual, 1, 3>( // 1,1,1 (wucha , m , c)
|
||||||
|
new SpeedfitResidual(t_data_[i], speed_data_[i])),
|
||||||
|
new ceres::CauchyLoss(0.5),
|
||||||
|
curve_param_);
|
||||||
|
// nullptr,
|
||||||
|
// &a,
|
||||||
|
// &w,
|
||||||
|
// &b);
|
||||||
|
}
|
||||||
|
|
||||||
|
// problem.SetParameterLowerBound(curve_param_, 0, 0.7);
|
||||||
|
// problem.SetParameterUpperBound(curve_param_, 0, 1.1);
|
||||||
|
// problem.SetParameterLowerBound(curve_param_, 1, 1.8);
|
||||||
|
// problem.SetParameterUpperBound(curve_param_, 1, 2.1);
|
||||||
|
problem.SetParameterLowerBound(curve_param_, 2, 0);
|
||||||
|
|
||||||
|
ceres::Solver::Options options;
|
||||||
|
options.max_num_iterations = 50;
|
||||||
|
options.linear_solver_type = ceres::DENSE_QR;
|
||||||
|
options.function_tolerance = 1e-15; // 设置函数值的收敛精度
|
||||||
|
options.gradient_tolerance = 1e-15; // 设置梯度的收敛精度
|
||||||
|
options.minimizer_progress_to_stdout = true;
|
||||||
|
ceres::Solver::Summary summary;
|
||||||
|
Solve(options, &problem, &summary);
|
||||||
|
std::cout << summary.BriefReport() << "\n";
|
||||||
|
std::cout << "Initial a: " << 0.78 << " w: " << 1.884 << " b: " << 2.090 - 0.78 <<" phi: " << 0<< "\n";
|
||||||
|
std::cout << "Final a: " << curve_param_[0] << " w: " << curve_param_[1] << " b: " << 2.090 - curve_param_[0] <<" phi: " << curve_param_[2] << "\n";
|
||||||
|
|
||||||
|
fit_flag = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void rune_fitter::draw_result()
|
||||||
|
{
|
||||||
|
cv::Mat img(800, 800, CV_8UC3, cv::Scalar(255, 255, 255));
|
||||||
|
|
||||||
|
// 绘制坐标轴
|
||||||
|
cv::line(img, cv::Point(0, 0), cv::Point(0, img.rows), cv::Scalar(128, 128, 128), 1);
|
||||||
|
cv::line(img, cv::Point(0, 0), cv::Point(img.cols, 0), cv::Scalar(128, 128, 128), 1);
|
||||||
|
|
||||||
|
// 绘制正弦函数曲线
|
||||||
|
std::vector<cv::Point> curvePoints;
|
||||||
|
for (int x = 0; x < img.cols; x++)
|
||||||
|
{
|
||||||
|
double y = curve_param_[0] * sin(curve_param_[1] * x * 5.0 / img.cols) + (2.090 - curve_param_[0]);
|
||||||
|
y = y / 2.090;
|
||||||
|
y *= img.rows;
|
||||||
|
// double y = 800* (a*sin(w*5.0*x/800.0)+(2.090))/2.090;
|
||||||
|
// double y = sin(m * (5.0*x/double(img.cols)) + c)+a;
|
||||||
|
curvePoints.push_back(cv::Point(x, y));
|
||||||
|
}
|
||||||
|
|
||||||
|
for (size_t i = 0; i < fitdataCount_; i++)
|
||||||
|
{
|
||||||
|
// auto max_it = std::max_element(y_data.begin(),y_data.end());
|
||||||
|
// double max_y = *max_it;
|
||||||
|
|
||||||
|
cv::Point data(t_data_[i] * img.cols / 5.0, speed_data_[i] * img.rows / 2.090);
|
||||||
|
cv::circle(img, data, 2, cv::Scalar(255, 0, 0), -1);
|
||||||
|
}
|
||||||
|
|
||||||
|
cv::polylines(img, curvePoints, false, cv::Scalar(0, 0, 255), 3);
|
||||||
|
|
||||||
|
cv::imshow("Curve", img);
|
||||||
|
}
|
||||||
|
|
||||||
|
void rune_fitter::draw_result_matplot() {
|
||||||
|
matplotlibcpp::scatter(t_data_,speed_data_);
|
||||||
|
std::vector<double> x;
|
||||||
|
std::vector<double> y;
|
||||||
|
|
||||||
|
double start = 0.0;
|
||||||
|
double end = 6 * M_PI;
|
||||||
|
double step = 0.1;
|
||||||
|
|
||||||
|
for (double i = start; i <= end; i += step) {
|
||||||
|
x.push_back(i);
|
||||||
|
y.push_back(curve_param_[0]*std::sin(curve_param_[1]*i+curve_param_[2]) + (2.090 - curve_param_[0]));
|
||||||
|
}
|
||||||
|
|
||||||
|
// 绘制图形
|
||||||
|
matplotlibcpp::plot(x, y,"r-");
|
||||||
|
matplotlibcpp::show();
|
||||||
|
}
|
||||||
|
|
||||||
|
bool rune_fitter::is_data_enough() {
|
||||||
|
if(speed_data_.size() >= fitdataCount_ )
|
||||||
|
return true;
|
||||||
|
else
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool rune_fitter::is_fitted() {
|
||||||
|
return fit_flag;
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace rune_fit
|
||||||
|
|
@ -0,0 +1,89 @@
|
||||||
|
//
|
||||||
|
// Created by hshine on 23-10-10.
|
||||||
|
//
|
||||||
|
|
||||||
|
#include "rune_tracker.h"
|
||||||
|
#include <iostream>
|
||||||
|
|
||||||
|
namespace rm_power_rune {
|
||||||
|
rune_tracker::rune_tracker(double a0,double w0,double phi0,double r0):state_(6),observe_(2) {
|
||||||
|
// state: a, w, phi, r, v, t
|
||||||
|
// measurement: r, v
|
||||||
|
state_ << a0,w0,phi0,r0,0,0;
|
||||||
|
// f - Process function
|
||||||
|
auto f = [this](const Eigen::VectorXd & x) {
|
||||||
|
Eigen::VectorXd x_new = x;
|
||||||
|
double a = x(0),w = x(1),phi = x(2),t = x(5);
|
||||||
|
x_new(4) = a*sin(w*t+phi)+(2.090-a);
|
||||||
|
x_new(5) += dt_;
|
||||||
|
//w*a*cos(w*t+phi)
|
||||||
|
return x_new;
|
||||||
|
};
|
||||||
|
// h - Observation function
|
||||||
|
auto h = [](const Eigen::VectorXd & x) {
|
||||||
|
Eigen::VectorXd z(2);
|
||||||
|
|
||||||
|
z(0) = x(3);
|
||||||
|
z(1) = x(4);
|
||||||
|
return z;
|
||||||
|
};
|
||||||
|
// J_f - Jacobian of process function
|
||||||
|
auto j_f = [](const Eigen::VectorXd & x) {
|
||||||
|
Eigen::MatrixXd f(6, 6);
|
||||||
|
double a = x(0),w = x(1),phi = x(2),t = x(5);
|
||||||
|
// clang-format off
|
||||||
|
f << 1, 0, 0, 0, 0, 0,
|
||||||
|
0, 1, 0, 0, 0, 0,
|
||||||
|
0, 0, 1, 0, 0, 0,
|
||||||
|
0, 0, 0, 1, 0, 0,
|
||||||
|
sin(w*t+phi)-1, a*t*cos(w*t+phi), a*cos(w*t+phi), 0, 0, w*a*cos(w*t+phi),
|
||||||
|
0, 0, 0, 0, 0, 1;
|
||||||
|
// clang-format on
|
||||||
|
return f;
|
||||||
|
};
|
||||||
|
auto j_h = [](const Eigen::VectorXd & x) {
|
||||||
|
Eigen::MatrixXd h(2, 6);
|
||||||
|
|
||||||
|
// clang-format off
|
||||||
|
// a w phi r v t
|
||||||
|
h << 0, 0, 0, 1, 0, 0,
|
||||||
|
0, 0, 0, 0, 1, 0;
|
||||||
|
// clang-format on
|
||||||
|
return h;
|
||||||
|
};
|
||||||
|
auto u_q = []() {
|
||||||
|
Eigen::DiagonalMatrix<double, 6> q;
|
||||||
|
q.diagonal() << 1e-5,1e-5,1e-5,1e-5,1e-5,1e-5;
|
||||||
|
// clang-format on
|
||||||
|
return q;
|
||||||
|
};
|
||||||
|
auto u_r = [](const Eigen::VectorXd & z) {
|
||||||
|
Eigen::DiagonalMatrix<double, 2> r;
|
||||||
|
r.diagonal() << 1.5,1.5;
|
||||||
|
// clang-format on
|
||||||
|
return r;
|
||||||
|
};
|
||||||
|
// P - error estimate covariance matrix
|
||||||
|
Eigen::DiagonalMatrix<double, 6> p0;
|
||||||
|
p0.setIdentity();
|
||||||
|
|
||||||
|
rune_ekf_ = ExtendedKalmanFilter(f, h, j_f, j_h, u_q, u_r, p0);
|
||||||
|
|
||||||
|
rune_ekf_.setState(state_);
|
||||||
|
}
|
||||||
|
|
||||||
|
void rune_tracker::track(double r, double v, double dt) {
|
||||||
|
this->dt_=dt;
|
||||||
|
observe_<<r,v;
|
||||||
|
Eigen::VectorXd ekf_prediction = rune_ekf_.predict();
|
||||||
|
state_ = ekf_prediction;
|
||||||
|
state_ = rune_ekf_.update(observe_);
|
||||||
|
std::cout<<state_<<std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
double rune_tracker::get_velocity() {
|
||||||
|
return this->state_(4);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
} // rm_rune
|
||||||
|
|
@ -0,0 +1,109 @@
|
||||||
|
#include "tuneParameter.h"
|
||||||
|
|
||||||
|
|
||||||
|
#define ENEMY BLUE
|
||||||
|
#define RED 0
|
||||||
|
#define BLUE 1
|
||||||
|
|
||||||
|
|
||||||
|
//声明调参全局变量
|
||||||
|
cv::Mat src;
|
||||||
|
cv::Mat subtracted, imageGray ,subthreshold, thresholdImg, andmask, mask;
|
||||||
|
char OUTPUT_WIN[] = "output image";
|
||||||
|
|
||||||
|
|
||||||
|
void CallBack_directThreshold(int pos, void* userdata); //声明二值化的回调函数
|
||||||
|
void CallBack_threshold(int pos, void* userdata); //声明二值化的回调函数
|
||||||
|
void CallBack_dilate(int pos, void* userdata); //声明膨胀的回调函数
|
||||||
|
//void CallBack_erode(int pos, void* userdata); //声明腐蚀的回调函数
|
||||||
|
|
||||||
|
|
||||||
|
void tuneParameter(cv::Mat input){
|
||||||
|
if (input.empty())
|
||||||
|
{
|
||||||
|
cout << "preprocessing could not load image..." << endl;
|
||||||
|
getchar();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
input.copyTo(src);
|
||||||
|
cv::namedWindow("input image", CV_WINDOW_AUTOSIZE);//命名一个窗口显示输入图像
|
||||||
|
cv::resizeWindow("input image", 741, 620);
|
||||||
|
imshow("input image", src);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
cvtColor(src, imageGray, cv::COLOR_BGR2GRAY);
|
||||||
|
//通道相减 突出识别颜色
|
||||||
|
std::vector<cv::Mat> channels;
|
||||||
|
split(src, channels);
|
||||||
|
if (ENEMY == RED)
|
||||||
|
subtract(channels[2], channels[0], subtracted);
|
||||||
|
else if (ENEMY == BLUE)
|
||||||
|
subtract(channels[0], channels[2], subtracted);
|
||||||
|
|
||||||
|
int element_size = 3;//此处定义滑动条当前值
|
||||||
|
int max_size = 21;//定义滑动条最大值
|
||||||
|
cv::Mat strElement;
|
||||||
|
cv::namedWindow(OUTPUT_WIN, CV_WINDOW_AUTOSIZE);//命名一个窗口显示输出图像
|
||||||
|
//创建直接二值化滑动条
|
||||||
|
cv::createTrackbar("directThreshold :", OUTPUT_WIN, &element_size, 255, CallBack_directThreshold);
|
||||||
|
//创建通道相减二值化滑动条
|
||||||
|
cv::createTrackbar("threshold :", OUTPUT_WIN, &element_size, 255, CallBack_threshold);
|
||||||
|
//创建膨胀滑动条
|
||||||
|
cv::createTrackbar("dilateKernel :", OUTPUT_WIN, &element_size, max_size, CallBack_dilate);
|
||||||
|
////创建腐蚀滑动条
|
||||||
|
//createTrackbar("erodeKernel :", OUTPUT_WIN, &element_size, max_size, CallBack_erode);
|
||||||
|
|
||||||
|
cv::waitKey(0);
|
||||||
|
return ;
|
||||||
|
}
|
||||||
|
|
||||||
|
//定义直接二值化的回调函数
|
||||||
|
void CallBack_directThreshold(int element_size, void* strElement) {
|
||||||
|
|
||||||
|
threshold(imageGray, thresholdImg, element_size, 255, cv::THRESH_BINARY);
|
||||||
|
imshow(OUTPUT_WIN, thresholdImg);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
//定义通道相减二值化的回调函数
|
||||||
|
void CallBack_threshold(int element_size, void* strElement) {
|
||||||
|
|
||||||
|
threshold(subtracted, subthreshold, element_size, 255, cv::THRESH_BINARY);
|
||||||
|
andmask = subthreshold & thresholdImg;
|
||||||
|
imshow(OUTPUT_WIN, subthreshold);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
//定义膨胀的回调函数
|
||||||
|
void CallBack_dilate(int element_size, void* strElement) {
|
||||||
|
int s;
|
||||||
|
if (element_size % 2 == 0)
|
||||||
|
s = element_size + 1;
|
||||||
|
else
|
||||||
|
s = element_size;
|
||||||
|
cv::Mat structureElement = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(s, s), cv::Point(-1, -1));
|
||||||
|
dilate(andmask, mask, structureElement, cv::Point(-1, -1), 1);
|
||||||
|
imshow(OUTPUT_WIN, mask);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
////定义腐蚀的回调函数
|
||||||
|
//void CallBack_erode(int element_size, void* userdata) {
|
||||||
|
// int s = element_size * 2 + 1;
|
||||||
|
// Mat structureElement = getStructuringElement(MORPH_RECT, Size(s, s), Point(-1, -1));
|
||||||
|
// erode(mask, mask, structureElement);
|
||||||
|
// imshow(OUTPUT_WIN, mask);
|
||||||
|
// return;
|
||||||
|
//}
|
||||||
|
//
|
||||||
|
|
||||||
|
//
|
||||||
|
////定义内圈遮罩的回调函数
|
||||||
|
//void CallBack_circleIn(int element_size, void* strElement) {
|
||||||
|
//
|
||||||
|
// circle(mask, center, element_size, Scalar(0,0,0), 1);
|
||||||
|
// return;
|
||||||
|
//}
|
||||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
After Width: | Height: | Size: 109 KiB |
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
After Width: | Height: | Size: 51 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 51 KiB |
Loading…
Reference in New Issue