Click here to Skip to main content
Click here to Skip to main content
Add your own
alternative version

Image Alignment Algorithms - Part II

, 21 Apr 2008 CPOL
Implementing and comparing the forwards compositional and the Hager-Belhumeur algorithms.
// 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);
}

By viewing downloads associated with this article you agree to the Terms of Service and the article's licence.

If a file you wish to view isn't highlighted, and is a text file (not binary), please let us know and we'll add colourisation support for it.

License

This article, along with any associated source code and files, is licensed under The Code Project Open License (CPOL)

Share

About the Author

Oleg Krivtsov
Software Developer
Russian Federation Russian Federation
No Biography provided

| Advertise | Privacy | Mobile
Web04 | 2.8.141022.2 | Last Updated 21 Apr 2008
Article Copyright 2008 by Oleg Krivtsov
Everything else Copyright © CodeProject, 1999-2014
Terms of Service
Layout: fixed | fluid