GSLでホモグラフィ行列を求めてみる
出典: Wikimura
[1]でMatlabを使ってホモグラフィ行列を求めたので、今度はGSLを使って同様に求めてみる。
4点対応から求める場合
横長の行列に対する特異値分解関数が無いため、固有値分解を使用する。 ATAを計算し、ATAに格納する。 固有値と固有ベクトルの順番は整列されていない。 一番小さな固有値(これは0と決まってる)に対する固有ベクトルを調べたいので、まず最小の固有値のインデックスを調べる。 最小の固有値をとるインデックスに対する固有ベクトルが答えとなる。
得られた固有値から行列を構築し、スケーリングすることでホモグラフィ行列が得られる。
#include<gsl/gsl_matrix.h>
#include<gsl/gsl_vector.h>
#include<gsl/gsl_eigen.h>
#include<gsl/gsl_blas.h>
#include<stdio.h>
double Psrc[3][4] = { {1, -1, -1, 1},
{1, 1, -1, -1},
{1, 1, 1, 1},
};
double Qsrc[3][4] = { {5, -5, -5, 5},
{5, 5, -5, -5},
{1, 1, 1, 1},
};
void printmat( const gsl_matrix *mat)
{
for( int i=0; i<mat->size1; i++)
{
for( int j=0; j<mat->size2; j++)
{
printf( "%5.3f\t", gsl_matrix_get( mat, i, j));
}
printf( "\n");
}
}
void printvec( const gsl_vector *vec)
{
for( int i=0; i<vec->size; i++)
{
printf( "%5.3f\t", gsl_vector_get( vec, i));
}
printf( "\n");
}
void main( void)
{
gsl_matrix_view Pview = gsl_matrix_view_array( &Psrc[0][0], sizeof(Psrc) / sizeof(Psrc[0]), sizeof(Psrc[0])/sizeof(Psrc[0][0]));
gsl_matrix_view Qview = gsl_matrix_view_array( &Qsrc[0][0], sizeof(Qsrc) / sizeof(Qsrc[0]), sizeof(Qsrc[0])/sizeof(Qsrc[0][0]));
gsl_matrix *P = &Pview.matrix; // 同次座標で表現した点列
gsl_matrix *Q = &Qview.matrix; // Pに対応する点列
gsl_matrix *H = gsl_matrix_calloc( 3, 3); // ホモグラフィ行列
gsl_matrix *A = gsl_matrix_calloc( 8, 9); // PとQから決定される方程式の係数行列
gsl_matrix *ATA = gsl_matrix_calloc( 9, 9); // Aの転置とAの積。ここから固有値を計算する
gsl_matrix *V = gsl_matrix_calloc( 9, 9); // 固有ベクトル
gsl_vector *E = gsl_vector_calloc( 9); // 固有値
gsl_eigen_symmv_workspace *work = gsl_eigen_symmv_alloc( 9); // 固有ベクトル計算の為のワークスペース
int imin; // 最小の固有値のインデックス
double scale; // h33 = 1 にするためのスケール
gsl_vector_view h; // ホモグラフィ行列の解
// Aを構築する
for( int j=0; j<P->size2; j++)
{
gsl_matrix_view block = gsl_matrix_submatrix( A, j<<1, 0, 2, 9); // 9行2列ずつ書き込む
gsl_vector_view Psrc = gsl_matrix_column( P, j);
gsl_vector_view Qsrc = gsl_matrix_column( Q, j);
gsl_vector_view dst1 = gsl_matrix_subrow( &block.matrix, 0, 0, 3);
gsl_vector_view dst2 = gsl_matrix_subrow( &block.matrix, 1, 3, 3);
gsl_vector_view dst3 = gsl_matrix_subrow( &block.matrix, 0, 6, 3);
gsl_vector_view dst4 = gsl_matrix_subrow( &block.matrix, 1, 6, 3);
gsl_vector_memcpy( &dst1.vector, &Psrc.vector);
gsl_vector_memcpy( &dst2.vector, &Psrc.vector);
gsl_vector_memcpy( &dst3.vector, &Psrc.vector);
gsl_vector_memcpy( &dst4.vector, &Psrc.vector);
gsl_vector_scale( &dst3.vector, -gsl_vector_get( &Qsrc.vector, 0));
gsl_vector_scale( &dst4.vector, -gsl_vector_get( &Qsrc.vector, 1));
}
// ATA を計算する
gsl_blas_dgemm( CblasTrans, CblasNoTrans, 1, A, A, 0, ATA);
// 固有ベクトルを求める
gsl_eigen_symmv( ATA, E, V, work);
// 最小の固有値をもつ列を探す(整列されていないため)
imin = gsl_vector_min_index( E);
h = gsl_matrix_column( V, imin);
scale = 1 / gsl_vector_get( &h.vector, 8);
gsl_vector_scale( &h.vector, scale);
// 行列へ代入
for( int i=0; i<3; i++)
{
gsl_vector_view Hrow = gsl_matrix_row( H, i);
gsl_vector_view hrow = gsl_vector_subvector( &h.vector, i*3, 3);
gsl_vector_memcpy( &Hrow.vector, &hrow.vector);
}
// 結果の表示
printf("Eigen vector:\n");
printmat( V);
printf("Eigen value:\n");
printvec( E);
printf("Homography matrix:\n");
printmat(H);
// キー待機
fgetc( stdin);
gsl_matrix_free( H);
gsl_matrix_free( A);
gsl_matrix_free( ATA);
gsl_matrix_free( V);
gsl_vector_free( E);
gsl_eigen_symmv_free( work);
}
特異値分解を使わない理由
[1]で書いたように、ホモグラフィ行列は点対応から得られる同次一次方程式を解くことで得られた。同次方程式の解を求めるのに、ATAの固有値分解かAの特異値分解を行った(Aは係数行列)。 固有値分解を利用する場合、ATAを計算する必要があるため、特異値分解が使えればそちらを使いたいところ。
Wikipedia[2]によると、特異値分解はm行n列の行列を3つの行列の積にすることをいうらしい(複素なら転置ではなく
4点対応からホモグラフィ行列を求める場合、8x9の横長な係数行列を扱うことになる。 ところが、GSLリファレンスマニュアル[3]によれば、GSLが提供する特異値分解関数は、行数 < 列数の横長な行列に対応していない。
