// Module: forwcomp.cpp
// Brief: Contains implementation of forwards compositional image alignment algorithm.
// Author: Oleg A. Krivtsov
// Email: olegkrivtsov@mail.ru
// Date: April 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.
// Forwards-compositional method
// @param[in] pImgT Template image T
// @param[in] omega Rectangular template region
// @param[in] pImgI Another image I
void align_image_forwards_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("forwcomp.txt", "wt");
if(f==NULL)
{
printf("Error opening log file 'forwcomp.txt'\n");
return;
}
// Here we will store internally used images.
IplImage* pImgIWarped = 0; // Image I(W)
IplImage* pGradIWx = 0; // Gradient of I(W) in X direction.
IplImage* pGradIWy = 0; // Gradient of I(W) in Y direction.
IplImage* pJacobian = 0; // Image contining precomputed Jacobian for each point.
// Here we will store matrices.
CvMat* W = 0; // Current value of warp W(x,p)
CvMat* dW = 0; // Warp increment W(x, delta_p)
CvMat* M = 0; // Temporary used matrix.
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 vector delta_p=(delta_wz, delta_tx, delta_ty, delta_s).
// Create matrices.
W = cvCreateMat(3, 3, CV_32F);
dW = cvCreateMat(3, 3, CV_32F);
M = 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);
pImgIWarped = cvCreateImage(image_size, IPL_DEPTH_8U, 1);
pGradIWx = cvCreateImage(image_size, IPL_DEPTH_16S, 1);
pGradIWy = cvCreateImage(image_size, IPL_DEPTH_16S, 1);
pJacobian = cvCreateImage(image_size, IPL_DEPTH_32F, 8);
// Get current time. We will use it later to obtain total calculation time.
clock_t start_time = clock();
/*
* Precomputation stage.
*/
// Calculate Jacobian for T(x).
// (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;
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;
float* pj = &CV_IMAGE_ELEM(pJacobian, float, v, u*8);
pj[0] = (float)(-v);
pj[1] = 1.0f;
pj[2] = 0.0f;
pj[3] = (float)u;
pj[4] = (float)u;
pj[5] = 0.0f;
pj[6] = 1.0f;
pj[7] = (float)v;
}
}
/*
* Iteration stage.
*/
// Here we will store mean registration error.
float mean_error=0;
// Init warp matrix 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(H, cvScalar(0)); // Set Hessian with zeroes
cvSet(b, cvScalar(0)); // Set b matrix with zeroes
// Warp image I with W(x,p)
cvSet(pImgIWarped, cvScalar(0));
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++;
CV_IMAGE_ELEM(pImgIWarped, uchar, v, u) =
CV_IMAGE_ELEM(pImgI, uchar, v2, u2);
}
}
}
// Calculate gradient of I(W)(x).
cvSobel(pImgIWarped, pGradIWx, 1, 0); // Gradient in X direction
cvSobel(pImgIWarped, pGradIWy, 0, 1); // Gradient in Y direction
// Walk through pixels of template T.
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 I(W)(x).
short IWx = CV_IMAGE_ELEM(pGradIWx, short, v, u);
short IWy = CV_IMAGE_ELEM(pGradIWy, short, v, u);
// Calculate steepest descent image's element.
float* pj = &CV_IMAGE_ELEM(pJacobian, float, v, u*8);
float stdesc[4]; // an element of steepest descent image
stdesc[0] = (float)(pj[0]*IWx + pj[4]*IWy);
stdesc[1] = (float)(pj[1]*IWx + pj[5]*IWy);
stdesc[2] = (float)(pj[2]*IWx + pj[6]*IWy);
stdesc[3] = (float)(pj[3]*IWx + pj[7]*IWy);
// Calculate image difference D = T(x)-I(W(x,p)).
int D = CV_IMAGE_ELEM(pImgT, uchar, v, u) -
CV_IMAGE_ELEM(pImgIWarped, uchar, v, u);
// 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!=0)
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.
init_warp(dW, delta_wz, delta_tx, delta_ty, 1.0f+delta_s);
cvGEMM(dW, W, 1, 0, 0, M);
cvCopy(M, 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: forwards 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.
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(&M);
cvReleaseMat(&X);
cvReleaseMat(&Z);
cvReleaseMat(&H);
cvReleaseMat(&iH);
cvReleaseMat(&b);
cvReleaseMat(&delta_p);
cvReleaseImage(&pImgIWarped);
cvReleaseImage(&pGradIWx);
cvReleaseImage(&pGradIWy);
cvReleaseImage(&pJacobian);
}