int main ( int argc,char **argv )
{
openAndInitSpi();
time_t timer_begin,timer_end;
RaspiCam_Cv Camera;
Mat image;
int nCount=100;
//set camera params
Camera.set( CV_CAP_PROP_FORMAT , CV_8UC1 );
Camera.set( CV_CAP_PROP_FRAME_WIDTH , frame_width );
Camera.set( CV_CAP_PROP_FRAME_HEIGHT, frame_height);
//Open camera
cout<<"Opening Camera..."<<endl;
if (!Camera.open()) {cerr<<"Error opening the camera"<<endl;return -1;}
//Start capture
cout<<"Capturing "<<nCount<<" frames ...."<<endl;
time ( &timer_begin );
for ( int image_counter=0; image_counter<nCount; image_counter++ )
{
Camera.grab();
Camera.retrieve ( image );
//threshold(image,image,low_threshold,high_threshold,cv::THRESH_BINARY);
//if(image_counter==50)
//{
cout << "Log: beléptem a " << image_counter << ". képbe" << endl;
vector<Point> points;
image_processing(image,points);
cout << "A " << image_counter << ". képen a vonal helye: " << vector_average(points,2) << endl;
//}
}
Camera.grab();
Camera.retrieve(image);
time ( &timer_end );
Camera.release();
cout<<"Stop camera..."<<endl;
//show time statistics
double secondsElapsed = difftime ( timer_end,timer_begin );
cout<< secondsElapsed<<" seconds for "<< nCount<<" frames : FPS = "<< ( float ) ( ( float ) ( nCount ) /secondsElapsed ) <<endl;
/**
*Képek itt kerülnek mentésre:
* 1.: az eredeti kép (grayscale)
* 2.: a küszöbözött kép - egyenlőre nincs felhasználva ez a függvény, hiszen
* még nincs rá szükség
* 3.: az eredet kép immáron színesben: rajta a vonal két szélének (piros) és a
* középének (zöld) a becslőjével
*/
imwrite("raspicam_cv_image_original.jpg",image);
Mat colourful_image = image.clone();
cvtColor(colourful_image,colourful_image,CV_GRAY2BGR);
vector<Point> points;
image_processing(image,points);
int sum_left = vector_average(points,1);
int sum_average = vector_average(points,2);
int sum_right = vector_average(points,3);
cout<<"Average: "<< sum_average << endl;
cout<<"Sum_left: "<< sum_left << endl;
cout<<"Sum_right: "<< sum_right << endl;
line(colourful_image,Point(sum_left ,1), Point(sum_left ,frame_height),Scalar(0,0,255),2);
line(colourful_image,Point(sum_right ,1), Point(sum_right ,frame_height),Scalar(0,0,255),2);
line(colourful_image,Point(sum_average,1), Point(sum_average,frame_height),Scalar(0,255,0),2);
threshold(image,image,low_threshold,high_threshold,cv::THRESH_BINARY);
imwrite("raspicam_cv_image_threshold.jpg",image);
imwrite("raspicam_cv_image_colourful.jpg",colourful_image);
closeSpi();
}
void Robot::trackLine() {
RaspiCam_Cv camera;
if (camera.open()) {
Mat captured;
vector<vector<Point> > contours;
vector<Vec4i> hierarchy;
while (1) {
camera.grab();
camera.retrieve(captured);
Point capturedImageCenter(captured.cols / 2, captured.rows / 2);
circle(captured, capturedImageCenter, 5, Scalar(0, 255, 0), -1, 8,
0);
// region of interest. currently around the center of the
// captured image. might need to be moved depending on the
// camera angle, speed of the robot etc.
Rect roi(0, capturedImageCenter.y - 45, captured.cols, 100);
Mat roiImg = captured(roi).clone();
// extract the line to follow from the background
cvtColor(roiImg, roiImg, COLOR_BGR2GRAY);
threshold(roiImg, roiImg, lineTrackingThreshold, MAX_BINARY_VALUE, 1);
// displaying this is useful to see if the threshold is OK
if (!headless) {
imshow(THRESHOLD_WINDOW_NAME, roiImg);
waitKey(30);
}
Mat erodeElmt = getStructuringElement(MORPH_RECT, Size(3, 3));
Mat dilateElmt = getStructuringElement(MORPH_RECT, Size(5, 5));
erode(roiImg, roiImg, erodeElmt);
dilate(roiImg, roiImg, dilateElmt);
// determine the angle the robot needs to move
findContours(roiImg, contours, hierarchy, CV_RETR_TREE,
CV_CHAIN_APPROX_SIMPLE, Point(0, 0));
if (contours.size() == 0) {
cout << "no tracking line detected." << endl;
pushMotor->stop();
// return;
}
for (size_t i = 0; i < contours.size(); i++) {
float area = contourArea(contours[i]);
if (area > 2000) {
Moments mu;
mu = moments(contours[i], false);
Point2f roiCenter(mu.m10 / mu.m00,
capturedImageCenter.y - LOOK_AHEAD); // point in center (x only)
circle(captured, roiCenter, 5, Scalar(0, 255, 0), -1, 8, 0);
line(captured, capturedImageCenter, roiCenter,
Scalar(0, 255, 0));
float ankathete = LOOK_AHEAD;
float gegenkathete = capturedImageCenter.x - roiCenter.x;
float bogenmass = atan(gegenkathete / ankathete);
float gradmass = bogenmass * 180 / 3.1415926535;
string caption = format("%f Grad", gradmass);
putText(captured, caption,
Point(capturedImageCenter.x + 20,
capturedImageCenter.y + 20),
FONT_HERSHEY_SIMPLEX, 1, Scalar(0, 255, 0));
if (!pushMotor->isStarted()) {
pushMotor->startCounterClockwise();
}
steer(gradmass);
}
}
if (!headless) {
imshow(MAIN_WINDOW_NAME, captured);
imshow(ROI_WINDOW_NAME, roiImg);
waitKey(30);
}
}
} else {
cout << "failed to open camera." << endl;
exit(EXIT_FAILURE);
}
}
