C++ YARPImageOf::GetArray方法代码示例

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;
		}
	}

}

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 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;
//.........这里部分代码省略.........

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));
}