Beispiel #1
0
void callback(const sensor_msgs::ImageConstPtr &imgPtr){
    cvImageFromROS =cv_bridge::toCvCopy(imgPtr);
    diff =cv_bridge::toCvCopy(imgPtr);

    cvImageFromROS->image.copyTo(image);
    diff->image.copyTo(DiffImage);
    out_msg.header   = imgPtr->header;
    out_msg.encoding = "mono16";


    int cols=image.cols;
    int rows=image.rows;
    ushort v;
    cv::Point p;
    for(int i=0;i<cols;i++){
        for(int j=0;j<rows;j++){
            p.x=i;
            p.y=j;
            v=((float)image.at<ushort>(p));
            v*=multiplier->cell(p.y,p.x,v);
            image.at<ushort>(p)=(ushort)v;
        }
    }

    out_msg.image    =  image;
    pub.publish(out_msg.toImageMsg());
    out_msg.image    =  image-DiffImage;
    pub2.publish(out_msg.toImageMsg());
    //std::cout<<"global diff: "<< cv::sum(out_msg.image)/(out_msg.image.rows*out_msg.image.cols)<<std::endl;

}
void faceDetect(){
  //Detect faces
   cv::cvtColor( frame, frame_gray, CV_BGR2GRAY );
   cv::equalizeHist( frame_gray, frame_gray );
   face_cascade.detectMultiScale( frame_gray, faces, 1.1, 2, 0|CV_HAAR_SCALE_IMAGE, cv::Size(30, 30) );
  
   //for each face draws an ellipse arround and look for the red color at a distance from 
   for( i = 0; i < faces.size(); i++ )
    {
      cv::Point center( faces[i].x + faces[i].width*0.5, faces[i].y + faces[i].height*0.5 );
      cv::ellipse( frame, center, cv::Size( faces[i].width*0.5, faces[i].height*0.5), 0, 0, 360, cv::Scalar( 0, 255, 0 ), 2, 8, 0 );
      faceX = (float) faces[i].x;
      faceY = (float) faces[i].y;
     
      if( ((faceX + faceColorThresh) > (posX ) | (faceX - faceColorThresh) < (posX )) ) {
	face = true; 
	//publishing camera image
	out_msg.image    = frame; //frame
	out_msg.encoding = sensor_msgs::image_encodings::BGR8;
	msg = out_msg.toImageMsg();
	pub.publish(msg);
	ROS_FATAL("PERSON DETECTED");
	break;
      }
    }
}
	inline void publish(Mat img, imgColorType t, ros::Time now =
			ros::Time::now())
	{
		hasListener=(img_pub.getNumSubscribers() > 0);
		if (!enable || !hasListener)
			return;
		Mat res;
		switch (t)
		{
		case gray:
			msg.encoding = sensor_msgs::image_encodings::MONO8;
			res = img;
			break;
		case hsv:
			msg.encoding = sensor_msgs::image_encodings::BGR8;
			cvtColor(img, res, CV_HSV2BGR);
			break;
		case bgr:
		default:
			msg.encoding = sensor_msgs::image_encodings::BGR8;
			res = img;
			break;
		}
		msg.header.stamp = now;
		msg.image = res;
		img_pub.publish(msg.toImageMsg());
	}
Beispiel #4
0
void callback(const sensor_msgs::ImageConstPtr &imgPtr){
    cvImageFromROS =cv_bridge::toCvCopy(imgPtr);

    cvImageFromROS->image.copyTo(image);
    out_msg.header   = imgPtr->header;
    out_msg.encoding = "mono16";
    out_msg.image    = image;
    pub.publish(out_msg.toImageMsg());
}
/***********************************************************
* @fn main(int argc, char **argv)
* @brief starts the Pot_Nav node and publishises twist when 
* it gets a new image asuming 30 hz
***********************************************************/
int main(int argc, char **argv)
{
	ros::init(argc, argv, "Pot_Nav");
    ros::NodeHandle n;
    image_transport::ImageTransport it(n);
    
    
	dynamic_reconfigure::Server<MST_Potential_Navigation::Pot_Nav_ParamsConfig> srv;
    dynamic_reconfigure::Server<MST_Potential_Navigation::Pot_Nav_ParamsConfig>::CallbackType f;
    f = boost::bind(&setparamsCallback, _1, _2);
	srv.setCallback(f);

	//create subsctiptions
    image_sub_edges = it.subscribe( n.resolveName("image_edges") , 1, edgesCallback );

    //create publishers
    twist_pub = n.advertise<geometry_msgs::Twist>( "nav_twist" , 5 );
    map_pub = it.advertise( "map" , 5 );
    
    //set rate to 30 hz
    ros::Rate loop_rate(30);
    
    
    
    //run main loop
	while (ros::ok())
    {
		//check calbacks
		ros::spinOnce();
		
		//finds and publishes new twist
		if(map_changed)
		{
			geometry_msgs::Twist twist;
			
			twist = find_twist(); 
			
			//publish twist
			twist_pub.publish(twist);
			
			//publish map
			map_pub.publish(map_dis.toImageMsg());
			
			map_changed = 0;
		}
		
		loop_rate.sleep();
    }
    
    return 0;
}
void HiwrCameraControllerNodelet::publishFrame(Mat frame ) {
    out_msg_.encoding = sensor_msgs::image_encodings::MONO8;
    out_msg_.image    = frame;
    out_msg_.header.seq = published_frame_;


    char frame_id[20];
    sprintf(frame_id , "%d" , published_frame_);
    out_msg_.header.frame_id = frame_id;
    out_msg_.header.stamp = ros::Time::now();

    published_frame_++;
    image_publisher_.publish(out_msg_.toImageMsg() );
}
Beispiel #7
0
void mapSubCallback(const nav_msgs::OccupancyGrid map) {
  //================================================================MAP TO IMAGE
  MAP_X = map.info.width;
  MAP_Y = map.info.height;

  MAP_RESOLUTION = map.info.resolution;

  if ((MAP_X < 3) || (MAP_Y < 3) ) {
    ROS_INFO("Map size is only x: %ld,  y: %ld . Not running map to image conversion", MAP_X, MAP_Y);
    return;
  }

  cv::Mat *map_mat  = &cv_img.image;

  // resize cv image if it doesn't have the same dimensions as the map
  if ((map_mat->rows != MAP_Y) && (map_mat->cols != MAP_X)) {
    *map_mat = cv::Mat(MAP_Y, MAP_X, CV_8U);
  }

  // ROS_INFO("Fault_1");

  const std::vector<int8_t>&map_data(map.data);

  unsigned char *map_mat_data_p = (unsigned char *)map_mat->data;

  // ROS_INFO("Fault_2");

  //We have to flip around the y axis, y for image starts at the top and y for map at the bottom

  int MAP_Y_rev = MAP_Y - 1;

  // ROS_INFO_STREAM("Fault for :" << MAP_Y_rev);

  for (int y = MAP_Y_rev; y >= 0; --y) {
    int idx_map_y = MAP_X * (MAP_Y - y);
    int idx_img_y = MAP_X * y;

    // ROS_INFO_STREAM("Fault_3 :" << y);

    // if (y == 0) {
    //   ROS_INFO_STREAM("MAP_X :" << MAP_X);
    // }

    for (int x = 0; x < MAP_X; ++x) {
      int idx = idx_img_y + x;
      //
      // if (y == 0 && x > 1000) {
      //   ROS_INFO_STREAM("x :" << x);
      //   try {
      //     //ROS_INFO_STREAM("valid:" << isValidPtr(map_data, idx_map_y + x));
      //     ROS_INFO_STREAM("Map data :" << map_data[idx_map_y + x]);
      //   } catch (std::exception& e) {
      //     std::cout << e.what() << std::endl;
      //   }
      // }

      switch (map_data[idx_map_y + x]) {
        case -1:
          map_mat_data_p[idx] = 255; //grey color:unknown default value of 127
          break;

        case 0:
          map_mat_data_p[idx] = 255; // laser ray:default 255
          break;

        case 100:
          map_mat_data_p[idx] = 0; // obstacle:default 0
          break;
      }

      // if (y == 0 && x > 1000) {
      //   ROS_INFO_STREAM("x :" << x);
      // }
    }

    // ROS_INFO_STREAM("Fault_3 :" << y);
  }

  // ROS_INFO("Fault_4");


//======================================================================== DETECT TREES
  cv::Mat im, im_with_keypoints;

// Reduce the noise
  if (GaussianBlur_kernelSize > 1) {
    cv::GaussianBlur(cv_img.image, im, cv::Size(GaussianBlur_kernelSize, GaussianBlur_kernelSize), 0);
  } else {
    im = cv_img.image;
  }

// Setup SimpleBlobDetector parameters.
  cv::SimpleBlobDetector::Params params;

// Change thresholds
  params.minThreshold = minThreshold;
  params.maxThreshold = maxThreshold;

// Filter by Area.
  if (minArea < maxArea) {
    params.filterByArea = true;
    params.minArea = minArea;
    params.maxArea = maxArea;
  } else {
    params.filterByArea = false;
  }

// Filter by Circularity
  if (minCircularity < maxCircularity) {
    params.filterByCircularity = true;
    params.minCircularity = minCircularity;
    params.maxCircularity = maxCircularity;
  } else {
    params.filterByCircularity = false;
  }

// Filter by Convexity
  if (minConvexity < maxConvexity) {
    params.filterByConvexity = true;
    params.minConvexity = minConvexity;
    params.maxConvexity = maxConvexity;
  } else {
    params.filterByConvexity = false;
  }

// Filter by Inertia
// params.filterByInertia = true;
// params.minInertiaRatio = 0.01;

// Storage for blobs
  std::vector<cv::KeyPoint> keypoints;

// Set up detector with params
  cv::Ptr<cv::SimpleBlobDetector> detector = cv::SimpleBlobDetector::create(params);

// Detect blobs
  detector->detect(im, keypoints);

//================================================================ Publish Markers
  visualization_msgs::Marker marker;
  marker.header.frame_id = "map";
  marker.header.stamp = ros::Time();
  marker.ns = "agri_mapper";
  marker.id = 0;
  marker.type = visualization_msgs::Marker::SPHERE_LIST;
  marker.action = visualization_msgs::Marker::ADD;
  marker.pose.position.x = -(MAP_X * MAP_RESOLUTION / 2) - 0.2;
  marker.pose.position.y = -(MAP_Y * MAP_RESOLUTION / 2) - 0.2;
  marker.pose.position.z = 0;
  marker.pose.orientation.x = 0;
  marker.pose.orientation.y = 0;
  marker.pose.orientation.z = 0;
  marker.pose.orientation.w = 1;
  marker.scale.x = 0.1;
  marker.scale.y = 0.1;
  marker.scale.z = 0.1;
  marker.color.a = 1.0; // Don't forget to set the alpha!
  marker.color.r = 0.0;
  marker.color.g = 1.0;
  marker.color.b = 0.0;

  marker.points.resize(keypoints.size());

  std::vector<cv::KeyPoint>::iterator it = keypoints.begin();

  ROS_INFO_STREAM("making markers");
  ROS_INFO_STREAM("\n Size : " << keypoints.size() << "\n");

  for (int i = 0; it != keypoints.end(); ++it, i++) {
    marker.points[i].x = (it->pt.x) * MAP_RESOLUTION;
    marker.points[i].y = (MAP_Y - it->pt.y) * MAP_RESOLUTION;
    marker.points[i].z = 0;
  }

  ROS_INFO_STREAM("made markers");

  vis_pub.publish(marker);

//============================================================================PUBLISH IMAGE

// Draw detected blobs as red circles.
// DrawMatchesFlags::DRAW_RICH_KEYPOINTS flag ensures
// the size of the circle corresponds to the size of blob

  cv::drawKeypoints(im, keypoints, im_with_keypoints, cv::Scalar(255, 0, 0), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);

  cv_detectionImg.image = im_with_keypoints;

  image_pub.publish(cv_img.toImageMsg());
  detected_pub.publish(cv_detectionImg.toImageMsg());
}
int main(int argc, char **argv)
{
    ros::init(argc, argv, "people_counting");
    ros::NodeHandle node;

    ros::param::param<std::string>(ros::this_node::getName() + "dataset", fileDataset_param, "haarcascade_upperbody.xml");
    ros::param::param<double>(ros::this_node::getName() + "scaleFactor", scaleFactor_param, 0.1);
    ros::param::param<int>(ros::this_node::getName() + "minNeighbors", minNeighbors_param, 0);
    // ros::param::param<double>("minSize", minSize_param, 0);
    ros::param::param<double>(ros::this_node::getName() + "maxSize", maxSize_param, 1.0);

    // argc--; argv++;
    // while( argc && *argv[0] == '-' )
    // {
    //     if( !strcmp(*argv, "-fps") && ( argc > 1 ) )
    //     {
    //         fps = atof(*(argv+1));
    //         printf("With %ffps\n",fps);
    //         argc--; argv++;
    //     }
    //     if( !strcmp(*argv, "-crowd_dataset") && ( argc > 1 ) )
    //     {
    //         file_dataset = *(argv+1);
    //         printf("Using dataset form %s\n",file_dataset.c_str());
    //         argc--; argv++;
    //     }
    //     argc--; argv++;
    // }

    std::vector<cv::Rect> objects;
    std::vector<cv::Scalar> colors;
    cv::RNG rng(12345);

    img_msg.encoding = "bgr8";

    if ( ! classifier.load(fileDataset_param))
        ROS_ERROR("Can't open %s", fileDataset_param.c_str());

    ros::Publisher img_pub = node.advertise<sensor_msgs::Image>("crowd/count_img", 100);
    ros::Publisher count_pub = node.advertise<std_msgs::UInt16>("crowd/people_count", 100);

    image_transport::ImageTransport it(node);
    image_transport::Subscriber img_sub = it.subscribe("image", 1, imageCallback);

    ros::Rate loop_rate(fps);

    while (ros::ok())
    {
        if (! img_msg.image.empty()) {
            classifier.detectMultiScale(img_msg.image, objects, 1.0+scaleFactor_param, minNeighbors_param+1, 0, cv::Size(), maxSize);
            for (int j = 0; j < objects.size(); ++j) {
                if ( j > max_detected) {
                    colors.push_back(cv::Scalar(rng.uniform(0,255),rng.uniform(0, 255),rng.uniform(0, 255)));
                    max_detected = j;
                }
                cv::rectangle(img_msg.image, objects[j], colors[j], 2);
            }
            people_count.data = (u_int16_t)objects.size();
        }
        img_pub.publish(img_msg.toImageMsg());
        count_pub.publish(people_count);

        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}
void BallDetector::imageCb(const sensor_msgs::ImageConstPtr& msg){
  static cv_bridge::CvImageConstPtr cv_const_ptr;

#ifdef BALLDETECTOR_DEBUG
  static cv_bridge::CvImage cv_img;
  cv_img.header.stamp = ros::Time::now();

  if(debugLevel.sendDebugTimes){
    debugTimeInit();
    debugTimeStart();
  }
#endif

  try{
    cv_const_ptr = cv_bridge::toCvShare(msg, enc::BGR8);
  }catch (cv_bridge::Exception& e){
    ROS_ERROR("cv_bridge exception: %s", e.what());
    return;
  }

#ifdef BALLDETECTOR_DEBUG
  if(debugLevel.sendDebugTimes){
    debugTimeRecordAndReStart("cvconvert");
  }
#endif

#ifdef BALLDETECTOR_DEBUG
  //Get a the image just as a Mat
  static cv::Mat colorImg;
  if(debugLevel.sendDebugImages){
    colorImg = cv_const_ptr->image.clone();
    //cv_const_ptr->image.copyTo(colorImg);
  }else{
    colorImg = cv_const_ptr->image;
  }
#else 
  static cv::Mat colorImg;
  colorImg = cv_const_ptr->image;
#endif

  //Create the hsv image
  static cv::Mat hsvImg;
#ifdef BALLDETECTOR_DEBUG
  if(debugLevel.sendDebugTimes){
    debugTimeRecordAndReStart("allocate");
  }
#endif
  
  //colorImg is type CV_8U
  cv::cvtColor(colorImg,hsvImg,CV_RGB2HSV);
  //cv::cvtColor(colorImg,hsvImg,CV_RGB2HLS);
  //cv::cvtColor(colorImg,hsvImg,CV_RGB2Lab); //faster but not on gumstix?

#ifdef BALLDETECTOR_DEBUG
  if(debugLevel.sendDebugTimes){
    debugTimeRecordAndReStart("hsv");
  }
#endif

  //Perform thresholding
  static cv::Mat threshImg;
  cv::inRange(hsvImg,lowThresh,highThresh,threshImg);


#ifdef BALLDETECTOR_DEBUG
  if(debugLevel.sendDebugTimes){
    debugTimeRecordAndReStart("threshold");
  }
#endif

#ifdef BALLDETECTOR_DEBUG
  if(debugLevel.sendDebugImages){
    //Send some images out
    cv_img.image = threshImg;
    cv_img.encoding = enc::MONO8; // TYPE_8UC1;
    debugImg2_pub_.publish(cv_img.toImageMsg());

    cv_img.image = hsvImg;
    cv_img.encoding = enc::BGR8;
    debugImg3_pub_.publish(cv_img.toImageMsg());

  }
#endif /** BALLDETECTOR_DEBUG **/

#ifdef BALLDETECTOR_DEBUG
  if(debugLevel.sendDebugTimes){
    debugTimeStart();
  }
#endif
  //Contours
  static vector<vector<cv::Point> > contours;
  contours.clear();
  static vector<cv::Vec4i> hierarchy;
  hierarchy.clear();

  //Some quick testing indicates CV_RETR_* doesn't make too much
  //difference in performance.  The CV_CHAIN_APPROX_* also doesn't
  //seem to make much difference, but the "NONE" draws the full
  //outline with debug mode (taking more time), but is easier to look
  //at the output.
  cv::findContours( threshImg, contours, hierarchy,
  CV_RETR_CCOMP, CV_CHAIN_APPROX_NONE);
  //cv::findContours( threshImg, contours, hierarchy,
  //CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE );


#ifdef BALLDETECTOR_DEBUG
  if(debugLevel.sendDebugTimes){
    debugTimeRecordAndReStart("findcontours");
  }
#endif

  // iterate through all the top-level contours,
  // draw each connected component with its own random color
  int idxMax = -1;
  unsigned int idxMaxValue = 0;
  int idx = 0;
  if(hierarchy.size() > 0){
    for( ; idx >= 0; idx = hierarchy[idx][0] ){
      //If it is large enough, fit an ellipse to it
      if(contours.at(idx).size() > 15){
        //Slower than the below methods
        //cv::RotatedRect r = cv::fitEllipse(cv::Mat(contours.at(idx)));
        //cv::Rect rect = r.boundingRect();

        //Find the min/max pixel locatiosn of x and y
        int maxX = 0, maxY = 0;
        unsigned int maxXidx = 0, maxYidx = 0;
        //Just needs to be bigger than max image width/height
        int minX = 999999999, minY = 999999999;
        unsigned int minXidx = 0, minYidx = 0;
        for(unsigned int i = 0; i < contours.at(idx).size(); i++){
          cv::Point p = contours.at(idx).at(i);
          if(p.x > maxX){
            maxXidx = i;
            maxX = p.x;
          }
          else if(p.x < minX){
            minXidx = i;
            minX = p.x;
          }
          if(p.y > maxY){
            maxYidx = i;
            maxY = p.y;
          }
          else if(p.y < minY){
            minYidx = i;
            minY = p.y;
          }
        }

        //Keep the ellipse if it is mostly round
          //if((rect.width/(1.0*rect.height) > 0.75) && (rect.height/(1.0*rect.width) > 0.75)){

        if(((1.0*(contours.at(idx).at(maxXidx).x-contours.at(idx).at(minXidx).x)/
             (contours.at(idx).at(maxYidx).y-contours.at(idx).at(minYidx).y))) > 0.75
           &&
           ((1.0*(contours.at(idx).at(maxYidx).y-contours.at(idx).at(minYidx).y)/
             (contours.at(idx).at(maxXidx).x-contours.at(idx).at(minXidx).x)) > 0.75)){
#ifdef BALLDETECTOR_DEBUG
          if(debugLevel.sendDebugImages){
            //cv::ellipse(colorImg,r,cv::Scalar(255,0,0,0),3);

            //Color all the points white (it isn't all points, but a lot on the boarders)
            for(unsigned int i = 0; i < contours.at(idx).size(); i++){
              cv::Point p = contours.at(idx).at(i);
              //Get a pointer to the row, then access the pixel elements
              colorImg.ptr<uchar>(p.y)[3*p.x] = 255;
              colorImg.ptr<uchar>(p.y)[3*p.x+1] = 255;
              colorImg.ptr<uchar>(p.y)[3*p.x+2] = 255;
            }

            //draw max/min points
            /*
            cv::rectangle(colorImg,
                          cv::Point(contours.at(idx).at(maxXidx).x - 1, 
                                    contours.at(idx).at(maxXidx).y- 1),
                          cv::Point(contours.at(idx).at(maxXidx).x + 1, 
                                    contours.at(idx).at(maxXidx).y+ 1),
                          cv::Scalar(255,0,0,0),2);
            cv::rectangle(colorImg,
                          cv::Point(contours.at(idx).at(maxYidx).x - 1, 
                                    contours.at(idx).at(maxYidx).y- 1),
                          cv::Point(contours.at(idx).at(maxYidx).x + 1, 
                                    contours.at(idx).at(maxYidx).y+ 1),
                          cv::Scalar(255,0,0,0),2);
            cv::rectangle(colorImg,
                          cv::Point(contours.at(idx).at(minXidx).x - 1, 
                                    contours.at(idx).at(minXidx).y- 1),
                          cv::Point(contours.at(idx).at(minXidx).x + 1, 
                                    contours.at(idx).at(minXidx).y+ 1),
                          cv::Scalar(255,0,0,0),2);
            cv::rectangle(colorImg,
                          cv::Point(contours.at(idx).at(minYidx).x - 1, 
                              contours.at(idx).at(minYidx).y- 1),
                          cv::Point(contours.at(idx).at(minYidx).x + 1, 
                                    contours.at(idx).at(minYidx).y+ 1),
                          cv::Scalar(255,0,0,0),2);
            */
          }
#endif /** BALLDETECTOR_DEBUG **/
          //If there are more points in it than our previous max, then record it
          //N.B. contours.at(idx).size() isn't all points, rather a
          //representation of them...but it seems to map fairly well.
          if(idxMaxValue < contours.at(idx).size()){
            idxMaxValue = contours.at(idx).size();
            idxMax = idx;
          }
        }
      }
    }  
  }


#ifdef BALLDETECTOR_DEBUG
  if(debugLevel.sendDebugTimes){
    debugTimeRecordAndReStart("iteratecontours");
  }
#endif

  //Make sure we found a good one
  if(idxMax >= 0){

    //Now publish and draw the best
    cv::RotatedRect ellipse = cv::fitEllipse(cv::Mat(contours.at(idxMax)));
    cv::Rect rect = ellipse.boundingRect();
    //Publish the message for the best
    ballDetector::ballLocation circ;
    circ.header.stamp = ros::Time::now();
    //Use the average of the height and width as the radius
    double radius = (rect.height/2.0 + rect.width/2.0)/2.0;
    circ.imageWidth = colorImg.cols;
    circ.imageHeight = colorImg.rows;
    //Make it relative to the center of the image
    circ.x = rect.x + radius - threshImg.cols/2;
    circ.y = -1*(rect.y + radius - threshImg.rows/2);
    circ.radius = radius;
    circle_pub_.publish(circ);
#ifdef BALLDETECTOR_DEBUG
    if(debugLevel.sendDebugImages){
      //Draw the best one bold
      cv::ellipse(colorImg,ellipse,cv::Scalar(0,255,0,0),4);
      //And the center point
      cv::rectangle(colorImg,
                    cv::Point(rect.x + radius-2,rect.y + radius-2),
                    cv::Point(rect.x + radius+2,rect.y + radius+2),
                    cv::Scalar(0,255,0,0),4);

      //Find min/max x,y pixel locations to draw them
      //cout << "---------------------\n";
      int maxX = 0, maxY = 0;
      unsigned int maxXidx = 0, maxYidx = 0;
      //Just needs to be bigger than max image width/height
      int minX = 999999999, minY = 999999999;
      unsigned int minXidx = 0, minYidx = 0;
      for(unsigned int i = 0; i < contours.at(idxMax).size(); i++){
        cv::Point p = contours.at(idxMax).at(i);
        //printf("%d: [%d,%d] ",i,p.x,p.y);
        if(p.x > maxX){
          maxXidx = i;
          maxX = p.x;
        }
        else if(p.x < minX){
          minXidx = i;
          minX = p.x;
        }
        if(p.y > maxY){
          maxYidx = i;
          maxY = p.y;
        }
        else if(p.y < minY){
          minYidx = i;
          minY = p.y;
        }
      }


      /*
      printf("\nminX: %d, %d, %d, %d\n",minX,minXidx,
             contours.at(idxMax).at(minXidx).x,contours.at(idxMax).at(minXidx).y);
      printf("maxX: %d, %d, %d, %d\n",maxX,maxXidx,
             contours.at(idxMax).at(maxXidx).x,contours.at(idxMax).at(maxXidx).y);
      printf("minY: %d, %d, %d, %d\n",minY,minYidx,
             contours.at(idxMax).at(minYidx).x,contours.at(idxMax).at(minYidx).y);
      printf("maxY: %d, %d, %d, %d\n",maxY,maxYidx,
             contours.at(idxMax).at(maxYidx).x,contours.at(idxMax).at(maxYidx).y);
      printf("diffX: %d\n",contours.at(idxMax).at(maxXidx).x-contours.at(idxMax).at(minXidx).x);
      printf("diffY: %d\n",contours.at(idxMax).at(maxYidx).y-contours.at(idxMax).at(minYidx).y);
      printf("ratio: %f %f\n",
             1.0*(contours.at(idxMax).at(maxXidx).x-contours.at(idxMax).at(minXidx).x)/
             (contours.at(idxMax).at(maxYidx).y-contours.at(idxMax).at(minYidx).y),
             1.0*(contours.at(idxMax).at(maxYidx).y-contours.at(idxMax).at(minYidx).y)/
             (contours.at(idxMax).at(maxXidx).x-contours.at(idxMax).at(minXidx).x));
      */

      //draw max/min points
      cv::rectangle(colorImg,
                    cv::Point(contours.at(idxMax).at(maxXidx).x - 1, 
                              contours.at(idxMax).at(maxXidx).y - 1),
                    cv::Point(contours.at(idxMax).at(maxXidx).x + 1, 
                              contours.at(idxMax).at(maxXidx).y + 1),
                    cv::Scalar(0,0,255,0),2);
      cv::rectangle(colorImg,
                    cv::Point(contours.at(idxMax).at(maxYidx).x - 1, 
                              contours.at(idxMax).at(maxYidx).y - 1),
                    cv::Point(contours.at(idxMax).at(maxYidx).x + 1, 
                              contours.at(idxMax).at(maxYidx).y + 1),
                    cv::Scalar(0,0,255,0),2);
      cv::rectangle(colorImg,
                    cv::Point(contours.at(idxMax).at(minXidx).x - 1, 
                              contours.at(idxMax).at(minXidx).y - 1),
                    cv::Point(contours.at(idxMax).at(minXidx).x + 1, 
                              contours.at(idxMax).at(minXidx).y + 1),
                    cv::Scalar(0,0,255,0),2);
      cv::rectangle(colorImg,
                    cv::Point(contours.at(idxMax).at(minYidx).x - 1, 
                              contours.at(idxMax).at(minYidx).y - 1),
                    cv::Point(contours.at(idxMax).at(minYidx).x + 1, 
                              contours.at(idxMax).at(minYidx).y + 1),
                    cv::Scalar(0,0,255,0),2);
    }
#endif /** BALLDETECTOR_DEBUG **/
  }


#ifdef BALLDETECTOR_DEBUG
  if(debugLevel.sendDebugTimes){
    debugTimeRecordAndReStart("publish");
  }
#endif

#ifdef BALLDETECTOR_DEBUG
  if(debugLevel.sendDebugImages){
    cv_img.image = colorImg;
    cv_img.encoding = enc::BGR8;
    debugImg1_pub_.publish(cv_img.toImageMsg());

  }

  if(debugLevel.sendDebugTimes){
    debugTimeSend();
  }
#endif /** BALLDETECTOR_DEBUG **/

}
void imageDataHandler(const sensor_msgs::Image::ConstPtr& imageData) 
{
  timeLast = timeCur;
  timeCur = imageData->header.stamp.toSec() - 0.1163;

  cv_bridge::CvImageConstPtr imageDataCv = cv_bridge::toCvShare(imageData, "mono8");

  if (!systemInited) {
    remap(imageDataCv->image, image0, mapx, mapy, CV_INTER_LINEAR);
    oclImage0 = oclMat(image0);
    systemInited = true;

    return;
  }

  Mat imageLast, imageCur;
  oclMat oclImageLast, oclImageCur;
  if (isOddFrame) {
    remap(imageDataCv->image, image1, mapx, mapy, CV_INTER_LINEAR);
    oclImage1 = oclMat(image1);

    imageLast = image0;
    imageCur = image1;

    oclImageLast = oclImage0;
    oclImageCur = oclImage1;
  } else {
    remap(imageDataCv->image, image0, mapx, mapy, CV_INTER_LINEAR);
    oclImage0 = oclMat(image0);

    imageLast = image1;
    imageCur = image0;

    oclImageLast = oclImage1;
    oclImageCur = oclImage0;
  }
  isOddFrame = !isOddFrame;

  resize(oclImageLast, oclImageShow, showSize);
  oclImageShow.download(imageShow);
  cornerHarris(oclImageShow, oclHarrisLast, 2, 3, 0.04);
  oclHarrisLast.download(harrisLast);

  vector<Point2f> *featuresTemp = featuresLast;
  featuresLast = featuresCur;
  featuresCur = featuresTemp;

  pcl::PointCloud<ImagePoint>::Ptr imagePointsTemp = imagePointsLast;
  imagePointsLast = imagePointsCur;
  imagePointsCur = imagePointsTemp;
  imagePointsCur->clear();

  int recordFeatureNum = totalFeatureNum;
  detectionCount = (detectionCount + 1) % (detectionSkipNum + 1);
  if (detectionCount == detectionSkipNum) {
    oclMat oclFeaturesSub;
    for (int i = 0; i < ySubregionNum; i++) {
      for (int j = 0; j < xSubregionNum; j++) {
        int ind = xSubregionNum * i + j;
        int numToFind = maxFeatureNumPerSubregion - subregionFeatureNum[ind];

        if (numToFind > maxFeatureNumPerSubregion / 5.0) {
          int subregionLeft = xBoundary + (int)(subregionWidth * j);
          int subregionTop = yBoundary + (int)(subregionHeight * i);
          Rect subregion = Rect(subregionLeft, subregionTop, (int)subregionWidth, (int)subregionHeight);

          oclFeatureDetector.maxCorners = numToFind;
          oclFeatureDetector(oclImageLast(subregion), oclFeaturesSub);
          if (oclFeaturesSub.cols > 0) {
            oclFeatureDetector.downloadPoints(oclFeaturesSub, featuresSub);
            numToFind = featuresSub.size();
          } else {
            numToFind = 0;
          }

          int numFound = 0;
          for(int k = 0; k < numToFind; k++) {
            featuresSub[k].x += subregionLeft;
            featuresSub[k].y += subregionTop;

            int xInd = (featuresSub[k].x + 0.5) / showDSRate;
            int yInd = (featuresSub[k].y + 0.5) / showDSRate;

            if (harrisLast.at<float>(yInd, xInd) > 1e-7) {
              featuresLast->push_back(featuresSub[k]);
              featuresInd.push_back(featuresIndFromStart);

              numFound++;
              featuresIndFromStart++;
            }
          }
          totalFeatureNum += numFound;
          subregionFeatureNum[ind] += numFound;
        }
      }
    }
  }

  if (totalFeatureNum > 0) {
    Mat featuresLastMat(1, totalFeatureNum, CV_32FC2, (void*)&(*featuresLast)[0]);
    oclMat oclFeaturesLast(featuresLastMat);
    oclMat oclFeaturesCur, oclFeaturesStatus;

    oclOpticalFlow.sparse(oclImageLast, oclImageCur, oclFeaturesLast, oclFeaturesCur, oclFeaturesStatus);
    if (oclFeaturesCur.cols > 0 && oclFeaturesCur.cols == oclFeaturesStatus.cols) {
      download(oclFeaturesCur, *featuresCur);
      download(oclFeaturesStatus, featuresStatus);
      totalFeatureNum = featuresCur->size();
    } else {
      totalFeatureNum = 0;
    }
  }

  for (int i = 0; i < totalSubregionNum; i++) {
    subregionFeatureNum[i] = 0;
  }

  ImagePoint point;
  int featureCount = 0;
  for (int i = 0; i < totalFeatureNum; i++) {
    double trackDis = sqrt(((*featuresLast)[i].x - (*featuresCur)[i].x) 
                    * ((*featuresLast)[i].x - (*featuresCur)[i].x)
                    + ((*featuresLast)[i].y - (*featuresCur)[i].y) 
                    * ((*featuresLast)[i].y - (*featuresCur)[i].y));

    if (!(trackDis > maxTrackDis || (*featuresCur)[i].x < xBoundary || 
      (*featuresCur)[i].x > imageWidth - xBoundary || (*featuresCur)[i].y < yBoundary || 
      (*featuresCur)[i].y > imageHeight - yBoundary) && featuresStatus[i]) {

      int xInd = (int)(((*featuresLast)[i].x - xBoundary) / subregionWidth);
      int yInd = (int)(((*featuresLast)[i].y - yBoundary) / subregionHeight);
      int ind = xSubregionNum * yInd + xInd;

      if (subregionFeatureNum[ind] < maxFeatureNumPerSubregion) {
        (*featuresCur)[featureCount] = (*featuresCur)[i];
        (*featuresLast)[featureCount] = (*featuresLast)[i];
        featuresInd[featureCount] = featuresInd[i];

        point.u = -((*featuresCur)[featureCount].x - kImage[2]) / kImage[0];
        point.v = -((*featuresCur)[featureCount].y - kImage[5]) / kImage[4];
        point.ind = featuresInd[featureCount];
        imagePointsCur->push_back(point);

        if (i >= recordFeatureNum) {
          point.u = -((*featuresLast)[featureCount].x - kImage[2]) / kImage[0];
          point.v = -((*featuresLast)[featureCount].y - kImage[5]) / kImage[4];
          imagePointsLast->push_back(point);
        }

        featureCount++;
        subregionFeatureNum[ind]++;
      }
    }
  }
  totalFeatureNum = featureCount;
  featuresCur->resize(totalFeatureNum);
  featuresLast->resize(totalFeatureNum);
  featuresInd.resize(totalFeatureNum);

  sensor_msgs::PointCloud2 imagePointsLast2;
  pcl::toROSMsg(*imagePointsLast, imagePointsLast2);
  imagePointsLast2.header.stamp = ros::Time().fromSec(timeLast);
  imagePointsLastPubPointer->publish(imagePointsLast2);

  showCount = (showCount + 1) % (showSkipNum + 1);
  if (showCount == showSkipNum) {
    bridge.image = imageShow;
    bridge.encoding = "mono8";
    sensor_msgs::Image::Ptr imageShowPointer = bridge.toImageMsg();
    imageShowPubPointer->publish(imageShowPointer);
  }
}