CELLON Kart(Tracking)
Vision_Kart 2.1v
박민혀기
2024. 3. 28. 18:33
※find_angle() 함수 각도 값 수정 필요
Vision_Kart 2.1v
Vision_Kart 2.1v에 기능은 예외처리 기능만 추가 되었다.
기존 환경 또한 동일하다.
Environment of Vision_Kart
Processor : RaspberryPi 4
OS : Raspbian
Lidar : YDLidar G2
Lidar Frequency : 10Hz ~ 12Hz(recommend 7Hz)
New function
- handling exceptions(If Target going outside, Vision_Kart start Auto-Disconnection.)
Goals
만약 타겟이 라이다 시야 범위를 벗어난 경우 예외 인터럽트 처리 기능이다.(실제 인터럽트 X, 표현만 인터럽트)
(타겟이 먼 거리를 가서 시야에서 사라지는 경우는 있을 수 없다고 가정하고, 0.12 ~ 10m 사이 시야 각도에서 벗어난 경우에 초점을 맞췄다. 알고리즘 이론상 전자에 경우에도 올바르게 예외 처리가 된다.)
Algorithm
처음 시도했던 방식은 가장 단순하게 이전 타겟과의 거리 값이 급격하게 증가하게되면 인터럽트 발생으로 구현하였다.
하지만 생각보다 정교하게 처리되지 못했다.
우선 튀는 값들의 편차가 제각각이었다. 아래는 이전 타겟과의 거리가 15이상일 경우에만 출력한 것이다.
15.0333cm
15.2971cm
40.6079cm // Jump!
16.0312cm
15.5563cm
15.6205cm
15.1327cm
16.1245cm
25.0799cm
40.7063cm // Jump!
18.0278cm
20.2485cm
42.0119cm // Jump!
23.7065cm
133.27cm //Interrupt!
75.7166cm
71.0282cm위 테스트 환경은 장애물이 없던 경우였다. 따라서 이전 타켓과의 거리가 110cm 정도 차이나는 것을 확인할 수 있다.
물론 근처 장애물이 없으면 거리로만 처리해도 충분할 것이다. 하지만 장애물이 많은 경우에는 위 결과보다 훨씬 적은 40, 50cm 정도도 충분히 나올 수 있다.
당장 Jump한 경우(순간적으로 100cm 이상 움직인 상황)에도 크게는 25cm 이상 차이가 난다.
(공간이 넓없다면 더 큰 값이 나올 것이다.)
그래서 선택하게된 방법이 각도 값 참조이다.
우선 타겟이 옆으로 사라질 때 각도를 구해야한다.
각도 연산은 1.x버전에서 사용했던 find_angle() 함수를 불러와 사용했다.
//float error = find_distance(find_target_center(boundingRects[targetIdx]), ptTarget);
//if(error > 15) cout << error << endl;
float error = find_angle(ptTarget, sight_center);
string angle_text = "Angle : " + to_string(error);
putText(dotMat, angle_text, Point(50, 50), FONT_HERSHEY_SIMPLEX, 1,
Scalar(255, 255, 255), true);
현재 시야를 -45º ~ 45º로 설정한 상태이다.
객체가 선 가까이 가면 45º ~ 49º까지 나온다. 이제 이걸 활용해서 예외 상황을 잡아야한다.
알고리즘은 여러가지가 있다. 단순하게 생각해 보면 이렇게 네 가지가 생각난다.
- 거리
- 각도
- 각도, 거리
- 사이즈
내가 생각했을 때에는 거리를 추가하면 오히려 오작동 상황이 더 많아질 것 같다. 실제로 1번은 위에서 시도해 봤지만 불안정했다. 사이즈 활용을 늦게 생각해 냈는데 가장 안정적일 수도 있을 것이라고 생각했다.
하지만 각도, 사이즈를 각각 테스트해본 결과 분명히 약점은 존재했고, 그에 따라 각도, 사이즈를 둘 다 필터에 거치면 정말 견고할 수도 있겠다는 생각을 했다.
if(60 > targetSize && error > 45)임계값을 이렇게 설정하고 해봤더니 완벽하게 구현됐다.!(생각보다 정말 안정적으로 동작한다)
점프도 해보고 달려가기도 하고 천천히 움직이기도 했는데 정말 다 성공했다.
Size & Angle : 49, 46.8965
Size & Angle : 49, 48.0128
Size & Angle : 49, 49.0283
Size & Angle : 49, 47.5336대충 이런수치에서 프로그램이 종료되었다.
Angle 값은 Rect의 사이즈가 크면 조금의 오차는 생기는 것 같은데
Size 값은 소수점을 제외하고 49로 일정한 것을 확인할 수 있다.
우선 이정도에서 Vision_Kart 2.1v을 마무리 해도 될 것 같다.
Source Code
// The new version Vision_Kart!! //
// Vision_Kart 2.0v //
// 2024_03_25 ~ 27 //
// Vision_Kart 2.1v //
// 2024_03_28 ~ 28 //
/******************************///
#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);
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);
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(51 > targetSize && targetAngle > 45){
cout << "Size & Angle : " << targetSize << ", " << targetAngle << endl;
break;
}
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]);
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;
}
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 90 - (asin(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 = 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;
}