/**
 *
 *  Horn & Schunck method for optical flow estimation at a single scale
 *
 */
void horn_schunck_optical_flow(
	const float *I1,             // source image
	const float *I2,             // target image
	float       *u,              // x component of optical flow
	float       *v,              // y component of optical flow
	const int    nx,             // image width
	const int    ny,             // image height
	const float  alpha,          // smoothing parameter
	const int    warps,          // number of warpings per scale
	const float  TOL,            // stopping criterion threshold
	const int    maxiter,        // maximum number of iterations
	const bool   verbose         // switch on messages
)
{
	if (verbose) fprintf(stderr, "Single-scale Horn-Schunck of a %dx%d "
		       "image\n\ta=%g nw=%d eps=%g mi=%d v=%d\n", nx, ny,
			alpha, warps, TOL, maxiter, verbose);

	const int   size   = nx * ny;
	const float alpha2 = alpha * alpha;

	//allocate memory
	int sf = sizeof(float);
	float *I2x  = xmalloc(size * sf); // x derivative of I2
	float *I2y  = xmalloc(size * sf); // y derivative of I2
	float *I2w  = xmalloc(size * sf); // warping of I2
	float *I2wx = xmalloc(size * sf); // warping of I2x
	float *I2wy = xmalloc(size * sf); // warping of I2y
	float *Au   = xmalloc(size * sf); // constant part of numerator of u
	float *Av   = xmalloc(size * sf); // constant part of numerator of v
	float *Du   = xmalloc(size * sf); // denominator of u
	float *Dv   = xmalloc(size * sf); // denominator of v
	float *D    = xmalloc(size * sf); // common numerator of u and v

	// compute the gradient of the second image
	gradient(I2, I2x, I2y, nx, ny);

	// iterative approximation to the Taylor expansions
	for(int n = 0; n < warps; n++)
	{
		if(verbose) fprintf(stderr, "Warping %d:", n);

		// warp the second image and its derivatives
		bicubic_interpolation_warp(I2,  u, v, I2w,  nx, ny, true);
		bicubic_interpolation_warp(I2x, u, v, I2wx, nx, ny, true);
		bicubic_interpolation_warp(I2y, u, v, I2wy, nx, ny, true);

		// store the constant parts of the system
		for(int i = 0; i < size; i++)
		{
			const float I2wl = I2wx[i] * u[i] + I2wy[i] * v[i];
			const float dif  = I1[i] - I2w[i] + I2wl;

			Au[i] = dif * I2wx[i];
			Av[i] = dif * I2wy[i];
			Du[i] = I2wx[i] * I2wx[i] + alpha2;
			Dv[i] = I2wy[i] * I2wy[i] + alpha2;
			D[i]  = I2wx[i] * I2wy[i];
		}

		int niter = 0;
		float error = 1000;

		// iterations of the SOR numerical scheme
		while(error > TOL && niter < maxiter)
		{
			niter++;
			error = 0;

			//process the central part of the optical flow
			#pragma omp parallel for reduction(+:error)
			for(int i = 1; i < ny-1; i++)
			for(int j = 1; j < nx-1; j++)
			{
				const int k = i * nx + j;
				error += sor_iteration(
						Au, Av, Du, Dv, D, u, v, alpha2,
						k, k-nx-1, k-nx+1, k+nx-1,
						k+nx+1, k-nx, k-1, k+nx, k+1
						);
			}

			// process the first and last rows
			for(int j = 1; j < nx-1; j++)
			{
				// first row
				int k = j;
				error += sor_iteration(
						Au, Av, Du, Dv, D, u, v, alpha2,
						k, k-1, k+1, k+nx-1, k+nx+1,
						k, k-1, k+nx, k+1
						);

				// last row
				k = (ny-1) * nx + j;
				error += sor_iteration(
						Au, Av, Du, Dv, D, u, v, alpha2,
						k, k-nx-1, k-nx+1, k-1, k+1,
						k-nx, k-1, k, k+1
						);
			}

			// process the first and last columns
			for(int i = 1; i < ny-1; i++)
			{
				// first column
				int k = i * nx;
				error += sor_iteration(
						Au, Av, Du, Dv, D, u, v, alpha2,
						k, k-nx, k-nx+1, k+nx, k+nx+1,
						k-nx, k, k+nx, k+1
						);

				// last column
				k = (i+1) * nx - 1;
				error += sor_iteration(
						Au, Av, Du, Dv, D, u, v, alpha2,
						k, k-nx-1, k-nx, k+nx-1, k+nx,
						k-nx, k-1, k+nx, k
						);
			}

			// process the corners
			// up-left corner
			error += sor_iteration(
					Au, Av, Du, Dv, D, u, v, alpha2,
					0, 0, 1, nx, nx+1,
					0, 0, nx, 1
					);

			// up-right corner
			int k = nx - 1;
			error += sor_iteration(
					Au, Av, Du, Dv, D, u, v, alpha2,
					k, k-1, k, k+nx-1, k+nx,
					k, k-1, k+nx, k
					);

			// bottom-left corner
			k = (ny-1) * nx;
			error += sor_iteration(
					Au, Av, Du, Dv, D, u, v, alpha2,
					k, k-nx, k-nx+1,k, k+1,
					k-nx, k, k, k+1
					);

			// bottom-right corner
			k = ny * nx - 1;
			error += sor_iteration(
					Au, Av, Du, Dv, D, u, v, alpha2,
					k, k-1, k, k-nx-1, k-nx,
					k-nx, k-1, k, k
					);

			error = sqrt(error / size);
		}

		if(verbose)
			fprintf(stderr, "Iterations %d (%g)\n", niter, error);
	}

	// free the allocated memory
	free(I2x);
	free(I2y);
	free(I2w);
	free(I2wx);
	free(I2wy);
	free(Au);
	free(Av);
	free(Du);
	free(Dv);
	free(D);
}
예제 #2
0
/**
 *
 *  Horn & Schunck method for optical flow estimation at a single scale
 *
 */
static void horn_schunck_optical_flow(
	const float *I1,             // source image
	const float *I2,             // target image
	float       *u,              // x component of optical flow
	float       *v,              // y component of optical flow
	const int    nx,             // image width
	const int    ny,             // image height
	const float  alpha,          // smoothing parameter
	const int    nwarps,         // number of warpings per scale
	const float  epsilon,        // stopping criterion threshold
	const int    nmaxiter,       // maximum number of iterations
	const bool   verbose         // switch on messages
)
{
	if (verbose) fprintf(stderr, "Single-scale Horn-Schunck of a %dx%d "
		       "image\n\ta=%g nw=%d eps=%g mi=%d v=%d\n", nx, ny,
			alpha, nwarps, epsilon, nmaxiter, verbose);

	const int   npixels = nx * ny;
	const float alpha2  = alpha * alpha;

	//allocate memory
	int sf = sizeof(float);
	float *I2x  = xmalloc(npixels * sf); // x derivative of I2
	float *I2y  = xmalloc(npixels * sf); // y derivative of I2
	float *I2w  = xmalloc(npixels * sf); // warping of I2
	float *I2wx = xmalloc(npixels * sf); // warping of I2x
	float *I2wy = xmalloc(npixels * sf); // warping of I2y
	float *Au   = xmalloc(npixels * sf); // constant part of numerator of u
	float *Av   = xmalloc(npixels * sf); // constant part of numerator of v
	float *Du   = xmalloc(npixels * sf); // denominator of u
	float *Dv   = xmalloc(npixels * sf); // denominator of v
	float *D    = xmalloc(npixels * sf); // common numerator of u and v

	// compute the gradient of the second image
	compute_gradient_using_centered_differences(I2, I2x, I2y, nx, ny);

	// iterative approximation to the Taylor expansions
	for (int n = 0; n < nwarps; n++)
	{
		if(verbose) fprintf(stderr, "Warping %d:", n);

		// warp the second image and its derivatives
		bicubic_interpolation_warp(I2,  u, v, I2w,  nx, ny, true);
		bicubic_interpolation_warp(I2x, u, v, I2wx, nx, ny, true);
		bicubic_interpolation_warp(I2y, u, v, I2wy, nx, ny, true);

		// store the constant parts of the system
		// (including the starting values of (u,v) at this warp,
		// denoted by u^n and v^n in the pseudocode of the article)
		for(int i = 0; i < npixels; i++)
		{
			const float I2wl = I2wx[i] * u[i] + I2wy[i] * v[i];
			const float dif  = I1[i] - I2w[i] + I2wl;

			Au[i] = dif * I2wx[i];
			Av[i] = dif * I2wy[i];
			Du[i] = I2wx[i] * I2wx[i] + alpha2;
			Dv[i] = I2wy[i] * I2wy[i] + alpha2;
			D[i]  = I2wx[i] * I2wy[i];
		}

		// counter for the loop below (named "r" on article pseudocode)
		int niter = 0;

		// this variable starts with a dummy value to enter the loop
		float stopping_criterion = epsilon + 1;

		// iterations of the SOR numerical scheme
		while (stopping_criterion > epsilon && niter < nmaxiter)
		{
			niter++;
			stopping_criterion = 0;

			//process the central part of the optical flow
#ifdef _OPENMP
			#pragma omp parallel for reduction(+:stopping_criterion)
#endif
			for(int i = 1; i < ny-1; i++)
			for(int j = 1; j < nx-1; j++)
			{
				const int k = i * nx + j;
				stopping_criterion += sor_iteration(
						Au, Av, Du, Dv, D, u, v, alpha2,
						k, k-nx-1, k-nx+1, k+nx-1,
						k+nx+1, k-nx, k-1, k+nx, k+1
						);
			}

			// process the first and last rows
			for(int j = 1; j < nx-1; j++)
			{
				// first row
				int k = j;
				stopping_criterion += sor_iteration(
						Au, Av, Du, Dv, D, u, v, alpha2,
						k, k-1, k+1, k+nx-1, k+nx+1,
						k, k-1, k+nx, k+1
						);

				// last row
				k = (ny-1) * nx + j;
				stopping_criterion += sor_iteration(
						Au, Av, Du, Dv, D, u, v, alpha2,
						k, k-nx-1, k-nx+1, k-1, k+1,
						k-nx, k-1, k, k+1
						);
			}

			// process the first and last columns
			for(int i = 1; i < ny-1; i++)
			{
				// first column
				int k = i * nx;
				stopping_criterion += sor_iteration(
						Au, Av, Du, Dv, D, u, v, alpha2,
						k, k-nx, k-nx+1, k+nx, k+nx+1,
						k-nx, k, k+nx, k+1
						);

				// last column
				k = (i+1) * nx - 1;
				stopping_criterion += sor_iteration(
						Au, Av, Du, Dv, D, u, v, alpha2,
						k, k-nx-1, k-nx, k+nx-1, k+nx,
						k-nx, k-1, k+nx, k
						);
			}

			// process the corners
			// up-left corner
			stopping_criterion += sor_iteration(
					Au, Av, Du, Dv, D, u, v, alpha2,
					0, 0, 1, nx, nx+1,
					0, 0, nx, 1
					);

			// up-right corner
			int k = nx - 1;
			stopping_criterion += sor_iteration(
					Au, Av, Du, Dv, D, u, v, alpha2,
					k, k-1, k, k+nx-1, k+nx,
					k, k-1, k+nx, k
					);

			// bottom-left corner
			k = (ny-1) * nx;
			stopping_criterion += sor_iteration(
					Au, Av, Du, Dv, D, u, v, alpha2,
					k, k-nx, k-nx+1,k, k+1,
					k-nx, k, k, k+1
					);

			// bottom-right corner
			k = ny * nx - 1;
			stopping_criterion += sor_iteration(
					Au, Av, Du, Dv, D, u, v, alpha2,
					k, k-1, k, k-nx-1, k-nx,
					k-nx, k-1, k, k
					);

			stopping_criterion = sqrt(stopping_criterion / npixels);
		}

		if(verbose)
			fprintf(stderr, "Iterations %d (%g)\n",
				       	niter, stopping_criterion);
	}

	// free the allocated memory
	free(I2x);
	free(I2y);
	free(I2w);
	free(I2wx);
	free(I2wy);
	free(Au);
	free(Av);
	free(Du);
	free(Dv);
	free(D);
}