void CamCalibPort::onRead(ImageOf<PixelRgb> &yrpImgIn)
{
double t=Time::now();
yarp::os::Stamp s;
this->getEnvelope(s);
double time = s.getTime();
if (!updatePose(time)) {
return;
}
unsigned char *pixel = yrpImgIn.getPixelAddress(0, 0);
double *stamp = reinterpret_cast<double*>(pixel);
double backdoorTime = stamp[0];
double backdoorRoll = stamp[1];
double backdoorPitch = stamp[2];
double backdoorYaw = stamp[3];
if (time != backdoorTime) {
yWarning() << "Backdoor time:" << backdoorTime << "Imu time:" << time << "diff:" << (backdoorTime - time);
}
Bottle& b = rpyPort.prepare();
b.clear();
b.addDouble(roll);
b.addDouble(pitch);
b.addDouble(yaw);
b.addDouble(backdoorRoll);
b.addDouble(backdoorPitch);
b.addDouble(backdoorYaw);
b.addDouble(backdoorRoll - roll);
b.addDouble(backdoorPitch - pitch);
b.addDouble(backdoorYaw - yaw);
rpyPort.write();
// execute calibration
if (portImgOut!=NULL) {
yarp::sig::ImageOf<PixelRgb> &yrpImgOut=portImgOut->prepare();
if (verbose) {
yDebug("received input image after %g [s] ... ",t-t0);
}
double t1=Time::now();
if (calibTool!=NULL) {
calibTool->apply(yrpImgIn,yrpImgOut);
for (int r =0; r <yrpImgOut.height(); r++) {
for (int c=0; c<yrpImgOut.width(); c++) {
unsigned char *pixel = yrpImgOut.getPixelAddress(c,r);
double mean = (1.0/3.0)*(pixel[0]+pixel[1]+pixel[2]);
for(int i=0; i<3; i++) {
double s=pixel[i]-mean;
double sn=currSat*s;
sn+=mean;
if(sn<0.0)
sn=0.0;
else if(sn>255.0)
sn=255.0;
pixel[i]=(unsigned char)sn;
}
}
}
if (verbose)
yDebug("calibrated in %g [s]\n",Time::now()-t1);
} else {
yrpImgOut=yrpImgIn;
if (verbose)
yDebug("just copied in %g [s]\n",Time::now()-t1);
}
m.lock();
//timestamp propagation
//yarp::os::Stamp stamp;
//BufferedPort<ImageOf<PixelRgb> >::getEnvelope(stamp);
//portImgOut->setEnvelope(stamp);
Bottle pose;
pose.addDouble(roll);
pose.addDouble(pitch);
pose.addDouble(yaw);
portImgOut->setEnvelope(pose);
portImgOut->writeStrict();
m.unlock();
}
t0=t;
}
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;
}
