Skip to content

Commit 007eb5b

Browse files
author
cuixing158
committed
first commit
0 parents  commit 007eb5b

31 files changed

+3373
-0
lines changed

‎.gitignore

+40
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,40 @@
1+
# cuixingxing 定义的.gitgnore文件,参考：https://www.zhihu.com/question/26247816 ，以后项目依据此文件适当修改，2021.9.26
2+
# 文件夹前面必须要用"/"才奏效，2022.3.8
3+
#忽略所有文件和目录
4+
/*
5+
!.gitignore
6+
!*.json
7+
!*.md
8+
9+
# license
10+
!LICENSE
11+
12+
# python
13+
!*.py
14+
!*.ipynb
15+
16+
# matlab
17+
!*.m
18+
!*.mlx
19+
!*.slx
20+
!*.p
21+
22+
# c/c++
23+
!*.cpp
24+
!*.hpp
25+
!*.h
26+
!*.c
27+
28+
# cmake
29+
!CMakeLists.txt
30+
!Makefile
31+
32+
# 以下根据自己项目工程修改
33+
!/data/
34+
!/data/**/
35+
36+
!*.prj
37+
!*.txt
38+
!*.cgt
39+
!*.csv
40+
!*.xlsx

‎README.md

+14
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,14 @@
1+
# 基于先验里程计+车位检测实时构建costmap 2D地图及路径规划
2+
3+
![cost map gif](./data/mapOnLine.gif)
4+
5+
## C++ implementation
6+
7+
C++实现此[Repo](https://github.com/cuixing158/costmap_pathplan)同等功能!
8+
9+
## Reference
10+
11+
1. [Create 360° Bird's-Eye-View Image Around a Vehicle](https://www.mathworks.com/help/driving/ug/create-360-birds-eye-view-image.html)
12+
1. [Automated Parking Valet](https://www.mathworks.com/help/driving/ug/automated-parking-valet.html)
13+
14+
1. [Automated Parking Systems](https://www.mathworks.com/help/driving/automated-parking-systems.html)

‎YOLO.cpp

+155
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,155 @@
1+
#include "YOLO.h"
2+
3+
YOLO::YOLO(Net_config config) {
4+
this->confThreshold = config.confThreshold;
5+
this->nmsThreshold = config.nmsThreshold;
6+
this->xthr = config.xthr;
7+
this->ythr = config.xthr;
8+
this->objThreshold = config.objThreshold;
9+
10+
this->net = readNet(config.modelpath);
11+
ifstream ifs("./costMap/class.txt");
12+
std::string line;
13+
while (getline(ifs, line)) {
14+
printf("%s", line.c_str());
15+
this->class_names.push_back(line);
16+
}
17+
this->num_class = class_names.size();
18+
19+
/*if (endsWith(config.modelpath, "6.onnx"))
20+
{
21+
anchors = (float*)anchors_1280;
22+
this->num_stride = 4;
23+
this->inpHeight = 1280;
24+
this->inpWidth = 1280;
25+
}*/
26+
if (config.modelpath.find("ps")) {
27+
anchor_yolov5n[0][0][0] = 96;
28+
anchor_yolov5n[0][1][0] = 224;
29+
slot_stride = 32;
30+
this->inpHeight = 640;
31+
this->inpWidth = 640;
32+
}
33+
else {
34+
float anchor[3][3][2] = { 10, 13, 16, 30, 33, 23, 30, 61, 62, 45, 59, 119, 116, 90, 156, 198, 373, 326 };
35+
memcpy(anchor_yolov5n, anchor, sizeof(anchor));
36+
this->num_stride = 3;
37+
this->inpHeight = 640;
38+
this->inpWidth = 640;
39+
}
40+
}
41+
42+
Mat YOLO::resize_image(Mat srcimg, int* newh, int* neww, int* top, int* left) {
43+
int srch = srcimg.rows, srcw = srcimg.cols;
44+
*newh = this->inpHeight;
45+
*neww = this->inpWidth;
46+
Mat dstimg;
47+
if (this->keep_ratio && srch != srcw) {
48+
float hw_scale = (float)srch / srcw;
49+
if (hw_scale > 1) {
50+
*newh = this->inpHeight;
51+
*neww = int(this->inpWidth / hw_scale);
52+
resize(srcimg, dstimg, Size(*neww, *newh), INTER_AREA);
53+
*left = int((this->inpWidth - *neww) * 0.5);
54+
copyMakeBorder(dstimg, dstimg, 0, 0, *left, this->inpWidth - *neww - *left, BORDER_CONSTANT, 114);
55+
}
56+
else {
57+
*newh = (int)this->inpHeight * hw_scale;
58+
*neww = this->inpWidth;
59+
resize(srcimg, dstimg, Size(*neww, *newh), INTER_AREA);
60+
*top = (int)(this->inpHeight - *newh) * 0.5;
61+
copyMakeBorder(dstimg, dstimg, *top, this->inpHeight - *newh - *top, 0, 0, BORDER_CONSTANT, 114);
62+
}
63+
}
64+
else {
65+
resize(srcimg, dstimg, Size(*neww, *newh), INTER_AREA);
66+
}
67+
return dstimg;
68+
}
69+
70+
void YOLO::slot_detect_1280_720(Mat& src, vector<parkingSpaceInPixel>& spaces) {
71+
spaces.clear();
72+
cv::Mat frame = src.clone();
73+
if ((frame.rows != 720) || (frame.cols != 1280)) {
74+
std::cerr << "input bird eye image is not 1280*720." << std::endl;
75+
return;
76+
}
77+
int startX = frame.cols / 2 - 340;
78+
int startY = frame.rows / 2 - 340;
79+
frame = frame(Rect(startX, startY, 680, 680));
80+
// cv::imwrite("aa.jpg", frame);
81+
82+
int newh = 0, neww = 0, padh = 0, padw = 0;
83+
Mat dstimg = this->resize_image(frame, &newh, &neww, &padh, &padw);
84+
Mat blob = blobFromImage(dstimg, 1 / 255.0, Size(this->inpWidth, this->inpHeight), Scalar(0, 0, 0), true, false);
85+
this->net.setInput(blob);
86+
vector<Mat> outs;
87+
this->net.forward(outs, this->net.getUnconnectedOutLayersNames());
88+
int Input_w = frame.cols;
89+
int Input_h = frame.rows;
90+
int feat_width = dstimg.cols / slot_stride;
91+
int feat_height = dstimg.rows / slot_stride;
92+
int anchor_num = 2;
93+
int yoloOutNum = 1;
94+
int cls_num = this->num_class;
95+
float conf_thr = this->confThreshold;
96+
97+
vector<SlotInfo> slots;
98+
get_yolo_slot_output(outs, feat_width, feat_height, slot_stride, anchor_num, cls_num,
99+
conf_thr, yoloOutNum, Input_w, Input_h, anchor_yolov5n, slots);
100+
slot_nms(slots, this->xthr, this->ythr);
101+
102+
for (int iter = 0; iter < slots.size(); iter++) {
103+
int x1 = slots[iter].x1;
104+
int y1 = slots[iter].y1;
105+
int x2 = slots[iter].x2;
106+
int y2 = slots[iter].y2;
107+
float parkingSpaceWidth = std::sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
108+
float parkingSpaceLength = PARKING_SPACE_RATIO * parkingSpaceWidth;
109+
int x3 = slots[iter].cos2 * parkingSpaceLength + x2;
110+
int y3 = slots[iter].sin2 * parkingSpaceLength + y2;
111+
int x4 = slots[iter].cos2 * parkingSpaceLength + x1;
112+
int y4 = slots[iter].sin2 * parkingSpaceLength + y1;
113+
114+
#if SHOW_PARKING_DEBUG
115+
float score = slots[iter].score;
116+
if (slots[iter].label == 0) {
117+
// cv::fillPoly(srcimg,ppt,npt,1,cv::Scalar(0,255,0),0);
118+
cv::line(frame, cv::Point(x1, y1), cv::Point(x2, y2), cv::Scalar(0, 255, 0), 2);
119+
cv::line(frame, cv::Point(x1, y1), cv::Point(x4, y4), cv::Scalar(0, 255, 0), 2);
120+
cv::line(frame, cv::Point(x2, y2), cv::Point(x3, y3), cv::Scalar(0, 255, 0), 2);
121+
cv::circle(frame, cv::Point((x1 + x2) / 2, (y1 + y2) / 2), 3, cv::Scalar(255, 255, 0), 2);
122+
cv::putText(frame, std::to_string(score), cv::Point((x1 + x2) / 2, (y1 + y2) / 2 + 5), cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(0, 255, 0));
123+
cv::putText(frame, std::to_string(1), cv::Point(x1, y1), cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(0, 255, 0));
124+
cv::putText(frame, std::to_string(2), cv::Point(x2, y2), cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(0, 255, 0));
125+
cv::putText(frame, std::to_string(3), cv::Point(x3, y3), cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(0, 255, 0));
126+
}
127+
else if (slots[iter].label == 1) {
128+
cv::line(frame, cv::Point(x1, y1), cv::Point(x2, y2), cv::Scalar(0, 0, 255), 2);
129+
cv::line(frame, cv::Point(x1, y1), cv::Point(x4, y4), cv::Scalar(0, 0, 255), 2);
130+
cv::line(frame, cv::Point(x2, y2), cv::Point(x3, y3), cv::Scalar(0, 0, 255), 2);
131+
cv::circle(frame, cv::Point((x1 + x2) / 2, (y1 + y2) / 2), 3, cv::Scalar(255, 255, 0), 2);
132+
cv::putText(frame, std::to_string(score), cv::Point((x1 + x2) / 2, (y1 + y2) / 2 + 5), cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(0, 0, 255));
133+
// cv::fillPoly(srcimg,ppt,npt,1,cv::Scalar(0,0,255),0);
134+
}
135+
cv::imshow("yolo detect", frame);
136+
#endif // SHOW_PARKING_DEBUG
137+
138+
139+
parkingSpaceInPixel sp;
140+
sp.x1 = x1+ startX;
141+
sp.y1 = y1+ startY;
142+
sp.x2 = x2+ startX;
143+
sp.y2 = y2+ startY;
144+
sp.x3 = x3+ startX;
145+
sp.y3 = y3+ startY;
146+
sp.x4 = x4+ startX;
147+
sp.y4 = y4+ startY;
148+
149+
sp.label = slots[iter].label;
150+
sp.score = slots[iter].score;
151+
spaces.push_back(sp);
152+
}
153+
/*SlotTrack tracker;
154+
tracker.process(frame, slots);*/
155+
}

‎YOLO.h

+43
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,43 @@
1+
#pragma once
2+
#include <fstream>
3+
#include <sstream>
4+
#include <iostream>
5+
#include <opencv2/dnn.hpp>
6+
#include <opencv2/imgproc.hpp>
7+
#include <opencv2/highgui.hpp>
8+
#include "det_pp.h"
9+
#include "path.h"
10+
11+
using namespace cv;
12+
using namespace dnn;
13+
using namespace std;
14+
15+
class YOLO {
16+
public:
17+
YOLO(Net_config config);
18+
void detect(Mat& frame);
19+
void slot_detect_1280_720(Mat& frame, vector<parkingSpaceInPixel>& spaces);
20+
21+
//void slot_track(vector<SlotInfo> slots);
22+
float anchor_yolov5n[3][3][2];
23+
int OD_stride[3];
24+
int slot_stride;
25+
26+
private:
27+
float* anchors;
28+
int num_stride;
29+
int inpWidth;
30+
int inpHeight;
31+
vector<string> class_names;
32+
int num_class;
33+
34+
float confThreshold;
35+
float nmsThreshold;
36+
float xthr;
37+
float ythr;
38+
float objThreshold;
39+
const bool keep_ratio = true;
40+
Net net;
41+
void drawPred(float conf, int left, int top, int right, int bottom, Mat& frame, int classid);
42+
Mat resize_image(Mat srcimg, int* newh, int* neww, int* top, int* left);
43+
};

‎class.txt

+2
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,2 @@
1+
slot
2+
engaged

0 commit comments

Comments
 (0)