int main(int argc, char *argv[]) { // Check command arguments int c; int DEVICE, SEGMENTATION_RATIO, OFFSET, ANGLE, THRESHOLD, RESOLUTION; while ((c = getopt(argc, argv, "v:s:o:a:t:r:")) != -1) { switch (c) { case 'v': DEVICE = atoi(optarg); break; case 's': SEGMENTATION_RATIO = atoi(optarg); break; case 'o': OFFSET = atoi(optarg); break; case 'a': ANGLE = atoi(optarg); break; case 't': THRESHOLD = atoi(optarg); break; case 'r': RESOLUTION = atoi(optarg); break; default: // '?' std::cout << "Usage: " << argv[0] << " [-v device] [-s segmentation_ratio] [-o offset] [-a angle] [-t threshold] [-r resolution]" << std::endl; return -1; } } if (argc != 13) { std::cout << "Usage: " << argv[0] << " [-v device] [-s segmentation_ratio] [-o offset] [-a angle] [-t threshold] [-r resolution]" << std::endl; return -1; } /*ROS*/ r_init( Road_Detecter); r_hdlr( hdl); r_newPub( pubRoad, hdl, challenge1::road, Road, 1000); ros::Rate loop_rate(10); challenge1::road rd; /**/ cv::VideoCapture myVideoCapture(DEVICE); if (!myVideoCapture.isOpened()) { std::cout << "ERROR: Cannot open device " << DEVICE << std::endl; return -1; } cv::namedWindow("Video Captured"); cv::namedWindow("Road Detected"); int imgCenter = myVideoCapture.get(CV_CAP_PROP_FRAME_WIDTH) / 2; int preCenter = imgCenter; int movingDirection = GO_STRAIGHT; //while (1) { while( ros::ok()){ int key = cv::waitKey(1); if ((key & 0xFF) == 27) // 'Esc' key return 0; cv::Mat inputFrame, segmentedInputFrame; myVideoCapture >> inputFrame; inputFrame(cv::Range(inputFrame.rows - inputFrame.rows / SEGMENTATION_RATIO, inputFrame.rows - 1), cv::Range::all()).copyTo(segmentedInputFrame); // Detect road. In general, there are only vertical lines cv::Mat road = detectRoad(segmentedInputFrame, OFFSET, ANGLE); // Compute the road center int curCenter = computeCenter(road); // Compute the moving direction movingDirection = computeMovingDirection(imgCenter, preCenter, curCenter, movingDirection, THRESHOLD, RESOLUTION); std::cout << movingDirection << std::endl; cv::imshow("Video Captured", segmentedInputFrame); cv::imshow("Road Detected", road); /*ROS*/ /*switch( movingDirection){ case GO_STRAIGHT: case TURN_LEFT: case TURN_RIGHT: default: break; }*/ rd.direction = movingDirection; pubRoad.publish( rd); ros::spinOnce(); loop_rate.sleep(); /*\ROS*/ } return 0; }
//event handler for the next button to go from one image to another void callbackBtnNext(int state, void* dataPassed) { //increase the file counter by 1 countFile++; //if (countFile == 203) // cout << "asd" << endl; //construct the next file names string countFileStr = static_cast<ostringstream*>( &(ostringstream() << countFile) )->str(); nameImg = folderRoot + folderImg + "(" + countFileStr + ")" + fileExtImg; namePtsFile = folderRoot + folderPts + "(" + countFileStr + ")" + fileExtPts; nameResultsFile = folderWrite + prefixResultsFile + "(" + countFileStr + ")" + fileExtResult; nameResultsImg = folderWrite + prefixResultsFile + "(" + countFileStr + ")" + fileExtImg; //Read the image and store in a matrix //cout << nameImg << " is loading..." << endl; //bgrMat = imread(nameImg, cv::IMREAD_COLOR); //cvtColor(bgrMat, hsvMat, CV_BGR2HSV); //Read the pts file and store x,y,z in a matrix //cout << namePtsFile << " is loading..." << endl; //Mat xyzMat = Mat::zeros(height, width, CV_64FC3); //readPtsDataFromFile(namePtsFile, xyzMat); //Calculate the variance of Y and the max diff of Y in patches //cout << "Patches are constructing..." << endl; //cout << "Variance and Difference in Y calulcation..." << endl; //varPatches = calculateVarianceYforPatches(xyzMat, size_patch); //diffPatches = calculateDifferenceMaxYforPatches(xyzMat, size_patch); //Calculate image patch histograms //cout << "Image patch histograms are building..." << endl; //histsBGR = calculateHistograms(bgrMat, SIZE_PATCH, CHANNELS, 3, SIZE_HIST, RANGES); //histsHS = calculateHistograms(hsvMat, size_patch, channels_hs, 2, size_hist_hs, ranges_hs, false); //show the original image //imshow(nameWindowMain, bgrMat); //Tracbar stuff //create slider for training sample threshold //createTrackbar( nameTrackbarVarYThreshold, nameWindowMain, &thresholdVarY, threshold_var_y_max, callbackTrackbarVarYThreshold ); //createTrackbar( nameTrackbarDiffYThreshold, nameWindowMain, &thresholdDiffY, THRESHOLD_DIFF_Y_MAX, callbackTrackbarDiffYThreshold ); //create next button to jump to the next image //createButton(nameBtnNext, callbackBtnNext, NULL); // Show some stuff //callbackTrackbarVarYThreshold( thresholdVarY, 0 ); //callbackTrackbarDiffYThreshold( thresholdDiffY, 0 ); //Read the image and store in a matrix cout << nameImg << " is loading..." << endl; bgrMat = imread(nameImg, cv::IMREAD_COLOR); //Read the pts file and store x,y,z in a matrix cout << namePtsFile << " is loading..." << endl; Mat xyzMat = Mat::zeros(height, width, CV_64FC3); readPtsDataFromFile(namePtsFile, xyzMat); //convert the variance threshold value obtained from the trackbar to double thresholdVarYd = ((double)thresholdVarY)/threshold_var_y_divider; //Get ticks before algorithm double t = (double)getTickCount(); //Mat classifiedMat = detectRoadSingleSVM(bgrMat, xyzMat, trainingBuffer, trainingBufferLib, model, weights, thresholdVarYd, false, distribution); Mat classifiedMat = detectRoad(bgrMat, xyzMat, trainingBuffer, means, covs, weights, thresholdVarYd, false, distribution); //Mat classifiedMat = detectRoadDebug(bgrMat, xyzMat, trainingBuffer, means, covs, weights, thresholdVarYd, false, distribution); //Mat classifiedMat = detectRoadBanded(bgrMat, xyzMat, trainingBuffer, means, covs, weights, thresholdVarYd, false, distribution); //cout << "Number of Training Sets : " << trainingBufferLib.size() << endl; //for (int i = 0; i < trainingBufferLib.size(); i++) // cout << "Set " << i << " : " << trainingBufferLib[i].rows << endl; //for (int i = 0; i < trainingBufferLib.size(); i++) // cout << "Set " << i << " : " << trainingBufferLib[i] << endl; //Get tocks after algorithm, find the difference and write in sec form t = ((double)getTickCount() - t)/getTickFrequency(); cout << "Times passed in seconds: " << t << endl; //Show the classification result for the Y variance namedWindow(nameWindowClassifiedVar, CV_WINDOW_AUTOSIZE); imshow(nameWindowClassifiedVar, classifiedMat); ////////////////////////////////////////////////////////////////// ///////////////////// WRITING THE RESULTS //////////////////////// ////////////////////////////////////////////////////////////////// //writing the classification results as image and/or text cout << "Writing to Image : " << nameResultsImg << endl; vector<int> output_params; output_params.push_back(CV_IMWRITE_PXM_BINARY); output_params.push_back(1); imwrite(nameResultsImg, classifiedMat, output_params); cout << "Writing to File: " << nameResultsFile << endl; writeResultDataToFile(nameResultsFile, classifiedMat); cout << "Done..." << endl; }