<commit_before>#include "FrontCameraVision.h"
#include "SimpleImageThresholder.h"
#include "ThreadedImageThresholder.h"
#include "ParallelImageThresholder.h"
#include "TBBImageThresholder.h"
#include "GateFinder.h"
#include "BallFinder.h"
#include "AutoCalibrator.h"
#include <queue>          // std::priority_queue
#include <functional>     // std::greater
#include "kdNode2D.h"
#include "DistanceCalculator.h"
#include "VideoRecorder.h"


extern DistanceCalculator gDistanceCalculator;


FrontCameraVision::FrontCameraVision(ICamera *pCamera, IDisplay *pDisplay, FieldState *pFieldState) : ConfigurableModule("FrontCameraVision"), ThreadedClass("FrontCameraVision")
{
	m_pCamera = pCamera;
	m_pDisplay = pDisplay;
	m_pState = pFieldState;

	ADD_BOOL_SETTING(gaussianBlurEnabled);
	ADD_BOOL_SETTING(greenAreaDetectionEnabled);
	ADD_BOOL_SETTING(gateObstructionDetectionEnabled);
	ADD_BOOL_SETTING(borderDetectionEnabled);
	ADD_BOOL_SETTING(nightVisionEnabled);
	videoRecorder = new VideoRecorder("videos/", 30, m_pCamera->GetFrameSize(true));
	LoadSettings();
	Start();
}


FrontCameraVision::~FrontCameraVision()
{
	WaitForStop();
	if (videoRecorder != NULL) {
		videoRecorder->Stop();
		delete videoRecorder;
		videoRecorder = NULL;
	}
}
bool FrontCameraVision::captureFrames(){
	return videoRecorder->isRecording;
}

void FrontCameraVision::captureFrames(bool start){
	if (start) {
		videoRecorder->Start();
	}
	else {
		videoRecorder->Stop();
	}
}
void FrontCameraVision::Run() {
	ThresholdedImages thresholdedImages;


	try {
		CalibrationConfReader calibrator;
		for (int i = 0; i < NUMBER_OF_OBJECTS; i++) {
			objectThresholds[(OBJECT)i] = calibrator.GetObjectThresholds(i, OBJECT_LABELS[(OBJECT)i]);
		}
	}
	catch (...){
		std::cout << "Calibration data is missing!" << std::endl;

	}
	TBBImageThresholder thresholder(thresholdedImages, objectThresholds);
	GateFinder blueGateFinder;
	GateFinder yellowGateFinder;
	BallFinder ballFinder;

	auto frameSize = m_pCamera->GetFrameSize();

	/*
	m_pState->blueGate.setFrameSize(frameSize);
	m_pState->yellowGate.setFrameSize(frameSize);
	*/
	

	cv::Mat white(frameSize.height, frameSize.width, CV_8UC3, cv::Scalar(255, 255, 255));
	cv::Mat black(frameSize.height, frameSize.width, CV_8UC3, cv::Scalar(40, 40, 40));
	cv::Mat green(frameSize.height, frameSize.width, CV_8UC3, cv::Scalar(21, 188, 80));
	cv::Mat yellow(frameSize.height, frameSize.width, CV_8UC3, cv::Scalar(61, 255, 244));
	cv::Mat blue(frameSize.height, frameSize.width, CV_8UC3, cv::Scalar(236, 137, 48));
	cv::Mat orange(frameSize.height, frameSize.width, CV_8UC3, cv::Scalar(48, 154, 236));

	bool notEnoughtGreen = false;
	int mouseControl = 0;
	bool somethingOnWay = false;

	while (!stop_thread){
		double t2 = (double)cv::getTickCount();
		double dt = (t2 - time) / cv::getTickFrequency();

		cv::Mat frameBGR = m_pCamera->Capture();
		if (videoRecorder->isRecording){
			videoRecorder->RecordFrame(frameBGR, "");
		}
		/**************************************************/
		/*	STEP 1. Convert picture to HSV colorspace	  */
		/**************************************************/
		if (gaussianBlurEnabled) {
			cv::GaussianBlur(frameBGR, frameBGR, cv::Size(3, 3), 4);
		}
		cvtColor(frameBGR, frameHSV, cv::COLOR_BGR2HSV); //Convert the captured frame from BGR to HSV

		/**************************************************/
		/*	STEP 2. thresholding in parallel	          */
		/**************************************************/

		thresholder.Start(frameHSV, { BALL, BLUE_GATE, YELLOW_GATE, FIELD, INNER_BORDER, OUTER_BORDER});

		/**************************************************/
		/*	STEP 3. check that path to gate is clean      */
		/* this is done here, because finding contures	  */
		/* corrupts thresholded imagees					  */
		/**************************************************/
		if (false && gateObstructionDetectionEnabled) {
			cv::Mat selected(frameSize.height, frameSize.width, CV_8U, cv::Scalar::all(0));
			cv::Mat mask(frameSize.height, frameSize.width, CV_8U, cv::Scalar::all(0));
			cv::Mat	tmp(frameSize.height, frameSize.width, CV_8U, cv::Scalar::all(0));
			//cv::line(mask, cv::Point(frameSize.width / 2, 200), cv::Point(frameSize.width / 2 - 40, frameSize.height - 100), cv::Scalar(255, 255, 255), 40);
			//cv::line(mask, cv::Point(frameSize.width / 2, 200), cv::Point(frameSize.width / 2 + 40, frameSize.height - 100), cv::Scalar(255, 255, 255), 40);
			std::vector<cv::Point2i> triangle;
			triangle.push_back(cv::Point(frameSize.width / 2, 50));
			triangle.push_back(cv::Point(frameSize.width / 2 - 80, frameSize.height - 100));
			triangle.push_back(cv::Point(frameSize.width / 2 + 80, frameSize.height - 100));
			cv::fillConvexPoly(mask, triangle, cv::Scalar::all(255));
			tmp = 255 - (thresholdedImages[INNER_BORDER] + thresholdedImages[OUTER_BORDER] + thresholdedImages[FIELD]);
			tmp.copyTo(selected, mask); // perhaps use field and inner border
			thresholdedImages[SIGHT_MASK] = selected;
			//sightObstructed = countNonZero(selected) > 10;
		}




		/**************************************************/
		/* STEP 4. extract closest ball and gate positions*/
		/**************************************************/
		cv::Point2f r1[4], r2[4];
		cv::Point g1, g2;
		//Blue gate pos
		bool blueFound = blueGateFinder.Locate(thresholdedImages[BLUE_GATE], frameHSV, frameBGR, g1, r1);

		//if (!found) {
		//	m_pDisplay->ShowImage(frameBGR);
		//	continue; // nothing to do :(
		//}
		//Yellow gate pos
		bool yellowFound = yellowGateFinder.Locate(thresholdedImages[YELLOW_GATE], frameHSV, frameBGR, g2, r2);
		//if (!found) {
		//	m_pDisplay->ShowImage(frameBGR);
		//	continue; // nothing to do :(
		//}
		// ajust gate positions to ..
		// find closest points to opposite gate centre
		if (blueFound && yellowFound) {
			auto min_i1 = 0, min_j1 = 0, min_i2 = 0, min_j2 = 0;
			double min_dist1 = INT_MAX, min_dist2 = INT_MAX;
			for (int i = 0; i < 4; i++){
				double dist1 = cv::norm(r1[i] - (cv::Point2f)g2);
				double dist2 = cv::norm(r2[i] - (cv::Point2f)g1);
				if (dist1 < min_dist1) {
					min_dist1 = dist1;
					min_i1 = i;
				}
				if (dist2 < min_dist2) {
					min_dist2 = dist2;
					min_j1 = i;
				}
			}
			auto next = (min_i1 + 1) % 4;
			auto prev = (min_i1 + 3) % 4;
			// find longest side
			min_i2 = (cv::norm(r1[min_i1] - r1[next]) > cv::norm(r1[min_i1] - r1[prev])) ? next : prev;
			next = (min_j1 + 1) % 4;
			prev = (min_j1 + 3) % 4;
			// find longest side
			min_j2 = (cv::norm(r2[min_j1] - r2[next]) > cv::norm(r2[min_j1] - r2[prev])) ? next : prev;<commit_msg>mask out balls in gate<commit_after>#include "FrontCameraVision.h"
#include "SimpleImageThresholder.h"
#include "ThreadedImageThresholder.h"
#include "ParallelImageThresholder.h"
#include "TBBImageThresholder.h"
#include "GateFinder.h"
#include "BallFinder.h"
#include "AutoCalibrator.h"
#include <queue>          // std::priority_queue
#include <functional>     // std::greater
#include "kdNode2D.h"
#include "DistanceCalculator.h"
#include "VideoRecorder.h"


extern DistanceCalculator gDistanceCalculator;


FrontCameraVision::FrontCameraVision(ICamera *pCamera, IDisplay *pDisplay, FieldState *pFieldState) : ConfigurableModule("FrontCameraVision"), ThreadedClass("FrontCameraVision")
{
	m_pCamera = pCamera;
	m_pDisplay = pDisplay;
	m_pState = pFieldState;

	ADD_BOOL_SETTING(gaussianBlurEnabled);
	ADD_BOOL_SETTING(greenAreaDetectionEnabled);
	ADD_BOOL_SETTING(gateObstructionDetectionEnabled);
	ADD_BOOL_SETTING(borderDetectionEnabled);
	ADD_BOOL_SETTING(nightVisionEnabled);
	videoRecorder = new VideoRecorder("videos/", 30, m_pCamera->GetFrameSize(true));
	LoadSettings();
	Start();
}


FrontCameraVision::~FrontCameraVision()
{
	WaitForStop();
	if (videoRecorder != NULL) {
		videoRecorder->Stop();
		delete videoRecorder;
		videoRecorder = NULL;
	}
}
bool FrontCameraVision::captureFrames(){
	return videoRecorder->isRecording;
}

void FrontCameraVision::captureFrames(bool start){
	if (start) {
		videoRecorder->Start();
	}
	else {
		videoRecorder->Stop();
	}
}
void FrontCameraVision::Run() {
	ThresholdedImages thresholdedImages;


	try {
		CalibrationConfReader calibrator;
		for (int i = 0; i < NUMBER_OF_OBJECTS; i++) {
			objectThresholds[(OBJECT)i] = calibrator.GetObjectThresholds(i, OBJECT_LABELS[(OBJECT)i]);
		}
	}
	catch (...){
		std::cout << "Calibration data is missing!" << std::endl;

	}
	TBBImageThresholder thresholder(thresholdedImages, objectThresholds);
	GateFinder blueGateFinder;
	GateFinder yellowGateFinder;
	BallFinder ballFinder;

	auto frameSize = m_pCamera->GetFrameSize();

	/*
	m_pState->blueGate.setFrameSize(frameSize);
	m_pState->yellowGate.setFrameSize(frameSize);
	*/
	

	cv::Mat white(frameSize.height, frameSize.width, CV_8UC3, cv::Scalar(255, 255, 255));
	cv::Mat black(frameSize.height, frameSize.width, CV_8UC3, cv::Scalar(40, 40, 40));
	cv::Mat green(frameSize.height, frameSize.width, CV_8UC3, cv::Scalar(21, 188, 80));
	cv::Mat yellow(frameSize.height, frameSize.width, CV_8UC3, cv::Scalar(61, 255, 244));
	cv::Mat blue(frameSize.height, frameSize.width, CV_8UC3, cv::Scalar(236, 137, 48));
	cv::Mat orange(frameSize.height, frameSize.width, CV_8UC3, cv::Scalar(48, 154, 236));

	bool notEnoughtGreen = false;
	int mouseControl = 0;
	bool somethingOnWay = false;

	while (!stop_thread){
		double t2 = (double)cv::getTickCount();
		double dt = (t2 - time) / cv::getTickFrequency();

		cv::Mat frameBGR = m_pCamera->Capture();
		if (videoRecorder->isRecording){
			videoRecorder->RecordFrame(frameBGR, "");
		}
		/**************************************************/
		/*	STEP 1. Convert picture to HSV colorspace	  */
		/**************************************************/
		if (gaussianBlurEnabled) {
			cv::GaussianBlur(frameBGR, frameBGR, cv::Size(3, 3), 4);
		}
		cvtColor(frameBGR, frameHSV, cv::COLOR_BGR2HSV); //Convert the captured frame from BGR to HSV

		/**************************************************/
		/*	STEP 2. thresholding in parallel	          */
		/**************************************************/

		thresholder.Start(frameHSV, { BALL, BLUE_GATE, YELLOW_GATE, FIELD, INNER_BORDER, OUTER_BORDER});

		/**************************************************/
		/*	STEP 3. check that path to gate is clean      */
		/* this is done here, because finding contures	  */
		/* corrupts thresholded imagees					  */
		/**************************************************/
		if (false && gateObstructionDetectionEnabled) {
			cv::Mat selected(frameSize.height, frameSize.width, CV_8U, cv::Scalar::all(0));
			cv::Mat mask(frameSize.height, frameSize.width, CV_8U, cv::Scalar::all(0));
			cv::Mat	tmp(frameSize.height, frameSize.width, CV_8U, cv::Scalar::all(0));
			//cv::line(mask, cv::Point(frameSize.width / 2, 200), cv::Point(frameSize.width / 2 - 40, frameSize.height - 100), cv::Scalar(255, 255, 255), 40);
			//cv::line(mask, cv::Point(frameSize.width / 2, 200), cv::Point(frameSize.width / 2 + 40, frameSize.height - 100), cv::Scalar(255, 255, 255), 40);
			std::vector<cv::Point2i> triangle;
			triangle.push_back(cv::Point(frameSize.width / 2, 50));
			triangle.push_back(cv::Point(frameSize.width / 2 - 80, frameSize.height - 100));
			triangle.push_back(cv::Point(frameSize.width / 2 + 80, frameSize.height - 100));
			cv::fillConvexPoly(mask, triangle, cv::Scalar::all(255));
			tmp = 255 - (thresholdedImages[INNER_BORDER] + thresholdedImages[OUTER_BORDER] + thresholdedImages[FIELD]);
			tmp.copyTo(selected, mask); // perhaps use field and inner border
			thresholdedImages[SIGHT_MASK] = selected;
			//sightObstructed = countNonZero(selected) > 10;
		}




		/**************************************************/
		/* STEP 4. extract closest ball and gate positions*/
		/**************************************************/
		cv::Point2f r1[4], r2[4];
		cv::Point g1, g2;
		//Blue gate pos
		bool blueFound = blueGateFinder.Locate(thresholdedImages[BLUE_GATE], frameHSV, frameBGR, g1, r1);
		if (blueFound) {
			cv::Point vertices[4];
			for (int i = 0; i < 4; ++i){
				vertices[i] = r1[i];
			}
			cv::fillConvexPoly(thresholdedImages[BALL], vertices, 4, cv::Scalar::all(0));
		}
		//if (!found) {
		//	m_pDisplay->ShowImage(frameBGR);
		//	continue; // nothing to do :(
		//}
		//Yellow gate pos
		bool yellowFound = yellowGateFinder.Locate(thresholdedImages[YELLOW_GATE], frameHSV, frameBGR, g2, r2);
		if (yellowFound) {
			cv::Point vertices[4];
			for (int i = 0; i < 4; ++i){
				vertices[i] = r2[i];
			}
			cv::fillConvexPoly(thresholdedImages[BALL], vertices, 4, cv::Scalar::all(0));
		}
		//if (!found) {
		//	m_pDisplay->ShowImage(frameBGR);
		//	continue; // nothing to do :(
		//}
		// ajust gate positions to ..
		// find closest points to opposite gate centre
		if (blueFound && yellowFound) {
			auto min_i1 = 0, min_j1 = 0, min_i2 = 0, min_j2 = 0;
			double min_dist1 = INT_MAX, min_dist2 = INT_MAX;
			for (int i = 0; i < 4; i++){
				double dist1 = cv::norm(r1[i] - (cv::Point2f)g2);
				double dist2 = cv::norm(r2[i] - (cv::Point2f)g1);
				if (dist1 < min_dist1) {
					min_dist1 = dist1;
					min_i1 = i;
				}
				if (dist2 < min_dist2) {
					min_dist2 = dist2;
					min_j1 = i;
				}
			}
			auto next = (min_i1 + 1) % 4;
			auto prev = (min_i1 + 3) % 4;
			// find longest side
			min_i2 = (cv::norm(r1[min_i1] - r1[next]) > cv::norm(r1[min_i1] - r1[prev])) ? next : prev;
			next = (min_j1 + 1) % 4;
			prev = (min_j1 + 3) % 4;
			// find longest side
			min_j2 = (cv::norm(r2[min_j1] - r2[next]) > cv::norm(r2[min_j1] - r2[prev])) ? next : prev;
{'fraction_non_alphanumeric': 0.20414201183431951, 'fraction_numerical': 0.032248520710059174, 'mean_word_length': 4.724386113463167, 'pattern_counts': {'":': 0, '<': 27, '<?xml version=': 0, '>': 41, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '5750471', 'n_tokens_mistral': 5417, 'n_tokens_neox': 4767, 'n_words': 963}