Mat TargetExtractor::extract(const Mat& frame, map<int, Target>& targets, bool track)
{
mFrame = frame;
/* for 2.avi:
* movement: 0.008;
* color: 120, 0.2;
* regionGrow: enable;
* for 6.avi:
* movement: 0.012;
* color: 150, 0.4;
* regionGrow: disable;
*/
movementDetect(0.012);
colorDetect(150, 0.4);
denoise(7, 5);
fill(7, 5);
medianBlur(mMask, mMask, 3);
// TODO: make use of accumulate result
//regionGrow();
//fill(7, 6);
//medianBlur(mMask, mMask, 3);
//Mat element = getStructuringElement(MORPH_CROSS, Size(3, 3));
//erode(mMask, mMask, element);
//dilate(mMask, mMask, element);
smallAreaFilter(12, 8);
#ifdef DEBUG_OUTPUT
namedWindow("mask");
moveWindow("mask", 350, 120);
imshow("mask", mMask);
#endif
if (track) {
blobTrack(targets);
}
return mMask;
}
int main( int argc, char** argv )
{
ros::init(argc, argv, "face_detect");
ros::NodeHandle n;
opencvCommands = n.advertise<robotBrain::opencvMessage>("opencv_commands",1000);
image_transport::ImageTransport it(n);
pub = it.advertise("camera/image", 1);
cv::VideoCapture cap(1);
if(!cap.isOpened()){
ROS_FATAL("opencv: COULD NOT OPEN CAMERA" );
sendError();
}
//Load the cascades
if( !face_cascade.load( face_cascade_name ) ){
ROS_FATAL("--(!)Error loading FACE CASCADE(!)--" );
sendError();
}
if( !eyes_cascade.load( eyes_cascade_name ) ){
ROS_FATAL("--(!)Error loading EYE CASCADE(!)--");
sendError();
}
cap.set(CV_CAP_PROP_FRAME_WIDTH, 320);
cap.set(CV_CAP_PROP_FRAME_HEIGHT, 240);
char key;
while ( n.ok() ){
cap.read(frame);
if( !frame.empty() ) colorDetect();//detectAndDisplay();
else {
ROS_FATAL("opencv: FRAME FAIL " );
sendError();
}
//showing image
cv::namedWindow( "Patrolling Android View", CV_WINDOW_AUTOSIZE );
cv::startWindowThread();
cv::imshow( "Patrolling Android View", imgHSV);
if (color & !face ) faceDetect(); //if we have a color and we havent previously detected a face we look for a face
if(face) personTracking(); //if we have a face we follow the color
else { //if not
message.camera = 's';
message.errorOpenCV = '0';
opencvCommands.publish( message );
}
key = cv::waitKey(100);
switch (key)
{
//hue
case 'q':
if (hue_low == hue_high)
ROS_INFO("hue_low must be less than hue_high");
else
hue_low = hue_low + 1;
break;
case 'a':
if (hue_low == 0)
ROS_INFO("Hue is minimum");
else
hue_low = hue_low - 1;
break;
case 'w':
if (hue_high == 255)
ROS_INFO("Hue is maximum");
else
hue_high = hue_high + 1;
break;
case 's':
if (hue_high == hue_low)
ROS_INFO("hue_high must be greater than hue_low");
else
hue_high = hue_high - 1;
break;
//saturation
case 'e':
if (sat_low == sat_high)
ROS_INFO("sat_low must be less than sat_high");
else
sat_low = sat_low + 1;
break;
case 'd':
if (sat_low == 0)
ROS_INFO("sat is minimum");
else
sat_low = sat_low - 1;
break;
case 'r':
if (sat_high == 255)
ROS_INFO("sat is maximum");
else
sat_high = sat_high + 1;
break;
case 'f':
if (sat_high == sat_low)
ROS_INFO("sat_high must be greater than sat_low");
else
sat_high = sat_high - 1;
break;
//value
case 't':
if (val_low == val_high)
ROS_INFO("val_low must be less than val_high");
else
val_low = val_low + 1;
break;
case 'g':
if (val_low == 0)
ROS_INFO("val is minimum");
else
val_low = val_low - 1;
break;
case 'y':
if (val_high == 255)
ROS_INFO("val is maximum");
else
val_high = val_high + 1;
break;
case 'h':
if (val_high == val_low)
ROS_INFO("val_high must be greater than val_low");
else
val_high = val_high - 1;
break;
}
//ROS_INFO("Frames");
ROS_INFO("Hue: %d-%d\tSat: %d-%d\tVal: %d-%d\n", hue_low, hue_high, sat_low, sat_high, val_low, val_high);
}
ROS_FATAL("COULD NOT CAPTURE FRAME");
return -1;
}
int main(void)
{
// variables
char val = 0;
float dist = 0;
int xx = 0;
struct timespec tim, tim2; // tim holds the desired sleep time, tim2 is filled with the remaining time if error occurs
tim.tv_sec = 0;
tim.tv_nsec = 500000000L; // 1/2 second sleep time
// gpio initialization
wiringPiSetup();
pinMode(0,OUTPUT); // stop sign signal
digitalWrite(0,LOW);
pinMode(7,OUTPUT); // vibration motor signal
digitalWrite(0,LOW);
pinMode(15,INPUT); // power switch
pullUpDnControl(15, PUD_DOWN);
// open default camera 0
VideoCapture capture(0);
if(!capture.isOpened())
{
// TODO need to send a message to pcb
cout<<"ERROR: webcam problem\n";
return 1;
}
// read current status of power switch
xx = digitalRead(15);
// 3 1/2 second vibrations to indicate power on
digitalWrite(7,HIGH);
if(nanosleep(&tim,&tim2) == EXIT_FAILURE)
return EXIT_FAILURE;
digitalWrite(7,LOW);
if(nanosleep(&tim,&tim2) == EXIT_FAILURE)
return EXIT_FAILURE;
digitalWrite(7,HIGH);
if(nanosleep(&tim,&tim2) == EXIT_FAILURE)
return EXIT_FAILURE;
digitalWrite(7,LOW);
if(nanosleep(&tim,&tim2) == EXIT_FAILURE)
return EXIT_FAILURE;
digitalWrite(7,HIGH);
if(nanosleep(&tim,&tim2) == EXIT_FAILURE)
return EXIT_FAILURE;
digitalWrite(7,LOW);
// play power on sound track over speaker
system("omxplayer -o local /home/pi/capstone/gdr_rasp/audio/ready2.mp3");
// main loop
while(1)
{
// check for shutdown command
xx = digitalRead(15);
if(xx == 0)
{
// 2 1/2 second vibrations to indicate power off
digitalWrite(7,HIGH);
if(nanosleep(&tim,&tim2) == EXIT_FAILURE)
return EXIT_FAILURE;
digitalWrite(7,LOW);
if(nanosleep(&tim,&tim2) == EXIT_FAILURE)
return EXIT_FAILURE;
digitalWrite(7,HIGH);
if(nanosleep(&tim,&tim2) == EXIT_FAILURE)
return EXIT_FAILURE;
digitalWrite(7,LOW);
// play power down audio file
system("omxplayer -o local /home/pi/capstone/gdr_rasp/audio/power_off.mp3");
// shutdown raspberry pi
system("shutdown now");
}
Mat frame; // holds a captured frame
capture >> frame; // get a new frame from camera
// process image
Mat red = colorDetect(frame);
Mat out = shapeDetect(red, &dist);
// display processed image (testing only) XXX
/* imshow("result", out); */
// print distance (testing only) XXX
/* printf("%f\n",dist); */
// write pin high if within 6 ft of stop sign
if(val <= 90 && val > 20)
{
digitalWrite(0,HIGH);
}
digitalWrite(0,LOW);
dist = 0;
// if any key is pressed exit (needed for displaying image) XXX
/* if(waitKey(30) >= 0) break; */
}
return 0;
}
