Image Alignment Algorithms - Part II

// Module: auxfunc.cpp
// Brief:  Contains implementation of auxilary functions.
// Author: Oleg A. Krivtsov
// Email:  olegkrivtsov@mail.ru
// Date:  March 2008

#include "auxfunc.h"

/* Sets elements of warp matrix W.
 *
 * Warp matrix looks like this one:
 *
 *        | s  -wz  tx |
 * W(p) = | wz  s   ty |
 *        | 0   0    1 |
 *
 * Exception is inverse compositional algorithm.  For inverse compositional algorithm we use
 *
 *        | 1+s  -wz   tx |
 * W(p) = | wz   1+s   ty |
 *        | 0     0     1 |
 *
 *
 */

void init_warp(CvMat* W, float wz, float tx, float ty, float s)
{
	CV_MAT_ELEM(*W, float, 0, 0) = s;
	CV_MAT_ELEM(*W, float, 1, 0) = wz;
	CV_MAT_ELEM(*W, float, 2, 0) = 0;

	CV_MAT_ELEM(*W, float, 0, 1) = -wz;
	CV_MAT_ELEM(*W, float, 1, 1) = s;
	CV_MAT_ELEM(*W, float, 2, 1) = 0;

	CV_MAT_ELEM(*W, float, 0, 2) = tx;
	CV_MAT_ELEM(*W, float, 1, 2) = ty;
	CV_MAT_ELEM(*W, float, 2, 2) = 1;
}


/* Warps image by transforming coordinates of each pixel with W(x;p) = W(p)*X,
 * where W(p) is 3x3 warp matrix; 
 *       X is 3x1 vector containing homogeneous coordinates of the pixel.
 *
 *   @param[in] pSrcFrame Source image.
 *   @param[in, out] pDstImage Warped image.
 *   @param[in] Warp matrix.
 *   @param[in] step  This parameter is used to warp image with subpixel accuracy.
 *
 */

void warp_image(IplImage* pSrcFrame, IplImage* pDstFrame, CvMat* W, float step)
{
	cvSet(pDstFrame, cvScalar(0));

	CvMat* X = cvCreateMat(3, 1, CV_32F);
	CvMat* Z = cvCreateMat(3, 1, CV_32F);
	float x, y;
	
	for(x=0;x<pSrcFrame->width; x+=step)
	{
		for(y=0;y<pSrcFrame->height; y+=step)
		{
			SET_VECTOR(X, x, y);

			cvGEMM(W, X, 1, 0, 0, Z);

			int x2, y2;
			GET_VECTOR(Z, x2, y2);

			if(x2>=0 && x2<pDstFrame->width &&
				y2>=0 && y2<pDstFrame->height)
			{
				CV_IMAGE_ELEM(pDstFrame, uchar, y2, x2) = 
					CV_IMAGE_ELEM(pSrcFrame, uchar, (int)y, (int)x);
			}
		}
	}

	cvSmooth(pDstFrame, pDstFrame);

	cvReleaseMat(&X);
	cvReleaseMat(&Z);
}

/*  Warps coordinates of given rectangle and draws the warped rectangle.
 *  This function is used to show result of image alignment.
 *  @param[in] pImage Image to draw rectangle on.
 *  @param[in] rect   Template rectangle.
 *  @param[in] W      Warp matrix.
 *
 */

void draw_warped_rect(IplImage* pImage, CvRect rect, CvMat* W)
{
	CvPoint lt, lb, rt, rb;
	
	CvMat* X = cvCreateMat(3, 1, CV_32F);
	CvMat* Z = cvCreateMat(3, 1, CV_32F);

	// left-top point
	SET_VECTOR(X, rect.x, rect.y);
	cvGEMM(W, X, 1, 0, 0, Z);
	GET_VECTOR(Z, lt.x, lt.y);

	// left-bottom point
	SET_VECTOR(X, rect.x, rect.y+rect.height);
	cvGEMM(W, X, 1, 0, 0, Z);
	GET_VECTOR(Z, lb.x, lb.y);

	// right-top point
	SET_VECTOR(X, rect.x+rect.width, rect.y);
	cvGEMM(W, X, 1, 0, 0, Z);
	GET_VECTOR(Z, rt.x, rt.y);

	// right-bottom point
	SET_VECTOR(X, rect.x+rect.width, rect.y+rect.height);
	cvGEMM(W, X, 1, 0, 0, Z);
	GET_VECTOR(Z, rb.x, rb.y);

	// draw rectangle
	cvLine(pImage, lt, rt, cvScalar(255));
	cvLine(pImage, rt, rb, cvScalar(255));
	cvLine(pImage, rb, lb, cvScalar(255));
	cvLine(pImage, lb, lt, cvScalar(255));

	// release resources and exit
	cvReleaseMat(&X);
	cvReleaseMat(&Z);
}