Image Alignment Algorithms - Part II

// Module: forwadditive.cpp
// Brief:  Contains implementation of forwards additive image alignment algorithm.
// Author: Oleg A. Krivtsov
// Email:  olegkrivtsov@mail.ru
// Date:  March 2008

#include <stdio.h>
#include <time.h>
#include <cv.h>			// Include header for computer-vision part of OpenCV.
#include <highgui.h>	// Include header for GUI part of OpenCV.
#include "auxfunc.h"    // Header for our warping functions.

// Lucas-Kanade method
// @param[in] pImgT   Template image T
// @param[in] omega   Rectangular template region
// @param[in] pImgI   Another image I

void align_image_forwards_additive(IplImage* pImgT, CvRect omega, IplImage* pImgI)
{
	// Some constants for iterative minimization process.
	const float EPS = 1E-5f; // Threshold value for termination criteria.
	const int MAX_ITER = 100;  // Maximum iteration count.
	
	// Open log file
	FILE* f = fopen("forwadditive.txt", "wt");
	if(f==NULL)
	{
		printf("Error opening log file 'forwadditive.txt'\n");
		return;
	}

	// Here we will store internally used images.
	IplImage* pGradIx = 0;	   // Gradient of I in X direction.
	IplImage* pGradIy = 0;	   // Gradient of I in Y direction.

	// Here we will store matrices.
	CvMat* W = 0;  // Current value of warp W(x,p)
	CvMat* X = 0;  // Point in coordinate frame of T.
	CvMat* Z = 0;  // Point in coordinate frame of I.
	CvMat* H = 0;  // The Hessian matrix.
	CvMat* iH = 0; // Inverse of the Hessian.
	CvMat* b = 0;  // Vector in the right side of the system of linear equations.
	CvMat* delta_p = 0; // Parameter update value.
	
	// Create matrices.
	W = cvCreateMat(3, 3, CV_32F);
	X = cvCreateMat(3, 1, CV_32F);
	Z = cvCreateMat(3, 1, CV_32F);

	H = cvCreateMat(4, 4, CV_32F);
	iH = cvCreateMat(4, 4, CV_32F);
	b = cvCreateMat(4, 1, CV_32F);
	delta_p = cvCreateMat(4, 1, CV_32F);
	

	// Create images.
	CvSize image_size = cvSize(pImgT->width, pImgT->height);
	pGradIx = cvCreateImage(image_size, IPL_DEPTH_16S, 1);
	pGradIy = cvCreateImage(image_size, IPL_DEPTH_16S, 1);

	// Get current time. We will use it later to obtain total calculation time.
	clock_t start_time = clock();

	/*
	 *  Precomputation stage.
	 */

	// Calculate gradient of I.
	cvSobel(pImgI, pGradIx, 1, 0); // Gradient in X direction
	cvSobel(pImgI, pGradIy, 0, 1); // Gradient in Y direction

	/*
	 *  Iteration stage.
	 */

	// Here we will store parameter approximation. 
	float wz_a=0, tx_a=0, ty_a=0, s_a=1.0f;

	// Here we will store mean error.
	float mean_error=0;

	// Iterate
	int iter=0; // Number of current iteration
	while(iter < MAX_ITER)
	{
		iter++; // Increment iteration counter

		int pixel_count=0; // Count of processed pixels
		
		init_warp(W, wz_a, tx_a, ty_a, s_a); // Init warp W(x, p)
		cvSet(H, cvScalar(0)); // Set Hessian with zeroes
		cvSet(b, cvScalar(0)); // Set b matrix with zeroes

		
		// (u,v) - pixel coordinates in the coordinate frame of T.
		int u, v;

		// (u2,v2) - pixel coordinates in the coordinate frame of I.
		int u2, v2;
		
		// Walk through pixels in the template T.
		int i, j;
		for(i=0; i<omega.width; i++)
		{
			u = i + omega.x;

			for(j=0; j<omega.height; j++)
			{
				v = j + omega.y;

				// Set vector X with pixel coordinates (u,v,1)
				SET_VECTOR(X, u, v);

				// Warp Z=W*X
				cvGEMM(W, X, 1, 0, 0, Z);

				// Get coordinates of warped pixel in coordinate frame of I.
				GET_VECTOR(Z, u2, v2);

				if(u2>=0 && u2<pImgI->width && // check if pixel is inside I.
					v2>=0 && v2<pImgI->height)
				{
					pixel_count++;

					// Evaluate gradient of I at W(x,p).
					short Ix = CV_IMAGE_ELEM(pGradIx, short, v2, u2);	
					short Iy = CV_IMAGE_ELEM(pGradIy, short, v2, u2);	

					// Calculate steepest descent image's element.
					float stdesc[4]; // an element of steepest descent image
					stdesc[0] = (float)(-v*Ix+u*Iy);
					stdesc[1] = (float)Ix;
					stdesc[2] = (float)Iy;
					stdesc[3] = (float)(u*Ix+v*Iy);

					// Calculate image difference D = T(x)-I(W(x,p)).
					int D = CV_IMAGE_ELEM(pImgT, uchar, v, u) - 
						CV_IMAGE_ELEM(pImgI, uchar, v2, u2);

					// Update mean error value.
					mean_error += abs(D);

					// Add a term to b matrix.
					float* pb = &CV_MAT_ELEM(*b, float, 0, 0);
					pb[0] += stdesc[0] * D;
					pb[1] += stdesc[1] * D;
					pb[2] += stdesc[2] * D;
					pb[3] += stdesc[3] * D;
					
					// Add a term to Hessian.
					int l,m;
					for(l=0;l<4;l++)
					{
						for(m=0;m<4;m++)
						{
							CV_MAT_ELEM(*H, float, l, m) += stdesc[l]*stdesc[m];
						}
					}
				}	
			}
		}

		// Finally, calculate mean error.
		if(pixel_count) mean_error /= pixel_count;

		// Invert Hessian.
		double inv_res = cvInvert(H, iH);
		if(inv_res==0)
		{
			printf("Error: Hessian is singular.\n");
			return;
		}

		// Find parameter increment. 
		cvGEMM(iH, b, 1, 0, 0, delta_p);
		float delta_wz = CV_MAT_ELEM(*delta_p, float, 0, 0);
		float delta_tx = CV_MAT_ELEM(*delta_p, float, 1, 0);
		float delta_ty = CV_MAT_ELEM(*delta_p, float, 2, 0);
		float delta_s = CV_MAT_ELEM(*delta_p, float, 3, 0);

		// Update parameter vector approximation.
		wz_a += delta_wz;
		tx_a += delta_tx;
		ty_a += delta_ty;
		s_a  += delta_s;

		// Print diagnostic information to file.
		fprintf(f, "%d %f\n", 	iter, mean_error);

		// Print dot symbol to screen.
		printf(".");

		// Check termination critera.
		if(fabs(delta_wz)<EPS && fabs(delta_tx)<EPS && 
			fabs(delta_ty)<EPS && fabs(delta_s)<EPS) break;
	}

	// Get current time and obtain total time of calculation.
	clock_t finish_time = clock();
	double total_time = (double)(finish_time-start_time)/CLOCKS_PER_SEC;

	// Print summary.
	printf("\n===============================================\n");
	printf("Algorithm: forwards additive.\n");
	printf("Caclulation time: %g sec.\n", total_time);
	printf("Iteration count: %d\n", iter);
	printf("Approximation: wz_a=%f tx_a=%f ty_a=%f s_a=%f\n", wz_a, tx_a, ty_a, s_a);
	printf("Epsilon: %f\n", EPS);
	printf("Resulting mean error: %f\n", mean_error);
	printf("===============================================\n");

	// Show result of image alignment.
	init_warp(W, wz_a, tx_a, ty_a, s_a);
	draw_warped_rect(pImgI, omega, W);
	cvSetImageROI(pImgT, omega);
	cvShowImage("template",pImgT);
	cvShowImage("image",pImgI);
	cvResetImageROI(pImgT);

	// Free used resources and exit.
	fclose(f);
	
	cvReleaseMat(&W);
	cvReleaseMat(&X);
	cvReleaseMat(&Z);
	
	cvReleaseMat(&H);
	cvReleaseMat(&iH);
	cvReleaseMat(&b);
	cvReleaseMat(&delta_p);

	cvReleaseImage(&pGradIx);
	cvReleaseImage(&pGradIy);
	
}