YARPOrientation::YARPOrientation(const YARPImageOf<YarpPixelMono> &image)
{
nAng = image.GetHeight() ;
nEcc = image.GetWidth() ;
rfMin = 0.31 ;
initialise() ;
}
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;
}
//
// only find object_no.
//
int YARPObjectContainer::Segment (int object_no, YARPImageOf<YarpPixelBGR>& scan, YARPImageOf<YarpPixelBGR>& out, int& xx, int& yy)
{
if (!m_active)
{
printf ("YARPObjectContainer: need to update stats first\n");
out.PeerCopy(scan);
xx = yy = 0;
return -1;
}
double x, y, quality;
m_locator[object_no].BackProject (scan, m_backp[object_no]);
double ex, ey;
m_locator[object_no].GetExtent (ex, ey);
ex *= SCALE;
ey *= SCALE;
bool valid = false;
if (m_locator[object_no].Find (ex, ey, x, y, quality) >= 0)
{
double mean = 0, stddev = 0;
const double THR = 4.0; // it was 2.0
m_locator[object_no].GetExpectancy (mean, stddev);
valid = (fabs(quality - mean) < stddev * THR) ? true : false;
#ifdef _DEBUG
printf ("object: %d location: %lf %lf q: %lf\n", object_no, x, y, quality);
#endif
}
if (valid)
{
YarpPixelBGR red;
red.r = 255;
red.g = red.b = 0;
double betterx = x;
double bettery = y;
AddRectangle (m_backp[object_no], red, int(betterx+.5), int(bettery+.5), int (ex/2+.5), int (ey/2+.5));
//AddCircleOutline (m_backp[object_no], red, int(max_x+.5), int(max_y+.5), 10);
AddCircle (m_backp[object_no], red, int(betterx+.5), int(bettery+.5), 5);
// return processed image.
out.PeerCopy (m_backp[object_no]);
xx = int (betterx + .5);
yy = int (bettery + .5);
}
else
{
xx = yy = 0;
}
return (valid) ? 0 : -1;
}
void YARPLpShifter::GetShiftedImage(const YARPImageOf<YarpPixelRGB>& im1, YARPImageOf<YarpPixelRGB>& dst, double * shift)
{
unsigned char **src1p0 = (unsigned char **)im1.GetArray();
unsigned char **src2p0 = (unsigned char **)dst.GetArray();
// shift (xi, eta);
int xi = int (shift[0] + .5);
int eta = int (shift[1] + .5);
assert (xi >= 0 && xi < lut_size && eta >= 0 && eta < nAng);
int i,j,i1,j1,jt;
for(i=0; i<nEcc; i++)
for(j=0; j<nAng; j++)
{
jt = (j + eta) % nAng;
i1 = lut[xi][i][jt].ecc;
j1 = lut[xi][i][jt].ang;
j1=(j1 + nAng - eta) % nAng;
if (i1>=0)
{
src2p0[j][i*3] = src1p0[j1][i1*3];
src2p0[j][i*3+1] = src1p0[j1][i1*3+1];
src2p0[j][i*3+2] = src1p0[j1][i1*3+2];
}
else
{
src2p0[j][i*3] = 0;
src2p0[j][i*3+1] = 0;
src2p0[j][i*3+2] = 0;
}
}
}
int
YARPFlowTracker::CenterOfMass (YARPImageOf<YarpPixelMono>& in, int& x, int& y)
{
double xx = 0, yy = 0;
int count = 0;
for (int i = 0; i < in.GetHeight(); i++)
for (int j = 0; j < in.GetWidth(); j++)
{
if (in(j, i) != 0)
{
xx += j;
yy += i;
count ++;
}
}
if (count != 0)
{
x = int(xx / count + .5);
y = int(yy / count + .5);
}
else
{
x = in.GetWidth()/2;
y = in.GetHeight()/2;
}
return 0;
}
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;
}
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;
}
}
}
void YARPLpHistoSegmentation::Apply(YARPImageOf<YarpPixelHSV> &src)
{
int i;
int j;
unsigned char *h;
unsigned char *s;
unsigned char *v;
for(i = 0; i < src.GetHeight(); i++)
{
h = (unsigned char *) src.GetArray()[i];
s = h+1;
v = h+2;
double w = pSize(1, i);
for(j = 0; j < src.GetWidth(); j++)
{
if (_checkThresholds(*h,*s,*v))
YARP3DHistogram::Apply(*h, *s, 0, w);
h += 3;
s += 3;
v += 3;
}
}
}
int main(int argc, const char *argv[])
{
const char *name = DEFAULT_NAME;
char buf[256];
if (argc>1)
{
name = argv[1];
}
sprintf(buf, "%s/i:img", name);
in_img.Register(buf);
sprintf(buf, "%s/o:img", name);
out_img.Register(buf);
sprintf(buf, "%s/i:mix", name);
in_mix.Register(buf);
sprintf(buf, "%s/o:box", name);
out_data.Register(buf);
box_thread.Begin();
while(1)
{
in_img.Read();
YARPImageOf<YarpPixelBGR> in;
YARPImageOf<YarpPixelBGR> out;
in.Refer(in_img.Content());
out_img.Content().SetID(YARP_PIXEL_BGR);
SatisfySize(in,out_img.Content());
out.Refer(out_img.Content());
Filter(in,out);
out_img.Write();
// YARPTime::DelayInSeconds(1000000);
}
return 0;
}
void YARPHistoSegmentation::Apply(YARPImageOf<YarpPixelBGR> &src)
{
int i;
int j;
unsigned char *r;
unsigned char *g;
unsigned char *b;
for(i = 0; i < src.GetHeight(); i++)
{
b = (unsigned char *) src.GetArray()[i];
g = b+1;
r = b+2;
// unsigned char rp, gp, bp;
for(j = 0; j < src.GetWidth(); j++)
{
// _normalize(*r, *g, *b, &rp, &gp, &bp);
// YARP3DHistogram::Apply(rp, gp, bp);
if (_checkThresholds(YarpPixelRGB(*r,*g,*b)))
YARP3DHistogram::Apply(*r, *g, *b);
b += 3;
g += 3;
r += 3;
}
}
}
//
// 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;
}
}
}
// returns the mask and frame of contact.
int
YARPFlowTracker::GetObjectProperties (YARPImageOf<YarpPixelMono>& mask, YARPImageOf<YarpPixelBGR>& frame)
{
// contact_frame
mask.CastCopy (segmentation_mask_copy);
frame.CastCopy (seq->GetImageRef(contact_frame));
return 0;
}
void YARPGaussianFeatures::Apply (const YARPImageOf<YarpPixelMono>& in)
{
assert (in.GetPadding() == 0);
for (int i = 1; i <= m_sigmas; i++)
{
m_features(i) = SpecialConvolveX (m_coeffs[i-1], (const unsigned char *)in.GetAllocatedArray());
m_features(i+m_sigmas) = SpecialConvolveY (m_coeffs[i-1], (const unsigned char *)in.GetAllocatedArray());
}
}
int YARPOrientation::Resize(const YARPImageOf<YarpPixelMono> &image)
{
cleanup() ;
nEcc = image.GetWidth() ;
nAng = image.GetHeight() ;
initialise() ;
return true ;
}
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 Filter(YARPImageOf<YarpPixelBGR>& src,
YARPImageOf<YarpPixelBGR>& dest)
{
FiveBoxesInARow& boxes = out_data.Content();
YARPImageOf<YarpPixelMono> mono;
mono.CastCopy(src);
dest.PeerCopy(src);
trackers.Update(mono,dest,boxes);
out_data.Write();
/*
static ImgTrackTool track;
dest.PeerCopy(src);
track.Apply(dest);
*/
}
void YARPLpConicFitter::plotCircle(int T0, int R0, double R, YARPImageOf<YarpPixelBGR> &output, const YarpPixelBGR &v)
{
int theta;
int rho;
double r0;
r0 = _moments.CsiToRo(R0);
for(theta = 0; theta < _logpolarParams::_stheta; theta++)
{
double c = cos((theta-T0)/_q);
double DELTA = (r0*r0*(c*c-1) + R*R);
if (DELTA>=0)
{
int r = (int) ((r0*c+sqrt(DELTA)) + 0.5);
if (r > 0)
{
rho = _moments.RoToCsi(r);
if ( (rho<=(_logpolarParams::_srho-1)) && (rho>0) )
output(theta,rho) = v;
}
r = (int) ((r0*c-sqrt(DELTA)) + 0.5);
if (r > 0)
{
rho = _moments.RoToCsi(r);
if ( (rho<=(_logpolarParams::_srho-1)) && (rho>0) )
output(theta,rho) = v;
}
}
}
// plot center
output.SafePixel(T0, R0) = v;
}
int YARPOrientation::centroid(const YARPImageOf<YarpPixelMono> &image,
double &x, double &y)
{
x = 0 ;
y = 0 ;
char**
pixels = image.GetArray() ;
double
numeratorX = 0.0,
numeratorY = 0.0,
area = 0.0 ;
for (int i = 0; i < nAng; i++)
{
for (int j = 0; j < nEcc; j++)
{
if ((unsigned char)pixels[i][j] > 250)
{
area += fabs(jacobian[j]) ;
numeratorX += power[j] * ac[i] * jacobian[j] ;
numeratorY += power[j] * as[i] * jacobian[j] ;
}
}
}
x = numeratorX / area ;
y = numeratorY / area ;
return true ;
}
void YARPConicFitter::plotEllipse(int X0, int Y0, double a11, double a12, double a22, YARPImageOf<YarpPixelBGR> &output, const YarpPixelBGR &v)
{
int t;
float theta = 0.0;
const int nThetaS = 200;
const float deltaTh= (float) PI/ (double) nThetaS;
float x,y,r;
if ( !_checkDet(a11, a12, a22) )
{
// sorry, not an ellipse...
return;
}
for(t = 0; t < nThetaS; t++)
{
theta = deltaTh*t;
double c = cos(theta);
double s = sin(theta);
double A = a11*c*c+2*a12*c*s+a22*s*s;
if (A > 0)
{
r = sqrt(1/A);
int xx = int (r*c+0.5);
int yy = int (r*s+0.5);
x = X0 + xx;
y = Y0 - yy;
output.SafePixel(x,y) = v;
x = X0 - xx;
y = Y0 + yy;
output.SafePixel(x,y) = v;
}
}
// plot center
output.SafePixel(X0, Y0) = v;
}
void YARPImageTrackTool::Apply(YARPImageOf<YarpPixelMono>& src, YARPImageOf<YarpPixelMono>& dest, int x, int y)
{
YARPImageOf<YarpPixelMono> prevImg, nextImg;
if ((src.GetWidth()!=IMG_W||src.GetHeight()!=IMG_H) ||
(dest.GetWidth()!=IMG_W||dest.GetHeight()!=IMG_H))
{
printf("Image tracking code is old, and specific to %dx%d images\n", IMG_W, IMG_H);
exit(1);
}
prevImg.Refer(src);
nextImg.Refer(dest);
tx = x; ty = y;
ImgInt& ii1 = *((ImgInt *)prevImg.GetRawBuffer());
ImgInt& ii2 = *((ImgInt *)nextImg.GetRawBuffer());
Apply(ii1, ii2, tx, ty);
}
// very very stupid hash generator.
NetInt32 YARPImageHash::GetHash(YARPImageOf<YarpPixelBGR>& src)
{
NetInt32 key = 0;
int y = src.GetHeight()/2;
int ent = 0;
for (int x=src.GetWidth()-10; x>=10 && ent<5; x--)
{
YarpPixelBGR& pix = src.SafePixel(x,y);
if ((pix.r>=10 && pix.r<=200) || x<=15)
{
key *= 17;
key += pix.r;
key *= 17;
key += pix.g;
key *= 17;
key += pix.b;
ent++;
}
}
return key;
}
void YARPLpConicFitter::plotEllipse(int T0, int R0, double a11, double a12, double a22, YARPImageOf<YarpPixelBGR> &output, const YarpPixelBGR &v)
{
int theta;
int rho;
double r0;
if ( !_checkDet(a11, a12, a22) )
{
// sorry, not an ellipse...
return;
}
r0 = _moments.CsiToRo(R0);
double c0 = cos((T0)/_q);
double s0 = sin((T0)/_q);
for(theta = 0; theta < _logpolarParams::_stheta; theta++)
{
double c = cos((theta)/_q);
double s = sin((theta)/_q);
double A;
double B;
double C;
A = a11*c*c + 2*a12*c*s + a22*s*s;
B = r0*(a11*c*c0 + a12*c*s0 + a12*c0*s + a22*s*s0);
C = r0*r0*(2*a12*c0*s0 + a22*s0*s0 + a11*c0*c0)-1;
double DELTA = B*B-A*C;
if (DELTA >= 0)
{
int r = (int) ((B+sqrt(DELTA))/A + 0.5);
if (r > 0)
{
rho = _moments.RoToCsi(r);
if ( (rho<=(_logpolarParams::_srho-1)) && (rho>0) )
output(theta,rho) = v;
}
r = (int) ((B-sqrt(DELTA))/A + 0.5);
if (r > 0)
{
rho = _moments.RoToCsi(r);
if ( (rho<=(_logpolarParams::_srho-1)) && (rho>0) )
output(theta,rho) = v;
}
}
}
// plot center
output.SafePixel(T0, R0) = v;
}
void YARPBlobFinder::DrawBoxes (YARPImageOf<YarpPixelMono>& id)
{
for (int i = 0; i < MaxBoxes; i++)
{
if (m_attn[i].valid)
{
DrawBox (id.GetIplPointer(),
m_attn[i].xmin,
m_attn[i].ymin,
m_attn[i].xmax,
m_attn[i].ymax);
}
}
}
void GetPlane (const YARPGenericImage& in, YARPImageOf<YarpPixelMono>& out, int shift)
{
ACE_ASSERT (in.GetIplPointer() != NULL && out.GetIplPointer() != NULL);
ACE_ASSERT (in.GetWidth() == out.GetWidth());
ACE_ASSERT (in.GetHeight() == out.GetHeight());
const int width = in.GetWidth();
const int height = in.GetHeight();
unsigned char *src = NULL;
unsigned char *dst = NULL;
for (int i = 0; i < height; i++)
{
src = (unsigned char *)in.GetArray()[i] + shift;
dst = (unsigned char *)out.GetArray()[i];
for (int j = 0; j < width; j++)
{
*dst++ = *src;
src += 3;
}
}
}
/// 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 YARPHistoSegmentation::backProjection(YARPImageOf<YarpPixelHSV> &in, YARPImageOf<YarpPixelMono> &out)
{
// complete histogram backprojection
int i;
int j;
YarpPixelHSV *src;
YarpPixelMono *dst;
for(j = 0; j < in.GetHeight(); j++)
{
src = (YarpPixelHSV *) in.GetArray()[j];
dst = (YarpPixelMono *) out.GetArray()[j];
for(i = 0; i < in.GetWidth(); i++)
{
if (_checkThresholds(src->h, src->s, src->v))
*dst = (unsigned char)(YARP3DHistogram::backProjection(src->h, src->s, 0)*255 + 0.5);
else
*dst = 0;
src++;
dst++;
}
}
}
void YARPHistoSegmentation::backProjection(YARPImageOf<YarpPixelBGR> &in, YARPImageOf<YarpPixelMono> &out)
{
// complete histogram backprojection
int i;
int j;
YarpPixelBGR *src;
YarpPixelRGB tmp;
YarpPixelMono *dst;
for(j = 0; j < in.GetHeight(); j++)
{
src = (YarpPixelBGR *) in.GetArray()[j];
dst = (YarpPixelMono *) out.GetArray()[j];
for(i = 0; i < in.GetWidth(); i++)
{
// _normalize(*src, tmp);
if (_checkThresholds(YarpPixelRGB(src->r, src->g, src->b)))
*dst = (unsigned char)(YARP3DHistogram::backProjection(*src)*255 + 0.5);
else
*dst = 0;
src++;
dst++;
}
}
}
void YARPBlobFinder::DrawBoxes (YARPImageOf<YarpPixelBGR>& id)
{
for (int i = 0; i < MaxBoxes; i++)
{
if (m_attn[i].valid)
{
DrawBox (id.GetIplPointer(),
m_attn[i].xmin,
m_attn[i].ymin,
m_attn[i].xmax,
m_attn[i].ymax,
0, 0, 255); // Use red.
}
}
}
int
YARPFlowTracker::WriteMask (YARPImageOf<YarpPixelMono>& mask, YARPImageOf<YarpPixelBGR>& out, YarpPixelBGR *color)
{
YarpPixelBGR c;
if (color == NULL)
{
c.r = 255;
c.g = 0;
c.b = 255;
}
else
c = *color;
for (int j = 0; j < mask.GetHeight(); j++)
for (int i = 0; i < mask.GetWidth(); i++)
if (mask(i,j) != 0)
{
//out(i,j) = c;
//out(i,j).r = c.r;
out(i,j).b = c.b;
}
return 0;
}