├── image
    ├── bev1.jpg
    ├── .DS_Store
    ├── segment.jpg
    └── freespace1.jpg
├── CMakeLists.txt
├── tic_toc.h
├── README.md
├── FreeSpaceDetector.h
├── main.cpp
└── FreeSpaceDetector.cpp


/image/bev1.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/tiger20/FreeSpaceDetect/HEAD/image/bev1.jpg


--------------------------------------------------------------------------------
/image/.DS_Store:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/tiger20/FreeSpaceDetect/HEAD/image/.DS_Store


--------------------------------------------------------------------------------
/image/segment.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/tiger20/FreeSpaceDetect/HEAD/image/segment.jpg


--------------------------------------------------------------------------------
/image/freespace1.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/tiger20/FreeSpaceDetect/HEAD/image/freespace1.jpg


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8)
 2 | set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )
 3 | 
 4 | project(freespace_detect)
 5 | 
 6 | find_package(PCL 1.8 REQUIRED)
 7 | find_package(OpenCV REQUIRED)
 8 | 
 9 | include_directories(${PCL_INCLUDE_DIRS})
10 | link_directories(${PCL_LIBRARY_DIRS})
11 | add_definitions(${PCL_DEFINITIONS})
12 | 
13 | include_directories("/usr/local/include/eigen3")
14 | 
15 | add_executable(freespace_detect main.cpp FreeSpaceDetector.cpp)
16 | target_link_libraries(freespace_detect ${PCL_LIBRARIES} ${OpenCV_LIBS})


--------------------------------------------------------------------------------
/tic_toc.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <ctime>
 4 | #include <cstdlib>
 5 | #include <chrono>
 6 | 
 7 | class TicToc
 8 | {
 9 |   public:
10 |     TicToc()
11 |     {
12 |         tic();
13 |     }
14 | 
15 |     void tic()
16 |     {
17 |         start = std::chrono::system_clock::now();
18 |     }
19 | 
20 |     double toc()
21 |     {
22 |         end = std::chrono::system_clock::now();
23 |         std::chrono::duration<double> elapsed_seconds = end - start;
24 |         return elapsed_seconds.count() * 1000;
25 |     }
26 | 
27 |   private:
28 |     std::chrono::time_point<std::chrono::system_clock> start, end;
29 | };
30 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # FreeSpaceDetect
 2 | Free space detection for automated vehicles
 3 | 
 4 | Prerequisites:
 5 | + OpenCV
 6 | + PCL
 7 | + Eigen
 8 | 
 9 | It's an implementation of a simple idea. I want to efficiently plan a region where the vehicle is free to go. The algorithm only uses a small subset of the Kitti lidar data per frame since most of the point cloud is redundant for this task.
10 | The algorithm only need about 6ms to locate the road plane and estimate the curve. With the file reading and display stuff, it can take longer in total.
11 | 
12 | The idea is quite simple, first we do the segmentation to locate the road plane.
13 | 
14 | <center>
15 |     <img src="https://github.com/tiger20/FreeSpaceDetect/blob/master/image/segment.jpg" width="45%">
16 | </center>
17 | 
18 | Then convert the rest of point cloud to BEV view and after a set of image processing techniques we extract the edge points of the free zone and fit them with two smooth curves.
19 | 
20 | <center>
21 |     <img src="https://github.com/tiger20/FreeSpaceDetect/blob/master/image/bev1.jpg" width="45%">
22 | </center>
23 | 
24 | Also we can project the curve onto the image with the knonw calibration parameters, which looks cooler.
25 | 
26 | <center>
27 |     <img src="https://github.com/tiger20/FreeSpaceDetect/blob/master/image/freespace1.jpg" width="60%">
28 | </center>


--------------------------------------------------------------------------------
/FreeSpaceDetector.h:
--------------------------------------------------------------------------------
 1 | #include <boost/thread/thread.hpp>
 2 | #include <pcl/common/common_headers.h>
 3 | #include <pcl/features/normal_3d.h>
 4 | #include <pcl/visualization/pcl_visualizer.h>
 5 | #include <pcl/console/parse.h>
 6 | #include <pcl/io/pcd_io.h>
 7 | #include <pcl/point_types.h>
 8 | 
 9 | #include <opencv2/opencv.hpp>
10 | #include <opencv2/core/core.hpp>
11 | 
12 | #include <eigen3/Eigen/Dense>
13 | 
14 | #include <iostream>
15 | #include <vector>
16 | #include <cmath>
17 | #include <algorithm>
18 | #include "tic_toc.h"
19 | using namespace std;
20 | using namespace cv;
21 | using namespace Eigen;
22 | 
23 | //const double PI = 3.14159265;
24 | extern bool Debug;
25 | struct LidarParam
26 | {
27 |     int N_SCAN;
28 |     int Horizon_SCAN;
29 |     float ang_res_x;
30 |     float ang_res_y;
31 |     float ang_bottom;
32 |     float sensorMountAngle;
33 |     int groundScanInd;
34 |     LidarParam(){
35 |         N_SCAN = 64;
36 |         Horizon_SCAN = 4500;
37 |         ang_res_x = 0.08;
38 |         ang_res_y = 26.9/63;
39 |         ang_bottom = -24.9f;
40 |         groundScanInd = 60;
41 |         sensorMountAngle = 0.0;
42 |     }
43 |     LidarParam(int ns, int hs, float rx, float ry, float aglb, int grs, float mountAngle){
44 |         N_SCAN = ns;
45 |         Horizon_SCAN = hs;
46 |         ang_res_x = rx;
47 |         ang_res_y = ry;
48 |         ang_bottom = aglb;
49 |         groundScanInd = grs;
50 |         sensorMountAngle = mountAngle;
51 |     }
52 | };
53 | struct Edge{
54 |     Vector3d coeffs;
55 |     Vector2d startpt;
56 |     Vector2d endpt;
57 | };
58 | class FreeSpaceDetector{
59 | public:
60 |     LidarParam ldr_param;
61 |     pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudRaw;
62 |     pcl::PointCloud<pcl::PointXYZI>::Ptr sparseCloud;
63 |     pcl::PointCloud<pcl::PointXYZRGB>::Ptr segResult;
64 |     Vector3d plane;
65 |     Mat frame;
66 |     Mat bev;
67 |     Mat labels;
68 |     int sprsWidth;
69 |     int sprsHeight;
70 |     double fov;
71 |     FreeSpaceDetector();
72 |     void setData(Mat& img, pcl::PointCloud<pcl::PointXYZI>::Ptr cloud);
73 |     void setParam(LidarParam param);
74 |     bool getFreeSpace();
75 |     void projectCurveOnImage(Mat& img, const Matrix3d& H, Vector3d coeffs0, Point2f startpt0, Point2f endpt0,
76 |                                             Vector3d coeffs1, Point2f startpt1, Point2f endpt1);
77 |     Point2f projectPoint(const Matrix3d &H, const Vector3d &point);
78 |     TicToc clock;
79 |     bool inBorder(const Mat &image, const Point2f &pt);
80 |     double fcurve(const Vector3d X, double x) { return X(0) * x * x + X(1) * x + X(2); }
81 | 
82 | private:
83 |     Edge leftEdge, rightEdge;
84 |     void downSample(int gap);
85 |     Matrix3d computeH();//plane modle: z = a_1x + a_2y + a_3
86 |     void segmentRoad();
87 |     void rangeConv(const Mat& input, Mat& output, int flag);
88 |     void genFreeArea(Mat& input, Mat& output, bool use_pad_up);
89 |     void threshold_range(Mat &range, int ths);
90 |     void drawCurve(Mat& img, Vector3d coeffs0, int starty0, int endy0, Vector3d coeffs1, int starty1, int endy1);
91 |     int fitRoadEdges(const Mat &area, Mat &output, Vector3d &coeffs0, Point2f& startpt0, Point2f& endpt0, Vector3d& coeffs1, Point2f& startpt1, Point2f& endpt1);
92 |     float scale;
93 | };


--------------------------------------------------------------------------------
/main.cpp:
--------------------------------------------------------------------------------
 1 | #include "FreeSpaceDetector.h"
 2 | #include "tic_toc.h"
 3 | #include <fstream>
 4 | #include <iostream>
 5 | 
 6 | using namespace std;
 7 | pcl::PointCloud<pcl::PointXYZI>::Ptr readfile(string filename){
 8 |     std::fstream input(filename, std::ios::in | std::ios::binary);
 9 |     if(!input.good()){
10 |         std::cerr << "Could not read file: " << std::endl;
11 |         exit(EXIT_FAILURE);
12 |     }
13 |     input.seekg(0, std::ios::beg);
14 |     pcl::PointCloud<pcl::PointXYZI>::Ptr points (new pcl::PointCloud<pcl::PointXYZI>);
15 |     int i;
16 |     for (i = 0; input.good() && !input.eof(); i++)
17 |     {
18 |         pcl::PointXYZI point;
19 |         input.read((char *) &point.x, 3*sizeof(float));
20 |         input.read((char *) &point.intensity, sizeof(float));
21 |         points->push_back(point);
22 |     }
23 |     input.close();
24 |     return points;
25 | }
26 | boost::shared_ptr<pcl::visualization::PCLVisualizer> simpleVis (pcl::PointCloud<pcl::PointXYZI>::ConstPtr cloud)
27 | {
28 |     // --------------------------------------------
29 |     // -----Open 3D viewer and add point cloud-----
30 |     // --------------------------------------------
31 |     boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
32 |     viewer->setBackgroundColor (0, 0, 0);
33 |     viewer->addPointCloud<pcl::PointXYZI> (cloud, "sample cloud");
34 |     viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
35 |     viewer->addCoordinateSystem (1.0);
36 |     viewer->initCameraParameters ();
37 |     return (viewer);
38 | }
39 | 
40 | boost::shared_ptr<pcl::visualization::PCLVisualizer> rgbVis (pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud)
41 | {
42 |   // --------------------------------------------
43 |   // -----Open 3D viewer and add point cloud-----
44 |   // --------------------------------------------
45 |   boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
46 |   viewer->setBackgroundColor (0, 0, 0);
47 |   pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
48 |   viewer->addPointCloud<pcl::PointXYZRGB> (cloud, rgb, "sample cloud");
49 |   viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
50 |   viewer->addCoordinateSystem (1.0);
51 |   viewer->initCameraParameters ();
52 |   return (viewer);
53 | }
54 | 
55 | int main(){
56 |     //TicToc clock;
57 | 
58 |     FreeSpaceDetector det;
59 |     LidarParam param;
60 |     det.setParam(param);
61 |     string datapath = "/Volumes/Orange/DataSet/paper_data/2011_09_26_93/2011_09_26_drive_0093_sync";
62 |     Mat img = imread(datapath + "/image_02/data/0000000302.png");
63 |     pcl::PointCloud<pcl::PointXYZI>::Ptr data = readfile(datapath + "/velodyne_points/data/0000000302.bin");
64 | 
65 |     //clock.tic();
66 |     det.setData(img, data);
67 |     det.getFreeSpace();
68 |     //cout<<clock.toc()<<" ms"<<endl;
69 |     if(Debug){
70 |         boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
71 |         viewer = rgbVis(det.segResult);
72 |         while (!viewer->wasStopped ())
73 |         {
74 |             viewer->spinOnce (100);
75 |             boost::this_thread::sleep (boost::posix_time::microseconds (100000));
76 |         }
77 |     }
78 |     return 0;
79 | }


--------------------------------------------------------------------------------
/FreeSpaceDetector.cpp:
--------------------------------------------------------------------------------
  1 | #include "FreeSpaceDetector.h"
  2 | 
  3 | bool Debug = false;
  4 | const int PAD_TOP = 150;
  5 | const int PAD_DOWN = 250;
  6 | const int CENT_Y = 395;
  7 | const int CENT_X = 256;
  8 | const int MAP_WIDTH = 512;
  9 | const int LINE_TOP = 150;
 10 | const int LINE_DOWN = 350;
 11 | 
 12 | FreeSpaceDetector::FreeSpaceDetector()
 13 | {
 14 |     sparseCloud.reset(new pcl::PointCloud<pcl::PointXYZI>());
 15 |     fov = 126.0;
 16 |     bev = Mat::zeros(512, 512, CV_8UC1);
 17 | }
 18 | void FreeSpaceDetector::setData(Mat& img, pcl::PointCloud<pcl::PointXYZI>::Ptr cloud){
 19 |     frame = img;
 20 |     laserCloudRaw = cloud;
 21 | }
 22 | void FreeSpaceDetector::setParam(LidarParam param){
 23 |     ldr_param = param;
 24 | }
 25 | void FreeSpaceDetector::downSample(int gap){
 26 |     int sz = laserCloudRaw->points.size();
 27 |     sprsHeight = 64;
 28 |     sprsWidth = static_cast<int>(fov / (0.08 * gap) + 0.5) + 1;
 29 | 
 30 |     labels = Mat::zeros(sprsHeight, sprsWidth, CV_8UC1);
 31 |     sparseCloud->points.resize(sprsHeight * sprsWidth);
 32 |     pcl::PointXYZI nanPoint;
 33 |     nanPoint.x = -1;
 34 |     nanPoint.y = -1;
 35 |     nanPoint.z = -1;
 36 |     nanPoint.intensity = -1;
 37 |     std::fill(sparseCloud->points.begin(), sparseCloud->points.end(), nanPoint);
 38 | 
 39 |     pcl::PointXYZI pt;
 40 |     for(int i = 0; i < sz; ++i){
 41 |         pt = laserCloudRaw->points[i];
 42 |         if(pt.x >= 5){
 43 |             double angle = atan2(pt.y, pt.x) * 180.0 / M_PI;
 44 |             if (angle < fov/2 && angle > -fov/2)
 45 |             {
 46 |                 int nth = static_cast<int>((fov / 2 - angle) / (0.08 * gap) + 0.5);
 47 |                 double verAngle = atan2(pt.z, sqrt(pt.x * pt.x + pt.y * pt.y)) * 180 / M_PI;
 48 |                 int rowid = static_cast<int>((verAngle - ldr_param.ang_bottom) / ldr_param.ang_res_y + 0.5);
 49 |                 int idx = rowid * sprsWidth + nth;
 50 |                 sparseCloud->points[idx] = pt;
 51 |                 labels.at<uchar>(rowid, nth) = 1;
 52 |             }
 53 |         }
 54 |     }
 55 |     // imshow("img", labels * 255);
 56 |     // waitKey(0);
 57 | }
 58 | void FreeSpaceDetector::segmentRoad(){
 59 |     size_t lowerInd, upperInd;
 60 |     float diffX, diffY, diffZ, angle;
 61 |     vector<Vector3d> samples;
 62 |     int sampleGap = 10;
 63 |     for (int col = 0; col < sprsWidth; ++col)
 64 |     {
 65 |         for(int row = 0; row < ldr_param.groundScanInd; ++row){
 66 |             lowerInd = col + row*sprsWidth;
 67 |             upperInd = col + (row+1)*sprsWidth;
 68 |             if(labels.at<uchar>(row,col) == 0 || labels.at<uchar>(row+1,col) == 0)
 69 |                 continue;
 70 |             diffX = sparseCloud->points[upperInd].x - sparseCloud->points[lowerInd].x;
 71 |             diffY = sparseCloud->points[upperInd].y - sparseCloud->points[lowerInd].y;
 72 |             diffZ = sparseCloud->points[upperInd].z - sparseCloud->points[lowerInd].z;
 73 |             angle = atan2(diffZ, sqrt(diffX*diffX + diffY*diffY) ) * 180 / M_PI;
 74 |             if (abs(angle - ldr_param.sensorMountAngle) <= 10){
 75 |                 labels.at<uchar>(row,col) = 2;
 76 |                 labels.at<uchar>(row+1,col) = 2;
 77 |                 if(lowerInd % sampleGap == 0)
 78 |                     samples.emplace_back(Vector3d(sparseCloud->points[lowerInd].x,
 79 |                                                   sparseCloud->points[lowerInd].y,
 80 |                                                   sparseCloud->points[lowerInd].z));
 81 |             }
 82 |         }
 83 |     }
 84 |     //cout << samples.size() << endl;
 85 |     const int l = samples.size();
 86 |     assert(l > 80);
 87 |     sort(samples.begin(), samples.end(), [&](Vector3d &a, Vector3d &b) { return a.z() < b.z(); });
 88 |     const int N = 50;
 89 |     Eigen::MatrixXd P1(N, 3), P2(N, 1), A, b;
 90 |     int start = l / 2 - N / 2;
 91 |     for (int i = start; i < start + N; ++i){
 92 |         P1.block<1, 3>(i-start, 0) = Vector3d(samples[i].x(), samples[i].y(), 1);
 93 |         P2(i-start,0) = samples[i].z();
 94 |     }
 95 |     A = P1.transpose() * P1;
 96 |     b = P1.transpose() * P2;
 97 |     Vector3d X = A.ldlt().solve(b);
 98 |     plane = X;
 99 |     double ths = 0.12;
100 |     if (Debug)
101 |         segResult.reset(new pcl::PointCloud<pcl::PointXYZRGB>());
102 |     pcl::PointXYZRGB point;
103 |     scale = 7.0;
104 |     for (int i = 0; i < sprsHeight; i++)
105 |     {
106 |         for (int j = 0; j < sprsWidth; j++){
107 |             if (labels.at<uchar>(i, j) != 0){
108 |                 int idx = j + i * sprsWidth;
109 |                 point.x = sparseCloud->points[idx].x;
110 |                 point.y = sparseCloud->points[idx].y;
111 |                 point.z = sparseCloud->points[idx].z;
112 |                 double error = point.z - (X(0) * point.x + X(1) * point.y + X(2));
113 |                 if(error > ths){
114 |                     float xp, yp, xx, yy;
115 |                     xp = point.x;
116 |                     yp = point.y;
117 |                     xx = CENT_Y - xp * scale;
118 |                     yy = CENT_X - yp * scale;
119 |                     if (xx >= 0 && xx < 512 && yy >= 0 && yy < 512)
120 |                         bev.at<uchar>(static_cast<int>(xx + 0.5), static_cast<int>(yy + 0.5)) = 255;
121 |                 }
122 |                 if(Debug){
123 |                     uint32_t rgb;
124 |                     if (error > ths)
125 |                         rgb = (static_cast<uint32_t>(255) << 16 | static_cast<uint32_t>(0) << 8 | static_cast<uint32_t>(0));
126 |                     else
127 |                         rgb = (static_cast<uint32_t>(0) << 16 | static_cast<uint32_t>(255) << 8 | static_cast<uint32_t>(0));
128 |                     point.rgb = *reinterpret_cast<float *>(&rgb);
129 |                     segResult->points.push_back(point);
130 |                 }
131 |             }
132 |         }
133 |     }
134 |     //imwrite("bev.jpg", bev);
135 |     // imshow("bev", bev);
136 |     // waitKey(0);
137 | }
138 | 
139 | void FreeSpaceDetector::rangeConv(const Mat& input, Mat& output, int flag){
140 |     output = input.clone();
141 |     int m1 = 3;
142 | 	int m3 = m1;
143 | 	int m2 = 1;
144 | 	int m4 = 3;
145 |     uchar *pd = output.data;
146 |     for (int i = 0; i < output.rows; ++i)
147 |         for (int j = 0; j < output.cols; ++j)
148 |             pd[j + i * output.cols] = 255 - pd[j + i * output.cols];
149 | 
150 |     uchar *p1 = output.data;
151 |     for (int i = 1; i < 511; i++){
152 |         for (int j = 1; j < 511; j++){
153 |             if (p1[512 * i + j] < 254){
154 |                 if (p1[512 * i + j] + m1 < p1[512 * (i + 1) + (j - 1)])
155 |                     p1[512 * (i + 1) + (j - 1)] = p1[512 * i + j] + m1;
156 |                 if (p1[512 * i + j] + m2 < p1[512 * (i + 1) + j])
157 |                     p1[512 * (i + 1) + j] = p1[512 * i + j] + m2;
158 |                 if (p1[512 * i + j] + m3 < p1[512 * (i + 1) + (j + 1)])
159 |                     p1[512 * (i + 1) + (j + 1)] = p1[512 * i + j] + m3;
160 |                 if (p1[512 * i + j] + m4 < p1[512 * i + (j + 1)])
161 |                     p1[512 * i + (j + 1)] = p1[512 * i + j] + m4;
162 |             }
163 |         }
164 | 	}
165 | 	for (int i = 510; i > 0; i--) {
166 | 		for (int j = 510; j > 0; j--) {
167 | 			if (p1[512 * i + j] < 254) {
168 | 				if (p1[512 * i + j] + m1 < p1[512 * (i - 1) + (j - 1)])
169 | 					p1[512 * (i - 1) + (j - 1)] = p1[512 * i + j] + m1;
170 | 				if (p1[512 * i + j] + m2 < p1[512 * (i - 1) + j])
171 | 					p1[512 * (i - 1) + j] = p1[512 * i + j] + m2;
172 | 				if (p1[512 * i + j] + m3 < p1[512 * (i - 1) + (j + 1)])
173 | 					p1[512 * (i - 1) + (j + 1)] = p1[512 * i + j] + m3;
174 | 				if (p1[512 * i + j] + m4 < p1[512 * i + (j - 1)])
175 | 					p1[512 * i + (j - 1)] = p1[512 * i + j] + m4;
176 | 			}
177 | 		}
178 | 	}
179 | }
180 | void FreeSpaceDetector::threshold_range(Mat& range, int ths){
181 |     uchar *pd = range.data;
182 |     for(int i = 0; i < range.rows; ++i)
183 |         for(int j = 0; j < range.cols; ++j)
184 |             if (pd[j + i * range.cols] <= ths)
185 |                 pd[j + i * range.cols] = 255;
186 |             else
187 |                 pd[j + i * range.cols] = 0;
188 | }
189 | void FreeSpaceDetector::genFreeArea(Mat& input, Mat& output, bool use_pad_up){
190 |     int left_start = 0;
191 | 	int right_start = 0;
192 | 	int up_start = 0;
193 |     output = Mat::zeros(input.rows, input.cols, CV_8UC1);
194 |     uchar *p0, *p1;
195 |     p0 = input.data;
196 |     p1 = output.data;
197 |     for (int ii = CENT_Y; ii >= 0; ii--){
198 |         int det = CENT_Y - ii;
199 | 		float k = det / (float)(CENT_X - 0);
200 | 		for (int tmp_j = CENT_X; tmp_j >= 0; tmp_j--){
201 | 			int tmp_i = CENT_Y - (CENT_X - tmp_j) * k;
202 | 			if (p0[MAP_WIDTH * tmp_i + tmp_j] == 0){
203 | 				if (left_start == 1){
204 | 					p1[MAP_WIDTH * tmp_i + tmp_j] = 255;
205 | 				}
206 | 			} else {
207 | 				left_start = 1;
208 | 				break;
209 | 			}
210 | 		}
211 | 		k = det / (float)(MAP_WIDTH - CENT_X);
212 | 		for (int tmp_j = CENT_X; tmp_j < MAP_WIDTH; tmp_j++) {
213 | 			int tmp_i = CENT_Y - (tmp_j - CENT_X) * k;
214 | 			if (p0[MAP_WIDTH * tmp_i + tmp_j] == 0) {
215 | 				if (right_start == 1) {
216 | 					p1[MAP_WIDTH * tmp_i + tmp_j] = 255;
217 | 				}
218 | 			} 
219 | 			else {
220 | 				right_start = 1;
221 | 				break;
222 | 			}
223 | 		}
224 | 	}
225 | 	for (int jj = 0; jj < MAP_WIDTH; jj++) {
226 | 		int det = CENT_X - jj;
227 | 		float k = det / float(CENT_Y - 0);
228 | 		for (int tmp_i = CENT_Y; tmp_i >=0; tmp_i--) {
229 | 			int tmp_j = CENT_X - (CENT_Y - tmp_i) * k;
230 | 			if (p0[MAP_WIDTH * tmp_i + tmp_j] == 0){
231 | 				if (up_start == 1) {
232 | 					p1[MAP_WIDTH * tmp_i + tmp_j] = 255;
233 | 				}
234 | 			}else{
235 | 				up_start = 1;
236 | 				break;
237 | 			}
238 | 		}
239 | 	}
240 |     
241 |     if (use_pad_up){
242 |         cout << "use pad up" << endl;
243 |         for (int jj = 0; jj < MAP_WIDTH; jj++) {
244 | 			int y_min = -1;
245 | 			for (int ii = PAD_DOWN; ii >= PAD_TOP; ii--) {
246 | 				if (p1[MAP_WIDTH * ii + jj] == 255) {
247 | 					y_min = ii;
248 | 				}
249 | 			}
250 | 			if (y_min > PAD_TOP) {
251 | 				for (int ii = y_min - 1; ii >= 0; ii--) {
252 | 					if (p0[MAP_WIDTH * ii + jj] == 0) {
253 | 						p1[MAP_WIDTH * ii + jj] = 200;
254 | 					} else {
255 | 						break;
256 | 					}
257 | 				}
258 | 			}
259 | 		}
260 | 	}
261 | }
262 | int FreeSpaceDetector::fitRoadEdges(const Mat &area, Mat &output, Vector3d &coeffs0, Point2f& startpt0, Point2f& endpt0, Vector3d &coeffs1, Point2f& startpt1, Point2f& endpt1){
263 |     uchar *p0 = area.data;
264 |     int left_x = -1;
265 | 	int right_x = -1;
266 |     vector<Point2f> leftline, rightline;
267 | 	for (int jj = 0; jj < MAP_WIDTH; jj++) {
268 | 		if (p0[MAP_WIDTH * LINE_TOP + jj] > 0) {
269 | 			if (left_x == -1){
270 | 				left_x = jj;
271 | 			}
272 | 			right_x = jj;
273 | 		}
274 | 	}
275 | 	if (left_x== -1 || right_x == -1) {
276 | 		return 0;
277 | 	}
278 |     leftline.emplace_back(Point2f(LINE_TOP, left_x));
279 |     rightline.emplace_back(Point2f(LINE_TOP, right_x));
280 |     for (int ii = LINE_TOP + 1; ii <= LINE_DOWN; ii++) {
281 | 		int left_find = 0;
282 | 		int right_find = 0;
283 | 		for (int jj = left_x - 1; jj >= left_x - 10 && jj >= 0; jj--) {
284 | 			if (p0[MAP_WIDTH * ii + jj] > 0){
285 | 				left_find = 1;
286 | 				left_x = left_x - 1;
287 | 				break;
288 | 			}
289 | 		}
290 | 		if (left_find == 0) {
291 | 			for (int jj = left_x; jj < left_x + 10 && jj < MAP_WIDTH; jj++) {
292 | 				if (p0[MAP_WIDTH * ii + jj] > 0){
293 | 					left_find = 1;
294 | 					if (left_x != jj) {
295 | 						left_x = left_x + 1;
296 | 					}
297 | 					break;
298 | 				}
299 | 			}
300 | 		}
301 | 		for (int jj = right_x + 1; jj < right_x + 10 && jj < MAP_WIDTH; jj++) {
302 | 			if (p0[MAP_WIDTH * ii + jj] > 0) {
303 | 				right_find = 1;
304 | 				right_x = right_x + 1;
305 | 				break;
306 | 			}
307 | 		}
308 | 		if (right_find == 0) {
309 | 			for (int jj = right_x; jj > right_x - 10 && jj >= 0; jj--){
310 | 				if (p0[MAP_WIDTH * ii + jj] > 0){
311 | 					right_find = 1;
312 | 					if (right_x != jj) {
313 | 						right_x = right_x -1;
314 | 					}
315 | 					break;
316 | 				}
317 | 			}
318 | 		}
319 | 		if (left_find == 0 || right_find == 0 || left_x >= right_x) {
320 | 			return 0;
321 | 		}
322 | 		if ((ii & 3) == 0) {
323 | 			leftline.emplace_back(Point2f(ii, left_x));
324 | 			rightline.emplace_back(Point2f(ii, right_x));
325 | 		}
326 | 	}
327 |     // int starty0 = leftline[0].x;
328 |     // int endy0 = leftline.back().x;
329 |     // int starty1 = rightline[0].x;
330 |     // int endy1 = rightline.back().x;
331 | 
332 |     MatrixXd P_l0(leftline.size(), 3), P_l1(leftline.size(), 1);
333 |     MatrixXd P_r0(rightline.size(), 3), P_r1(rightline.size(), 1);
334 |     double x,y;
335 |     for (int i = 0; i < leftline.size(); ++i){
336 |         x = leftline[i].x;
337 |         y = leftline[i].y;
338 |         P_l0.block<1,3>(i,0) = Vector3d(x*x, x, 1);
339 |         P_l1(i,0) = y;
340 | 
341 |         x = rightline[i].x;
342 |         y = rightline[i].y;
343 |         P_r0.block<1,3>(i,0) = Vector3d(x*x, x, 1);
344 |         P_r1(i,0) = y;
345 |     }
346 | 
347 |     double offset = 3.0;
348 |     MatrixXd A, b;
349 |     Vector3d X;
350 |     A = P_l0.transpose()*P_l0;
351 |     b = P_l0.transpose()*P_l1;
352 |     X = A.ldlt().solve(b);
353 |     X(2) -= offset;
354 |     coeffs0 = X;
355 | 
356 |     x = leftline[0].x;
357 |     startpt0.x = x;
358 |     startpt0.y = X(0) * x * x + X(1) * x + X(2);
359 |     x = leftline.back().x;
360 |     endpt0.x = x;
361 |     endpt0.y = X(0) * x * x + X(1) * x + X(2);
362 | 
363 |     A = P_r0.transpose() * P_r0;
364 |     b = P_r0.transpose()*P_r1;
365 |     X = A.ldlt().solve(b);
366 |     X(2) += offset;
367 |     coeffs1 = X;
368 | 
369 |     x = rightline[0].x;
370 |     startpt1.x = x;
371 |     startpt1.y = X(0) * x * x + X(1) * x + X(2);
372 |     x = rightline.back().x;
373 |     endpt1.x = x;
374 |     endpt1.y = X(0) * x * x + X(1) * x + X(2);
375 | }
376 | void FreeSpaceDetector::drawCurve(Mat& img, Vector3d coeffs0, int starty0, int endy0, Vector3d coeffs1, int starty1, int endy1){
377 |     vector<Point2f> pts0, pts1;
378 |     Point2f lastpt, curpt;
379 |     for(int i = starty0; i <= endy0; ++i){
380 |         float x = coeffs0[0]*i*i + coeffs0[1]*i+coeffs0[2];
381 |         curpt = Point2f(x, static_cast<float>(i));
382 |         if(i > starty0)
383 |             line(img, lastpt, curpt, Scalar(0, 255, 0), 1.5);
384 |         lastpt = curpt;
385 |     }
386 |     for(int i = starty1; i <= endy1; ++i){
387 |         float x = coeffs1[0]*i*i + coeffs1[1]*i+coeffs1[2];
388 |         curpt = Point2f(x, static_cast<float>(i));
389 |         if(i > starty1)
390 |             line(img, lastpt, curpt, Scalar(0, 255, 0), 1.5);
391 |         lastpt = curpt;
392 |     }
393 | }
394 | Matrix3d FreeSpaceDetector::computeH(){
395 |     Matrix<double, 4,3> plane_to_lidar;
396 |     plane_to_lidar << -1/scale, 0, CENT_Y/scale,
397 |                        0, -1/scale, CENT_X/scale,
398 |                        -plane[0]/scale, -plane[1]/scale, (plane[0]*CENT_Y+plane[1]*CENT_X)/scale+plane[2],
399 |                        0, 0, 1;
400 | 
401 |     Matrix<double, 3,4> P_lidar_to_img;
402 |     P_lidar_to_img << 609.6954, -721.4216, -1.2513, -123.0418,
403 |                       180.3842, 7.6448, -719.6515, -101.0167,
404 |                       0.9999,   0.0001, 0.0105,   -0.2694;
405 |     return (P_lidar_to_img*plane_to_lidar);
406 | }
407 | Point2f FreeSpaceDetector::projectPoint(const Matrix3d &H, const Vector3d &point){
408 |     Vector3d res = H * point;
409 |     res /= res.z();
410 |     return Point2f(res.x(), res.y());
411 | }
412 | bool FreeSpaceDetector::inBorder(const Mat& image, const Point2f& pt){
413 |     if(pt.x >=0 && pt.x < image.cols && pt.y >= 0 && pt.y < image.rows)
414 |         return true;
415 |     else
416 |         return false;
417 | }
418 | void FreeSpaceDetector::projectCurveOnImage(Mat& img, const Matrix3d& H, 
419 |                                             Vector3d coeffs0, Point2f startpt0, Point2f endpt0,
420 |                                             Vector3d coeffs1, Point2f startpt1, Point2f endpt1){
421 |     vector<Point> ptsl, ptsr;
422 |     Point2f p;
423 |     for (int i = static_cast<int>(startpt0.x + 0.5); i <= static_cast<int>(endpt0.x + 0.5); ++i){
424 |         double y = fcurve(coeffs0, i);
425 |         p = projectPoint(H, Vector3d(i, y, 1));
426 |         ptsl.emplace_back(Point(int(p.x+0.5), int(p.y + 0.5)));
427 | 
428 |         y = fcurve(coeffs1, i);
429 |         p = projectPoint(H, Vector3d(i, y, 1));
430 |         ptsr.emplace_back(Point(int(p.x+0.5), int(p.y + 0.5)));
431 |     }
432 |     int len = ptsl.size();
433 |     Point *ptsall = new Point[2*len];
434 |     for (int i = 0; i < len; ++i)
435 |         ptsall[i] = ptsl[i];
436 |     for (int i = 0; i < len; ++i)
437 |         ptsall[i + len] = ptsr[len - i - 1];
438 |     const Point *pts[] = {ptsall, ptsall + len};
439 |     int npts[2];
440 |     npts[0] = npts[1] = len;
441 |     Mat mask = Mat::zeros(img.size(), CV_8UC1);
442 |     int npts2[1];
443 |     npts2[0] = 2 * len;
444 |     fillPoly(mask, pts, npts2, 1, Scalar(255), 8, 0, Point());
445 | 
446 |     uchar *data0 = frame.data;
447 |     uchar *data1 = mask.data;
448 |     for(int i = 0; i < frame.rows; ++i)
449 |         for(int j = 0; j < frame.cols; ++j){
450 |             int idx0 = 3 * j + i * 3 * frame.cols;
451 |             int idx1 = j + i * frame.cols;
452 |             if(data1[idx1] > 50){
453 |                 float s = 0.5;
454 |                 data0[idx0] *= s;
455 |                 data0[idx0 + 1] = data0[idx0 + 1] * s + 255 * s;
456 |                 data0[idx0 + 2] *= s;
457 |             }
458 |         }
459 |     // imshow("mask", mask);
460 |     // waitKey();
461 |     polylines(img, pts, npts, 2, false, Scalar(0, 255, 255), 3, 8, 0);
462 |     imwrite("freespace.jpg", img);
463 | }
464 | bool FreeSpaceDetector::getFreeSpace(){
465 |     //clock.tic();
466 |     downSample(5);
467 |     // double dura1 = clock.toc();
468 |     // cout<<"Downsample : "<<dura1<<"ms"<<"    ";
469 | 
470 |     //clock.tic();
471 |     segmentRoad();
472 |     //double dura2 = clock.toc();
473 |     //cout<<"Segment Road : "<<dura2<<"ms"<<"    ";
474 |     Mat range;
475 |     rangeConv(bev, range, 1);
476 |     //imshow("rg1", range);
477 |     threshold_range(range, 20);
478 |     Mat freearea;
479 |     genFreeArea(range, freearea, false);
480 |     Vector3d coeffs0, coeffs1;
481 |     Point2f s0,e0,s1,e1;
482 |     fitRoadEdges(freearea, bev, coeffs0, s0, e0, coeffs1, s1, e1);
483 |     Matrix3d H = computeH();
484 |     projectCurveOnImage(frame, H, coeffs0, s0, e0, coeffs1, s1, e1);
485 | 
486 |     //cout<<s0<<" "<<e0<<endl;
487 |     Mat show = bev.clone();
488 |     cvtColor(show, show, CV_GRAY2BGR);
489 |     drawCurve(show, coeffs0, s0.x, e0.x, coeffs1, s1.x, e1.x);
490 |     imwrite("bev.jpg", show);
491 |     imshow("frme", frame);
492 |     waitKey(0);
493 |     return true;
494 | }


--------------------------------------------------------------------------------