master

分支 (1)

管理

管理

master

aust-rm-vision-2022
/
main_default.hpp

//
// Created by hoyin on 2022/4/13.
//
#ifndef MAIN_DEFAULT_CPP
#define MAIN_DEFAULT_CPP

#include <iostream>
#include <chrono>
#include "opencv2/opencv.hpp"
#include "MVCamera.h"
#include "SingleTrack.h"
#include "Matcher.h"
#include "UndistortImage.h"
#include "CvUtils.h"
#include "ImageStream.h"
#include "thread"
#include "Chrono.h"
#include "Filter.h"
#include "Optimizer.h"
#include "MathUtils.h"

using namespace st;
using namespace cv;
using namespace mu;
using namespace opt;

//#define FRAME_WIDTH 678
//#define FRAME_HEIGHT 512

#define RETURN_VALUE_SUCCESS 0
#define RETURN_VALUE_ERROR 1
#define RETURN_VALUE_ILLEGAL_COMMAND 2
#define RETURN_VALUE_UNDEFINED_TYPE_OF_STREAM 3

//像尺寸
#define FRAME_WIDTH 640
#define FRAME_HEIGHT 368

#define PI 3.14159265

// 帧间隔
#define FRAME_TIME_SPAN 42
// 飞行时间 + 机动时间 + 其他时间
#define TIME_SPAN 0.5
// 直线距离
#define STRAIGHT_DISTANCE 10
// 重力加速度
#define G 9.8
// 装甲板在图像中的像素
#define ARMOUR_WIDTH_PIX 35.86666463157895
#define ARMOUR_HEIGHT_PIX 24.679530213903742
// 装甲板的实际长度
#define ARMOUR_WIDTH_ACT 14.5329608467882
#define ARMOUR_HEIGHT_ACT 10
// 线性尺度倍数 实际长度(m)/像素个数(pix)
#define SCALE 0.4051940986448067
// 绘图框数量
#define CANVAS_COUNT 4


int main_default(ImageStream &ims) {
//	初始化匹配器
	Matcher matcher;
	Mat centerTemplate = imread("./media/centerTemplate.png", IMREAD_GRAYSCALE);
	matcher.setTargetImage(centerTemplate);
//	等待100ms跳过无效帧
	std::this_thread::sleep_for(std::chrono::milliseconds(100));
//	定义原始帧
	Mat frame;
//	散点绘图帧、绘图数据流
	Mat canvas[CANVAS_COUNT];
	for (Mat &c: canvas) {
		c = Mat(FRAME_HEIGHT, FRAME_WIDTH, CV_8UC3, Scalar(255, 255, 255));
	}
	vector<double> data_x[CANVAS_COUNT];
	vector<double> data_y[CANVAS_COUNT];
//	上一角度，用于计算角度差
	double prev_angle = 0;
//	上一时间戳，用于计算时间差
	double prev_ts = 0;
//	上一R标位置，用于防止位置差太大的误匹配
	Point2d prevRLogoPoint(-1, -1);
//	滤波器、结果流、原始流
	Filter filter(5);
	vector<vector<double>> filterResultStream;
	vector<vector<double>> unfilteredStream;
//	数据记录流
	ofstream originLogOFS("./logs/velocity");
	ofstream filteredLogOFS("./logs/filteredVelocity");
	ofstream sizeOFS("./logs/rectSize");
	if (!originLogOFS.is_open()) {
		cout << "open origin log failed" << endl;
	}

	if (!filteredLogOFS.is_open()) {
		cout << "open filtered log failed" << endl;
	}
//	优化器初始采样数、优化参数表、优化数据流
	int optN = 20;
	double ae = 0.785, we = 0.884, te = 0.0, ye = 1.305;
	double params[4] = {ae, we, te, ye};
	vector<double> opt_data_x, opt_data_y;
	Optimizer optimizer(params, optN);
//	预测角度、半径、直角坐标
	double esti_theta = 0;
	double R = 0;
	Point2d estiPoint_scr, estiPoint_pix;
//	Yaw, Pitch, Roll, v 参数
	double yaw, pitch, roll = 0, v;
	double z = STRAIGHT_DISTANCE;
//	计时器
	Chrono chrono;
//	开始
	while (ims.isOK()) {
//		时间戳
		double ts = 0;
		ims.read(frame, ts);
//		原视频有些影响匹配的
		if (frame.empty()) {
			continue;
		}
		for (int c = 0; c < 200; ++c) {
			for (int r = 300; r < 368; ++r) {
				frame.at<Vec3b>(r, c)[0] = 0;
				frame.at<Vec3b>(r, c)[1] = 0;
				frame.at<Vec3b>(r, c)[2] = 0;
			}
		}
		for (int c = 310; c < 325; ++c) {
			for (int r = 165; r < 180; ++r) {
				frame.at<Vec3b>(r, c)[0] = 20;
				frame.at<Vec3b>(r, c)[1] = 20;
				frame.at<Vec3b>(r, c)[2] = 200;
			}
		}
//		showWaitKey(frame, "frame");
		flip(frame, frame, 0);
//		查找R标中心点
		Mat frameBin;
		Mat element = getStructuringElement(MORPH_RECT, Size(5, 5));
		cvtColor(frame, frameBin, COLOR_BGR2GRAY);
		threshold(frameBin, frameBin, 50, 255, THRESH_BINARY);
		morphologyEx(frameBin, frameBin, MORPH_CLOSE, element);

		matcher.setOriginImage(frameBin);
		Rect2d centerRect = Rect2d(0, 0, 0, 0);
		matcher.contoursMatch(centerRect);
		if (centerRect.empty()) {
			continue;
		}
		Mat centerImg = frameBin(centerRect);
		Moments m = moments(centerImg, true);
		Point2d rLogoPoint(m.m10/m.m00 + centerRect.x, m.m01/m.m00 + centerRect.y);
		if (prevRLogoPoint.x == -1 && prevRLogoPoint.y == -1) {
			prevRLogoPoint = Point2d(rLogoPoint);
		} else {
			if (norm(prevRLogoPoint - rLogoPoint) > 10) {
				rLogoPoint = Point2d(prevRLogoPoint);
			}
		}
		circle(frame, rLogoPoint, 2, Scalar(200, 20, 20), 2);

//		查找未打击扇叶
		Mat armourTemplate, armourTemplateBin;
		armourTemplate = imread("./media/nohit.png", IMREAD_GRAYSCALE);
		threshold(armourTemplate, armourTemplateBin, 50, 255, THRESH_BINARY);
		morphologyEx(armourTemplateBin, armourTemplateBin, MORPH_CLOSE, element);
		matcher.setTargetImage(armourTemplateBin);
		Rect2d fanNotHitRect = Rect2d(0, 0, 0, 0);
		matcher.contoursMatch(fanNotHitRect);
		if (fanNotHitRect.empty()) {
			continue;
		}
//		rectangle(frameBin, fanNotHitRect, Scalar(20, 200, 200), 2);
//		showWaitKey(frameBin, "frame");

//		查找未打击装甲板
		armourTemplate = imread("./media/armour.png", IMREAD_GRAYSCALE);
		threshold(armourTemplate, armourTemplateBin, 50, 255, THRESH_BINARY);
		morphologyEx(armourTemplateBin, armourTemplateBin, MORPH_CLOSE, element);
		Mat fanNotHit = frameBin(fanNotHitRect);
		matcher.setTargetImage(armourTemplateBin);
		matcher.setOriginImage(fanNotHit);
		RotatedRect armourNotHitRect_rotate;
		matcher.contoursMatch(armourNotHitRect_rotate);
		if (armourNotHitRect_rotate.size.empty()) {
			continue;
		}
		Point2d armourNotHitCenter = armourNotHitRect_rotate.center;
		armourNotHitCenter.x += fanNotHitRect.x;
		armourNotHitCenter.y += fanNotHitRect.y;
		armourNotHitRect_rotate.center = armourNotHitCenter;

		double l1 = armourNotHitRect_rotate.size.width;
		double l2 = armourNotHitRect_rotate.size.height;
		if (l1 > l2) {
			sizeOFS << l1 << " " << l2 << endl;
		} else {
			sizeOFS << l2 << " " << l1 << endl;
		}

//		查找未打击装甲板的中心点
		drawRotatedRect(armourNotHitRect_rotate, frame, Scalar(20, 200, 20), 2, Point2d(0, 0));
		Point2f armourCenterPoint_scr;
		getCenterOfRect(armourNotHitRect_rotate, armourCenterPoint_scr);
		circle(frame, armourCenterPoint_scr, 2, Scalar(200, 200, 20), 2);

//		未打击装甲板中心点和R标的连线
		line(frame, rLogoPoint, armourCenterPoint_scr, Scalar(200, 20, 20));

//		计算半径
		R = sqrt(pow(armourCenterPoint_scr.x - rLogoPoint.x, 2) + pow(rLogoPoint.y - armourCenterPoint_scr.y, 2));

//		获取偏转角
		Point2d armourCenterPoint_pix;
		convertPos_Image2Calc(armourCenterPoint_scr, rLogoPoint, armourCenterPoint_pix);
//		Vec2d vecArmour(armourCenterPoint_scr.x - rLogoPoint.x, rLogoPoint.y - armourCenterPoint_scr.y);
		Vec2d vecArmour(armourCenterPoint_pix.x, armourCenterPoint_pix.y);
		Vec2d vecX_p(1, 0);
		double angle = getAngleOf2Vectors(vecArmour, vecX_p);
		if (getQuadrant(vecArmour) > 2) {
			angle = 2 * PI - angle;
		}
//		计算瞬时速度
		double velocity = (angle - prev_angle) / (ts - prev_ts);
//		为下一循环设置变量
		prev_angle = angle;
		prev_ts = ts;
//		输出数据流到文件
		originLogOFS << ts << " " << velocity << " " << angle << endl;
//		输出数据到原始数据流
		if (!isnan(velocity)) {
			unfilteredStream.push_back({ts, velocity});
//			进行滤波
			if (filter.readyToFilter()) {
				vector<vector<double>> filteredV = Filter::classifyFilter(unfilteredStream, filter.getFlag() -	filter.getCapacity(), filter.getFlag(), 0.25, 2.5);
//				滤波结果加入结果流和优化器输入流
				if (!filteredV.empty()) {
					for (auto &fv: filteredV) {
						filterResultStream.push_back(fv);
						opt_data_x.push_back(fv.at(0));
						opt_data_y.push_back(fv.at(1));
						optimizer.increaseFlag();
//								输出滤波结果到文件
						filteredLogOFS << fv.at(0) << " " << fv.at(1) << endl;
					}
				}
//				cout << chrono.timeSpan() << " s" << endl;
			}
			filter.updateFlag();
//			非线性优化
			if (optimizer.readyToOptimize()) {
				optimizer.solveTrigonometric(opt_data_x, opt_data_y);
				double d_theta = integrateSin(params[0], params[1], params[2], params[3], ts, ts + TIME_SPAN);
				esti_theta = angle + d_theta;
				estiPoint_pix.x = R * cos(esti_theta);
				estiPoint_pix.y = R * sin(esti_theta);
			}
		}
//		速度函数优化结果
		double optvelocity = params[0]* sin(params[1]*(ts + params[2])) + params[3];

//		输出数据到散点图
		data_x[0].push_back(ts);
		data_y[0].push_back(angle);
		drawPlot(data_x[0], data_y[0], canvas[0], Scalar(123, 104, 238), 0, 0, -10, 10);
		imshow("angle", canvas[0]);
		data_x[1].push_back(ts + TIME_SPAN);
		data_y[1].push_back(esti_theta);
		drawPlot(data_x[1], data_y[1], canvas[1], Scalar(84, 139, 84), 0, 0, -10, 10);
		imshow("esti_angle", canvas[1]);
		data_x[2].push_back(ts + TIME_SPAN);
		data_y[2].push_back(yaw);
		drawPlot(data_x[2], data_y[2], canvas[2], Scalar(238, 99, 99), 0, 0, -1, 1);
		imshow("yaw", canvas[2]);
		data_x[3].push_back(ts + TIME_SPAN);
		data_y[3].push_back(pitch);
		drawPlot(data_x[3], data_y[3], canvas[3], Scalar(139, 35, 35), 0, 0, -3, 3);
		imshow("pitch", canvas[3]);
//		显示预测点位置
		convertPos_Calc2Image(estiPoint_pix, rLogoPoint, estiPoint_scr);
		circle(frame, estiPoint_scr, 3, Scalar(20, 200, 200), 3);
//		计算ypr参数
		const double x = armourCenterPoint_pix.x * SCALE;
		const double y = armourCenterPoint_pix.y * SCALE;
		const double Sy = sqrt(x*x + z*z);

		yaw = atan(z / Sy);

		double y_ = 0.5*y + 0.25*G*TIME_SPAN*TIME_SPAN;
		double s_ = sqrt(x*x + pow(y - y_, 2) + z*z);
		double rho_ = acos((y - y_) / s_);

		v = Sy / (TIME_SPAN * sin(rho_));
		pitch = rho_;

//		显示ypr参数
		putText(frame, format("yaw: %f rad, pitch: %f rad, speed: %f m/s", yaw, pitch, v), Point(20, 40), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(20, 200, 200), 2);
//		显示追踪结果
		putText(frame, format("ts: %f s", ts), Point(200, 60), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 0, 255), 2);
		putText(frame, format("a: %f deg", angle * 180 / 3.14159), Point(20, 60), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 0, 255), 2);
//		namedWindow("frame");
//		显示单帧耗时
		putText(frame, format("time_cost: %f ms", 1000*chrono.timeSpan()), Point(20, 80), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 0, 255), 2);
//		namedWindow("frame");
		imshow("frame", frame);
//		startWindowThread();
		waitKey(10);
//		showWaitKey(frame, "frame");
	}

	return RETURN_VALUE_SUCCESS;
}


#endif