Vision_Kart 1.0V

※find_angle() 함수 각도 값 수정 필요

마지막 포스팅을 찾아보니 OpenCV 환경에서 라이다 데이터 값을 Dotting까지 했다.

(중간중간 3~4번 정도 더 해야 했어야 하는데 미뤄졌다..)

 

지난주 미팅에서 이 정도면 프로토 타입 정도 된다는 얘기를 듣고 거기서 조금? 더 업데이트된 이번 포스팅에서 소개할 기능을 Vision_Kart 1.0v으로 지정하게 됐다.

 

Lidar with OpenCV를 시작한지 한 달이 지났다. (Start from 2024-02-30)위에서 말한 대로 꽤 많이 진행됐다.물론 비전 파트가 남아있긴 하지만.. 라이다만 보면 크게는 필더링, 안정화만 남은 것 같다.

 

Vision_Kart 1.0v

Environment of Vision_Kart

Processor : RaspberryPi 4

OS : Raspbian

Lidar : YDLidar G2

Lidar Frequency : 12Hz

 

Function of Vision_Kart 1.0v

• Lidar viewer
• Sight angle variable
• Filtering for object tracking
• Object(target) tracking
• Show target angle & distance
• Adaptive circle size for proportion distance(possible any color)

 

Vision_Kart 1.0v Testing

1. Adaptive circle size for proportion distance

Figure 1은 거리 비례로 색 & Dotting 사이즈를 조절한 결과이다.

사실 색은 내가 알아보기 편하게 하기 위해 한 것이고 큰 의미는 없다..

거리 비례로 Dotting 사이즈를 조절해야 멀리 있는 물체도 하나의 객체로 인식하고, 가까이 있는 물체가 너무 큰 객체로 인식되는 문제를 해결할 수 있다.

Figure 1

 

2. Targeting with target angle, distance, size

Figure 2는 현재는 적용되지 않았지만 문제가 발생한 화면이다.

아래 실행 프로그램에서는 단순히 거리를 가지고 판단하는 것이다.

그 결과 파란색 테두리에 있는 객체가 목표물인데 영상처리에서는 초록색 테두리 객체를 잡고있다. (이런 상황은 내가 테스트 중에 가장 많이 일어났던 오류이다.)

이를 해결하기 위해 여러가지 방법을 시도 중인데 가장 유력한 방법 중 하나인 Angle, Distance, Size를 모두 관여하여 필터링 하는 것이다.

위 솔루션은 Vision_Kart 1.1v에 적용 예정이다.

Figure 2

Source Code of Vision_Kart 1.0v

// First YDlidar control with C++
// 2024-02-20
// based from tri_test.cpp

// YDLidar with OpenCV
// 2024-02-22

// Sight Line with OpenCV
// 2024-02-25

// Filter & Draw
// 2024-02-27

// Lidar Tracking
// 2024-02-28

// Show angle & real_distance
// 2024-03-13

// Adaptive circle size for proportion distance(possible any color)
// 2024-03-19
/*****************************/

#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;

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

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

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) * 100;
	else	return acos(compute_angle) * -100;
}

float find_distance(Point target_point, Point sight_center){
	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));

		return slide_dis;
	}
	else	return abs(target_point.y - sight_center.y);
}

//MI : Most Important
//Only Distance
void MI_Size_Tracking(Mat& dotMat, vector<Rect> Signi_Rect, Point sight_center){
	vector<int> MIST_List;
	vector<float> Signi_Pt;
	MIST_List.clear();
	Signi_Pt.clear();

	if(Signi_Rect.size() > 0){
		for(int i = 0; i < Signi_Rect.size(); i++){
			Point temp = Point(Signi_Rect[i].x + (Signi_Rect[i].width / 2),
				Signi_Rect[i].y + Signi_Rect[i].height);

			Point margin_temp = sight_center - temp;
			float dis = sqrt(pow(margin_temp.x, 2) + pow(margin_temp.y, 2));
			Signi_Pt.push_back(dis);
		}

		int min_idx = min_element(Signi_Pt.begin(), Signi_Pt.end()) - Signi_Pt.begin();
		Point target_center = find_target(Signi_Rect[min_idx]);
		string angle_text = "Angle : " + to_string(find_angle(target_center, sight_center));
		string distance_text = "Distance : " + to_string(find_distance(target_center, sight_center));

		putText(dotMat, angle_text, Point(target_center +
			Point(Signi_Rect[min_idx].width, Signi_Rect[min_idx].height * -1 - 20)),
			FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 255), true);
		putText(dotMat, distance_text, Point(target_center +
			Point(Signi_Rect[min_idx].width, Signi_Rect[min_idx].height * -1)),
			FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 255), true);
		rectangle(dotMat, Signi_Rect[min_idx], Scalar(0, 255, 0), 2, -1);
		circle(dotMat, target_center, 3, Scalar(0, 0, 255), -1);
	}
}

void FilterAndTracking(Mat& dotMat, float circle_area, Point sight_center, Point min_max_area){
	Mat gray_dotMat;
	vector<vector<Point>> contours;
	vector<Vec4i> hierarchy;
	cvtColor(dotMat, gray_dotMat, COLOR_BGR2GRAY);

	findContours(gray_dotMat, contours, hierarchy, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);

	float dis;
	vector<float> dis_vector;
	vector<Rect> Signi_Rect;
	dis_vector.clear();
	Signi_Rect.clear();

	for(size_t i = 0; i < contours.size(); i++){
		float area = contourArea(contours[i]);
		if(area >= circle_area * min_max_area.x && circle_area * min_max_area.y >= area){
			Rect boundRect = boundingRect(contours[i]);
			Point temp = Point(boundRect.x + (boundRect.width / 2),
				boundRect.y + boundRect.height);
			Point margin_temp = sight_center - temp;
			dis = sqrt(pow(margin_temp.x, 2) + pow(margin_temp.y, 2));
			if(300.0 >= dis){
				Signi_Rect.push_back(boundRect);
				dis_vector.push_back(dis);

				rectangle(dotMat, boundRect, Scalar(255,0 ,0), 2);
			}
		}
	}

	MI_Size_Tracking(dotMat, Signi_Rect, sight_center);
}

int main(){
	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 = 12.0;
	cout << "Frequency : " << frequency << "Hz" << endl;

	if(!os_isOk())	return -1;

	CYdLidar laser;
	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);

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

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

	//OpenCV Property
	//Variable at Green//
	/*	circle_radius => dotting size
		min_max_area => borderline
		View_Angle => sight angle
	*/
	/*********************/
	int circle_radius = 2;
	float circle_area = circle_radius * circle_radius * M_PI;
	int Viewer_X = 1000, Viewer_Y = 700;
	float View_Angle = 135.0;
	float cos_theta = View_Angle - 90.0;
	float cos_theta_rad = cos_theta * M_PI / 180.0;
	float height = Viewer_Y * tan(acos(View_Angle));
	Point sight_center = Point(Viewer_Y / 2, Viewer_X);

	Point min_max_area = Point(6, 15);

	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;
					if(proportion_dis >= 500)
						circle(dotMat, Point(x, y), 6, Scalar(0, 0, 255), -1);
					else if(proportion_dis >= 400)
						circle(dotMat, Point(x, y), 5, Scalar(0, 0, 175), -1);
					else if(proportion_dis >= 300)
						circle(dotMat, Point(x, y), 4, Scalar(0, 175, 0), -1);
					else if(proportion_dis >= 200)
						circle(dotMat, Point(x, y), 3, Scalar(255, 175, 0), -1);
					else
						circle(dotMat, Point(x, y), 2, Scalar(255, 0, 255), -1);

//					circle(dotMat, Point(x, y), circle_radius, Scalar(255, 255, 255), -1);
				}
			}

			transpose(dotMat, dotMat);
			flip(dotMat, dotMat, 1);

			//Filter & Draw
			FilterAndTracking(dotMat, circle_area, sight_center, min_max_area);

			//draw angle of sight
			float hypotenuse = (Viewer_Y / 2) / cos(cos_theta_rad);
			int height = hypotenuse * sin(cos_theta_rad);
			draw_line(dotMat, sight_center, Viewer_X, Viewer_Y, height);
			draw_sight_center(dotMat, sight_center);

			imshow("dotMat", dotMat);
			int key = waitKey(10);
			if(key == 27)	break;
		}
		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;
}