Vision_Kart 2.2v

Vision_Kart 2.2v

Vision_Kart 2.2v은 2.1v에서 키보드로 매개변수 값을 변경할 수 있다.

나머지는 환경은 동일하다.

Environment of Vision_Kart

Processor : RaspberryPi 4

OS : Raspbian

Lidar : YDLidar G2

Lidar Frequency : 10Hz ~ 12Hz(recommend 7Hz)

New function

• Keyboard command

Goals

키보드 명령으로 Circle size, Sight angle 값 조정!

입력 순서도

Enter -> 1 ~ 2(추후에 하나씩 늘릴 예정) -> 값(입력 범위 출력) -> Enter

Algorithm

인터페이스 과정은 그냥 하드코딩으로 구현했다.

더 중요한건 각도와 circle 사이즈마다 종료 임계값(Deadline)이 달라지는 것이다.

새로운 변수(circle_proportion)를 만들어서  프로그램을 종료하게 만들었다.

if(circle_proportion > targetSize && abs(targetAngle) > 180 - View_Angle)
	cout << "out!!!!" << endl;

중간에 보면 circle_proportion을 raius에 맞게 바꿔주는 것을 확인할 수 있다.

뭔가 최적화를 하고 싶었지만 시간 관계상 이제 최선..

아래 표는 내가 측정한 비례 최소 값이다. 저걸 참고해서 변수에 할당하였다.

circle size : circle_proportion
1 : 9
2 : 25
3 : 49
4 : 81
5 : 140

새로운 함수 inputKey()

void inputKey(Mat& dotMat, int& radius, int& circle_proportion, int& View_Angle, float& cos_theta,
        float& cos_theta_rad, float& hypotenuse, int& height, int Viewer_Y){
        Mat backupMat = dotMat.clone();
        putText(dotMat, "1. Circle size", Point(10, 900), FONT_HERSHEY_SIMPLEX, 0.8, Scalar(200, 200, 200), true);
        putText(dotMat, "2. View angle", Point(10, 930), FONT_HERSHEY_SIMPLEX, 0.8, Scalar(200, 200, 200), true);
        imshow("dotMat", dotMat);

        int key = waitKey(0);
        if(key == 49){
                putText(backupMat, "Circle size(1 ~ 5, Typical size 3)", Point(10, 900),
                        FONT_HERSHEY_SIMPLEX, 0.8, Scalar(200, 200, 200), true);
                imshow("dotMat", backupMat);

                radius = waitKey(0) - 48;
                if(radius == 1) circle_proportion = 14;
                else if(radius == 2)    circle_proportion = 40;
                else if(radius == 3)    circle_proportion = 70;
                else if(radius == 4)    circle_proportion = 100;
                else    circle_proportion = 200;

        }
        else if(key == 50){
                putText(backupMat, "View angle(10' ~ 80', Typical angle 45')", Point(10, 900),
                        FONT_HERSHEY_SIMPLEX, 0.8, Scalar(200, 200, 200), true);
                imshow("dotMat", backupMat);

                string putKey = to_string(waitKey(0) - 48);
                putText(backupMat, putKey, Point(10, 930),
                        FONT_HERSHEY_SIMPLEX, 0.8, Scalar(200, 200, 200), true);
                imshow("dotMat", backupMat);

                string putKey2 = to_string(waitKey(0) - 48);
                putText(backupMat, putKey2, Point(25, 930),
                        FONT_HERSHEY_SIMPLEX, 0.8, Scalar(200, 200, 200), true);
                imshow("dotMat", backupMat);
                waitKey(500);

                View_Angle = 180 - stoi(putKey + putKey2);
                cos_theta = View_Angle - 90.0;
                cos_theta_rad = cos_theta * M_PI / 180.0;
                hypotenuse = (Viewer_Y / 2) / cos(cos_theta_rad);
                height = hypotenuse * sin(cos_theta_rad);
        }
}

새로운 함수 inputKey()

 

Source Code

//	The new version Vision_Kart!!	//
//	Vision_Kart 2.0v	//
//	2024_03_25 ~ 27		//

//	Vision_Kart 2.1v	//
//	2024_03_28 ~ 28		//

//	Vision_Kart 2.2v	//
//	2024_03_29 ~ 30		//
/******************************///
#include <iostream>
#include <string>
#include "CYdLidar.h"
#include <core/base/timer.h>
#include <core/common/ydlidar_help.h>

#include <cmath>
#include <cstdlib>
#include <opencv2/opencv.hpp>

using namespace std;
using namespace ydlidar;
using namespace cv;

CYdLidar laser;
int c = 0;
uint32_t t = getms();

int ydlidarSet();
void draw_sight_center(Mat& drawMat, Point sight_center);
void draw_line(Mat& dotMat, Point sight_center, int Viewer_X, int Viewer_Y, int height);
void dotting(Mat& dotMat, int x, int y, int proportion_dis, int circle_radius);
vector<Rect> find_object(Mat& dotMat);
int setTarget(const vector<Rect>& boundingBoxs, Point sight_center);
int trkTarget(const vector<Rect>& boundingBoxs, Point ptTarget);
float find_distance(Point target_point, Point sight_center);
float find_angle(Point target_point, Point sight_center);
Point find_target_center(Rect target);
void putAngleSizeDistance(Mat& dotMat, float targetAngle, int targetSize, float targetDistance, Rect putPt);
void inputKey(Mat& dotMat, int& radius, int& circle_proportion, int& View_Angle, float& cos_theta,
	float& cos_theta_rad, float& hypotenuse, int& height, int Viewer_y);

int main(){
	if(!ydlidarSet()){
		cout << "ydLidar Setting Error!!" << endl;
		return -1;
	}

	//OpenCV Property
	/*********************/
	Point ptTarget;
	bool tracking_Flag = false;
	int circle_radius = 3;
	int circle_proportion = 55;
	int Viewer_X = 1000, Viewer_Y = 700;
	int View_Angle = 135;
	float cos_theta = View_Angle - 90.0;
	float cos_theta_rad = cos_theta * M_PI / 180.0;
	Point sight_center = Point(Viewer_Y / 2, Viewer_X);
	float hypotenuse = (Viewer_Y / 2) / cos(cos_theta_rad);
	int height = hypotenuse * sin(cos_theta_rad);

	namedWindow("dotMat");
	moveWindow("dotMat", 30, 30);

	LaserScan scan;
	while(os_isOk()){
		if(laser.doProcessSimple(scan)){
			Mat dotMat = Mat::zeros(Viewer_Y, Viewer_X, CV_8UC3);
			Point center = Point(dotMat.cols, dotMat.rows / 2);

			for(size_t i = 0; i < scan.points.size(); i++){
				const LaserPoint &p = scan.points.at(i);
				float Angle = p.angle * 180.0 / M_PI;
				if(Angle >= View_Angle || View_Angle * -1 >= Angle){
					float radian_Angle = p.angle;
					int dis = p.range * 100;
					int x = center.x + static_cast<int>(dis * cos(radian_Angle));
					int y = center.y + static_cast<int>(dis * sin(radian_Angle));

					int proportion_dis = Viewer_X - x;
					dotting(dotMat, x, y, proportion_dis, circle_radius);
				}
			}

			transpose(dotMat, dotMat);
			flip(dotMat, dotMat, 1);
			vector<Rect> boundingRects = find_object(dotMat);
			if(tracking_Flag && boundingRects.size() > 0){
				int targetIdx = trkTarget(boundingRects, ptTarget);
				rectangle(dotMat, boundingRects[targetIdx], Scalar(255, 255, 255), 2);

				float targetDistance = find_distance(find_target_center(boundingRects[targetIdx]), ptTarget);
				float targetAngle = find_angle(ptTarget, sight_center);
				int targetSize = boundingRects[targetIdx].width * boundingRects[targetIdx].height;
				putAngleSizeDistance(dotMat, targetAngle, targetSize, targetDistance, boundingRects[targetIdx]);

				if(circle_proportion > targetSize && abs(targetAngle) > 180 - View_Angle)
					cout << "out!!!!" << endl;

				ptTarget = find_target_center(boundingRects[targetIdx]);
			}

			int key = waitKey(1);
			if(key == 27)	break;
			else if(key == 32){
				int idxTarget = setTarget(boundingRects, sight_center);
				ptTarget = find_target_center(boundingRects[idxTarget]);
				tracking_Flag = !tracking_Flag;
			}
			else if(key == 13){
				inputKey(dotMat, circle_radius, circle_proportion, View_Angle,
					cos_theta, cos_theta_rad, hypotenuse, height, Viewer_Y);
				continue;
			}

			draw_line(dotMat, sight_center, Viewer_X, Viewer_Y, height);
			draw_sight_center(dotMat, sight_center);
			imshow("dotMat", dotMat);
		}
		else
			cerr << "Failed to get Lidar Data" << endl;
		if(!c++)
			cout << "Time consuming " << getms() - t <<
				" from initialization to parsing to point cloud data" << endl;
	}

	laser.turnOff();
	laser.disconnecting();

	return 0;
}

void inputKey(Mat& dotMat, int& radius, int& circle_proportion, int& View_Angle, float& cos_theta,
	float& cos_theta_rad, float& hypotenuse, int& height, int Viewer_Y){
	Mat backupMat = dotMat.clone();
	putText(dotMat, "1. Circle size", Point(10, 900), FONT_HERSHEY_SIMPLEX, 0.8, Scalar(200, 200, 200), true);
	putText(dotMat, "2. View angle", Point(10, 930), FONT_HERSHEY_SIMPLEX, 0.8, Scalar(200, 200, 200), true);
	imshow("dotMat", dotMat);

	int key = waitKey(0);
	if(key == 49){
		putText(backupMat, "Circle size(1 ~ 5, Typical size 3)", Point(10, 900),
			FONT_HERSHEY_SIMPLEX, 0.8, Scalar(200, 200, 200), true);
		imshow("dotMat", backupMat);

		radius = waitKey(0) - 48;
		if(radius == 1)	circle_proportion = 14;
		else if(radius == 2)	circle_proportion = 40;
		else if(radius == 3)    circle_proportion = 70;
		else if(radius == 4)    circle_proportion = 100;
		else	circle_proportion = 200;

	}
	else if(key == 50){
		putText(backupMat, "View angle(10' ~ 80', Typical angle 45')", Point(10, 900),
			FONT_HERSHEY_SIMPLEX, 0.8, Scalar(200, 200, 200), true);
		imshow("dotMat", backupMat);

		string putKey = to_string(waitKey(0) - 48);
		putText(backupMat, putKey, Point(10, 930),
                        FONT_HERSHEY_SIMPLEX, 0.8, Scalar(200, 200, 200), true);
		imshow("dotMat", backupMat);
		string putKey2 = to_string(waitKey(0) - 48);
		putText(backupMat, putKey2, Point(25, 930),
			FONT_HERSHEY_SIMPLEX, 0.8, Scalar(200, 200, 200), true);
		imshow("dotMat", backupMat);
		waitKey(500);

		View_Angle = 180 - stoi(putKey + putKey2);
		cos_theta = View_Angle - 90.0;
		cos_theta_rad = cos_theta * M_PI / 180.0;
		hypotenuse = (Viewer_Y / 2) / cos(cos_theta_rad);
		height = hypotenuse * sin(cos_theta_rad);
	}
}

void putAngleSizeDistance(Mat& dotMat, float targetAngle, int targetSize, float targetDistance, Rect targetRect){
	string angle_text = "Angle : " + to_string(targetAngle);
	string size_text = "Size : " + to_string(targetSize);
	string distance_text = "Distance : " + to_string(targetDistance) + "m";
	Point target_center = find_target_center(targetRect);

	putText(dotMat, angle_text, Point(target_center +
		Point(targetRect.width, targetRect.height * -1 - 40)),
		FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 255), true);
	putText(dotMat, size_text, Point(target_center +
		Point(targetRect.width, targetRect.height * -1 - 20)),
		FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 255), true);
	putText(dotMat, distance_text, Point(target_center +
		Point(targetRect.width, targetRect.height * -1)),
		FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 255), true);
}

float find_angle(Point target_point, Point sight_center){
	float compute_angle = 0.0;
	if(target_point.x != sight_center.x){
		Point dis_pt = target_point - sight_center;
		float slide_dis = sqrt(pow(dis_pt.x, 2) + pow(dis_pt.y, 2));
		compute_angle = abs(dis_pt.y) / slide_dis;
	}
	else	return 0.00;

	if(target_point.x > sight_center.x)	return acos(compute_angle) * (180.0 / M_PI);
	else	return acos(compute_angle) * -(180.0 / M_PI);
}

float find_distance(Point target_point, Point sight_center){
	Point dis_pt = target_point - sight_center;
	float slide_dis = sqrt(pow(dis_pt.x, 2) + pow(dis_pt.y, 2));

	return slide_dis;
}

int trkTarget(const vector<Rect>& boundingBoxs, Point ptTarget){
	//Index 0 is Sight_Circle so start index is Index 1
	int idxTarget = 1;
	float mostTargetDis = find_distance(find_target_center(boundingBoxs[idxTarget]), ptTarget);

	for(size_t i = 2; i < boundingBoxs.size(); i++){
		float compareTargetDis = find_distance(find_target_center(boundingBoxs[i]), ptTarget);
		if(compareTargetDis < mostTargetDis){
			idxTarget = i;
			mostTargetDis = compareTargetDis;
		}
	}

	return idxTarget;
}

int setTarget(const vector<Rect>& boundingBoxs, Point sight_center){
	//Index 0 is Sight_Circle so start index is Index 1
	int idxTarget = 1;
	float mostTargetDis = find_distance(find_target_center(boundingBoxs[idxTarget]), sight_center);

	for(size_t i = 2; i < boundingBoxs.size(); i++){
		float compareTargetDis = find_distance(find_target_center(boundingBoxs[i]), sight_center);
		if(compareTargetDis < mostTargetDis){
			idxTarget = i;
			mostTargetDis = compareTargetDis;
		}
	}

	return idxTarget;
}

Point find_target_center(Rect target){
	return Point(target.x + target.width / 2, target.y + target.height);
}

vector<Rect> find_object(Mat& dotMat){
	Mat bin_dotMat;
	cvtColor(dotMat, bin_dotMat, COLOR_BGR2GRAY);
	threshold(bin_dotMat, bin_dotMat, 1, 255, THRESH_BINARY);

	vector<vector<Point>> contours;
	vector<Vec4i> hierarchy;
	findContours(bin_dotMat, contours, hierarchy, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);

	vector<Rect> boundingBoxs;
	for (size_t i = 0; i < contours.size(); ++i)
		boundingBoxs.push_back(boundingRect(contours[i]));

	return boundingBoxs;
}

void dotting(Mat& dotMat, int x, int y, int proportion_dis, int circle_radius){
	if(proportion_dis >= 500)
		circle(dotMat, Point(x, y), circle_radius + 4, Scalar(139, 0, 0), -1);
	else if(proportion_dis >= 400)
		circle(dotMat, Point(x, y), circle_radius + 3, Scalar(0, 200, 0), -1);
	else if(proportion_dis >= 300)
		circle(dotMat, Point(x, y), circle_radius + 2, Scalar(0, 255, 255), -1);
	else if(proportion_dis >= 200)
		circle(dotMat, Point(x, y), circle_radius + 1, Scalar(0, 165, 255), -1);
	else
		circle(dotMat, Point(x, y), circle_radius, Scalar(0, 0, 255), -1);
}

void draw_sight_center(Mat& draw_mat, Point sight_center){
        circle(draw_mat, sight_center, 6, Scalar(0, 255, 0), -1);
}

void draw_line(Mat& dotMat, Point sight_center, int Viewer_X, int Viewer_Y, int height){
	line(dotMat, sight_center, Point(Viewer_Y, Viewer_X - height), Scalar(0, 0, 255), 3);
	line(dotMat, sight_center, Point(0, Viewer_X - height), Scalar(0, 0, 255), 3);
}

int ydlidarSet(){
	string port;
	os_init();

	map<string, string> ports = lidarPortList();
	map<string, string>::iterator it;

	if(ports.size() == 1)   port = ports.begin()->second;
	else{
		int id = 0;
		for (it = ports.begin(); it != ports.end(); it++)
		{
			printf("[%d] %s %s\n", id, it->first.c_str(), it->second.c_str());
			id++;
		}

		if(ports.empty()){
			cout << "Not Lidar was detected. Please enter the lidar serial port : ";
			cin >> port;
		}
		else{
			while(os_isOk()){
				cout << "Please select the lidar port : ";
				string number;
				cin >> number;

				if ((size_t)atoi(number.c_str()) >= ports.size())
					continue;

				it = ports.begin();
				id = atoi(number.c_str());

				while (id)
				{
					id--;
					it++;
				}

				port = it->second;
	                        break;
			}
		}
	}

	int baudrate = 230400;
	cout << "Baudrate : " << baudrate << endl;

	bool isSingleChannel = false;
	cout << "One-way communication : " << isSingleChannel  << endl;

	float frequency = 10.0;
	cout << "Frequency : " << frequency << "Hz" << endl;

	if(!os_isOk())  return -1;

	int optval = TYPE_TRIANGLE;
	string ignore_array;
	ignore_array.clear();
	laser.setlidaropt(LidarPropSerialPort, port.c_str(), port.size());
	laser.setlidaropt(LidarPropIgnoreArray, ignore_array.c_str(), ignore_array.size());
	laser.setlidaropt(LidarPropSerialBaudrate, &baudrate, sizeof(int));
	laser.setlidaropt(LidarPropLidarType, &optval, sizeof(int));
	optval = YDLIDAR_TYPE_SERIAL;
	laser.setlidaropt(LidarPropDeviceType, &optval, sizeof(int));
	optval = 5;
	laser.setlidaropt(LidarPropSampleRate, &optval, sizeof(int));
	optval = 4;
	laser.setlidaropt(LidarPropAbnormalCheckCount, &optval, sizeof(int));
	optval = 10;
	laser.setlidaropt(LidarPropIntenstiyBit, &optval, sizeof(int));

	bool b_optvalue = false;
	laser.setlidaropt(LidarPropFixedResolution, &b_optvalue, sizeof(bool));
	b_optvalue = false;
	laser.setlidaropt(LidarPropReversion, &b_optvalue, sizeof(bool));
	b_optvalue = false;
	laser.setlidaropt(LidarPropInverted, &b_optvalue, sizeof(bool));
	b_optvalue = true;
	laser.setlidaropt(LidarPropAutoReconnect, &b_optvalue, sizeof(bool));
	laser.setlidaropt(LidarPropSingleChannel, &isSingleChannel, sizeof(bool));
	b_optvalue = false;
	laser.setlidaropt(LidarPropIntenstiy, &b_optvalue, sizeof(bool));
	b_optvalue = true;
	laser.setlidaropt(LidarPropSupportMotorDtrCtrl, &b_optvalue, sizeof(bool));
	b_optvalue = false;
	laser.setlidaropt(LidarPropSupportHeartBeat, &b_optvalue, sizeof(bool));

	/// unit: °
	float f_optvalue = 180.0f;
	laser.setlidaropt(LidarPropMaxAngle, &f_optvalue, sizeof(float));
	f_optvalue = -180.0f;
	laser.setlidaropt(LidarPropMinAngle, &f_optvalue, sizeof(float));
	/// unit: m
	f_optvalue = 64.f;
	laser.setlidaropt(LidarPropMaxRange, &f_optvalue, sizeof(float));
	f_optvalue = 0.05f;
	laser.setlidaropt(LidarPropMinRange, &f_optvalue, sizeof(float));
	/// unit: Hz
	laser.setlidaropt(LidarPropScanFrequency, &frequency, sizeof(float));

	laser.enableGlassNoise(false);
	laser.enableSunNoise(false);
	laser.setBottomPriority(true);

	bool ret = laser.initialize();
	if(!ret){
		cerr << "Fail to initalize " << laser.DescribeError() << endl;
		return -1;
	}

	ret = laser.turnOn();
	if(!ret){
		cerr << "Fail to start "<< laser.DescribeError() << endl;
		return -1;
	}

	if(ret && os_isOk()){
		string userVersion;
		if(laser.getUserVersion(userVersion))
			cout << "User Version : " << userVersion.c_str() << endl;
	}

	if(ret){
		device_info di;
		memset(&di, 0, DEVICEINFOSIZE);
		if(laser.getDeviceInfo(di, EPT_Module)) core::common::printfDeviceInfo(di, EPT_Module);
		else    cout << "Fail to get module device info" << endl;
		if(laser.getDeviceInfo(di, EPT_Base))   core::common::printfDeviceInfo(di, EPT_Base);
		else    cout << "Fail to get baseplate device info" << endl;
	}

	return 1;
}