void imageCb(const sensor_msgs::ImageConstPtr& msg) { //cv_bridge::CvImagePtr cvPtr = getThreshImageColor( msg ); //cv_bridge::CvImagePtr cvPtr = getThreshImageInfrared( msg ); cv_bridge::CvImagePtr cvPtr = getImage( msg ); equalizeHistogram( cvPtr ); // static int counter = 0; // if( counter++ % 128 == 0 ) // { // every 128 frames do a full match cv_bridge::CvImagePtr threshImagePtr(new cv_bridge::CvImage( cvPtr->header, cvPtr->encoding, cvPtr->image.clone() ) ); thresholdImage( threshImagePtr, /* byColor = */ false ); cv::vector< cv::vector<cv::Point> > contours; // applyMorphology( threshImagePtr ); contours = getContours( threshImagePtr->image ); cv::vector< double > areaBasedProbs = getAreaBasedProbs( contours ); // cv::vector< double > momentMatchBasedProbs = getMomentMatchBasedProbs( contours ); // cv::vector< double > surfMatchBasedProbs = getSurfMatchBasedProbs( contours, cvPtr ); double maxProb = 0; int chosenContour = -1; for( int i = 0; i < contours.size(); i++ ) { double prob = areaBasedProbs[i]; // * momentMatchBasedProbs[i] * surfMatchBasedProbs[i]; if( prob > maxProb ) { maxProb = 0; chosenContour = i; } } // Draw centers & contours cv::Mat drawing = cv::Mat::zeros( cvPtr->image.size(), CV_8UC3 ); for( int i = 0; i < contours.size(); i++ ) { if( i == chosenContour ) continue; cv::Scalar color = cv::Scalar( 50, 150, 150 ); cv::drawContours( drawing, contours, i, color, 2, 8 ); } geometry_msgs::Point point; if( chosenContour > -1 ) { // Get the mass center of the chosen contour cv::Moments chosenMoments = cv::moments( contours[chosenContour], false ); cv::Point foundCenter = cv::Point( chosenMoments.m10/chosenMoments.m00, chosenMoments.m01/chosenMoments.m00 ); cv::drawContours( drawing, contours, chosenContour, cv::Scalar( 255, 255, 255 ), 2, 8 ); cv::circle( drawing, foundCenter, 10, cv::Scalar( 255, 255, 255 ), -1, 8, 0 ); point.x = foundCenter.x * 100.0 / cvPtr->image.size().width; point.y = foundCenter.y * 100.0 / cvPtr->image.size().height; // distance is the size of the contour relative to targetSize_; double contourArea = cv::contourArea( contours[chosenContour] ); double imageArea = cvPtr->image.size().width * cvPtr->image.size().height; // 50 means we are at the target size, lower too close, higher too far // if contourArea/imageArea is very close to 1 something is wrong, // regard it as a miss if( fabs( contourArea/imageArea - 1.0 ) < .05 ) { point.x = point.y = 0.0; point.z = -1.0; missesInARow_++; } else { point.z = fmax( ( targetSize_ - contourArea/imageArea * 100.0 ) / 2.0 + 50.0, 0.0 ); missesInARow_ = 0; } } else { point.x = point.y = 0.0; point.z = -1.0; missesInARow_++; } if( missesInARow_ > 30 || pointPublished_.z == -1.0 ) { pointPublished_ = point; missesInARow_ = 0; } else if( point.z != -1.0 ) { pointPublished_.x = ( point.x + pointPublished_.x * dampingFactor_ ) / ( 1.0 + dampingFactor_ ); pointPublished_.y = ( point.y + pointPublished_.y * dampingFactor_ ) / ( 1.0 + dampingFactor_ ); pointPublished_.z = ( point.z + pointPublished_.z * dampingFactor_ ) / ( 1.0 + dampingFactor_ ); } // draw the pointPublished cv::Point drawPointPublished( pointPublished_.x * cvPtr->image.size().width / 100.0, pointPublished_.y * cvPtr->image.size().height / 100.0 ); cv::circle( drawing, drawPointPublished, 5, cv::Scalar( 255, 0, 255 ), -1, 8, 0 ); // Publish cv_bridge::CvImage drawingImage( cvPtr->header, enc::BGR8, drawing ); centersPub_.publish( drawingImage.toImageMsg() ); pointPub_.publish( pointPublished_ ); }
int main(int argc, char *argv[]) { IplImage* img = cvLoadImage("lowContrastPollen.jpg"); //IplImage* img = cvLoadImage("darkpollen.jpg"); setbuf(stdout, NULL); if(!img){ printf("Could not load image file: %s\n",argv[1]); exit(0); } int height,width,step,channels; // get the image data height = img->height; width = img->width; step = img->widthStep; channels = img->nChannels; // Checking... printf("Processing a %dx%d image with %d channels\n",height,width,channels); // Histogram. int numBins = 256; float range[] = {0, 255}; float *ranges[] = { range }; CvHistogram *hist = cvCreateHist(1, &numBins, CV_HIST_ARRAY, ranges, 1); cvClearHist(hist); // Create image(s). IplImage* imgRed = cvCreateImage(cvGetSize(img), 8, 1); IplImage* imgGreen = cvCreateImage(cvGetSize(img), 8, 1); IplImage* imgBlue = cvCreateImage(cvGetSize(img), 8, 1); IplImage* imgGrayscale = cvCreateImage(cvGetSize(img), 8, 1); IplImage* imgHistEqualized = cvCreateImage(cvGetSize(img), 8, 1); // Split image into the color planes. cvSplit(img, imgBlue, imgGreen, imgRed, NULL); // Convert the image into its corresponding gray-scale image. // Add weight to rgb values // Y = 0.2126*R + 0.7152*G + 0.0722*B. cvAddWeighted( imgRed, 0.2126 , imgGreen, 0.7152 , 0.0, imgGrayscale ); cvAddWeighted( imgGrayscale, 1.0, imgBlue, 0.0722 , 0.0, imgGrayscale ); // Compute and draw histogram(s). cvClearHist(hist); cvCalcHist(&imgGrayscale, hist, 0, 0); IplImage* imgHistGrayscale = DrawHistogram(hist); // Work on the histogram equalization. cvCopy(imgGrayscale, imgHistEqualized); equalizeHistogram(hist, imgHistEqualized); //cvEqualizeHist(imgGrayscale, imgHistEqualized); // Compute and draw histogram(s) of the equalized-histogram image. cvClearHist(hist); cvCalcHist(&imgHistEqualized, hist, 0, 0); IplImage* imgHistHistEqualized = DrawHistogram(hist); //show window(s). cvNamedWindow("Gray scale"); cvNamedWindow("Gray scale image"); cvNamedWindow("Histogram equalization"); cvNamedWindow("Histogram equalization image"); cvShowImage("Gray scale", imgHistGrayscale); cvShowImage("Gray scale image", imgGrayscale); cvShowImage("Histogram equalization", imgHistHistEqualized); cvShowImage("Histogram equalization image", imgHistEqualized); cvWaitKey(0); // destroy window(s). cvDestroyWindow("Gray scale"); cvDestroyWindow("Gray scale image"); cvDestroyWindow("Histogram equalization"); cvDestroyWindow("Histogram equalization image"); // release the image cvReleaseImage(&imgGrayscale); cvReleaseImage(&imgHistGrayscale); cvReleaseImage(&imgHistEqualized); cvReleaseImage(&imgHistHistEqualized); return 0; }