void RGBDSensorWrapper::shallowCopyImages(const ImageOf<PixelFloat>& src, ImageOf<PixelFloat>& dest)
{
dest.setQuantum(src.getQuantum());
dest.setExternal(src.getRawImage(), src.width(), src.height());
}
bool ImageSplitter::updateModule()
{
ImageOf<PixelRgb> *inputImage = inputPort.read();
yarp::os::Stamp stamp;
inputPort.getEnvelope(stamp);
ImageOf<PixelRgb> &outLeftImage = outLeftPort.prepare();
ImageOf<PixelRgb> &outRightImage = outRightPort.prepare();
inWidth = inputImage->width();
inHeight = inputImage->height();
if(horizontal) // input image is horizontally aligned
{
outWidth = inWidth/2;
outHeight = inHeight;
}
else
{
outWidth = inWidth;
outHeight = inHeight/2;
}
outLeftImage.setQuantum(inputImage->getQuantum());
outRightImage.setQuantum(inputImage->getQuantum());
outLeftImage.resize(outWidth, outHeight);
outRightImage.resize(outWidth, outHeight);
// alloc and compute some vars for efficency
int h2, w2;
unsigned char *pixelLeft, *pixelRight;
unsigned char *pixelInputL, *pixelInputR;
unsigned char *pixelInput = inputImage->getRawImage();
int dualImage_rowSizeByte = inputImage->getRowSize();
int singleImage_rowSizeByte = outLeftImage.getRowSize();
int singleImage_wholeSizeByte = outWidth * outHeight * outLeftImage.getPixelSize();
static int counter = 0;
static double start = 0;
start = yarp::os::Time::now();
switch(method)
{
case 0: // pixel by pixel
{
if(horizontal)
{
for(int h=0; h<outHeight; h++)
{
for(int w1=0; w1<outWidth; w1++)
{
w2 = w1+outWidth;
pixelLeft = outLeftImage.getPixelAddress(w1, h);
pixelLeft[0] = *(inputImage->getPixelAddress(w1, h)+0);
pixelLeft[1] = *(inputImage->getPixelAddress(w1, h)+1);
pixelLeft[2] = *(inputImage->getPixelAddress(w1, h)+2);
pixelRight = outRightImage.getPixelAddress(w1, h);
pixelRight[0] = *(inputImage->getPixelAddress(w2, h)+0);
pixelRight[1] = *(inputImage->getPixelAddress(w2, h)+1);
pixelRight[2] = *(inputImage->getPixelAddress(w2, h)+2);
}
}
}
else
{
for(int h1=0; h1<outHeight; h1++)
{
for(int w=0; w<outWidth; w++)
{
h2 = h1+outHeight;
pixelLeft = outLeftImage.getPixelAddress(w, h1);
pixelLeft[0] = *(inputImage->getPixelAddress(w, h1)+0);
pixelLeft[1] = *(inputImage->getPixelAddress(w, h1)+1);
pixelLeft[2] = *(inputImage->getPixelAddress(w, h1)+2);
pixelRight = outRightImage.getPixelAddress(w, h1);
pixelRight[0] = *(inputImage->getPixelAddress(w, h2)+0);
pixelRight[1] = *(inputImage->getPixelAddress(w, h2)+1);
pixelRight[2] = *(inputImage->getPixelAddress(w, h2)+2);
}
}
}
} break;
case 1: // pixel by pixel, a bit better
{
if(horizontal)
{
pixelLeft = outLeftImage.getRawImage();
pixelRight = outRightImage.getRawImage();
pixelInputL = pixelInput;
pixelInputR = pixelInput+singleImage_rowSizeByte;
for(int h=0, idx=0, idx2=0; h<outHeight; h++)
{
for(int w=0; w<outWidth; w++)
{
pixelLeft[idx++] = *(pixelInputL++);
pixelLeft[idx++] = *(pixelInputL++);
pixelLeft[idx++] = *(pixelInputL++);
pixelRight[idx2++] = *(pixelInputR++);
pixelRight[idx2++] = *(pixelInputR++);
pixelRight[idx2++] = *(pixelInputR++);
}
pixelInputL += singleImage_rowSizeByte;
pixelInputR += singleImage_rowSizeByte;
}
}
else
{
}
} break;
case 2: // line by line
{
if(horizontal)
{
pixelLeft = outLeftImage.getRawImage();
pixelRight = outRightImage.getRawImage();
for(int h=0; h<inHeight; h++)
{
memcpy(pixelLeft + h*singleImage_rowSizeByte, pixelInput, singleImage_rowSizeByte);
memcpy(pixelRight + h*singleImage_rowSizeByte, pixelInput+=singleImage_rowSizeByte, singleImage_rowSizeByte);
pixelInput+= dualImage_rowSizeByte/2;
}
}
else
{
pixelLeft = outLeftImage.getRawImage();
pixelRight = outRightImage.getRawImage();
pixelInputL = pixelInput;
pixelInputR = pixelInput+singleImage_wholeSizeByte;
for(int h=0; h<outHeight; h++)
{
memcpy(pixelLeft + h*singleImage_rowSizeByte, pixelInputL, singleImage_rowSizeByte);
memcpy(pixelRight + h*singleImage_rowSizeByte, pixelInputR, singleImage_rowSizeByte);
pixelInputL+= singleImage_rowSizeByte;
pixelInputR+= singleImage_rowSizeByte;
}
}
} break;
case 3: // whole image, only if input image is vertically aligned
{
if(horizontal)
{
yError() << "Cannot use this copy method with horizontally aligned source image.";
}
else
{
pixelLeft = outLeftImage.getRawImage();
pixelRight = outRightImage.getRawImage();
memcpy(pixelLeft, pixelInput, singleImage_wholeSizeByte);
memcpy(pixelRight, pixelInput+ singleImage_wholeSizeByte, singleImage_wholeSizeByte);
}
} break;
default:
{
yError() << " @line " << __LINE__ << "unhandled switch case, we should not be here!";
}
}
static double end = 0;
static double elapsed = 0;
end = yarp::os::Time::now();
elapsed += (end-start);
counter++;
if((counter % 100) == 0)
{
yInfo() << "Elapsed time: " << elapsed;
elapsed = 0;
}
outLeftPort.setEnvelope(stamp);
outRightPort.setEnvelope(stamp);
outLeftPort.write();
outRightPort.write();
return true;
}
