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;
}
}
}
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;
}
}
}
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 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 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 YARPLpConicFitter::_radius(YARPImageOf<YarpPixelMono> &in, int theta, int rho, int *Rmin, int *Rmax, int *Rav)
{
int t,c;
double avR = 0.0;
double maxR = 0.0;
double minR = 255*255;
int x0;
int y0;
unsigned char *src;
_moments.Logpolar2Cartesian(rho, theta, x0, y0);
for(c = 0; c < _srho; c++)
{
src = (unsigned char *) in.GetArray()[c];
for(t = 0; t < _stheta; t++)
{
if ( in(t,c) == 255 )
{
int x;
int y;
_moments.Logpolar2Cartesian(c, t, x, y);
double R;
R = (x-x0)*(x-x0)+(y-y0)*(y-y0);
avR += R;
if (R>maxR)
maxR = R;
}
else
{
int x;
int y;
_moments.Logpolar2Cartesian(c, t, x, y);
double R;
R = (x-x0)*(x-x0)+(y-y0)*(y-y0);
avR += R;
if (R<minR)
minR = R;
}
src++;
}
}
*Rmax = (int) sqrt(maxR);
*Rmin = (int) sqrt(minR);
*Rav = (int) (*Rmax+*Rmin)/2;
}
int YARPOrientation::arguments(const YARPImageOf<YarpPixelMono> &image,
double x, double y,
double &arcsin, double &arccos)
{
arcsin = 0.0 ;
arccos = 0.0 ;
char**
pixels = image.GetArray() ;
double
A = 0.0,
B = 0.0,
C = 0.0 ;
for (int i = 0; i < nAng; i++)
{
for (int j = 0; j < nEcc; j++)
{
if ((unsigned char)pixels[i][j] > 250)
{
A += (power[j] * ac[i] - x) * (power[j] * ac[i] - x) * jacobian[j] ;
//Logfile::msg (Logfile::info, "A = %f\n", A) ;
B += 2* (power[j] * ac[i] - x) * (power[j] * as[i] - y) * jacobian[j] ;
//Logfile::msg (Logfile::info, "B = %f\n", B) ;
C += (power[j] * as[i] - y) * (power[j] * as[i] - y) * jacobian[j] ;
//Logfile::msg (Logfile::info, "C = %f\n", C) ;
}
}
}
arcsin = B / sqrt(B * B + (A - C) * (A - C)) ;
arccos = (A - C) / sqrt(B * B + (A - C) * (A - C)) ;
return true ;
}
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;
}
}
}
//
// computes also the global correlation (not used!)
//
double YARPLpDisparity::CorrelationMap(const YARPImageOf<YarpPixelRGB>& im1, const YARPImageOf<YarpPixelRGB>& im2)
{
const int border = 2;
int i,j,pos_i;
int xi = 0, eta = 0;
double correl;
float Cmedia1r = .0f, Cmedia2r = .0f;
float Cmedia1g = .0f, Cmedia2g = .0f;
float Cmedia1b = .0f, Cmedia2b = .0f;
float Cnumeratore = .0f, Cdenom1 = .0f, Cdenom2 = .0f;
float numeratore = .0f, denom1 = .0f, denom2 = .0f;
float d1 = .0f, d2 = .0f;
int count=0;
int Ccount=0;
unsigned char **src1 = (unsigned char **)im1.GetArray();
unsigned char **src2 = (unsigned char **)im2.GetArray();
// global average.
for(eta=0; eta<nAng; eta++)
for(xi=0; xi<nEcc-border; xi++)
{
Cmedia1r += src1[eta][xi*3];
Cmedia2r += src2[eta][xi*3];
Cmedia1g += src1[eta][xi*3+1];
Cmedia2g += src2[eta][xi*3+1];
Cmedia1b += src1[eta][xi*3+2];
Cmedia2b += src2[eta][xi*3+2];
Ccount++;
}
Cmedia1r /= float(Ccount);
Cmedia2r /= float(Ccount);
Cmedia1g /= float(Ccount);
Cmedia2g /= float(Ccount);
Cmedia1b /= float(Ccount);
Cmedia2b /= float(Ccount);
float media1r, media2r;
float media1g, media2g;
float media1b, media2b;
// 5 x 5 average and cross-correlation.
for(eta=0; eta<nAng; eta++)
for(xi=border; xi<nEcc-border ; xi++)
{
media1r = .0; media2r = .0;
media1g = .0; media2g = .0;
media1b = .0; media2b = .0;
for(j=-border;j<=border;j++)
for(i=-border;i<=border;i++)
{
if (eta + i >= nAng)
pos_i = eta + i - nAng ;
else
if (eta + i <0)
pos_i = nAng + eta + i;
else
pos_i = eta + i;
media1r += src1[pos_i][(xi+j)*3];
media2r += src2[pos_i][(xi+j)*3];
media1g += src1[pos_i][(xi+j)*3+1];
media2g += src2[pos_i][(xi+j)*3+1];
media1b += src1[pos_i][(xi+j)*3+2];
media2b += src2[pos_i][(xi+j)*3+2];
}
media1r /= _sqr(2*border+1);
media2r /= _sqr(2*border+1);
media1g /= _sqr(2*border+1);
media2g /= _sqr(2*border+1);
media1b /= _sqr(2*border+1);
media2b /= _sqr(2*border+1);
numeratore = .0f;
denom1 = .0f;
denom2 = .0f;
for(j=-border;j<=border;j++)
for(i=-border;i<=border;i++)
{
if (eta + i >= nAng)
pos_i = eta +i - nAng;
else
if (eta + i <0)
pos_i = nAng + eta + i;
else
pos_i = eta + i;
d1 = src1[pos_i][(xi+j)*3] - media1r;
d2 = src2[pos_i][(xi+j)*3] - media2r;
d1 += src1[pos_i][(xi+j)*3+1] - media1g;
d2 += src2[pos_i][(xi+j)*3+1] - media2g;
d1 += src1[pos_i][(xi+j)*3+2] - media1b;
d2 += src2[pos_i][(xi+j)*3+2] - media2b;
numeratore += (d1*d2);
denom1 += (d1*d1);
denom2 += (d2*d2);
if (i==0 && j==0)
{
d1 = src1[pos_i][(xi+j)*3] - Cmedia1r;
d2 = src2[pos_i][(xi+j)*3] - Cmedia2r;
d1 += src1[pos_i][(xi+j)*3+1] - Cmedia1g;
d2 += src2[pos_i][(xi+j)*3+1] - Cmedia2g;
d1 += src1[pos_i][(xi+j)*3+2] - Cmedia1b;
d2 += src2[pos_i][(xi+j)*3+2] - Cmedia2b;
Cnumeratore += (d1*d2);
Cdenom1 += (d1*d1);
Cdenom2 += (d2*d2);
}
}
//
// Correlation could be 1 for uniform areas.
//
correl = 1.0-(double(numeratore)/(denom1+0.00001))*(double(numeratore)/(denom2+0.00001));
if (correl < 0.0)
correl = 0;
else
if (correl > 1.0)
correl = 1;
if (correl < 80.0 / 255.0)
//m_corrMap(xi,eta) = (unsigned char)(correl*255.0);
m_corrMap(xi, eta) = 255;
else
m_corrMap(xi, eta) = 0;
count++;
}
return 1.0-(double(Cnumeratore)/(Cdenom1+0.00001))*(double(Cnumeratore)/(Cdenom2+0.00001));
}
double YARPLpDisparity::ShiftAndCorrelate (const YARPImageOf<YarpPixelMono>& l, const YARPImageOf<YarpPixelMono>& r, int shift)
{
int i, j, i1, j1;
int count = 0;
float d1 = 0;
int jt;
unsigned char **src1p0 = (unsigned char **)l.GetArray();
unsigned char **src2p0 = (unsigned char **)r.GetArray();
int thetaint;
if (shift >= 0)
thetaint = 0;
else
{
shift = -shift;
thetaint = nAng / 2;
}
// use normalized cross correlation.
double average_1 = 0;
double average_2 = 0;
double d_1 = 0;
double d_2 = 0;
// blind implementation.
// there should be a smarter formula.
for(i = 0; i < nEcc; i++)
for(j = 0; j < nAng; j++)
{
jt = (j + thetaint) % nAng;
i1 = lut[shift][i][jt].ecc;
j1 = lut[shift][i][jt].ang;
j1 = (j1 + nAng - thetaint) % nAng;
if (i1 >= 0)
{
average_1 += src1p0[j][i];
average_2 += src2p0[j1][i1];
count++;
}
}
average_1 /= count;
average_2 /= count;
double num = 0;
double den_1 = 0;
double den_2 = 0;
for(i = 0; i < nEcc; i++)
for(j = 0; j < nAng; j++)
{
jt = (j + thetaint) % nAng;
i1 = lut[shift][i][jt].ecc;
j1 = lut[shift][i][jt].ang;
j1 = (j1 + nAng - thetaint) % nAng;
if (i1 >= 0)
{
d_1 = src1p0[j][i] - average_1;
d_2 = src2p0[j1][i1] - average_2;
num += (d_1 * d_2);
den_1 += (d_1 * d_1);
den_2 += (d_2 * d_2);
}
}
return (1.0 - (num * num) / (den_1 * den_2 + 0.00001));
}
//
// computes also the global correlation (not used!)
//
double YARPLpDisparity::CorrelationMap(const YARPImageOf<YarpPixelMono>& im1, const YARPImageOf<YarpPixelMono>& im2)
{
const int border = 2;
int i,j,pos_i;
int xi = 0, eta = 0;
double correl;
float Cmedia1 = .0f, Cmedia2 = .0f;
float Cnumeratore = .0f, Cdenom1 = .0f, Cdenom2 = .0f;
float numeratore = .0f, denom1 = .0f, denom2 = .0f;
float d1 = .0f, d2 = .0f;
int count=0;
int Ccount=0;
unsigned char **src1p = (unsigned char **)im1.GetArray();
unsigned char **src2p = (unsigned char **)im2.GetArray();
for(eta=0; eta<nAng; eta++)
for(xi=0; xi<nEcc-border; xi++)
{
Cmedia1+=src1p[eta][xi];
Cmedia2+=src2p[eta][xi];
Ccount++;
}
Cmedia1 /= float(Ccount);
Cmedia2 /= float(Ccount);
float media1, media2;
for(eta=0; eta<nAng; eta++)
for(xi=border; xi<nEcc-border ; xi++)
{
media1 = .0; media2 = .0;
for(j=-border;j<=border;j++)
for(i=-border;i<=border;i++)
{
if (eta + i >= nAng)
pos_i = eta + i - nAng ;
else
if (eta + i <0)
pos_i = nAng + eta + i;
else
pos_i = eta + i;
media1 += src1p[pos_i][xi+j];
media2 += src2p[pos_i][xi+j];
}
media1 /= _sqr(2*border+1);
media2 /= _sqr(2*border+1);
numeratore = .0f;
denom1 = .0f;
denom2 = .0f;
for(j=-border;j<=border;j++)
for(i=-border;i<=border;i++)
{
if (eta + i >= nAng)
pos_i = eta +i - nAng;
else
if (eta + i <0)
pos_i = nAng + eta + i;
else
pos_i = eta + i;
d1=src1p[pos_i][xi+j]-media1;
d2=src2p[pos_i][xi+j]-media2;
numeratore += (d1*d2);
denom1 += (d1*d1);
denom2 += (d2*d2);
if (i==0 && j==0)
{
d1=src1p[pos_i][xi+j]-Cmedia1;
d2=src2p[pos_i][xi+j]-Cmedia2;
Cnumeratore += (d1*d2);
Cdenom1 += (d1*d1);
Cdenom2 += (d2*d2);
}
}
//
// Correlation could be 1 for uniform areas.
//
correl = 1.0-(double(numeratore)/(denom1+0.00001))*(double(numeratore)/(denom2+0.00001));
if (correl < 0.0)
correl = 0;
else
if (correl > 1.0)
correl = 1;
m_corrMap(xi,eta) = (unsigned char)(correl*255.0);
count++;
}
return 1.0-(double(Cnumeratore)/(Cdenom1+0.00001))*(double(Cnumeratore)/(Cdenom2+0.00001));
}
