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;
}
