예제 #1
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;
}
예제 #2
0
int
YARPFlowTracker::ComputeRotation (
	YARPImageOf<YarpPixelMono>& mask, 
	int *vx, 
	int *vy, 
	int ox, 
	int oy, 
	CVisDVector& trsf, 
	int thr)
{
	const int border = 1;

	trsf = 0;
	trsf(1) = 1;

	YARPImageOf<YarpPixelMono> tmp;
	tmp.Resize (mask.GetWidth(), mask.GetHeight());
	tmp.Zero();

	double avex = 0;
	double avey = 0;
	double average = 0;
	int count = 0;

	// compute average displacement.
	for (int i = border; i < oy-border; i++)
		for (int j = border; j < ox-border; j++)
		{
			if (vx[i*ox+j] <= OOVERFLOW &&
				vy[i*ox+j] <= OOVERFLOW &&
				mask (j*BLOCKINC+BLOCKSIZE/2, i*BLOCKINC+BLOCKSIZE/2) != 0 
				&& 
				(fabs(vx[i*ox+j]) > 0 || fabs(vy[i*ox+j]) > 0) 
				)
			{
				avex += vx[i*ox+j];
				avey += vy[i*ox+j];
				average += sqrt(vx[i*ox+j]*vx[i*ox+j]+vy[i*ox+j]*vy[i*ox+j]);
				count++;
			}
		}	

	if (count > 0)
	{
		avex /= count;
		avey /= count;
		average /= count;
	}

	//
	if (count >= thr)
	{
		CVisDMatrix A (count * 2, 4);
		CVisDMatrix At (4, count * 2);
		CVisDMatrix sqA (4, 4);
		CVisDVector sqB (4);

		CVisDVector b (count * 2);

		CVisDVector solution(4);

		count = 1;
		for (int i = border; i < oy-border; i++)
			for (int j = border; j < ox-border; j++)
			{
			if (vx[i*ox+j] <= OOVERFLOW &&
				vy[i*ox+j] <= OOVERFLOW &&
					mask (j*BLOCKINC+BLOCKSIZE/2, i*BLOCKINC+BLOCKSIZE/2) != 0 
					&& 
					(fabs(vx[i*ox+j]) > 0 || fabs(vy[i*ox+j]) > 0) 
				)
				{
					A(count,1) = j*BLOCKINC+BLOCKSIZE/2;
					A(count,2) = i*BLOCKINC+BLOCKSIZE/2;
					A(count,3) = 1;
					A(count,4) = 0;
					b(count) = j*BLOCKINC+BLOCKSIZE/2+vx[i*ox+j];
					count++;
					A(count,1) = i*BLOCKINC+BLOCKSIZE/2;
					A(count,2) = -(j*BLOCKINC+BLOCKSIZE/2);
					A(count,3) = 0;
					A(count,4) = 1;
					b(count) = i*BLOCKINC+BLOCKSIZE/2+vy[i*ox+j];
					count++;
				}
			}	
		
		// solve by LU.
		At = A.Transposed ();
		sqA = At * A;
		sqB = At * b;
		VisDMatrixLU (sqA, sqB, solution);

		trsf = solution;

		// apply tranformation to mask.
		double& aa = solution(1);
		double& bb = solution(2);
		double& t1 = solution(3);
		double& t2 = solution(4);

	  for (int i = 0; i < mask.GetHeight(); i++)
			for (int j = 0; j < mask.GetWidth(); j++)
			{
				if (mask (j, i) != 0)
				{
					int dx = int(aa * j + bb * i + t1 +.5);
					int dy = int(-bb * j + aa * i + t2 + .5);

					tmp.SafePixel (dx, dy) = 255;
				}
			}

		mask = tmp;

		return 0;
	}
	else
		return -2;

	return -1;
}