Go to the documentation of this file.00001
00002 #include "vmal_homog2d.h"
00003 #include <vnl/algo/vnl_svd.h>
00004
00005 vmal_homog2d::vmal_homog2d()
00006 {
00007 }
00008
00009 vmal_homog2d::~vmal_homog2d()
00010 {
00011 }
00012
00013 void vmal_homog2d::compute_homo(const vcl_vector<vnl_double_3 > &pima1,
00014 const vcl_vector<vnl_double_3 > &pima2,
00015 vnl_double_3x3 &hmatrix)
00016 {
00017 int numpoints=pima1.size();
00018 vnl_matrix<double> A(2*numpoints,9);
00019 A.fill(0.0);
00020 for (int row=0,i=0; i<numpoints; ++i)
00021 {
00022 A(row, 0) = pima1[i][0] * pima2[i][2];
00023 A(row, 1) = pima1[i][1] * pima2[i][2];
00024 A(row, 2) = pima1[i][2] * pima2[i][2];
00025 A(row, 3) = 0;
00026 A(row, 4) = 0;
00027 A(row, 5) = 0;
00028 A(row, 6) = -pima1[i][0] * pima2[i][0];
00029 A(row, 7) = -pima1[i][1] * pima2[i][0];
00030 A(row, 8) = -pima1[i][2] * pima2[i][0];
00031 ++row;
00032
00033 A(row, 0) = 0;
00034 A(row, 1) = 0;
00035 A(row, 2) = 0;
00036 A(row, 3) = pima1[i][0] * pima2[i][2];
00037 A(row, 4) = pima1[i][1] * pima2[i][2];
00038 A(row, 5) = pima1[i][2] * pima2[i][2];
00039 A(row, 6) = -pima1[i][0] * pima2[i][1];
00040 A(row, 7) = -pima1[i][1] * pima2[i][1];
00041 A(row, 8) = -pima1[i][2] * pima2[i][1];
00042 ++row;
00043 }
00044
00045 A.normalize_rows();
00046 vnl_svd<double> SVD(A);
00047 vnl_vector<double> x=SVD.nullvector();
00048 hmatrix[0][0]=x[0]; hmatrix[0][1]=x[1]; hmatrix[0][2]=x[2];
00049 hmatrix[1][0]=x[3]; hmatrix[1][1]=x[4]; hmatrix[1][2]=x[5];
00050 hmatrix[2][0]=x[6]; hmatrix[2][1]=x[7]; hmatrix[2][2]=x[8];
00051 }
