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