能量机关状态估计测试

.gitignore

@@ -0,0 +1,2 @@
+/cmake-build-debug/
+/.idea

CMakeLists.txt

@@ -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)

TestRecord.md

@@ -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

include/ExtendedKalmanFilter.h

@@ -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

include/PnP.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);

include/bufferDetector.h

@@ -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);

include/parameter.h

@@ -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>

include/preprocessing.h

@@ -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);

include/rune_fit.hpp

@@ -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

include/rune_tracker.h

@@ -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

include/tuneParameter.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); //声明图像预处理的函数
+

out_camera_data.xml

@@ -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>

src/ExtendedKalmanFilter.cpp

@@ -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

src/PnP.cpp

@@ -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);
+	
+
+}
+

src/bufferDetector.cpp

@@ -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;
+
+}
+

src/main.cpp

@@ -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;
+	
+	
+	
+	
+}

src/parameter.cpp

@@ -0,0 +1,3 @@
+#include "parameter.h"
+
+

src/preprocessing.cpp

@@ -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;
+}

src/rune_fit.cpp

@@ -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

src/rune_tracker.cpp

@@ -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

src/tuneParameter.cpp

@@ -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;
+//}