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;
    }
Exemple #2
0
    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;
}
Exemple #4
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;
}
Exemple #5
0
 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");
 }
Exemple #6
0
 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;
}
Exemple #8
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");
        }
    }
Exemple #9
0
    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");
        }
    }
Exemple #10
0
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;
}