Image Alignment Algorithms - Part II

// Module: invcomp.cpp
// Brief:  Contains implementation of inverse compositional 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.

// Baker-Dellaert-Matthews inverse compositional method
// @param[in] pImgT   Template image T
// @param[in] omega   Rectangular template region
// @param[in] pImgI   Another image I

void align_image_inverse_compositional(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("invcomp.txt", "wt");
	if(f==NULL)
	{
		printf("Error opening log file 'invcomp.txt'\n");
		return;
	}

	// Here we will store internally used images.
	IplImage* pGradTx = 0;	   // Gradient of I in X direction.
	IplImage* pGradTy = 0;	   // Gradient of I in Y direction.
	IplImage* pStDesc = 0;		// Steepest descent images.

	// Here we will store matrices.
	CvMat* W = 0;  // Current value of warp W(x,p)
	CvMat* dW = 0;  // Warp update.
	CvMat* idW = 0; // Inverse of warp update.
	CvMat* X = 0;  // Point in coordinate frame of T.
	CvMat* Z = 0;  // Point in coordinate frame of I.

	CvMat* H = 0;  // Hessian.
	CvMat* iH = 0; // Inverse of 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);
	dW = cvCreateMat(3, 3, CV_32F);
	idW = 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);
	
	CvSize image_size = cvSize(pImgT->width, pImgT->height);
	pGradTx = cvCreateImage(image_size, IPL_DEPTH_16S, 1);
	pGradTy = cvCreateImage(image_size, IPL_DEPTH_16S, 1);
	pStDesc = cvCreateImage(image_size, IPL_DEPTH_32F, 4);

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

	/*
	 *  Precomputation stage.
	 */

	// Calculate gradient of T.
	cvSobel(pImgT, pGradTx, 1, 0); // Gradient in X direction
	cvSobel(pImgT, pGradTy, 0, 1); // Gradient in Y direction
	

	// Compute steepest descent images and Hessian

	cvSet(H, cvScalar(0)); // Set Hessian with zeroes

	int u, v;	// (u,v) - pixel coordinates in the coordinate frame of T.
	int u2, v2; // (u2,v2) - pixel coordinates in the coordinate frame of I.
	
	// 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;

			// Evaluate gradient of T.
			short Tx = CV_IMAGE_ELEM(pGradTx, short, v, u);	
			short Ty = CV_IMAGE_ELEM(pGradTy, short, v, u);	

			// Calculate steepest descent image's element.
			float* stdesc = &CV_IMAGE_ELEM(pStDesc, float, v, u*4); // an element of steepest descent image
			stdesc[0] = (float)(-Tx*+Ty*u);
			stdesc[1] = (float)Tx;
			stdesc[2] = (float)Ty;
			stdesc[3] = (float)(Tx*u+Ty*v);

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

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


	/*
	 *   Iteration stage.
	 */

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

	// Set warp with identity.
	cvSetIdentity(W);

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

		int pixel_count=0; // Count of processed pixels
		
		cvSet(b, cvScalar(0)); // Set b matrix with zeroes
			
		// Walk through pixels in the template T.
		int i, j;
		for(i=0; i<omega.width; i++)
		{
			int u = i + omega.x;

			for(j=0; j<omega.height; j++)
			{
				int 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++;

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

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

					// Add a term to b matrix.
					float* stdesc = &CV_IMAGE_ELEM(pStDesc, float, v, u*4);
					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;					
				}	
			}
		}

		// Finally, calculate mean_error.
		if(pixel_count!=0)
			mean_error/=pixel_count;

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

		init_warp(dW, delta_wz, delta_tx, delta_ty, 1.0f+delta_s);
		// Invert warp.
		inv_res = cvInvert(dW, idW);
		if(inv_res==0)
		{
			printf("Error: Warp matrix is singular.\n");
			return;
		}

		cvGEMM(W, idW, 1, 0, 0, dW);
		cvCopy(dW, W);

		// 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: inverse compositional.\n");
	printf("Caclulation time: %g sec.\n", total_time);
	printf("Iteration count: %d\n", iter);
	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);
	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(&dW);
	cvReleaseMat(&idW);
	cvReleaseMat(&X);
	cvReleaseMat(&Z);

	cvReleaseMat(&H);
	cvReleaseMat(&iH);
	cvReleaseMat(&b);
	cvReleaseMat(&delta_p);
	
	cvReleaseImage(&pGradTx);
	cvReleaseImage(&pGradTy);
	cvReleaseImage(&pStDesc);
}