void merge(const ImageOf<PixelRgb> &imgR, const ImageOf<PixelRgb> &imgL, ImageOf<PixelRgb> &out)
{
if (out.width()!=imgR.width()/scale)
out.resize(imgR.width()/scale, imgR.height()/scale);
int rr=0;
for(int r=0; r<out.height(); r++)
{
const unsigned char *tmpR=imgR.getRow(rr);
const unsigned char *tmpL=imgL.getRow(rr);
unsigned char *tmpO=out.getRow(r);
for(int c=0; c<out.width(); c++)
{
tmpO[0]=(unsigned char) (1/3.0*(tmpL[0]+tmpL[1]+tmpL[2]));
tmpO[1]=(unsigned char) (1/3.0*(tmpR[0]+tmpR[1]+tmpR[2]));
tmpO+=3;
tmpL+=(3*scale);
tmpR+=(3*scale);
}
rr+=scale;
}
}
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();
}
bool DevicePipe::updateService() {
IFrameGrabberImage *imgSource;
IAudioGrabberSound *sndSource;
IAudioVisualGrabber *imgSndSource;
IAudioVisualStream *sourceType;
IAudioRender *sndSink;
IFrameWriterImage *imgSink;
IFrameWriterAudioVisual *imgSndSink;
IAudioVisualStream *sinkType;
source.view(imgSource);
source.view(sndSource);
source.view(imgSndSource);
source.view(sourceType);
sink.view(imgSink);
sink.view(sndSink);
sink.view(imgSndSink);
sink.view(sinkType);
if (sourceType!=NULL) {
if (!(sourceType->hasAudio()&&sourceType->hasVideo())) {
imgSndSource = NULL;
}
}
if (sinkType!=NULL) {
if (!(sinkType->hasAudio()&&sinkType->hasVideo())) {
imgSndSink = NULL;
}
}
if (imgSndSource!=NULL&&imgSndSink!=NULL) {
ImageOf<PixelRgb> tmp;
Sound tmpSound;
imgSndSource->getAudioVisual(tmp,tmpSound);
imgSndSink->putAudioVisual(tmp,tmpSound);
printf("piped %dx%d image, %dx%d sound\n",
tmp.width(), tmp.height(),
tmpSound.getSamples(), tmpSound.getChannels());
} else if (imgSource!=NULL&&imgSink!=NULL) {
ImageOf<PixelRgb> tmp;
imgSource->getImage(tmp);
imgSink->putImage(tmp);
printf("piped %dx%d image\n", tmp.width(), tmp.height());
} else if (sndSource!=NULL&&sndSink!=NULL) {
Sound tmp;
sndSource->getSound(tmp);
sndSink->renderSound(tmp);
printf("piped %dx%d sound\n", tmp.getSamples(), tmp.getChannels());
} else {
printf("Don't know how to pipe between these devices.\n");
printf("Piping is very limited at the moment.\n");
printf("You're probably better off writing some short custom code.\n");
return false;
}
return true;
}
cv::Rect CalibModule::extractFingerTip(ImageOf<PixelMono> &imgIn, ImageOf<PixelBgr> &imgOut,
const Vector &c, Vector &px)
{
cv::Mat imgInMat=cv::cvarrToMat((IplImage*)imgIn.getIplImage());
// produce a colored image
imgOut.resize(imgIn);
cv::Mat imgOutMat=cv::cvarrToMat((IplImage*)imgOut.getIplImage());
// proceed iff the center is within the image plane
if ((c[0]<10.0) || (c[0]>imgIn.width()-10) ||
(c[1]<10.0) || (c[1]>imgIn.height()-10))
{
cv::cvtColor(imgInMat,imgOutMat,CV_GRAY2BGR);
return cv::Rect();
}
// saturate the top-left and bottom-right corners
int roi_side2=roi_side>>1;
cv::Point ct((int)c[0],(int)c[1]);
cv::Point tl((int)(c[0]-roi_side2),(int)(c[1]-roi_side2));
cv::Point br((int)(c[0]+roi_side2),(int)(c[1]+roi_side2));
tl.x=std::max(1,tl.x); tl.x=std::min(tl.x,imgIn.width()-1);
tl.y=std::max(1,tl.y); tl.y=std::min(tl.y,imgIn.height()-1);
br.x=std::max(1,br.x); br.x=std::min(br.x,imgIn.width()-1);
br.y=std::max(1,br.y); br.y=std::min(br.y,imgIn.height()-1);
cv::Rect rect(tl,br);
// run Otsu algorithm to segment out the finger
cv::Mat imgInMatRoi(imgInMat,rect);
cv::threshold(imgInMatRoi,imgInMatRoi,0,255,cv::THRESH_BINARY|cv::THRESH_OTSU);
cv::cvtColor(imgInMat,imgOutMat,CV_GRAY2BGR);
px.resize(2,0.0);
bool ok=false;
for (int y=tl.y; y<br.y; y++)
{
for (int x=tl.x; x<br.x; x++)
{
if (imgIn(x,y)>0)
{
// predict the center of the finger a bit shifted
x+=3; y+=5;
px[0]=x; px[1]=y;
cv::circle(imgOutMat,cv::Point(x,y),5,cv::Scalar(0,0,255),-1);
ok=true;
break;
}
}
if (ok)
break;
}
cv::circle(imgOutMat,ct,5,cv::Scalar(0,255,0),-1);
cv::rectangle(imgOutMat,tl,br,cv::Scalar(255,255,255),4);
return rect;
}
static bool ImageReadRGB(ImageOf<PixelRgb> &img, const char *filename)
{
int width, height, color, num;
FILE *fp=0;
fp = fopen(filename, "rb");
if(fp==0)
{
fprintf(stderr, "Error opening %s, check if file exists.\n", filename);
return false;
}
if (!ReadHeader(fp, &height, &width, &color))
{
fclose (fp);
fprintf(stderr, "Error reading header, is file a valid ppm/pgm?\n");
return false;
}
if (!color)
{
ImageOf<PixelMono> tmp;
tmp.resize(width,height);
const int w = tmp.width() * tmp.getPixelSize();
const int h = tmp.height();
const int pad = tmp.getRowSize();
unsigned char *dst = tmp.getRawImage ();
num = 0;
for (int i = 0; i < h; i++)
{
num += (int)fread((void *) dst, 1, (size_t) w, fp);
dst += pad;
}
fclose(fp);
img.copy(tmp);
return true;
}
img.resize(width,height);
const int w = img.width() * img.getPixelSize();
const int h = img.height();
const int pad = img.getRowSize();
unsigned char *dst = img.getRawImage ();
num = 0;
for (int i = 0; i < h; i++)
{
num += (int)fread((void *) dst, 1, (size_t) w, fp);
dst += pad;
}
fclose(fp);
return true;
}
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;
}
// From iCub staticgrabber device.
// DF2 bayer sequence.
// -- in staticgrabber: first row GBGBGB, second row RGRGRG.
// -- changed here to: first row GRGRGR, second row BGBGBG.
bool TestFrameGrabber::makeSimpleBayer(
ImageOf<PixelRgb>& img,
ImageOf<PixelMono>& bayer) {
bayer.resize(img.width(), img.height());
const int w = img.width();
const int h = img.height();
int i, j;
for (i = 0; i < h; i++) {
PixelRgb *row = (PixelRgb *)img.getRow(i);
PixelMono *rd = (PixelMono *)bayer.getRow(i);
for (j = 0; j < w; j++) {
if ((i%2) == 0) {
switch (j%4) {
case 0:
case 2:
*rd++ = row->g;
row++;
break;
case 1:
case 3:
*rd++ = row->r;
row++;
break;
}
}
if ((i%2) == 1) {
switch (j%4) {
case 1:
case 3:
*rd++ = row->g;
row++;
break;
case 0:
case 2:
*rd++ = row->b;
row++;
break;
}
}
}
}
return true;
}
/**
* Read an image from the grabber.
*
* @param image The image to read. The image will be resized to
* the dimensions the grabber is using, and the captured image
* data will be written to it.
*
* @return True if an image was successfully captured. If false
* returned, the image will be resized to the dimensions used by
* the grabber, but all pixels will be zeroed.
*/
bool OpenCVGrabber::sendImage(IplImage* iplFrame, ImageOf<PixelRgb> & image)
{
// Resize the output image, this should not result in new
// memory allocation if the image is already the correct size
image.resize(iplFrame->width, iplFrame->height);
if (!m_saidSize) {
yDebug("Received image of size %dx%d\n", image.width(), image.height());
m_saidSize = true;
}
// Get an IplImage, the Yarp Image owns the memory pointed to
IplImage * iplImage = (IplImage*)image.getIplImage();
// create the timestamp
m_laststamp.update();
// Copy the captured image to the output image, flipping it if
// the coordinate origin is not the top left
if (IPL_ORIGIN_TL == iplFrame->origin)
cvCopy(iplFrame, iplImage, 0);
else
cvFlip(iplFrame, iplImage, 0);
if (iplFrame->channelSeq[0] == 'B') {
cvCvtColor(iplImage, iplImage, CV_BGR2RGB);
}
if (m_w <= 0) {
m_w = image.width();
}
if (m_h <= 0) {
m_h = image.height();
}
if (fromFile) {
if (m_w>0 && m_h>0) {
if (image.width() != m_w || image.height() != m_h) {
if (!m_saidResize) {
yDebug("Software scaling from %dx%d to %dx%d", image.width(), image.height(), m_w, m_h);
m_saidResize = true;
}
image.copy(image, m_w, m_h);
}
}
}
DBG yDebug("%d by %d %s image\n", image.width(), image.height(), iplFrame->channelSeq);
return true;
}
int main(int argc, char *argv[]) {
DriverCreator *fake_factory =
new DriverCreatorOf<FakeFrameGrabber>("fake_grabber","grabber","FakeFrameGrabber");
Drivers::factory().add(fake_factory); // hand factory over to YARP
PolyDriver dd("fake_grabber");
if (!dd.isValid()) {
printf("fake_grabber not available\n");
return 1;
}
IFrameGrabberImage *grabber;
dd.view(grabber);
if (grabber==NULL) {
printf("*** Device failed to supply images\n");
return 1;
}
printf("*** Device can supply images\n");
ImageOf<PixelRgb> img;
if (grabber->getImage(img)) {
printf("*** Got a %dx%d image\n", img.width(), img.height());
} else {
printf("Failed to read an image\n");
return 1;
}
return 0;
}
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;
}
bool OverlayEffect::draw(ImageOf<PixelRgb>& src2, ImageOf<PixelRgb>& dest2) {
if (needRead) {
readEffectData();
needRead = false;
}
dest2 = src2;
if (img.isValid()) {
for (int x=0; x<dest2.width()&&x<img.width(); x++) {
for (int y=0; y<dest2.height()&&y<img.height(); y++) {
PixelBgra v = img.pixel(x,y);
if (v.a==0) {
dest2(x,y) = PixelRgb(v.r,v.g,v.b);
} else if (v.a>=127) {
// do nothing, leave copied value
} else {
PixelRgb& o = dest2(x,y);
float f = v.a/127.0;
if (f>1) f = 1;
int r = (int)((1-f)*v.r+f*o.r);
int g = (int)((1-f)*v.g+f*o.g);
int b = (int)((1-f)*v.b+f*o.b);
dest2(x,y) = PixelRgb(r,g,b);
}
}
}
}
return true;
}
void testScale() {
report(0,"checking scaling...");
ImageOf<PixelRgb> img;
ImageOf<PixelMono> img2;
ImageOf<PixelRgb> img3;
img.resize(64,64);
img.zero();
for (int i=0; i<img.width()/2; i++) {
for (int j=0; j<img.height()/2; j++) {
img(i,j).r = 255;
img(i,j).g = 255;
img(i,j).b = 255;
}
}
img2.copy(img,32,32);
checkEqual(img2.width(),32,"dimension check");
checkEqual(img2(0,0),255,"logic check");
checkEqual(img2(img2.width()-2,0),0,"logic check");
checkEqual(img2(0,img2.height()-2),0,"logic check");
img3.copy(img,16,16);
checkEqual(img3.width(),16,"dimension check");
checkEqual(img3(0,0).r,255,"logic check");
checkEqual(img3(img3.width()-2,0).r,0,"logic check");
checkEqual(img3(0,img3.height()-2).r,0,"logic check");
img.copy(img3,4,4);
checkEqual(img.width(),4,"dimension check");
}
void AcousticMap::renderGaussianDistribution(ImageOf<PixelFloat> &img,
int posX, int posY,
int sizex, int sizey,
float sigmaX, float sigmaY,
float height) {
IplImage *ipl = (IplImage*)img.getIplImage();
//cout << "going to render.." << posX << " " << posY << " " << sizex << " " << sizey << " " << sigmaX << " " << sigmaY << " " << height << endl;
if (((sizex-1)%2 == 0) && ((sizey-1)%2 == 0)) {
int extx = (sizex-1)/2;
int exty = (sizey-1)/2;
int currX, currY;
for(int y=-exty; y<=exty; y++) {
for(int x=-extx; x<=extx; x++) {
currX = x+posX;
currY = y+posY;
// if inside acoustic map
//cout << "rendering at: " << posX << " " << posY << endl;
if (posX >= 0 && posX < img.width() &&
posY >= 0 && posY < img.height()) {
//cout << "rendering at: " << posX << " " << posY << " value: " << height*(float)(exp(-0.5*(((float)x/sigmaX)*((float)x/sigmaX)+((float)y/sigmaY)*((float)y/sigmaY)))) << endl;
((float*)(ipl->imageData + ipl->widthStep*(currY)))[currX] = height*(float)(exp(-0.5*(((float)x/sigmaX)*((float)x/sigmaX)+((float)y/sigmaY)*((float)y/sigmaY))));
}
}
}
}
else
{
cout << "AcousticMap::renderGaussianDistribution not possible due to invalid size (has to be odd)" << endl;
}
}
void IntensitySalience::applyImpl(ImageOf<PixelRgb>& src,
ImageOf<PixelRgb>& dest,
ImageOf<PixelFloat>& sal) {
dest.resize(src);
sal.resize(src);
if (_sizeOld.width != src.width() ||
_sizeOld.height != src.height())
initImages(cvSize(src.width(), src.height()));
cvCvtColor(src.getIplImage(), _imgGray, CV_RGB2GRAY);
cvCvtColor(_imgGray, dest.getIplImage(), CV_GRAY2RGB);
gray2Float(_imgGray, (IplImage*)sal.getIplImage());
_sizeOld.width = src.width();
_sizeOld.height = src.height();
}
void testExternalRepeat() {
report(0,"checking that setExternal can be called multiple times...");
unsigned char buf[EXT_HEIGHT*EXT_WIDTH*3];
unsigned char buf2[EXT_HEIGHT*2*EXT_WIDTH*2*3];
ImageOf<PixelRgb> img;
img.setExternal(&buf[0],EXT_WIDTH,EXT_HEIGHT);
checkEqual(img.width(),EXT_WIDTH,"width check");
checkEqual(img.height(),EXT_HEIGHT,"height check");
img.setExternal(&buf2[0],EXT_WIDTH*2,EXT_HEIGHT*2);
checkEqual(img.width(),EXT_WIDTH*2,"width check");
checkEqual(img.height(),EXT_HEIGHT*2,"height check");
}
/**
* Read an image from the grabber.
*
* @param image The image to read. The image will be resized to
* the dimensions the grabber is using, and the captured image
* data will be written to it.
*
* @return True if an image was successfully captured. If false
* returned, the image will be resized to the dimensions used by
* the grabber, but all pixels will be zeroed.
*/
bool OpenCVGrabber::sendImage(const cv::Mat & frame, ImageOf<PixelRgb> & image)
{
// Resize the output image, this should not result in new
// memory allocation if the image is already the correct size
image.resize(frame.cols, frame.rows);
if (!m_saidSize) {
yDebug("Received image of size %zux%zu\n", image.width(), image.height());
m_saidSize = true;
}
// create the timestamp
m_laststamp.update();
// Convert to RGB color space
cv::Mat frame_rgb;
cv::cvtColor(frame, frame_rgb, cv::COLOR_BGR2RGB);
// Copy the captured image to the output image
memcpy(image.getRawImage(), frame_rgb.data, sizeof(unsigned char) * frame_rgb.rows * frame_rgb.cols * frame_rgb.channels());
if (m_w == 0) {
m_w = image.width();
}
if (m_h == 0) {
m_h = image.height();
}
if (fromFile) {
if (m_w>0 && m_h>0) {
if (image.width() != m_w || image.height() != m_h) {
if (!m_saidResize) {
yDebug("Software scaling from %zux%zu to %zux%zu", image.width(), image.height(), m_w, m_h);
m_saidResize = true;
}
image.copy(image, m_w, m_h);
}
}
}
DBG yDebug("%zu by %zu image\n", image.width(), image.height());
return true;
}
int main() {
Network::setLocalMode(true);
FakeFrameGrabber fake;
fake.start();
PolyDriver dd("(device test_grabber) (local /client) (remote /server)");
if (!dd.isValid()) {
printf("Device not available\n");
exit(1);
}
printf("*** Device created\n");
IFrameGrabberImage *grabber;
dd.view(grabber);
if (grabber!=NULL) {
printf("*** It can supply images\n");
ImageOf<PixelRgb> img;
if (grabber->getImage(img)) {
printf("*** Got a %dx%d image\n", img.width(), img.height());
} else {
printf("*** Failed to actually read an image\n");
}
IFrameGrabberControls *ctrl;
dd.view(ctrl);
if (ctrl!=NULL) {
printf("*** It can be controlled as a framegrabber\n");
double x = ctrl->getBrightness();
printf("*** brightness before setting is reported as %g\n", x);
ctrl->setBrightness(100);
printf("*** brightness set\n");
x = ctrl->getBrightness();
printf("*** brightness after setting reported as %g\n", x);
} else {
printf("*** It can <<<<<NOT>>>>> be controlled as a framegrabber\n");
}
} else {
printf("*** It can <<<<<NOT>>>>> supply images\n");
}
IPidControl *pid;
dd.view(pid);
if (pid!=NULL) {
printf("*** It can do PID control\n");
} else {
printf("*** It can <<<<<NOT>>>>> do PID control\n");
}
return 0;
}
int main(int argc, char *argv[]) {
printf("Show a circle for 3 seconds...\n");
ImageOf<PixelRgb> yarpImage;
printf("Creating a YARP image of a nice circle\n");
yarpImage.resize(300,200);
addCircle(yarpImage,PixelRgb(255,0,0),
yarpImage.width()/2,yarpImage.height()/2,
yarpImage.height()/4);
addCircle(yarpImage,PixelRgb(255,50,50),
yarpImage.width()/2,yarpImage.height()/2,
yarpImage.height()/5);
printf("Copying YARP image to an OpenCV/IPL image\n");
IplImage *cvImage = cvCreateImage(cvSize(yarpImage.width(),
yarpImage.height()),
IPL_DEPTH_8U, 3 );
cvCvtColor((IplImage*)yarpImage.getIplImage(), cvImage, CV_RGB2BGR);
printf("Showing OpenCV/IPL image\n");
cvNamedWindow("test",1);
cvShowImage("test",cvImage);
printf("Taking image back into YARP...\n");
ImageOf<PixelBgr> yarpReturnImage;
yarpReturnImage.wrapIplImage(cvImage);
yarp::sig::file::write(yarpReturnImage,"test.ppm");
printf("Saving YARP image to test.ppm\n");
cvWaitKey(3000);
cvDestroyWindow("test");
cvReleaseImage(&cvImage);
printf("...done\n");
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 readAndShow(ImageOf<PixelRgb> &reconstructYarp, bool refreshGui)
{
if (reconstructWidth == -1 || reconstructHeight == -1)
{
//Sample the subdivision sizes
std::cout << "Waiting for input..." << std::endl;
ImageOf<PixelRgb>* testImg = ports[0][0]->read(true);
reconstructWidth = testImg->width() * ports.size();
reconstructHeight = testImg->height() * ports[0].size();
std::cout << "Input detected with size : " << testImg->width() << "x" << testImg->height() << std::endl;
std::cout << "Reconstruct size will: " << reconstructWidth << "x" << reconstructHeight << std::endl;
reconstruct = cvCreateImage(cvSize(reconstructWidth, reconstructHeight), 8, 3);
}
for (unsigned int x = 0; x < ports.size(); x++)
{
for (unsigned int y = 0; y < ports[x].size(); y++)
{
ImageOf<PixelRgb>* imgYarp = ports[x][y]->read(true);
if (imgYarp)
{
IplImage* partImg = (IplImage*) imgYarp->getIplImage();
cvCvtColor(partImg, partImg, CV_BGR2RGB);
int rectW = reconstructWidth / ports.size();
int rectH = reconstructHeight / ports[0].size();
cvSetImageROI(reconstruct, cvRect(x*rectW, y*rectH, rectW, rectH));
cvCopy(partImg,reconstruct);
cvResetImageROI(reconstruct);
}
}
}
if (refreshGui)
cvShowImage((name + "/Reconstruct").c_str(), reconstruct);
reconstructYarp.wrapIplImage(reconstruct);
cvWaitKey(1);
}
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");
}
}
bool file::write(const ImageOf<PixelFloat>& src, const ConstString& dest) {
FILE *fp = fopen(dest.c_str(), "w");
if (fp==NULL) {
return false;
}
for (int i=0; i<src.height(); i++) {
for (int j=0; j<src.width(); j++) {
fprintf(fp,"%g ", src(j,i));
}
fprintf(fp,"\n");
}
fclose(fp);
return false;
}
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 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");
}
static bool ImageReadBGR(ImageOf<PixelBgr> &img, const char *filename)
{
int width, height, color, num;
FILE *fp=0;
fp = fopen(filename, "rb");
if(fp==0)
{
fprintf(stderr, "Error opening %s, check if file exists.\n", filename);
return false;
}
if (!ReadHeader(fp, &height, &width, &color))
{
fclose (fp);
fprintf(stderr, "Error reading header, is file a valid ppm/pgm?\n");
return false;
}
if (!color)
{
fclose(fp);
fprintf(stderr, "File is grayscale, conversion not yet supported\n");
return false;
}
ImageOf<PixelRgb> tmpImg;
tmpImg.resize(width, height);
const int w = tmpImg.width() * img.getPixelSize();
const int h = tmpImg.height();
const int pad = tmpImg.getRowSize();
unsigned char *dst = tmpImg.getRawImage ();
num = 0;
for (int i = 0; i < h; i++)
{
num += (int)fread((void *) dst, 1, (size_t) w, fp);
dst += pad;
}
fclose(fp);
return img.copy(tmpImg);
}
yarp::os::Things& ZfpMonitorObject::update(yarp::os::Things& thing)
{
if(shouldCompress) {
ImageOf<PixelFloat>* img = thing.cast_as< ImageOf<PixelFloat> >();
// .... buffer, len
int sizeCompressed;
compress((float*)img->getRawImage(), compressed, sizeCompressed, img->width(),img->height(),1e-3);
if(!compressed){
yError()<<"ZfpMonitorObject:Failed to compress, exiting...";
return thing;
}
data.clear();
data.addInt32(img->width());
data.addInt32(img->height());
data.addInt32(sizeCompressed);
Value v(compressed, sizeCompressed);
data.add(v);
th.setPortWriter(&data);
}
else
{
Bottle* compressedbt= thing.cast_as<Bottle>();
int width=compressedbt->get(0).asInt32();
int height=compressedbt->get(1).asInt32();
int sizeCompressed=compressedbt->get(2).asInt32();
// cast thing to compressed.
decompress((float*)compressedbt->get(3).asBlob(), decompressed, sizeCompressed, width, height,1e-3);
if(!decompressed){
yError()<<"ZfpMonitorObject:Failed to decompress, exiting...";
return thing;
}
imageOut.resize(width,height);
memcpy(imageOut.getRawImage(),decompressed,width*height*4);
th.setPortWriter(&imageOut);
}
return th;
}
void CRMainEstimation::estimate() {
// get depth image
int depth_width=320;
int depth_height=240;
ImageOf<PixelMono16>* img;
IplImage* depthCV;
depthCV=cvCreateImageHeader(cvSize(depth_width,depth_height),IPL_DEPTH_16U,1);
if ((img=(ImageOf<PixelMono16>*)depthPort.read(false)))
{
unsigned short* pBuff=(unsigned short*)img->getRawImage();
for (int i=0; i<img->width()*img->height(); i++)
{
//We take only the first 13 bits, that contain the depth value in mm
unsigned short realDepth = (pBuff[i]&0xFFF8)>>3;
buf[i]=realDepth;
}
cvSetData(depthCV,buf,depth_width*2);
}
void PROCThread::allocate( ImageOf<PixelRgb> &img )
{
origsize.width = img.width();
origsize.height = img.height();
srcsize.width = origsize.width + 2 * KERNSIZEMAX;
srcsize.height = origsize.height + KERNSIZEMAX;
cout << "Received input image dimensions: " << origsize.width << " " << origsize.height << endl;
cout << "Will extend these to: " << srcsize.width << " " << srcsize.height << endl;
orig = cv::Mat(srcsize.width, srcsize.height, CV_8UC3);
csTot32f = cv::Mat( srcsize.height, srcsize.width, CV_32FC1 );
uvimg = cv::Mat( srcsize.height, srcsize.width, CV_32FC1 );
ncsscale = 4;
centerSurr = new CentSur( srcsize , ncsscale );
inputExtImage = new ImageOf<PixelRgb>;
inputExtImage->resize( srcsize.width, srcsize.height );
img_Y = new ImageOf<PixelMono>;
img_Y->resize( srcsize.width, srcsize.height );
img_out_Y = new ImageOf<PixelMono>;
img_out_Y->resize( origsize.width, origsize.height );
img_UV = new ImageOf<PixelMono>;
img_UV->resize( srcsize.width, srcsize.height );
img_out_UV = new ImageOf<PixelMono>;
img_out_UV->resize( origsize.width, origsize.height );
img_V = new ImageOf<PixelMono>;
img_V->resize( srcsize.width, srcsize.height );
img_out_V = new ImageOf<PixelMono>;
img_out_V->resize( origsize.width, origsize.height );
allocated = true;
cout << "done allocating" << endl;
}
bool autobiographicalMemory::saveImageFromPort(const string &fullPath, const string &fromPort, BufferedPort<ImageOf<PixelRgb> >* imgPort)
{
//Extract the incoming images from yarp
ImageOf<PixelRgb> *yarpImage = imgPort->read(false);
//yDebug() << "imgPort name : " << imgPort->getName();
if (yarpImage != NULL) { // check we actually got something
//use opencv to convert the image and save it
IplImage *cvImage = cvCreateImage(cvSize(yarpImage->width(), yarpImage->height()), IPL_DEPTH_8U, 3);
cvCvtColor((IplImage*)yarpImage->getIplImage(), cvImage, CV_RGB2BGR);
cvSaveImage(fullPath.c_str(), cvImage);
//yDebug() << "img created : " << fullPath;
cvReleaseImage(&cvImage);
return true;
}
else {
//yWarning() << "[saveImageFromPort] No image received from: " << imgPort->getName();
return false;
}
}
int main(int argc, char *argv[]) {
Network yarp;
// give YARP a factory for creating instances of FakeFrameGrabber
DriverCreator *fakey_factory =
new DriverCreatorOf<FakeFrameGrabber>("fakey",
"grabber",
"FakeFrameGrabber");
Drivers::factory().add(fakey_factory); // hand factory over to YARP
// use YARP to create and configure an instance of FakeFrameGrabber
Property config;
if (argc==1) {
// no arguments, use a default
config.fromString("(device fakey) (w 640) (h 480)");
} else {
// expect something like "--device fakey --w 640 --h 480"
// or "--device dragonfly"
// or "--device test_grabber --period 0.5 --mode [ball]"
config.fromCommand(argc,argv);
}
PolyDriver dd(config);
if (!dd.isValid()) {
printf("Failed to create and configure a device\n");
return 1;
}
IFrameGrabberImage *grabberInterface;
if (!dd.view(grabberInterface)) {
printf("Failed to view device through IFrameGrabberImage interface\n");
return 1;
}
ImageOf<PixelRgb> img;
grabberInterface->getImage(img);
printf("Got a %dx%d image\n", img.width(), img.height());
dd.close();
return 0;
}
