Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

Homography33.cc

Go to the documentation of this file.
00001 //-*-c++-*-
00002 
00003 #include "Shared/newmat/newmat.h"
00004 #include "Shared/newmat/newmatap.h"
00005 
00006 #include "Homography33.h"
00007 
00008 Homography33::Homography33(const std::pair<float,float> &opticalCenter) : cxy(opticalCenter), fA(), H(), valid(false) {}
00009 
00010 fmat::Matrix<3,3>& Homography33::getH() {
00011   compute();
00012   return H;
00013 }
00014 
00015 void Homography33::addCorrespondence(float worldx, float worldy, float imagex, float imagey) {
00016   valid = false;
00017   imagex -= cxy.first;
00018   imagey -= cxy.second;
00019 
00020   /* Here are the rows of matrix A.  We will compute A'*A
00021    * A[3*i+0][3] = -worldxyh[i][0]*imagexy[i][2];
00022    * A[3*i+0][4] = -worldxyh[i][1]*imagexy[i][2];
00023    * A[3*i+0][5] = -worldxyh[i][2]*imagexy[i][2];
00024    * A[3*i+0][6] =  worldxyh[i][0]*imagexy[i][1];
00025    * A[3*i+0][7] =  worldxyh[i][1]*imagexy[i][1];
00026    * A[3*i+0][8] =  worldxyh[i][2]*imagexy[i][1];
00027    *       
00028    * A[3*i+1][0] =  worldxyh[i][0]*imagexy[i][2];
00029    * A[3*i+1][1] =  worldxyh[i][1]*imagexy[i][2];
00030    * A[3*i+1][2] =  worldxyh[i][2]*imagexy[i][2];
00031    * A[3*i+1][6] = -worldxyh[i][0]*imagexy[i][0];
00032    * A[3*i+1][7] = -worldxyh[i][1]*imagexy[i][0];
00033    * A[3*i+1][8] = -worldxyh[i][2]*imagexy[i][0];
00034    *
00035    * A[3*i+2][0] = -worldxyh[i][0]*imagexy[i][1];
00036    * A[3*i+2][1] = -worldxyh[i][1]*imagexy[i][1];
00037    * A[3*i+2][2] = -worldxyh[i][2]*imagexy[i][1];
00038    * A[3*i+2][3] =  worldxyh[i][0]*imagexy[i][0];
00039    * A[3*i+2][4] =  worldxyh[i][1]*imagexy[i][0];
00040    * A[3*i+2][5] =  worldxyh[i][2]*imagexy[i][0];
00041    */
00042 
00043   // only update upper-right. A'A is symmetric, we'll finish the lower left later.
00044   float a03 = -worldx;
00045   float a04 = -worldy;
00046   float a05 = -1;
00047   float a06 = worldx*imagey;
00048   float a07 = worldy*imagey;
00049   float a08 = imagey;
00050 
00051   fA(3, 3) += a03*a03;
00052   fA(3, 4) += a03*a04;
00053   fA(3, 5) += a03*a05;
00054   fA(3, 6) += a03*a06;
00055   fA(3, 7) += a03*a07;
00056   fA(3, 8) += a03*a08;
00057 
00058   fA(4, 4) += a04*a04;
00059   fA(4, 5) += a04*a05;
00060   fA(4, 6) += a04*a06;
00061   fA(4, 7) += a04*a07;
00062   fA(4, 8) += a04*a08;
00063 
00064   fA(5, 5) += a05*a05;
00065   fA(5, 6) += a05*a06;
00066   fA(5, 7) += a05*a07;
00067   fA(5, 8) += a05*a08;
00068 
00069   fA(6, 6) += a06*a06;
00070   fA(6, 7) += a06*a07;
00071   fA(6, 8) += a06*a08;
00072 
00073   fA(7, 7) += a07*a07;
00074   fA(7, 8) += a07*a08;
00075 
00076   fA(8, 8) += a08*a08;
00077 
00078   float a10 = worldx;
00079   float a11 = worldy;
00080   float a12 = 1;
00081   float a16 = -worldx*imagex;
00082   float a17 = -worldy*imagex;
00083   float a18 = -imagex;
00084 
00085   fA(0, 0) += a10*a10;
00086   fA(0, 1) += a10*a11;
00087   fA(0, 2) += a10*a12;
00088   fA(0, 6) += a10*a16;
00089   fA(0, 7) += a10*a17;
00090   fA(0, 8) += a10*a18;
00091 
00092   fA(1, 1) += a11*a11;
00093   fA(1, 2) += a11*a12;
00094   fA(1, 6) += a11*a16;
00095   fA(1, 7) += a11*a17;
00096   fA(1, 8) += a11*a18;
00097 
00098   fA(2, 2) += a12*a12;
00099   fA(2, 6) += a12*a16;
00100   fA(2, 7) += a12*a17;
00101   fA(2, 8) += a12*a18;
00102 
00103   fA(6, 6) += a16*a16;
00104   fA(6, 7) += a16*a17;
00105   fA(6, 8) += a16*a18;
00106 
00107   fA(7, 7) += a17*a17;
00108   fA(7, 8) += a17*a18;
00109 
00110   fA(8, 8) += a18*a18;
00111 
00112   float a20 = -worldx*imagey;
00113   float a21 = -worldy*imagey;
00114   float a22 = -imagey;
00115   float a23 = worldx*imagex;
00116   float a24 = worldy*imagex;
00117   float a25 = imagex;
00118 
00119   fA(0, 0) += a20*a20;
00120   fA(0, 1) += a20*a21;
00121   fA(0, 2) += a20*a22;
00122   fA(0, 3) += a20*a23;
00123   fA(0, 4) += a20*a24;
00124   fA(0, 5) += a20*a25;
00125 
00126   fA(1, 1) += a21*a21;
00127   fA(1, 2) += a21*a22;
00128   fA(1, 3) += a21*a23;
00129   fA(1, 4) += a21*a24;
00130   fA(1, 5) += a21*a25;
00131 
00132   fA(2, 2) += a22*a22;
00133   fA(2, 3) += a22*a23;
00134   fA(2, 4) += a22*a24;
00135   fA(2, 5) += a22*a25;
00136 
00137   fA(3, 3) += a23*a23;
00138   fA(3, 4) += a23*a24;
00139   fA(3, 5) += a23*a25;
00140 
00141   fA(4, 4) += a24*a24;
00142   fA(4, 5) += a24*a25;
00143 
00144   fA(5, 5) += a25*a25;
00145 }
00146 
00147 void Homography33::compute() {
00148   if ( valid ) return;
00149 
00150   // make symmetric
00151   for (int i = 0; i < 9; i++)
00152     for (int j = i+1; j < 9; j++)
00153       fA(j,i) = fA(i,j);
00154 
00155   NEWMAT::Matrix nmA(9,9), U, V;
00156   NEWMAT::DiagonalMatrix D;
00157 
00158   fA.exportToNewmat(nmA);
00159 
00160   // compute using singular value decomposition
00161   NEWMAT::SVD(nmA,D,U,V);
00162 
00163   for (int i = 0; i < 3; i++)
00164     for (int j = 0; j < 3; j++)
00165       H(i,j) = V(i*3+j+1, V.ncols());
00166 
00167   valid = true;
00168 }
00169 
00170 std::pair<float,float> Homography33::project(float worldx, float worldy) {
00171   compute();
00172 
00173   std::pair<float,float> ixy;
00174   ixy.first = H(0,0)*worldx + H(0,1)*worldy + H(0,2);
00175   ixy.second = H(1,0)*worldx + H(1,1)*worldy + H(1,2);
00176   float z = H(2,0)*worldx + H(2,1)*worldy + H(2,2);
00177   ixy.first = ixy.first/z + cxy.first;
00178   ixy.second = ixy.second/z + cxy.second;
00179   return ixy;
00180 }
00181 
00182 std::ostream& operator<<(std::ostream &os, Homography33 &homography) {
00183   os << "Homography33(cx=[" << homography.getCXY().first
00184      << "," << homography.getCXY().second << "], ";
00185   os << homography.getH() << ")";
00186   return os;
00187 }
00188 
00189 

Tekkotsu v5.1CVS
Generated Mon May 9 04:58:41 2016 by Doxygen 1.6.3