int compute_inliers(const IPointMatch *matches, const int number_of_correspondences, const double *H, int *inlier_ids, const double threshold)
{
int i, num_inliers = 0;
double x, y, Z;
double eup, evp;
const double m11 = H[0], m12 = H[1], m13 = H[2], m21 = H[3], m22 = H[4], m23 = H[5], m31 = H[6], m32 = H[7], m33 = H[8];
for(i = 0; i < number_of_correspondences; i++)
{
x = (double)matches[i].second->x, y = (double)matches[i].second->y;
Z = (double)1.0 / (m31*x + m32*y + m33);
eup = (m11*x + m12*y + m13) * Z;
evp = (m21*x + m22*y + m23) * Z;
double distance = dSquare((double)matches[i].first->x - eup) + dSquare((double)matches[i].first->y - evp);
if(distance < threshold)
{
inlier_ids[num_inliers] = i;
num_inliers++;
}
}
return num_inliers;
}
int nice_homography(const double * H)
{
double det = H[0] * H[4] - H[3] * H[1];
if (det < 0) return 0;
double N1 = fast_sqrt( dSquare(H[0]) + dSquare(H[3]) );
if (N1 > 4) return 0;
if (N1 < 0.1) return 0;
double N2 = fast_sqrt( dSquare(H[1]) + dSquare(H[4]) );
if (N2 > 4) return 0;
if (N2 < 0.1) return 0;
double N3 = fast_sqrt( dSquare(H[6]) + dSquare(H[7]) );
if (N3 > 0.002) return 0;
return 1;
}
void display() {
float x, z;
float deltaTime;
float p2Speed = 0.20f;
float stepX, stepZ;
int i = 0;
Vec chase;
Vec move;
float len = 0;
now_t = glutGet(GLUT_ELAPSED_TIME);
deltaTime = (now_t - prev_t) / 1000.0f;
prev_t = now_t;
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glLoadIdentity();
glPushMatrix();
gluLookAt (rotateX, rotateY, startZ + rotateZ,
0.0, 0.0, 0.0,
0.0, 1.0, 0.0);
glRotatef(10, 0, 1, 0);
glRotatef(20, 1, 0, 0);
drawLines();
stepX = 0.08f;
stepZ = 0.08f;
//Draw surface
for (z = -1; z < 0.99; z += stepZ) {
for (x = -1; x < 0.99f; x += stepX) {
if ((i++) % 2) {
dSquare( x, 0.0f, z,
x + stepX, 0.0f, z,
x + stepX, 0.0f, z + stepZ,
x, 0.0f, z + stepZ,
1.0f, 1.0f, 1.0f,
1.0f, 1.0f, 1.0f,
1.0f, 1.0f, 1.0f,
1.0f, 1.0f, 1.0f);
} else {
dSquare( x, 0.0f, z,
x + stepX, 0.0f, z,
x + stepX, 0.0f, z + stepZ,
x, 0.0f, z + stepZ,
1.0f, 0.0f, 0.0f,
1.0f, 0.0f, 0.0f,
1.0f, 0.0f, 0.0f,
1.0f, 0.0f, 0.0f);
}
}
}
glPopMatrix();
glPushMatrix();
gluLookAt (rotateX, rotateY, startZ + rotateZ,
0.0, 0.0, 0.0,
0.0, 1.0, 0.0);
glRotatef(10, 0, 1, 0);
glRotatef(20, 1, 0, 0);
chase.x = pCurr[1].x - pCurr[2].x;
chase.y = pCurr[1].y - pCurr[2].y;
chase.z = pCurr[1].z - pCurr[2].z;
len = sqrt((chase.x * chase.x) + (chase.y * chase.y) +
(chase.z * chase.z));
//Normalize vector
move.x = chase.x / len;
move.y = chase.y / len;
move.z = chase.z / len;
pCurr[2].x += (move.x * p2Speed) * deltaTime;
pCurr[2].y += (move.y * p2Speed) * deltaTime;
pCurr[2].z += (move.z * p2Speed) * deltaTime;
glTranslatef(pCurr[2].x - p[2].x,
pCurr[2].y - p[2].y,
pCurr[2].z - p[2].z);
DrawPlayer(&player2);
glPopMatrix();
glPushMatrix();
gluLookAt (rotateX, rotateY, startZ + rotateZ,
0.0, 0.0, 0.0,
0.0, 1.0, 0.0);
glRotatef(10, 0, 1, 0);
glRotatef(20, 1, 0, 0);
glTranslatef(pCurr[1].x, pCurr[1].y, pCurr[1].z);
DrawPlayer(&player1);
glPopMatrix();
glFlush();
glutSwapBuffers();
glutPostRedisplay();
}
void normalizePoints(const int number_of_correspondences, const double *u_v_up_vp, double *normalized_u_v_up_vp, double *T1, double *T2inv)
{
const double invN = (double)1.0 / (double)number_of_correspondences;
const double sqrt2N = SQRT2 * (double)number_of_correspondences;
double u_sum = 0., v_sum = 0., up_sum = 0., vp_sum = 0;
int i, j;
for(i = 0, j = 0; i < number_of_correspondences; i++) {
u_sum += u_v_up_vp[j++];
v_sum += u_v_up_vp[j++];
up_sum += u_v_up_vp[j++];
vp_sum += u_v_up_vp[j++];
}
double u_mean = u_sum * invN;
double v_mean = v_sum * invN;
double up_mean = up_sum * invN;
double vp_mean = vp_sum * invN;
// translate mean to origin, compute sum of distances from origin
double dist_sum = 0, distp_sum = 0;
double n_up_vp_um, n_up_vp_vm;
for(i = 0; i < number_of_correspondences; i++)
{
normalized_u_v_up_vp[4 * i ] = ( n_up_vp_um = u_v_up_vp[4 * i ] - u_mean );
normalized_u_v_up_vp[4 * i + 1] = ( n_up_vp_vm = u_v_up_vp[4 * i + 1] - v_mean );
dist_sum += fast_sqrt( dSquare(n_up_vp_um) + dSquare(n_up_vp_vm) );
normalized_u_v_up_vp[4 * i + 2] = ( n_up_vp_um = u_v_up_vp[4 * i + 2] - up_mean );
normalized_u_v_up_vp[4 * i + 3] = ( n_up_vp_vm = u_v_up_vp[4 * i + 3] - vp_mean );
distp_sum += fast_sqrt( dSquare(n_up_vp_um) + dSquare(n_up_vp_vm) );
}
// compute normalizing scale factor ( average distance from origin = sqrt(2) )
double scale = sqrt2N / dist_sum;
double scalep = sqrt2N / distp_sum;
// apply scaling
for(i = 0, j = 0; i < number_of_correspondences; i++) {
normalized_u_v_up_vp[j++] *= scale;
normalized_u_v_up_vp[j++] *= scale;
normalized_u_v_up_vp[j++] *= scalep;
normalized_u_v_up_vp[j++] *= scalep;
}
// assemble transformation Matrices, used at denormalization
T1[1] = T1[3] = T1[6] = T1[7] = 0.0;
T1[0] = scale;
T1[2] = -scale * u_mean;
T1[4] = scale;
T1[5] = -scale * v_mean;
T1[8] = (double)1.0;
T2inv[1] = T2inv[3] = T2inv[6] = T2inv[7] = 0.0;
T2inv[0] = (double)1. / scalep;
T2inv[2] = up_mean;
T2inv[4] = (double)1. / scalep;
T2inv[5] = vp_mean;
T2inv[8] = (double)1.0;
}
