bool getCOG(ImageOf<PixelRgb> &img, Vector &cog)
{
int xMean=0;
int yMean=0;
int ct=0;
for (int x=0; x<img.width(); x++)
{
for (int y=0; y<img.height(); y++)
{
PixelRgb &pixel=img.pixel(x,y);
if ((pixel.b>5.0*pixel.r) && (pixel.b>5.0*pixel.g))
{
xMean+=x;
yMean+=y;
ct++;
}
}
}
if (ct>0)
{
cog.resize(2);
cog[0]=xMean/ct;
cog[1]=yMean/ct;
return true;
}
else
return false;
}
void testTransmit() {
report(0,"testing image transmission...");
ImageOf<PixelRgb> img1;
img1.resize(128,64);
for (int x=0; x<img1.width(); x++) {
for (int y=0; y<img1.height(); y++) {
PixelRgb& pixel = img1.pixel(x,y);
pixel.r = x;
pixel.g = y;
pixel.b = 42;
}
}
PortReaderBuffer< ImageOf<PixelRgb> > buf;
Port input, output;
buf.setStrict();
buf.attach(input);
input.open("/in");
output.open("/out");
output.addOutput(Contact("/in", "tcp"));
Time::delay(0.2);
report(0,"writing...");
output.write(img1);
output.write(img1);
output.write(img1);
report(0,"reading...");
ImageOf<PixelRgb> *result = buf.read();
checkTrue(result!=NULL,"got something check");
if (result!=NULL) {
checkEqual(img1.width(),result->width(),"width check");
checkEqual(img1.height(),result->height(),"height check");
if (img1.width()==result->width() &&
img1.height()==result->height()) {
int mismatch = 0;
for (int x=0; x<img1.width(); x++) {
for (int y=0; y<img1.height(); y++) {
PixelRgb& pix0 = img1.pixel(x,y);
PixelRgb& pix1 = result->pixel(x,y);
if (pix0.r!=pix1.r ||
pix0.g!=pix1.g ||
pix0.b!=pix1.b) {
mismatch++;
}
}
}
checkTrue(mismatch==0,"pixel match check");
}
}
output.close();
input.close();
}
int main() {
Network yarp; // set up yarp
BufferedPort<ImageOf<PixelRgb> > imagePort; // make a port for reading images
BufferedPort<Vector> targetPort;
imagePort.open("/tutorial/image/in"); // give the port a name
targetPort.open("/tutorial/target/out");
Network::connect("/icubSim/cam/left","/tutorial/image/in");
while (1) { // repeat forever
ImageOf<PixelRgb> *image = imagePort.read(); // read an image
ImageOf<PixelRgb> *image_cropped;
if (image!=NULL) { // check we actually got something
printf("We got an image of size %dx%d\n", image->width(), image->height());
double xMean = 0;
double yMean = 0;
int ct = 0;
for (int x=0; x<image->width(); x++) {
for (int y=0; y<image->height(); y++) {
PixelRgb& pixel = image->pixel(x,y);
/* very simple test for blueishness */
/* make sure blue level exceeds red and green by a factor of 2 */
if (pixel.b>pixel.r*1.2+10 && pixel.b>pixel.g*1.2+10) {
/* there's a blueish pixel at (x,y)! */
/* let's find the average location of these pixels */
xMean += x;
yMean += y;
ct++;
}
}
}
if (ct>0) {
xMean /= ct;
yMean /= ct;
}
if (ct>(image->width()/20)*(image->height()/20)) {
printf("Best guess at blue target: %g %g\n", xMean, yMean);
Vector& target = targetPort.prepare();
target.resize(3);
target[0] = xMean;
target[1] = yMean;
target[2] = 1;
targetPort.write();
} else {
Vector& target = targetPort.prepare();
target.resize(3);
target[0] = 0;
target[1] = 0;
target[2] = 0;
targetPort.write();
}
}
}
return 0;
}
yarp::os::Things& AsciiImageMonitorObject::update(yarp::os::Things& thing)
{
ImageOf<PixelRgb>* img = thing.cast_as< ImageOf<PixelRgb> >();
bt.clear();
int col = floor(img->width() / WINDOW_SIZE) + 1;
int row = floor(img->height() / WINDOW_SIZE) + 1;
char *str = (char*) malloc(2*col*(row+1) + 1);
int count = 0;
for(int i=0; i<img->height(); i+=WINDOW_SIZE) {
//Bottle& row = bt.addList();
for(int j=0; j<img->width(); j+=WINDOW_SIZE) {
// calc avg over a subrect
unsigned int sum_pix = 0;
int pix_count = 0;
for(int x=i; x< std::min(img->height(), i+WINDOW_SIZE); x++) {
for(int y=j; y<std::min(img->width(), j+WINDOW_SIZE); y++) {
sum_pix += img->pixel(y,x).r * 0.21+ img->pixel(y,x).g * 0.72 + img->pixel(y,x).b *0.07;
pix_count++;
}
}
sum_pix = sum_pix / pix_count;
int index = std::max(int(floor(sum_pix/11))-1, 0);
index = index % 11;
str[count++] = available[index];
str[count++] = ' ';
}
str[count++] = '\n';
//printf("count %d\n", count);
//sprintf(str, "\n");
}
str[count++] = '\0';
//printf("%s\n", str);
bt.addInt32(0);
bt.addString(str);
th.setPortWriter(&bt);
free(str);
return th;
}
void testCreate() {
report(0,"testing image creation...");
FlexImage image;
image.setPixelCode(VOCAB_PIXEL_RGB);
image.resize(256,128);
checkEqual(image.width(),256,"check width");
checkEqual(image.height(),128,"check height");
ImageOf<PixelInt> iint;
iint.resize(256,128);
long int total = 0;
for (int x=0; x<iint.width(); x++) {
for (int y=0; y<iint.height(); y++) {
int v = (x+y)%65537;
iint.pixel(x,y) = v;
total += v;
}
}
for (int x2=0; x2<iint.width(); x2++) {
for (int y2=0; y2<iint.height(); y2++) {
total -= iint.pixel(x2,y2);
}
}
checkEqual(total,0,"pixel assignment check");
}
void testDraw() {
report(0,"checking draw tools...");
ImageOf<PixelRgb> img;
img.resize(64,64);
img.zero();
addCircle(img,PixelRgb(255,0,0),32,32,200);
// full image should be colored blue
bool ok = true;
IMGFOR(img,x,y) {
if (img.pixel(x,y).r!=255) {
ok = false;
}
}
checkTrue(ok,"image is blue");
}
int main() {
Network yarp; // set up yarp
BufferedPort<ImageOf<PixelRgb> > imagePort_in; // make a port for reading images
BufferedPort<ImageOf<PixelRgb> > imagePort_out; // make a port for sending images
imagePort_in.open("/image/in"); // give the port a name
imagePort_out.open("/cropped_image/out");
// ImagePort_in receives input from webcam
Network::connect("/movie/out","/image/in");
while (1) { // repeat forever
ImageOf<PixelRgb> *image = imagePort_in.read(); // read an image
ImageOf<PixelRgb> &image_cropped = imagePort_out.prepare();
ImageOf<PixelRgb> *image_480= imagePort_in.read();
image_480->resize(480,480);
if (image!=NULL) { // check we actually got something
//printf("We got an image of size %dx%d\n", image->width(), image->height());
// Crop 640*480 image centrally to 480*480
for (int x=0; x<480; x++) {
for (int y=0; y<480; y++) {
PixelRgb& pixel_ = image->pixel(x+80,y);
image_480->pixel(x,y)=pixel_;
}
}
image_cropped.copy(*image_480, 256, 256);
imagePort_out.write();
}
}
return 0;
}
void testCopy() {
report(0,"testing image copying...");
ImageOf<PixelRgb> img1;
img1.resize(128,64);
for (int x=0; x<img1.width(); x++) {
for (int y=0; y<img1.height(); y++) {
PixelRgb& pixel = img1.pixel(x,y);
pixel.r = x;
pixel.g = y;
pixel.b = 42;
}
}
ImageOf<PixelRgb> result;
result.copy(img1);
checkEqual(img1.width(),result.width(),"width check");
checkEqual(img1.height(),result.height(),"height check");
if (img1.width()==result.width() &&
img1.height()==result.height()) {
int mismatch = 0;
for (int x=0; x<img1.width(); x++) {
for (int y=0; y<img1.height(); y++) {
PixelRgb& pix0 = img1.pixel(x,y);
PixelRgb& pix1 = result.pixel(x,y);
if (pix0.r!=pix1.r ||
pix0.g!=pix1.g ||
pix0.b!=pix1.b) {
mismatch++;
}
}
}
checkTrue(mismatch==0,"pixel match check");
}
}
void testCast() {
report(0,"testing image casting...");
ImageOf<PixelRgb> img1;
img1.resize(128,64);
for (int x=0; x<img1.width(); x++) {
for (int y=0; y<img1.height(); y++) {
PixelRgb& pixel = img1.pixel(x,y);
unsigned char v = x%30;
pixel.r = v;
pixel.g = v;
pixel.b = v;
}
}
ImageOf<PixelMono> result;
result.copy(img1);
checkEqual(img1.width(),result.width(),"width check");
checkEqual(img1.height(),result.height(),"height check");
if (img1.width()==result.width() &&
img1.height()==result.height()) {
int mismatch = 0;
for (int x=0; x<img1.width(); x++) {
for (int y=0; y<img1.height(); y++) {
PixelRgb& pix0 = img1.pixel(x,y);
PixelMono& pix1 = result.pixel(x,y);
if (pix0.r>pix1+1 || pix0.r<pix1-1) {
mismatch++;
}
}
}
checkTrue(mismatch==0,"pixel match check");
}
}
int
main ()
{
Network yarp;
BufferedPort<ImageOf<PixelRgb> > imagePort;
BufferedPort<Vector> target;
imagePort.open ("/target/image/in");
target.open ("/target/out");
Network::connect ("/icubSim/cam/left", "/target/image/in");
// gets the position of an object from left eye of iCub, and sends this
// to the port in "search.cpp"
while (true)
{
ImageOf<PixelRgb> *image = imagePort.read ();
if (image != NULL)
{
double xMean = 0;
double yMean = 0;
int ct = 0;
for (int x = 0; x < image->width (); x++)
{
for (int y = 0; y < image->height (); y++)
{
PixelRgb& pixel = image->pixel (x, y);
// selects the reddish object according to the threshold value
if (pixel.r > pixel.b * 1.2 + 20 && pixel.r > pixel.g * 1.2 + 20)
{
xMean += x;
yMean += y;
ct++;
}
}
}
if (ct > 0)
{
xMean /= ct;
yMean /= ct;
}
// to know whether an object is big enough to send its positions to the port
if (ct > (image->width () / 35) * (image->height () / 35))
{
printf ("Best guess at red target: %g %g\n", xMean, yMean);
Vector& tar = target.prepare ();
tar.resize (3);
tar[0] = xMean;
tar[1] = yMean;
tar[2] = 1;
target.write ();
}
else
{
Vector& tar = target.prepare ();
tar.resize (3);
tar[0] = 0;
tar[1] = 0;
tar[2] = 0;
target.write ();
}
}
else
{
cout << "image is not detected!" << endl;
}
}
return (0);
}
bool ARMarkerDetectorModule::updateModule()
{
int i, j;
ImageOf<PixelRgb> *yrpImgIn;
ImageOf<PixelRgb>* yrpCopyImgIn;
yrpImgIn = _imgPort.read();
yrpCopyImgIn= _imgPort.read();
if (yrpImgIn == NULL) // this is the case if module is requested to quit while waiting for image
return true;
double auxtime, aux;
auxtime = yarp::os::Time::now();
aux = auxtime - timelastround;
printf( "%g ms %g fps \n", aux*1000, 1/aux );
timelastround = auxtime;
IplImage *iplimg = (IplImage*)yrpImgIn->getIplImage();
tracker.detectMarkers( iplimg );
cvWaitKey( 5 );
Bottle &b = _outPort.prepare();
b.clear();
bool detected = false;
for(int cnt=0; cnt<tracker.nobjects(); cnt++)
{
if( tracker.isObjectDetected(cnt) )
{
double T[3][4];
tracker.getMatrix(cnt,T);
char name[256];
tracker.getObjectName(cnt, name);
printf("%s\nT\n",name);
b.addString( name );
b.addString( "T");
for(i = 0; i < 3; i++ )
{
b.addDouble( T[i][0]);
b.addDouble( T[i][1]);
b.addDouble( T[i][2]);
b.addDouble( T[i][3]);
char str[64];
printf("%g %g %g %g\n",T[i][0],T[i][1],T[i][2],T[i][3]);
}
int cx, cy;
tracker.getCenter( cnt, &cx, &cy);
b.addString("imgcoord");
b.addDouble( cx);
b.addDouble( cy);
b.addString("size");
b.addDouble( tracker.getSize(cnt));
b.addString("range");
b.addDouble(sqrt(T[0][3]*T[0][3] + T[1][3]*T[1][3] + T[2][3]*T[2][3]));
b.addString("imgcoordnorm");
b.addDouble( 2.0 * cx / (double)iplimg->width - 1.0 );
b.addDouble( - (2.0 * cy / (double)iplimg->height - 1.0) );
printf("img coord\n %d %d\t size %d\n", cx, cy, tracker.getSize(cnt));
printf("range: %f\n",sqrt(T[0][3]*T[0][3] + T[1][3]*T[1][3] + T[2][3]*T[2][3]));
printf("imgcoordnorm\n %1.3g %1.3g\n\n",
2.0 * cx / (double)iplimg->width - 1.0 ,
- (2.0 * cy / (double)iplimg->height - 1.0));
// edges
int x0,y0,x1,y1,x2,y2,x3,y3;
tracker.getEdge(cnt,0,&x0,&y0);
tracker.getEdge(cnt,1,&x1,&y1);
tracker.getEdge(cnt,2,&x2,&y2);
tracker.getEdge(cnt,3,&x3,&y3);
printf("Edges:\n %d %d - %d %d - %d %d - %d %d\n\n",x0,y0,x1,y1,x2,y2,x3,y3);
// compute the simple colour using chroma comparison - yields Red, green or Blue only!
float chromaTable[3]; // in this program we simply normalise for the 3 colours
for (int i=0;i<3;i++)
{
chromaTable[i] = 0;
}
int l = sqrt((double)tracker.getSize(cnt))/2.0; // this gives a slightly wider area than the ART central image - allows the bounding ART square to be coloured
int X1,Y1,X2,Y2;
X1 = cx - l;
Y1 = cy - l;
X2 = cx + l;
Y2 = cy + l;
// printf("x1 y1: %d %d x2 y2: %d %d\n",X1,Y1,X2,Y2);
// printf("=====================================\n");
for (int x = X1; x < X2; x++)
{
for (int y = Y1; y < Y2; y++)
{
PixelRgb& pixel = yrpCopyImgIn->pixel(x,y);
// printf("r: %d g: %d b: %d x: %d y: %d\n",pixel.r,pixel.g,pixel.b,x,y);
chromaTable[0] += (float)pixel.r / ((float)pixel.r + (float)pixel.g + (float)pixel.b);
chromaTable[1] += (float)pixel.g / ((float)pixel.r + (float)pixel.g + (float)pixel.b);
chromaTable[2] += (float)pixel.b / ((float)pixel.r + (float)pixel.g + (float)pixel.b);
}
}
// for (int i=0; i < 3; i++)
// {
// printf("%f ",chromaTable[i]);
// }
// printf( " \n");
// printf("=====================================\n");
if ((chromaTable[0] > chromaTable[1]) && (chromaTable[0] > chromaTable[2]))
{
printf("CHROMA: Red\n");
b.addString("red");
b.addInt(4); // the numbers represent the position of red in a 3 bit colour space
// in case we expand the selection later
}
if ((chromaTable[1] > chromaTable[0]) && (chromaTable[1] > chromaTable[2]))
{
printf("CHROMA: Green\n");
b.addString("green");
b.addInt(2); // the numbers represent the position of green in a 3 bit colour space
// in case we expand the selection later
}
if ((chromaTable[2] > chromaTable[0]) && (chromaTable[2] > chromaTable[1]))
{
printf("CHROMA: Blue\n");
b.addString("blue");
b.addInt(1); // the numbers represent the position of blue in 3 bit colour space
// in case we expand the selection later
}
Bottle time;
_imgPort.getEnvelope(time);
b.addDouble(time.get(0).asDouble());
detected = true;
}
}
if (detected) {
}
_outPort.write();
// ImageOf<PixelRgb> *imageOutput;
// imageOutput = yrpCopyImgIn;
// imageOut.prepare() = *imageOutput;
// imageOut.write();
return true;
}
