/**
 *
 *  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);
}
Exemplo n.º 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);
}
/**
 * calcMSCLG_OF
 *
 * Main Multiscale CLG-optical flow (CLG-OF) computation function.
 *
 * Parameters:
 *
 * image1          Pointer to the first image (the "previous" time frame).
 * image2          Pointer to the second image (the "current" time frame).
 * uOut            Pointer to the horizontal component of the CLG-OF solution.
 * vOut            Pointer to the vertical component of the CLG-OF solution.
 * nRows           Number of image rows (same for the CLG-OF vector field).
 * nCols           Number of image columns (same for the CLG-OF vector field).
 * iterations      Number of iterations for iterative solution.
 * alpha           Global smoothing coefficient of the CLG-OF.
 * rho             Local spatio-temporal smoothing coefficient of the CLG-OF.
 * sigma           Standard deviation for an optional gaussian smoothing kernel,
 *                 applied to the input images prior to the CLG-OF computation.
 * wFactor         SOR relaxation factor, between 0 and 2.
 * nScales         Total number of scales (at least 1).
 * scaleFactor     Downsampling factor, between 0.5 and 1.0.
 * coupledMode     Iteration type. 1->Pointwise-Coupled Gauss-Seidel, 0->SOR.
 * verbose         Display/hide messages to the stdout.
 *
 */
int calcMSCLG_OF(double* image1,
                 double* image2,
                 double* uOut,
                 double* vOut,
                 int nRows,
                 int nCols,
                 int iterations,
                 double alpha,
                 double rho,
                 double sigma,
                 double wFactor,
                 int nScales,
                 const double scaleFactor,
                 int coupledMode,
                 int verbose) {

    if (verbose) {
        printf("calcMSCLG_OF\n");
        printf("  parameters: nScales=%d, scaleFactor=%f\n",
               nScales, scaleFactor);
    }

    // Variables declaration.
    int s, i;
    double *image2warped;
    int size = nCols * nRows;

    double *I1s[nScales];
    double *I2s[nScales];
    double *us[nScales];
    double *vs[nScales];
    int nxx[nScales];
    int nyy[nScales];

    I1s[0] = image1;
    I2s[0] = image2;

    // Pre-smoothing the finest scale images.
    int filterSize = (int) 2*(DEFAULT_GAUSSIAN_WINDOW_SIZE * sigma) + 1;

    if (filterSize >= MIN_GAUSSIAN_SIZE) {
        printf("  apply gaussian smoothing for each frame\n");
        gaussian(I1s[0], nCols, nRows, sigma);
        gaussian(I2s[0], nCols, nRows, sigma);
    } else {
        printf("  no gaussian smoothing was applied to input frames\n");
    }

    us[0] = uOut;
    vs[0] = vOut;
    nxx[0] = nCols;
    nyy[0] = nRows;

    // Create the scales.
    for (s=1; s<nScales; s++) {

        if (verbose)
            printf("  scales %d init\n", s);

        zoom_size(nxx[s-1], nyy[s-1], nxx+s, nyy+s, scaleFactor);
        const int sizes = nxx[s] * nyy[s];

        I1s[s] = (double *) xmalloc(sizes * sizeof(double));
        I2s[s] = (double *) xmalloc(sizes * sizeof(double));
        us[s]  = (double *) xmalloc(sizes * sizeof(double));
        vs[s]  = (double *) xmalloc(sizes * sizeof(double));

        // Compute the zoom from the previous finer scale.
        zoom_out(I1s[s-1], I1s[s], nxx[s-1], nyy[s-1], scaleFactor);
        zoom_out(I2s[s-1], I2s[s], nxx[s-1], nyy[s-1], scaleFactor);
    }

    // Initialize the OF arrays.
    for (i=0; i < nxx[nScales-1] * nyy[nScales-1]; i++) {
        us[nScales-1][i] = 0.0;
        vs[nScales-1][i] = 0.0;
    }

    // Pyramidal approximation to the OF.
    for (s=nScales-1; s>=0; s--) {
        if (verbose)
            printf("Scale: %d %dx%d, nScales=%d\n",
			s, nxx[s], nyy[s], nScales);

        image2warped = (double *) xmalloc(nxx[s] * nyy[s] * sizeof(double));

        // Warp the second image before computing the derivatives.
        bicubic_interpolation_warp(I2s[s], us[s], vs[s], image2warped,
                                   nxx[s], nyy[s], false);

        // Compute the optical flow.
        calcCLG_OF(I1s[s], image2warped, us[s], vs[s], nyy[s], nxx[s],
                   iterations, alpha, rho, wFactor, verbose, coupledMode);

        free(image2warped);

        // If this was the last scale, finish now.
        if (!s) break;
        // Otherwise, upsample the optical flow.

        // Zoom the OF for the next finer scale.
        zoom_in(us[s], us[s-1], nxx[s], nyy[s], nxx[s-1], nyy[s-1]);
        zoom_in(vs[s], vs[s-1], nxx[s], nyy[s], nxx[s-1], nyy[s-1]);

        // Scale the OF with the appropriate zoom factor.
        for (i=0; i < nxx[s-1] * nyy[s-1]; i++) {
            us[s-1][i] *= 1.0 / scaleFactor;
            vs[s-1][i] *= 1.0 / scaleFactor;
        }
    }

    // Free the allocated memory.
    for (i=1; i<nScales; i++) {
        if (verbose)
            printf("Free Scale: %d %dx%d\n", s, nxx[s], nyy[s]);

        free(I1s[i]);
        free(I2s[i]);
        free(us[i]);
        free(vs[i]);
    }

    if (verbose)
        printf("calcMSCLG_OF: done\n");

    return 1;
}
Exemplo n.º 4
0
/**
 *
 * Function to compute the optical flow in one scale
 *
 **/
void Dual_TVL1_optic_flow(
		float *I0,           // source image
		float *I1,           // target image
		float *u1,           // x component of the optical flow
		float *u2,           // y component of the optical flow
		const int   nx,      // image width
		const int   ny,      // image height
		const float tau,     // time step
		const float lambda,  // weight parameter for the data term
		const float theta,   // weight parameter for (u - v)²
		const int   warps,   // number of warpings per scale
		const float epsilon, // tolerance for numerical convergence
		const bool  verbose  // enable/disable the verbose mode
		)
{
	const int   size = nx * ny;
	const float l_t = lambda * theta;

	size_t sf = sizeof(float);
	float *I1x    = malloc(size*sf);
	float *I1y    = xmalloc(size*sf);
	float *I1w    = xmalloc(size*sf);
	float *I1wx   = xmalloc(size*sf);
	float *I1wy   = xmalloc(size*sf);
	float *rho_c  = xmalloc(size*sf);
	float *v1     = xmalloc(size*sf);
	float *v2     = xmalloc(size*sf);
	float *p11    = xmalloc(size*sf);
	float *p12    = xmalloc(size*sf);
	float *p21    = xmalloc(size*sf);
	float *p22    = xmalloc(size*sf);
	float *div    = xmalloc(size*sf);
	float *grad   = xmalloc(size*sf);
	float *div_p1 = xmalloc(size*sf);
	float *div_p2 = xmalloc(size*sf);
	float *u1x    = xmalloc(size*sf);
	float *u1y    = xmalloc(size*sf);
	float *u2x    = xmalloc(size*sf);
	float *u2y    = xmalloc(size*sf);

	centered_gradient(I1, I1x, I1y, nx, ny);

	// initialization of p
	for (int i = 0; i < size; i++)
	{
		p11[i] = p12[i] = 0.0;
		p21[i] = p22[i] = 0.0;
	}

	for (int warpings = 0; warpings < warps; warpings++)
	{
		// compute the warping of the target image and its derivatives
		bicubic_interpolation_warp(I1,  u1, u2, I1w,  nx, ny, true);
		bicubic_interpolation_warp(I1x, u1, u2, I1wx, nx, ny, true);
		bicubic_interpolation_warp(I1y, u1, u2, I1wy, nx, ny, true);

#pragma omp parallel for
		for (int i = 0; i < size; i++)
		{
			const float Ix2 = I1wx[i] * I1wx[i];
			const float Iy2 = I1wy[i] * I1wy[i];

			// store the |Grad(I1)|^2
			grad[i] = (Ix2 + Iy2);

			// compute the constant part of the rho function
			rho_c[i] = (I1w[i] - I1wx[i] * u1[i]
						- I1wy[i] * u2[i] - I0[i]);
		}

		int n = 0;
		float error = INFINITY;
		while (error > epsilon * epsilon && n < MAX_ITERATIONS)
		{
			n++;
			// estimate the values of the variable (v1, v2)
			// (thresholding opterator TH)
#pragma omp parallel for
			for (int i = 0; i < size; i++)
			{
				const float rho = rho_c[i]
					+ (I1wx[i] * u1[i] + I1wy[i] * u2[i]);

				float d1, d2;

				if (rho < - l_t * grad[i])
				{
					d1 = l_t * I1wx[i];
					d2 = l_t * I1wy[i];
				}
				else
				{
					if (rho > l_t * grad[i])
					{
						d1 = -l_t * I1wx[i];
						d2 = -l_t * I1wy[i];
					}
					else
					{
						if (grad[i] < GRAD_IS_ZERO)
							d1 = d2 = 0;
						else
						{
							float fi = -rho/grad[i];
							d1 = fi * I1wx[i];
							d2 = fi * I1wy[i];
						}
					}
				}

				v1[i] = u1[i] + d1;
				v2[i] = u2[i] + d2;
			}

			// compute the divergence of the dual variable (p1, p2)
			divergence(p11, p12, div_p1, nx ,ny);
			divergence(p21, p22, div_p2, nx ,ny);

			// estimate the values of the optical flow (u1, u2)
			error = 0.0;
#pragma omp parallel for reduction(+:error)
			for (int i = 0; i < size; i++)
			{
				const float u1k = u1[i];
				const float u2k = u2[i];

				u1[i] = v1[i] + theta * div_p1[i];
				u2[i] = v2[i] + theta * div_p2[i];

				error += (u1[i] - u1k) * (u1[i] - u1k) +
					(u2[i] - u2k) * (u2[i] - u2k);
			}
			error /= size;

			// compute the gradient of the optical flow (Du1, Du2)
			forward_gradient(u1, u1x, u1y, nx ,ny);
			forward_gradient(u2, u2x, u2y, nx ,ny);

			// estimate the values of the dual variable (p1, p2)
#pragma omp parallel for
			for (int i = 0; i < size; i++)
			{
				const float taut = tau / theta;
				const float g1   = hypot(u1x[i], u1y[i]);
				const float g2   = hypot(u2x[i], u2y[i]);
				const float ng1  = 1.0 + taut * g1;
				const float ng2  = 1.0 + taut * g2;

				p11[i] = (p11[i] + taut * u1x[i]) / ng1;
				p12[i] = (p12[i] + taut * u1y[i]) / ng1;
				p21[i] = (p21[i] + taut * u2x[i]) / ng2;
				p22[i] = (p22[i] + taut * u2y[i]) / ng2;
			}
		}

		if (verbose)
			fprintf(stderr, "Warping: %d, "
					"Iterations: %d, "
					"Error: %f\n", warpings, n, error);
	}

	// delete allocated memory
	free(I1x);
	free(I1y);
	free(I1w);
	free(I1wx);
	free(I1wy);
	free(rho_c);
	free(v1);
	free(v2);
	free(p11);
	free(p12);
	free(p21);
	free(p22);
	free(div);
	free(grad);
	free(div_p1);
	free(div_p2);
	free(u1x);
	free(u1y);
	free(u2x);
	free(u2y);
}