bool updateModule() { if (imageIn.getInputCount()>0) { cv::Mat orig = (IplImage *) imageIn.read(true)->getIplImage(); ImageOf<PixelMono> &outImg = imageOut.prepare(); cv::Mat blueOnly = blueFilter(orig); float sumx=0, sumy=0; float num_pixel = 0; for(int x=0; x<blueOnly.cols; x++) { for(int y=0; y<blueOnly.rows; y++) { int val = blueOnly.at<uchar>(y,x); if( val >= 50) { sumx += x; sumy += y; num_pixel++; } } } cv::Point p(sumx/num_pixel, sumy/num_pixel); //cout << cv::Mat(p) << endl; cv::Moments m = cv::moments(blueOnly, false); cv::Point p1(m.m10/m.m00, m.m01/m.m00); //cout << cv::Mat(p1) << endl; cv::circle(blueOnly, p, 5, cv::Scalar(0,0,0), -1); Bottle &target=targetPort.prepare(); target.clear(); if (p.x < 0 && p.y <0) { target.addDouble(0); target.addDouble(0); target.addInt(0); } else { target.addDouble(p.x); target.addDouble(p.y); target.addInt(1); } IplImage tmp = blueOnly; outImg.resize(tmp.width, tmp.height); cvCopyImage( &tmp, (IplImage *) outImg.getIplImage()); imageOut.write(); targetPort.write(); } return true; }