YDLidar Application with OpenCV C++

이번 포스팅에서는 이전까지 YDLidar-SDK에서 제공되는 SW로만 확인하고 그것을 실제 시각화하여 처리하기 위해 OpenCV로 간단한 Viewer를 만들었다.(단순 Viewer지만 OpenCV를 통해 영상처리가 가능하다!!)

 

환경 : Raspberry Pi 4 Model B(Raspbian 32bit)

조건 : YDLidar-SDK & OpenCV must be installed.

※YDLidar-SDK install guide

https://forthe-future.tistory.com/110

※OpenCV 4.5.1 install guide

https://forthe-future.tistory.com/2

RaspiberryPi(CM4)에 OpenCV 4.5.1 Install [C++ 환경]

 

반드시 위에 두 단계가 정상적으로 설치된 뒤에 진행하길 바란다!!

사실 컴파일 방법(CMakeLists.txt)과 기본 소스코드는 매우 유사하다.

CMakeLists.txt

CMakeLists.txt에서는 OpenCV 라이브러리를 찾아 링크하는 부분 말곤 바뀐 곳이 없다.

cmake_minimum_required(VERSION 2.8)
project(Vision_Lidar)
add_compile_options(-std=c++11) # Use C++11

# Include directories
include_directories(
     ${CMAKE_SOURCE_DIR}
     ${CMAKE_SOURCE_DIR}/../../
     ${CMAKE_CURRENT_BINARY_DIR}
)

set(EXECUTABLE_OUTPUT_PATH ${CMAKE_BINARY_DIR})

set(APP_NAME Vision_Lidar)

find_package(OpenCV REQUIRED)

add_executable(${APP_NAME} ../Vision_Lidar.cpp)

target_link_libraries(${APP_NAME} ${OpenCV_LIBS} ydlidar_sdk pthread)

install(TARGETS ${APP_NAME}
    RUNTIME DESTINATION bin
)

소스코드

소스코드도 어렵고 많이 바뀐게 없다.

간단히 말해 검은 도화지(1000, 1000)를 만든 뒤에 라디안(Angle)과 거리(dis)를 계산하기 편하게 만들고 도화지 위에 하나하나 dotting하면 된다. (dot point 위치는 cos, sin를 활용하면 도출이 가능하다.)

그 외에 코드는 같다.

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

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

#include <iostream>
#include <string>
#include "CYdLidar.h"
#include <core/base/timer.h>
#include <core/common/ydlidar_help.h>

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

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

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 = true;
	cout << "One-way communication : " << isSingleChannel  << endl;

	float frequency = 5.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;
	}

        namedWindow("dotMat", WINDOW_AUTOSIZE);

	LaserScan scan;
	while(os_isOk()){
		if(laser.doProcessSimple(scan)){
/*			cout << "Scan received [" << scan.points.size() <<
				"] points scanFreq [" << scan.scanFreq << ']' << endl;

			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 > -90.0 && Angle < 90.0){
					cout << i << " " << "dis : " << p.range << ", angle : " <<
						Angle << endl;
				}
*/

			Mat dotMat(1000, 1000, CV_8UC1);
			Point center = Point(500, 500);
			dotMat.setTo(Scalar(0));

			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;
				float Angle = p.angle;
				int dis = p.range * 100;
//				cout << "dis : " << p.range << ", angle : " <<
//					Angle << endl;

				int x = center.x + static_cast<int>(dis * cos(Angle));
				int y = center.y + static_cast<int>(dis * sin(Angle));

				circle(dotMat, Point(x, y), 1, Scalar(255), -1);
			}
            
            transpose(dotMat, dotMat);
			flip(dotMat, dotMat, 1);
			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;
}

 

 

실행화면

각도가 90도 틀어진 것만 개선하면 좋을 것 같다.

자세한 성능은 계속 테스트할 예정..