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