Before start Vision_Kart 2.xv

이전 포스팅까지는 Vision_Kart 1.x를 개발, 테스트했다.
하지만 연구실과 집에서 테스트 결과 연구실에서는 동작이 안정적으로 작동했지만 집에서는 오류를 일으키는 상황이 생각보다 많이 일어났다. 1.x 버전 알고리즘을 가만히 생각을 해보니 극단적인 상황에서는 오류가 일어날 수 있는 알고리즘이란 걸 알고 아예 새롭게 2.x 버전을 개발하기로 했다.

 

기존에 생각하지 못했던 상황이 추가됐다.

• Target이 시야 사이드로 중간에 사라진 경우
• 현장에서 변수 변경 가능(특정 키 입력)

 

현재 생각나는 아이디어는 알고리즘 자체는 좀 더 단순해질 것 같다.

물론, 수많은 테스트를 해봐야 된다.

 

특정 버튼을 누르면 위 그림처럼 가장 가까운 객체를 인식하게된다.

타겟지정

 

외곽선까지 추출하는 것은 이전 1.x 버전과 동일하지만 이후에 처리 알고리즘은 완전 바뀐다.

추출된 외곽선을 Rect로 묶고 기본적인 필터를 거친다.(사이즈 필더링)

그 후에 중심점이 가장 가까운 Rect를 팔로잉 하게 된다.

물론 그 밖에 이것저것 추가 필터링이 있을 것인데 아직 개발 전이라 자세한 내용은 다음 포스팅에서..

화살표만큼 이동

Opimization Base code

Only Visualization & Color dotting

Vision_Lidar.cpp

0.01MB

//	Every thing renewal	//
//	Target disappeared midway to the side of the field of view
//	Variables can be changed on site (enter specific keys)

//	Vision_Kart 2.0v	//
//	Start 2024_03_25	//
/******************************///

#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& draw_mat, Point sight_center);
void draw_line(Mat& dotMat, Point sight_center, int Viewer_X, int Viewer_Y, int height);
void dotting(Mat& draw_mat, int x, int y, int proprtion_dis);
Point find_target(Rect target);

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

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

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

			transpose(dotMat, dotMat);
			flip(dotMat, dotMat, 1);
			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;
}

void dotting(Mat& dotMat, int x, int y, int proportion_dis){
	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(175, 175, 0), -1);
	else
		circle(dotMat, Point(x, y), 2, Scalar(175, 255, 175), -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);
}

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

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 = 12.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;
}