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;
}
}
}
void YARPColorConverter::RGB2Normalized (const YARPImageOf<YarpPixelRGB>& in, YARPImageOf<YarpPixelRGBFloat>& out, float threshold)
{
assert (out.GetIplPointer() != NULL && in.GetIplPointer() != NULL);
assert (out.GetHeight() == in.GetHeight());
assert (out.GetWidth() == in.GetWidth());
unsigned char *inTmp = (unsigned char *) in.GetAllocatedArray();
unsigned char *outTmp = (unsigned char *) out.GetAllocatedArray();
int r = 0;
int c = 0;
int padIn = in.GetPadding();
int padOut = out.GetPadding();
float lum;
float *tmp;
for(r = 0; r<in.GetHeight(); r++)
{
for(c = 0; c < in.GetWidth(); c++)
{
tmp = (float *) outTmp;
lum = (float)( inTmp[0] + inTmp[1] + inTmp[2]);
if (lum > threshold)
{
tmp[0] = inTmp[0]/lum;
tmp[1] = inTmp[1]/lum;
tmp[2] = inTmp[2]/lum;
}
else
{
tmp[0] = 0.0;
tmp[1] = 0.0;
tmp[2] = 0.0;
}
inTmp += 3;
outTmp += 3*sizeof(float);
}
inTmp += padIn;
outTmp += padOut;
}
}
// this function now is exactly like the RGB one; however, in the future we may want to use
// different weights for different colors.
void YARPColorConverter::RGB2Normalized (const YARPImageOf<YarpPixelBGR>& in, YARPImageOf<YarpPixelBGR>& out, float threshold)
{
assert (out.GetIplPointer() != NULL && in.GetIplPointer() != NULL);
assert (out.GetHeight() == in.GetHeight());
assert (out.GetWidth() == in.GetWidth());
unsigned char *inTmp = (unsigned char *) in.GetAllocatedArray();
unsigned char *outTmp = (unsigned char *) out.GetAllocatedArray();
int r = 0;
int c = 0;
int padIn = in.GetPadding();
int padOut = out.GetPadding();
float lum;
for(r = 0; r<in.GetHeight(); r++)
{
for(c = 0; c < in.GetWidth(); c++)
{
lum = (float) (inTmp[0] + inTmp[1] + inTmp[2]);
if (lum > threshold)
{
outTmp[0] = (unsigned char)((inTmp[0]/lum)*255 + 0.5); // B
outTmp[1] = (unsigned char)((inTmp[1]/lum)*255 + 0.5); // G
outTmp[2] = (unsigned char)((inTmp[2]/lum)*255 + 0.5); // R
}
else
{
outTmp[0] = 0;
outTmp[1] = 0;
outTmp[2] = 0;
}
inTmp += 3;
outTmp += 3;
}
inTmp += padIn;
outTmp += padOut;
}
}
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 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.
}
}
}
void SetPlane (const YARPImageOf<YarpPixelMono>& in, YARPGenericImage& 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];
dst = (unsigned char *)out.GetArray()[i] + shift;
for (int j = 0; j < width; j++)
{
*dst = *src++;
dst += 3;
}
}
}
// uses short instead of a std image.
int YARPBlobFinder::SpecialTagging(short *tagged, YARPImageOf<YarpPixelHSV>& img)
{
IplImage *source = img.GetIplPointer();
unsigned char *src = (unsigned char *)source->imageData;
short *dst = tagged;
int r, c;
int last_tag = 1;
int left_tag, up_tag;
const int w = source->width;
const int h = source->height;
const int size = w * h;
memset (dst, 0, sizeof(short) * w * h);
r = c = 0;
for (int i = 0; i < size; i++)
{
unsigned char hue = img (c, r).h;
unsigned char sat = img (c, r).s;
if (sat != 0)
{
if (c > 0)
{
if (SimilarSaturation (sat, img(c-1, r).s) &&
SimilarHue (hue, img(c-1, r).h))
left_tag = dst[i - 1];
else
left_tag = 0;
}
else
left_tag = 0;
if (r > 0)
{
if (SimilarSaturation (sat, img(c, r-1).s) &&
SimilarHue (hue, img(c, r-1).h))
up_tag = dst[i - w];
else
up_tag = 0;
}
else
up_tag = 0;
if (left_tag)
{
if (up_tag)
{
// both left and up tagged, may need to merge
if(left_tag != up_tag)
{
MergeRegions(dst, i, w);
}
else
dst[i] = left_tag;
}
else
{
// inherit from the left
dst[i] = left_tag;
}
}
else
{
if (up_tag)
{
// inherit from the top
dst[i] = up_tag;
}
else
{
// gets a new tag
last_tag++;
if (last_tag <= MaxTags)
dst[i] = last_tag;
else
{
// everything under tag MaxTags (?).
dst[i] = MaxTags;
}
}
}
}
else
{
dst[i] = 0;
}
c++;
if (c == w)
{
c = 0;
r++;
}
}
return last_tag;
}
void YARPColorConverter::RGB2HSV (const YARPImageOf<YarpPixelBGR>& in, YARPImageOf<YarpPixelHSV>& out)
{
assert (out.GetIplPointer() != NULL && in.GetIplPointer() != NULL);
iplRGB2HSV(in.GetIplPointer(), out.GetIplPointer());
}
void YARPColorConverter::RGB2Grayscale (const YARPImageOf<YarpPixelBGR>& in, YARPImageOf<YarpPixelMono>& out)
{
assert (out.GetIplPointer() != NULL && in.GetIplPointer() != NULL);
iplColorToGray(in.GetIplPointer(), out.GetIplPointer());
}
void YARPLpFirstOrderFlow::Apply (const YARPImageOf<YarpPixelMono>& currImg, const YARPImageOf<YarpPixelMono>& mask)
{
A_flow=0;
b_flow=0;
// Image FP conversion and log-polar border handling
_char2float(currImg.GetIplPointer(), Temp_img); //ok tested
// Apply Smoothing Filter
_compute_smoothing(Temp_img); //ok tested
// Apply Gaussian Filter
// LATER: Use separable kernel!!!!!!!
iplConvolve2DFP(Smoothing_img, Temp_img, &Gauss, 1, IPL_SUM);
// Apply Temporal Derivative Filter
_compute_derT(Temp_img); //ok tested
// Apply X Derivative Filter
iplConvolve2DFP(Temp_img, DerX_img, &DerivX, 1, IPL_SUM); //ok tested
// Apply Y Derivative Filter
iplConvolve2DFP(Temp_img, DerY_img, &DerivY, 1, IPL_SUM); //ok tested
// passing image array pointers
float *p_dx, *p_dy, *p_dt;
char *p_lineDerX, *p_lineDerY, *p_lineDerT;
unsigned char *p_maskVal;
char *p_lineMask;
IplImage *p_mask;
p_mask = mask.GetIplPointer();
p_lineDerX = DerX_img->imageData;
p_lineDerY = DerY_img->imageData;
p_lineDerT = DerT_img->imageData;
p_lineMask = p_mask->imageData;
double rad_quad;
int c, r;
int item = 1;
int limCol = nEcc - YARP_Border;
double dx, dy, dt;
// Skipping upper border
p_lineDerX += (DerX_img->widthStep * YARP_Border);
p_lineDerY += (DerY_img->widthStep * YARP_Border);
p_lineDerT += (DerT_img->widthStep * YARP_Border);
for (r=0; r<nAng; r++)
{
p_dx = (float *)p_lineDerX;
p_dy = (float *)p_lineDerY;
p_dt = (float *)p_lineDerT;
//skipping left border
p_dx += (YARP_Border);
p_dy += (YARP_Border);
p_dt += (YARP_Border);
p_maskVal = (unsigned char *)p_lineMask;
for (c=0; c<limCol; c++)
{
if (((int)*p_maskVal) < YARP_FuseThr)
{
dx = (double)*p_dx;
dy = (double)*p_dy;
dt = (double)*p_dt;
rad_quad = sqrt(dx*dx+dy*dy);
// Modified by Pasa (|| instead of &&).
if ((rad_quad > YARP_Threshold) || (fabs(dt) > YARP_ThreshLow))
{
SetMatrix(dx,dy,dt,item,c,r);
item++;
}
} // *maskVal.
p_dx++;
p_dy++;
p_dt++;
p_maskVal++;
}
p_lineDerX += DerX_img->widthStep;
p_lineDerY += DerY_img->widthStep;
p_lineDerT += DerT_img->widthStep;
p_lineMask += p_mask->widthStep;
}
assert (item >= YARP_MinPoints);
if (item >= YARP_MinPoints) // at least six components
{
A_trans=A_flow.Transposed();
sqA=A_trans*A_flow;
sqB=A_trans*b_flow;
VisDMatrixLU(sqA,sqB,flowComp);
}
else
{
flowComp = 0;
}
flowPoints = item-1;
}
void YARPLpFirstOrderFlow::Init(const YARPImageOf<YarpPixelMono>& first)
{
_char2float(first.GetIplPointer(), PreviousFrame);
_char2float(first.GetIplPointer(), Smoothing_img);
}