int YARPLogpolar::ReconstructGrays (const YARPImageOf<YarpPixelMono>& in, YARPImageOf<YarpPixelMono>& out)
{
using namespace _logpolarParams;
Reconstruct_Grays((unsigned char *)out.GetRawBuffer(), (unsigned char *)in.GetRawBuffer(), _srho, _stheta, _img.padding, _weightsMap, _img.Pix_Numb);
return YARP_OK;
}
int YARPLogpolar::Uniform2Sawt(const YARPImageOf<YarpPixelBGR>& in, YARPImageOf<YarpPixelBGR>& out)
{
using namespace _logpolarParams;
uniform2Sawt((unsigned char *)out.GetRawBuffer(), (unsigned char *)in.GetRawBuffer(), &_img, _padMap);
return YARP_OK;
}
/// out -> 256 x 256, in 152 x 256.
///
int YARPLogpolar::Logpolar2Cartesian (const YARPImageOf<YarpPixelBGR>& in, YARPImageOf<YarpPixelBGR>& out)
{
using namespace _logpolarParams;
ACE_ASSERT (in.GetWidth() == _stheta && in.GetHeight() == _srho);
ACE_ASSERT (out.GetWidth() == _xsize && out.GetHeight() == _ysize);
Remap ((unsigned char *)out.GetRawBuffer(), (unsigned char *)in.GetRawBuffer(), &_img, _remapMap);
return YARP_OK;
}
void TmpFilter (YARPImageOf<YarpPixelMono>& old, YARPImageOf<YarpPixelMono>& img)
{
char *src = img.GetRawBuffer();
char *optr = old.GetRawBuffer();
const int w = img.GetWidth ();
const int h = img.GetHeight ();
for (int i = 0; i < w * h; i++)
{
*optr = 0.5 * *src++ + 0.5 * *optr;
optr++;
}
memcpy (img.GetRawBuffer(), old.GetRawBuffer(), w * h);
}
void YARPImageTrackTool::Apply(YARPImageOf<YarpPixelBGR>& src)
{
YARPImageOf<YarpPixelBGR> nextImg, blah;
int ox, oy;
if (src.GetWidth() != IMG_W || src.GetHeight() != IMG_H)
{
printf("Image tracking code is old, and specific to %dx%d images\n", IMG_W, IMG_H);
exit(1);
}
nextImg.Refer(src);
if (first)
{
first = 0;
ResetXY();
prevImg3.PeerCopy(nextImg);
delta = 0;
}
else
{
ox = tx;
oy = ty;
ImgInt3& ii1 = *((ImgInt3 *)prevImg3.GetRawBuffer());
ImgInt3& ii2 = *((ImgInt3 *)src.GetRawBuffer());
Apply(ii1, ii2, tx, ty);
if (ox != tx || oy != ty || delta)
{
prevImg3.PeerCopy(nextImg);
delta = 0;
}
}
}
//
// helper.
int
YARPFlowTracker::GrowMask (const YARPImageOf<YarpPixelMono>& src, YARPImageOf<YarpPixelMono>& dest)
{
// block filter 7x7
const int w = segmentation_mask.GetWidth();
const int h = segmentation_mask.GetHeight();
unsigned char *ss = (unsigned char *)src.GetRawBuffer();
dest.Zero();
// save in +2,+2...
unsigned char *s[7];
for (int ll = 0; ll < 7; ll++)
s[ll] = ss+ll*w;
int accum = 0;
for (int i = 0; i < h-7; i++)
{
for (int j = 0; j < w-7; j++)
{
accum = 0;
for (int k = 0; k < 7; k++)
{
accum += s[0][k];
accum += s[1][k];
accum += s[2][k];
accum += s[3][k];
accum += s[4][k];
accum += s[5][k];
accum += s[6][k];
}
accum /= 49;
accum = (accum >= 64) ? 255 : 0;
dest (j+3, i+3) = accum;
s[0] ++;
s[1] ++;
s[2] ++;
s[3] ++;
s[4] ++;
s[5] ++;
s[6] ++;
}
ss += w;
s[0] = ss;
s[1] = s[0]+w;
s[2] = s[1]+w;
s[3] = s[2]+w;
s[4] = s[3]+w;
s[5] = s[4]+w;
s[6] = s[5]+w;
}
return 0;
}
void YARPImageUtils::PasteInto (const YARPImageOf<YarpPixelMono>& src, int x, int y, int zoom, YARPImageOf<YarpPixelMono>& dst)
{
char *bs = dst.GetRawBuffer ();
IplImage *ipl = src.GetIplPointer ();
const int dh = ipl->height;
const int dw = ipl->width;
char *dsY = ipl->imageData;
int depth = dst.GetPixelSize ();
ACE_ASSERT (depth == ipl->nChannels); // same # of chan.
const int h = dst.GetHeight();
ACE_ASSERT (h >= dh); // same height.
const int w = dst.GetWidth();
ACE_ASSERT (w >= dw); // same width.
const int rem_w = w - dw;
// crude limit check.
ACE_ASSERT (dw * zoom + x < w);
ACE_ASSERT (dh * zoom + y < h);
if (zoom == 1)
{
bs += (y * w);
for (int i = 0; i < dh; i++)
{
memcpy (bs + x, dsY, dw);
bs += w;
dsY += dw;
}
}
else
{
bs += (y * w);
for (int i = 0; i < dh; i++)
{
char * st_row = bs;
bs += x;
for (int j = 0; j < dw; j++)
{
for (int k = 0; k < zoom; k++)
{
*bs++ = *dsY;
}
dsY++;
}
for (int k = 1; k < zoom; k++)
memcpy (st_row + x + w * k, st_row + x, dw * zoom);
bs = st_row + w * zoom;
}
}
}
int YARPLogpolar::Logpolar2CartesianFovea (const YARPImageOf<YarpPixelBGR>& in, YARPImageOf<YarpPixelBGR>& out)
{
using namespace _logpolarParams;
ACE_ASSERT (in.GetWidth() == _stheta && in.GetHeight() == _srho);
ACE_ASSERT (out.GetWidth() == _xsizefovea && out.GetHeight() == _ysizefovea);
_img.Size_X_Remap = _xsizefovea;
_img.Size_Y_Remap = _ysizefovea;
_img.Size_Img_Remap = _xsizefovea * _ysizefovea;
_img.Zoom_Level = 512.0/1090.0;
Remap ((unsigned char *)out.GetRawBuffer(), (unsigned char *)in.GetRawBuffer(), &_img, _remapMapFovea);
_img.Size_X_Remap = _xsize;
_img.Size_Y_Remap = _ysize;
_img.Size_Img_Remap = _xsize * _ysize;
_img.Zoom_Level = 256.0/1090.0;
return YARP_OK;
}
bool FindCentroid (YARPImageOf<YarpPixelMono>& img, int *x, int *y)
{
char *ptri = img.GetRawBuffer ();
const int w = img.GetWidth ();
const int h = img.GetHeight ();
unsigned char max = 0;
unsigned char *tmp = (unsigned char *)ptri;
for (int i = 0; i < w * h; i++, tmp++)
if (*tmp > max)
{
max = *tmp;
}
int count = 0;
*x = 0;
*y = 0;
for (i = 0; i < h; i++)
{
unsigned char *row = (unsigned char *)ptri + i * w;
for (int j = 0; j < w; j++, row++)
{
if (*row == max)
{
*x += j;
*y += i;
count ++;
}
}
}
if (count != 0)
{
*x /= count;
*y /= count;
}
else
{
*x = img.GetWidth() / 2;
*y = img.GetHeight() / 2;
}
if (max > 0)
return true;
else
return false;
}
void DrawBoxes (FiveBoxesInARow boxes, YARPImageOf<YarpPixelMono>& img, int weight, int scalefactor)
{
char *ptri = img.GetRawBuffer ();
const int w = img.GetWidth ();
const int h = img.GetHeight ();
// weird stuff...
//
CBox2Send *array = &(boxes.box1);
for (int i = 0; i < 5; i++)
{
CBox2Send *tmp = array++;
if (tmp->valid && Area (tmp) > 500)
{
// draw box i-th
int minr = (tmp->ymin < 0) ? 0 : tmp->ymin;
int maxr = (tmp->ymax >= h) ? h-1 : tmp->ymax;
int minc = (tmp->xmin < 0) ? 0 : tmp->xmin;
int maxc = (tmp->xmax >= w) ? w-1 : tmp->xmax;
for (int r = minr; r <= maxr; r++)
{
char *row = ptri + r * w + tmp->xmin;
for (int c = minc; c <= maxc; c++)
{
int result = *row + weight * scalefactor;
if (result >= 255)
*row++ = 255;
else
*row++ = result;
}
}
}
}
}
void YARPDispMap::dispWrapper( YARPImageOf<YarpPixelMono> & left,
YARPImageOf<YarpPixelMono> & right,
YARPImageOf<YarpPixelMono> & mask,
YARPImageOf<YarpPixelMono> & disparity)
{
int i,j,m,n;
unsigned char * lmask = (unsigned char*) mask.GetRawBuffer();
unsigned char * rmask = (unsigned char*) _imaskImg.GetRawBuffer();
unsigned char * dilat = (unsigned char*) _dilatImg.GetRawBuffer();
unsigned char * l = (unsigned char*) left.GetRawBuffer();
unsigned char * r = (unsigned char*) right.GetRawBuffer();
unsigned char * disp = (unsigned char*) disparity.GetRawBuffer();
unsigned char * idisp = (unsigned char*) _idispImg.GetRawBuffer();
float * eps = (float*)_epsImg.GetRawBuffer();
_dilatImg = mask;
//Dilation
for(j=_dilMask; j<_ySize-_dilMask; j++)
for (i=_dilMask; i<_xSize-_dilMask; i++)
if (lmask[j*_xSize+i]!=0)
for (m=j-_dilMask; m<=j+_dilMask; m++)
for (n=i-_dilMask; n<=i+_dilMask; n++)
dilat[m*_xSize+n] = 255;
else
dilat[j*_xSize+i] = 0;
if (_epsVal)
{
edgePreservingSmooth(l);
for (j=0; j<_imgSize; j++)
if (dilat[j]==255)
l[j] = (unsigned char)(eps[j]+0.5f);
else
l[j] = 0;
edgePreservingSmooth(r);
for (j=1; j<_ySize-1; j++)
for (i=1; i<_xSize-1; i++)
r[j*_xSize+i] = (unsigned char)(eps[j*_xSize+i]+0.5f);
}
else
{
for (j=0; j<_imgSize; j++)
if (dilat[j]==0)
l[j] = 0;
}
getDisparity(l,r,disp);
for (j=0; j<_imgSize; j++)
{
if (lmask[j]==0)
disp[j] = 0;
}
getRightSegm(disp,rmask);
//Dilation
for(j=_dilMask; j<_ySize-_dilMask; j++)
for (i=_dilMask; i<_xSize-_dilMask; i++)
if (rmask[j*_xSize+i]==255)
for (m=j-_dilMask; m<=j+_dilMask; m++)
for (n=i-_dilMask; n<=i+_dilMask; n++)
dilat[m*_xSize+n] = 255;
else
dilat[j*_xSize+i] = 0;
if(_epsVal)
{
for (j=0; j<_imgSize; j++)
if (dilat[j]==255)
r[j] = (unsigned char)(eps[j]);
else
r[j] = 0;
}
else
{
for (j=0; j<_imgSize; j++)
if (dilat[j]==0)
r[j] = 0;
}
getDisparity(r,l,idisp);
invertDisparity(idisp);
for (j=0; j<_imgSize; j++)
if (idisp[j] != 0)
if (disp[j] == 255)
disp[j] = idisp[j];
else if (disp[j] != 0)
disp[j] = (idisp[j]+disp[j])/2;
}