박민혀기
Vision_Kart 2.0v 본문
Vision_Kart 2.0v
이제 본격적으로 2.0 개발을 시작한다.
솔직히 말하면 1.x 개발보다는 복합하거나 시간이 많이 소요될 것 같지는 않다.
물론, 테스트를 계속해보면서 수정해야겠지만..
테스트 환경이나 프로세서 환경을 기존과 같고 트레킹하는 알고리즘만 교체된다.
Environment of Vision_Kart
Processor : RaspberryPi 4
OS : Raspbian
Lidar : YDLidar G2
Lidar Frequency : 12Hz ~ 10Hz
Function of Vision_Kart 2.0v
- Lidar viewer
- Sight angle
- Adaptive circle size for proportion distance(possible any color)
- Target tracking(New Algorithm)
- Texting the angle, distance from target
- Variable edit
Goals
특정 버튼을 누르게 되면(테스트 단계에서는 Space Key로 지정) 가장 가까운 물체를 타겟으로 지정하게 되고 지속 적으로 팔로잉하게 된다. 하지만 외곽으로 벗어났다고 판단이 되면 종료!(추후에는 3초 룰 수행할 수도 있음)
Algorithm
외곽선 추출을 위해 바이너리 이미지로 변환한다.
아주 기본적인 코드로 구현 가능
cvtColor(dotMat, gray_dotMat, COLOR_BGR2GRAY);
threshold(gray_dotMat, gray_dotMat, 1, 255, THRESH_BINARY);
외곽선 추출 후 drawContours한 모습
지금 상태로는 뭐가 어떤 건지 명확하게 알아보기 힘들다.
BoundingRect를 활용하여 Rectangle 단위로 객체화가 가능해졌다.
위 두 사진은 동일한 프레임인데 조금은 다른 결과를 보여주고 있다.
동영상을 따로 녹화해서 살펴보니 오른쪽 그림이 아주 미세하게 더 빠른 것으로 확인됐다.
아마 find_object 함수를 호출하는 과정에서 주소연산자로 호출해서 그런 것 같다.
하지만 이 정도 딜레이는 무시해도 상관없을 것 같다. 나름 분석을 성공한 것에 의미를 둔다..
이젠 각 객체들을 여러 필터링과 알고리즘을 통해 내가 원하는대로 처리가 가능해진 상태가 됐다.
목표에서 얘기한 것처럼 특정 키(SpaceBar)를 입력하면 가장 가까운 객체를 타겟으로 지정해야한다.
아래 소스코드는 가장 가까운 객체를 추출하는 소스코드이다.
Vision_Kart 1.xv에서 사용됐던 find_distance와 find_target_center 함수를 가져와 사용했다.
복잡해 보이지만 그냥 거리 하나하나를 비교해 가장 작은 값을 구하는 코드이다.
# 여기서 인덱스 값의 시작 번째를 0으로 하지 않고 1로 한 이유는 이 라이다의 특징과 관련 있다.
# YDLidar는 값을 가져올 때 라이다가 있는 곳(정중앙)에도 값을 하나 가져온다.
# 그곳이 0번째 인덱스이다.
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;
}
테스트 해보니 이상한 현상이 일어났다.
아래 그림처럼 더 뒤에 있는 객체를 감지해서 타겟으로 지정한 것이다.
이것저것 테스트, 코드를 수정해보다가 내가 거리 기준을 사각형의 정중앙을 잡아서 그런 것이다.
※실제로는 정중앙이 아니라 Rect.x, Rect.y로 왼쪽 상단을 지정했다.. 이 코드 때문에 정말 몇 시간은 삽질했다..
디버깅 방법은 순간적으로 변화하는 point 값을 출력하여 어떻게 바뀌고 왜 바뀌는지를 하나하나 분석했다.
그다음은 처음 타겟을 정한 순간 그 타겟의 좌표를 저장해야한다.
그런데 이 과정을 최적화하고 압축 하려면 어떻게 해야할지 고민이 많이 됐다.
전역변수로 할지 아니면 지역변수로 할지.....
그리 복잡한 구조가 아니여서 지역변수로 하기로 했다. (사실 코딩할 때 변수가 시도 때도 없이 함수 매개변수로 호출되거나 굉장히 복잡하지 않는 이상 지역변수로 선언한다.)
변수는 변수인데 어떤 자료형으로 처리할지도 고민이 된다. (Point, int, Rect 등이 있지만 int로 인덱스르 알려주고 vector<Rect>에서 찾아 가는 방식으로 했다.)
코드는 위에 setTarget 함수와 비슷하다 단지 sight_center 포인트가 이전 타겟 포인트로 변화된 것 밖에 없다.
위 사진은 원활한 동작을 보이고 있으며 이론상 장애물 뒤로 숨어도 감지가 가능하다.(물론, 라이다 하드웨어 특성상 불가능하지만..) 지금 포스팅하는 동안에도 안정적으로 나를 디텍션 하고 있다.
Source Code
// The new version Vision_Kart!! //
// Vision_Kart 2.0v //
// 2024_03_25 ~ 27 //
/******************************///
#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);
Point find_target_center(Rect target);
int main(){
if(!ydlidarSet()){
cout << "ydLidar Setting Error!!" << endl;
return -1;
}
//OpenCV Property
/*********************/
Point ptTarget;
bool tracking_Flag = false;
int circle_radius = 3;
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);
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);
ptTarget = find_target_center(boundingRects[targetIdx]);
}
int key = waitKey(10);
if(key == 27) break;
else if(key == 32){
int idxTarget = setTarget(boundingRects, sight_center);
ptTarget = find_target_center(boundingRects[idxTarget]);
cout << ptTarget << endl;
cout << find_distance(ptTarget, sight_center) << endl;
tracking_Flag = !tracking_Flag;
}
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;
}
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 = 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;
}'CELLON Kart(Tracking)' 카테고리의 다른 글
| Vision_Kart 2.1v (0) | 2024.03.28 |
|---|---|
| Vision_Kart 2.xv MasterPlan (0) | 2024.03.28 |
| Before start Vision_Kart 2.xv (0) | 2024.03.25 |
| Vision_Kart 1.1V (0) | 2024.03.23 |
| Vision_Kart 1.0V (0) | 2024.03.19 |
'CELLON Kart(Tracking)' Related Articles
more
