diff --git a/linux/src/IPM.cc b/linux/src/IPM.cc index 91395ab..950ed04 100644 --- a/linux/src/IPM.cc +++ b/linux/src/IPM.cc @@ -13,6 +13,19 @@ IPM::IPM( const cv::Size& _origSize, const cv::Size& _dstSize, const std::vector createMaps(); } + +void IPM::setIPM( const cv::Size& _origSize, const cv::Size& _dstSize, const std::vector& _origPoints, const std::vector& _dstPoints ) +{ + m_origSize = _origSize; + m_dstSize = _dstSize; + m_origPoints = _origPoints; + m_dstPoints = _dstPoints; + assert( m_origPoints.size() == 4 && m_dstPoints.size() == 4 && "Orig. points and Dst. points must vectors of 4 points" ); + m_H = getPerspectiveTransform( m_origPoints, m_dstPoints ); + m_H_inv = m_H.inv(); + + createMaps(); +} void IPM::drawPoints( const std::vector& _points, cv::Mat& _img ) const { assert(_points.size() == 4); diff --git a/linux/src/IPM.h b/linux/src/IPM.h index 15f9e95..a92bb25 100644 --- a/linux/src/IPM.h +++ b/linux/src/IPM.h @@ -33,6 +33,8 @@ class IPM // Draw void drawPoints( const std::vector& _points, cv::Mat& _img ) const; + void setIPM( const cv::Size& _origSize, const cv::Size& _dstSize, + const std::vector& _origPoints, const std::vector& _dstPoints ); private: void createMaps(); diff --git a/linux/src/main2.cc b/linux/src/main2.cc index d5ca268..933b103 100644 --- a/linux/src/main2.cc +++ b/linux/src/main2.cc @@ -40,6 +40,7 @@ int main(int argc, char** argv) { } bool detection = false; bool python_on = false; + int width = 1024, height = 768; if(argc == 2){ if(strcmp(argv[1], "--Car_Detection")){ detection = true; @@ -101,6 +102,7 @@ int main(int argc, char** argv) { } } } + int move_mouse_pixel = 0; while (true) { auto begin = chrono::high_resolution_clock::now(); // ETS2 @@ -133,26 +135,25 @@ int main(int argc, char** argv) { //medianBlur(image, image, 3); //cv::cuda::bilateralFilter(imageGPU, imageGPU, ); - int width = 1024, height = 768; - - // The 4-points at the input image + IPM ipm; vector origPoints; - + vector dstPoints; + // The 4-points at the input image + origPoints.push_back(Point2f(0-400, (height-50))); origPoints.push_back(Point2f(width+400, height-50)); - origPoints.push_back(Point2f(width/2+80, height/2+30)); - origPoints.push_back(Point2f(width/2-80, height/2+30)); - + origPoints.push_back(Point2f(width/2+100, height/2+30)); + origPoints.push_back(Point2f(width/2-100, height/2+30)); // The 4-points correspondences in the destination image - vector dstPoints; + dstPoints.push_back(Point2f(0, height)); dstPoints.push_back(Point2f(width, height)); dstPoints.push_back(Point2f(width, 0)); dstPoints.push_back(Point2f(0, 0)); // IPM object - IPM ipm(Size(width, height), Size(width, height), origPoints, dstPoints); + ipm.setIPM(Size(width, height), Size(width, height), origPoints, dstPoints); // Process //clock_t begin = clock(); @@ -167,6 +168,8 @@ int main(int argc, char** argv) { cv::UMat blur; cv::UMat sobel; cv::Mat contours; + LineFinder ld; // 인스턴스 생성 + cv::resize(outputImg, outputImg, cv::Size(320, 240)); cv::cvtColor(outputImg, gray, COLOR_RGB2GRAY); cv::blur(gray, blur, cv::Size(10, 10)); @@ -174,18 +177,16 @@ int main(int argc, char** argv) { cv::threshold(sobel, contours, 145, 255, CV_THRESH_BINARY); //Thinning(contours, contours.rows, contours.cols); //cv::Canny(gray, contours, 125, 350); - - LineFinder ld; // 인스턴스 생성 - + + // 확률적 허프변환 파라미터 설정하기 - + ld.setLineLengthAndGap(20, 120); ld.setMinVote(55); std::vector li = ld.findLines(contours); ld.drawDetectedLines(contours); - //////////////////////////////////////////// // 자율 주행 @@ -215,9 +216,9 @@ int main(int argc, char** argv) { if (first_centerline == 0 ) { first_centerline = centerline; } - cv::circle(outputImg, Point(centerline, i), 1, Scalar(30, 255, 30) , 3); - cv::circle(outputImg, Point(centerline + center_to_right+20, i), 1, Scalar(255, 30, 30) , 3); - cv::circle(outputImg, Point(centerline - center_to_left+10, i), 1, Scalar(255, 30, 30) , 3); + //cv::circle(outputImg, Point(centerline, i), 1, Scalar(30, 255, 30) , 3); + //cv::circle(outputImg, Point(centerline + center_to_right+20, i), 1, Scalar(255, 30, 30) , 3); + //cv::circle(outputImg, Point(centerline - center_to_left+10, i), 1, Scalar(255, 30, 30) , 3); sum_centerline += centerline; avr_center_to_left = (avr_center_to_left * count_centerline + center_to_left)/count_centerline+1; avr_center_to_right = (avr_center_to_right * count_centerline + center_to_right)/count_centerline+1; @@ -234,9 +235,40 @@ int main(int argc, char** argv) { int degree = atan2 (last_centerline - first_centerline, count_centerline) * 180 / PI; //diff = (90 - degree); - int move_mouse_pixel = 0 - counter + diff; + move_mouse_pixel = 0 - counter + diff; cout << "Steer: "<< move_mouse_pixel << "px "; //goDirection(move_mouse_pixel); + origPoints[2]=Point2f((width/2+100)+(move_mouse_pixel*7), height/2+30); + origPoints[3]=Point2f((width/2-100)+(move_mouse_pixel*7), height/2+30); + + + // IPM object + ipm.setIPM(Size(width, height), Size(width, height), origPoints, dstPoints); + + // Process + //clock_t begin = clock(); + ipm.applyHomography(image, outputImg); + //clock_t end = clock(); + //double elapsed_secs = double(end - begin) / CLOCKS_PER_SEC; + //printf("%.2f (ms)\r", 1000 * elapsed_secs); + //ipm.drawPoints(origPoints, image); + + //Mat row = outputImg.row[0]; + cv::resize(outputImg, outputImg, cv::Size(320, 240)); + cv::cvtColor(outputImg, gray, COLOR_RGB2GRAY); + cv::blur(gray, blur, cv::Size(10, 10)); + cv::Sobel(blur, sobel, blur.depth(), 1, 0, 3, 0.5, 127); + cv::threshold(sobel, contours, 145, 255, CV_THRESH_BINARY); + //Thinning(contours, contours.rows, contours.cols); + //cv::Canny(gray, contours, 125, 350); + + // 확률적 허프변환 파라미터 설정하기 + + ld.setLineLengthAndGap(20, 120); + ld.setMinVote(55); + + std::vector li = ld.findLines(contours); + ld.drawDetectedLines(contours); moveMouse(move_mouse_pixel); int road_curve = (int)((sum_centerline/count_centerline-bottom_center)); @@ -268,7 +300,10 @@ int main(int argc, char** argv) { //////////////////////////////////////////// //cv::cvtColor(contours, contours, COLOR_GRAY2RGB); - imshow("Test", outputImg); + imshow("Road", outputImg); + imshow("Lines", contours); + moveWindow("Lines", 200, 100); + moveWindow("Road", 530, 100); waitKey(1); auto end = chrono::high_resolution_clock::now(); @@ -289,7 +324,8 @@ std::string exec(const char* cmd) { std::array buffer; std::string result; std::shared_ptr pipe(popen(cmd, "r"), pclose); - if (!pipe) throw std::runtime_error("popen() failed!"); + if (!pipe) + throw std::runtime_error("popen() failed!"); while (!feof(pipe.get())) { if (fgets(buffer.data(), 128, pipe.get()) != NULL) result += buffer.data();