Krisjanis Rijnieks
10 years ago
2 changed files with 197 additions and 0 deletions
@ -0,0 +1,174 @@ |
|||
/*
|
|||
* ofxHomographyHelper.cpp |
|||
* Created by Elliot Woods on 26/11/2010. |
|||
* |
|||
* Based entirely on arturo castro's homography implementation |
|||
* Created 08/01/2010, arturo castro |
|||
* |
|||
* http://www.openframeworks.cc/forum/viewtopic.php?f=9&t=3121
|
|||
*/ |
|||
|
|||
#include "ofxHomographyHelper.h" |
|||
|
|||
void ofxHomographyHelper::gaussian_elimination(float *input, int n) |
|||
{ |
|||
// ported to c from pseudocode in
|
|||
// http://en.wikipedia.org/wiki/Gaussian_elimination
|
|||
|
|||
float * A = input; |
|||
int i = 0; |
|||
int j = 0; |
|||
int m = n-1; |
|||
while (i < m && j < n){ |
|||
// Find pivot in column j, starting in row i:
|
|||
int maxi = i; |
|||
for(int k = i+1; k<m; k++){ |
|||
if(fabs(A[k*n+j]) > fabs(A[maxi*n+j])){ |
|||
maxi = k; |
|||
} |
|||
} |
|||
if (A[maxi*n+j] != 0){ |
|||
//swap rows i and maxi, but do not change the value of i
|
|||
if(i!=maxi) |
|||
for(int k=0;k<n;k++){ |
|||
float aux = A[i*n+k]; |
|||
A[i*n+k]=A[maxi*n+k]; |
|||
A[maxi*n+k]=aux; |
|||
} |
|||
//Now A[i,j] will contain the old value of A[maxi,j].
|
|||
//divide each entry in row i by A[i,j]
|
|||
float A_ij=A[i*n+j]; |
|||
for(int k=0;k<n;k++){ |
|||
A[i*n+k]/=A_ij; |
|||
} |
|||
//Now A[i,j] will have the value 1.
|
|||
for(int u = i+1; u< m; u++){ |
|||
//subtract A[u,j] * row i from row u
|
|||
float A_uj = A[u*n+j]; |
|||
for(int k=0;k<n;k++){ |
|||
A[u*n+k]-=A_uj*A[i*n+k]; |
|||
} |
|||
//Now A[u,j] will be 0, since A[u,j] - A[i,j] * A[u,j] = A[u,j] - 1 * A[u,j] = 0.
|
|||
} |
|||
|
|||
i++; |
|||
} |
|||
j++; |
|||
} |
|||
|
|||
//back substitution
|
|||
for(int i=m-2;i>=0;i--){ |
|||
for(int j=i+1;j<n-1;j++){ |
|||
A[i*n+m]-=A[i*n+j]*A[j*n+m]; |
|||
//A[i*n+j]=0;
|
|||
} |
|||
} |
|||
} |
|||
|
|||
|
|||
void ofxHomographyHelper::findHomography(float src[4][2], float dst[4][2], float homography[16]){ |
|||
|
|||
// create the equation system to be solved
|
|||
//
|
|||
// from: Multiple View Geometry in Computer Vision 2ed
|
|||
// Hartley R. and Zisserman A.
|
|||
//
|
|||
// x' = xH
|
|||
// where H is the homography: a 3 by 3 matrix
|
|||
// that transformed to inhomogeneous coordinates for each point
|
|||
// gives the following equations for each point:
|
|||
//
|
|||
// x' * (h31*x + h32*y + h33) = h11*x + h12*y + h13
|
|||
// y' * (h31*x + h32*y + h33) = h21*x + h22*y + h23
|
|||
//
|
|||
// as the homography is scale independent we can let h33 be 1 (indeed any of the terms)
|
|||
// so for 4 points we have 8 equations for 8 terms to solve: h11 - h32
|
|||
// after ordering the terms it gives the following matrix
|
|||
// that can be solved with gaussian elimination:
|
|||
|
|||
float P[8][9]={ |
|||
{-src[0][0], -src[0][1], -1, 0, 0, 0, src[0][0]*dst[0][0], src[0][1]*dst[0][0], -dst[0][0] }, // h11
|
|||
{ 0, 0, 0, -src[0][0], -src[0][1], -1, src[0][0]*dst[0][1], src[0][1]*dst[0][1], -dst[0][1] }, // h12
|
|||
|
|||
{-src[1][0], -src[1][1], -1, 0, 0, 0, src[1][0]*dst[1][0], src[1][1]*dst[1][0], -dst[1][0] }, // h13
|
|||
{ 0, 0, 0, -src[1][0], -src[1][1], -1, src[1][0]*dst[1][1], src[1][1]*dst[1][1], -dst[1][1] }, // h21
|
|||
|
|||
{-src[2][0], -src[2][1], -1, 0, 0, 0, src[2][0]*dst[2][0], src[2][1]*dst[2][0], -dst[2][0] }, // h22
|
|||
{ 0, 0, 0, -src[2][0], -src[2][1], -1, src[2][0]*dst[2][1], src[2][1]*dst[2][1], -dst[2][1] }, // h23
|
|||
|
|||
{-src[3][0], -src[3][1], -1, 0, 0, 0, src[3][0]*dst[3][0], src[3][1]*dst[3][0], -dst[3][0] }, // h31
|
|||
{ 0, 0, 0, -src[3][0], -src[3][1], -1, src[3][0]*dst[3][1], src[3][1]*dst[3][1], -dst[3][1] }, // h32
|
|||
}; |
|||
|
|||
gaussian_elimination(&P[0][0],9); |
|||
|
|||
// gaussian elimination gives the results of the equation system
|
|||
// in the last column of the original matrix.
|
|||
// opengl needs the transposed 4x4 matrix:
|
|||
float aux_H[]={ P[0][8],P[3][8],0,P[6][8], // h11 h21 0 h31
|
|||
P[1][8],P[4][8],0,P[7][8], // h12 h22 0 h32
|
|||
0 , 0,0,0, // 0 0 0 0
|
|||
P[2][8],P[5][8],0,1}; // h13 h23 0 h33
|
|||
|
|||
for(int i=0;i<16;i++) homography[i] = aux_H[i]; |
|||
} |
|||
|
|||
ofMatrix4x4 ofxHomographyHelper::findHomography(float src[4][2], float dst[4][2]){ |
|||
ofMatrix4x4 matrix; |
|||
|
|||
// create the equation system to be solved
|
|||
//
|
|||
// from: Multiple View Geometry in Computer Vision 2ed
|
|||
// Hartley R. and Zisserman A.
|
|||
//
|
|||
// x' = xH
|
|||
// where H is the homography: a 3 by 3 matrix
|
|||
// that transformed to inhomogeneous coordinates for each point
|
|||
// gives the following equations for each point:
|
|||
//
|
|||
// x' * (h31*x + h32*y + h33) = h11*x + h12*y + h13
|
|||
// y' * (h31*x + h32*y + h33) = h21*x + h22*y + h23
|
|||
//
|
|||
// as the homography is scale independent we can let h33 be 1 (indeed any of the terms)
|
|||
// so for 4 points we have 8 equations for 8 terms to solve: h11 - h32
|
|||
// after ordering the terms it gives the following matrix
|
|||
// that can be solved with gaussian elimination:
|
|||
|
|||
float P[8][9]={ |
|||
{-src[0][0], -src[0][1], -1, 0, 0, 0, src[0][0]*dst[0][0], src[0][1]*dst[0][0], -dst[0][0] }, // h11
|
|||
{ 0, 0, 0, -src[0][0], -src[0][1], -1, src[0][0]*dst[0][1], src[0][1]*dst[0][1], -dst[0][1] }, // h12
|
|||
|
|||
{-src[1][0], -src[1][1], -1, 0, 0, 0, src[1][0]*dst[1][0], src[1][1]*dst[1][0], -dst[1][0] }, // h13
|
|||
{ 0, 0, 0, -src[1][0], -src[1][1], -1, src[1][0]*dst[1][1], src[1][1]*dst[1][1], -dst[1][1] }, // h21
|
|||
|
|||
{-src[2][0], -src[2][1], -1, 0, 0, 0, src[2][0]*dst[2][0], src[2][1]*dst[2][0], -dst[2][0] }, // h22
|
|||
{ 0, 0, 0, -src[2][0], -src[2][1], -1, src[2][0]*dst[2][1], src[2][1]*dst[2][1], -dst[2][1] }, // h23
|
|||
|
|||
{-src[3][0], -src[3][1], -1, 0, 0, 0, src[3][0]*dst[3][0], src[3][1]*dst[3][0], -dst[3][0] }, // h31
|
|||
{ 0, 0, 0, -src[3][0], -src[3][1], -1, src[3][0]*dst[3][1], src[3][1]*dst[3][1], -dst[3][1] }, // h32
|
|||
}; |
|||
|
|||
gaussian_elimination(&P[0][0],9); |
|||
|
|||
matrix(0,0)=P[0][8]; |
|||
matrix(0,1)=P[1][8]; |
|||
matrix(0,2)=0; |
|||
matrix(0,3)=P[2][8]; |
|||
|
|||
matrix(1,0)=P[3][8]; |
|||
matrix(1,1)=P[4][8]; |
|||
matrix(1,2)=0; |
|||
matrix(1,3)=P[5][8]; |
|||
|
|||
matrix(2,0)=0; |
|||
matrix(2,1)=0; |
|||
matrix(2,2)=0; |
|||
matrix(2,3)=0; |
|||
|
|||
matrix(3,0)=P[6][8]; |
|||
matrix(3,1)=P[7][8]; |
|||
matrix(3,2)=0; |
|||
matrix(3,3)=1; |
|||
|
|||
return matrix; |
|||
} |
|||
@ -0,0 +1,23 @@ |
|||
/*
|
|||
* ofxHomographyHelper.cpp |
|||
* Created by Elliot Woods on 26/11/2010. |
|||
* |
|||
* Based entirely on arturo castro's homography implementation |
|||
* Created 08/01/2010, arturo castro |
|||
* |
|||
* http://www.openframeworks.cc/forum/viewtopic.php?f=9&t=3121
|
|||
*/ |
|||
|
|||
#pragma once |
|||
|
|||
#include "ofMatrix4x4.h" |
|||
|
|||
class ofxHomographyHelper |
|||
{ |
|||
public: |
|||
static void gaussian_elimination(float *input, int n); |
|||
|
|||
static void findHomography(float src[4][2], float dst[4][2], float homography[16]); |
|||
static ofMatrix4x4 findHomography(float src[4][2], float dst[4][2]); |
|||
}; |
|||
|
|||
Loading…
Reference in new issue