Basic Image AlgorithmS Library 2.8.0

LocalAffineFrame.cpp

00001 #include "Geometry/LocalAffineFrame.hh"
00002 #include "Base/ImageUtils/ImageDraw.hh"
00003 #include "Base/Image/ColourRGB.hh"
00004 #include "Geometry/HMatrix.hh"
00005 
00006 using namespace BIAS;
00007 using namespace std;
00008 
00009 ostream& BIAS::operator<<(ostream& os, const LocalAffineFrame& fp)
00010 {
00011   os << " "<< fp.t_<< " " << fp.A_ << endl;
00012   return os;
00013 }
00014 
00015 istream& BIAS::operator>>(istream& is, LocalAffineFrame& fp)
00016 {
00017   is >> fp.t_;
00018   BIASASSERT(is.good());
00019   is >> fp.A_;
00020   return is;
00021 }
00022 
00023 #ifdef BIAS_HAVE_XML2
00024 int LocalAffineFrame::XMLGetClassName(std::string& TopLevelTag,
00025                                       double& Version) const {
00026   TopLevelTag = "LocalAffineFrame";
00027   Version = 0.1;
00028   return 0;
00029 }
00030 
00031 int LocalAffineFrame::XMLOut(const xmlNodePtr Node,
00032                              XMLIO& XMLObject) const {
00033 
00034   BIASERR("not yet implemented");
00035   BIASABORT;
00036   return 0;
00037 }
00038 
00039 int LocalAffineFrame::XMLIn(const xmlNodePtr Node, XMLIO& XMLObject) {
00040   BIASERR("not yet implemented");
00041   BIASABORT;
00042   return 0;
00043 }
00044 #endif
00045 
00046 
00047 void LocalAffineFrame::Draw(Image<unsigned char>& targetImage,
00048                             const double& positionCovScale,
00049                             const unsigned int linethickness) const {
00050 
00051   // draw elliptic shape and orientation
00052 
00053   double center[2]={t_[0], t_[1]}, a[2], b[2];
00054   // length of ellipse main axes
00055   double eva,evb;
00056   Matrix2x2<double> phi;
00057   Matrix2x2<double> theta;
00058 
00059   HMatrix::FactorizeAffineMatrixRRight(A_, phi,
00060                                        theta, eva, evb);
00061 
00062 
00063   // ellipse
00064   a[0] = phi[0][0] * eva;
00065   a[1] = phi[1][0] * eva;
00066   b[0] = phi[0][1] * evb;
00067   b[1] = phi[1][1] * evb;
00068   unsigned char value[]={127, 255, 0};
00069   BIAS::ImageDraw<unsigned char>::Ellipse(targetImage, center, a, b, value);
00070 
00071   // orientation
00072   Vector2<double> oriLine(0,1);
00073   oriLine =  A_ * oriLine;
00074   if (targetImage.IsPositionInImage((int)rint(center[0]),
00075                                     (int)rint(center[1])) &&
00076       targetImage.IsPositionInImage((int)rint(center[0])+
00077                                     (int)rint(oriLine[0]),
00078                                     (int)rint(center[1]) +
00079                                     (int)rint(oriLine[1]))) {
00080     BIAS::ImageDraw<unsigned char>::
00081       Line(targetImage,
00082            (int)rint(center[0]), (int)rint(center[1]),
00083            (int)rint(center[0])+ (int)rint(oriLine[0]),
00084            (int)rint(center[1]) + (int)rint(oriLine[1]));
00085   }
00086 
00087 
00088   // draw covariance ellipse if desired
00089   if (positionCovScale>0.0) {
00090     double a[2], b[2], eva, evb;
00091     Vector2<double> na, nb;
00092     double center[2]={t_[0], t_[1]};
00093     unsigned char col[]={255, 0, 0};
00094     Matrix2x2<double> curcov;
00095     for (unsigned int i=0; i<2; i++) {
00096       for (unsigned int j=0; j<2; j++) {
00097         curcov[i][j] = Cov_[4+i][4+j];
00098       }
00099     }
00100     curcov.EigenvalueDecomposition(eva, na, evb, nb);
00101     // eigenvalues are the \sigma^2
00102     eva = sqrt(eva);
00103     evb = sqrt(evb);
00104     //out << eva<<", "<<evb<<"\n";
00105     a[0] = na[0] * eva * positionCovScale;
00106     a[1] = na[1] * eva * positionCovScale;
00107     b[0] = nb[0] * evb * positionCovScale;
00108     b[1] = nb[1] * evb * positionCovScale;
00109     //cout<<"drawing ellipse at "<<center<<" with a="<<a[0]<<" "
00110     //    <<a[1]<<" b="<<b[0]<<" "<<b[1]<<endl;
00111     ImageDraw<unsigned char>::Ellipse(targetImage, center, a, b, col);
00112   }
00113 }
00114 
00115 LocalAffineFrame LocalAffineFrame::
00116 GetRelativeTransform(const LocalAffineFrame& F2, bool defaultCov) const {
00117   LocalAffineFrame relativeTransform;
00118   // first set transform
00119   //cout<<GetAsMatrix()<<" "<< F2.GetAsInverseMatrix()<<endl;
00120   relativeTransform.SetFromMatrix(GetAsMatrix() * F2.GetAsInverseMatrix());
00121 
00122   if (defaultCov) {
00123     // use only standard cov without actually computing it
00124     relativeTransform.SetDefaultCov();
00125   } else {
00126     // linear error propagation!
00127 
00128     // make big cov with uncertainties of both transforms:
00129     Matrix<double> covbc(12,12,MatrixZero);
00130     for (unsigned int i=0; i<6; i++) {
00131       for (unsigned int j=i; j<6; j++) {
00132         covbc[i][j]     = covbc[j][i]     = 0.5 * (Cov_[j][i]+Cov_[i][j]);
00133         covbc[i+6][j+6] = covbc[j+6][i+6] = 0.5 * (F2.Cov_[j][i]+F2.Cov_[i][j]);
00134       }
00135     }
00136     // short names for vars:
00137     const double b11=A_[0][0], b12=A_[0][1], b21=A_[1][0], b22=A_[1][1];
00138     const double c11=F2.A_[0][0], c12=F2.A_[0][1], c21=F2.A_[1][0],
00139       c22=F2.A_[1][1], c13=F2.t_[0], c23=F2.t_[1];
00140 
00141     const double detc = c11*c22-c21*c12;
00142 
00143     // compute jacobian of parameter change (this(6), f2(6) --> relativ(6))
00144     Matrix<double> dadbc(6,12,MatrixZero);
00145     // da/db:
00146     dadbc[0][0] = c22/detc;
00147     dadbc[0][1] = -c21/detc;
00148     dadbc[1][0] = -c12/detc;
00149     dadbc[1][1] = c11/detc;
00150     dadbc[4][0] = (c12*c23-c22*c13)/detc;
00151     dadbc[4][1] = (c21*c13-c11*c23)/detc;
00152     dadbc[4][4] = 1;
00153 
00154     dadbc[2][2] = c22/detc;
00155     dadbc[2][3] = -c21/detc;
00156     dadbc[3][2] = -c12/detc;
00157     dadbc[3][3] = c11/detc;
00158     dadbc[5][2] = (c12*c23-c22*c13)/detc;
00159     dadbc[5][3] = (c21*c13-c11*c23)/detc;
00160     dadbc[5][5] = 1;
00161 
00162     const double detc2 = detc*detc;
00163     // da/dc11
00164     dadbc[0][6] = -c22*(b11*c22-b12*c21)/detc2;
00165     dadbc[1][6] = (b12*detc-c22*(b12*c11-b11*c12))/detc2;
00166     dadbc[4][6] = (-c23*b12*detc-c22*(b11*(c12*c23-c22*c13) +
00167                                       b12*(c21*c13-c11*c23)))/detc2;
00168     dadbc[2][6] = -c22*(b21*c22-b22*c21)/detc2;
00169     dadbc[3][6] = (b22*detc-c22*(b22*c11-b21*c12))/detc2;
00170     dadbc[5][6] = (-c23*b22*detc-c22*(b21*(c12*c23-c22*c13) +
00171                                       b22*(c21*c13-c11*c23)))/detc2;
00172 
00173     // da/dc12
00174     dadbc[0][7] = c21*(b11*c22-b12*c21)/detc2;
00175     dadbc[1][7] = (-b11*detc+c21*(b12*c11-b11*c12))/detc2;
00176     dadbc[4][7] = (c23*b11*detc+c21*(b11*(c12*c23-c22*c13)+
00177                                      b12*(c21*c13-c11*c23)))/detc2;
00178     dadbc[2][7] = c21*(b21*c22-b22*c21)/detc2;
00179     dadbc[3][7] = (-b21*detc+c21*(b22*c11-b21*c12))/detc2;
00180     dadbc[5][7] = (c23*b21*detc+c21*(b21*(c12*c23-c22*c13)+
00181                                      b22*(c21*c13-c11*c23)))/detc2;
00182 
00183     // da/dc13
00184     dadbc[4][10] = (b12*c21-c22*b11)/detc;
00185     dadbc[5][10] = (b22*c21-c22*b21)/detc;
00186 
00187     // da/dc21
00188     dadbc[0][8] = (c12*(b11*c22-b12*c21)-b21*detc)/detc2;
00189     dadbc[1][8] = c12*(b12*c11-b11*c12)/detc2;
00190     dadbc[4][8] = (b12*c13*detc+c12*(b11*(c12*c23-c22*c13)+
00191                                      b12*(c21*c13-c11*c23)))/detc2;
00192     dadbc[2][8] = (c12*(b21*c22-b22*c21)-b22*detc)/detc2;
00193     dadbc[3][8] = c12*(b22*c11-b21*c12)/detc2;
00194     dadbc[5][8] = (b22*c13*detc+c12*(b21*(c12*c23-c22*c13)+
00195                                      b22*(c21*c13-c11*c23)))/detc2;
00196 
00197     // da/dc22
00198     dadbc[0][9]= (-c11*(b11*c22-b12*c21)+b11*detc)/detc2;
00199     dadbc[1][9]= c11*(b12*c11-b11*c12)/detc2;
00200     dadbc[4][9]= (-b11*c13*detc-c11*(b11*(c12*c23-c22*c13)+
00201                                       b12*(c21*c13-c11*c23)))/detc2;
00202     dadbc[2][9]= (-c11*(b21*c22-b22*c21)+b21*detc)/detc2;
00203     dadbc[3][9]= c11*(b22*c11-b21*c12)/detc2;
00204     dadbc[5][9]= (-b21*c13*detc-c11*(b21*(c12*c23-c22*c13)+
00205                                      b22*(c21*c13-c11*c23)))/detc2;
00206 
00207     // da/dc23
00208     dadbc[4][11]= (b11*c12-b12*c11)/detc;
00209     dadbc[5][11]= (b21*c12-b22*c11)/detc;
00210 
00211     // linear error propagation
00212     relativeTransform.Cov_ = dadbc * covbc * dadbc.Transpose();
00213   }
00214 
00215   return relativeTransform;
00216 }
00217 
00218 
00219 LocalAffineFrame LocalAffineFrame::
00220 GetHomographyTransformed(const HMatrix& H) const {
00221   LocalAffineFrame result;
00222   Matrix2x2<double> jac;
00223   HomgPoint2D x(t_);
00224   H.GetJacobian(x,jac);
00225   x = H*x;
00226   x.Homogenize();
00227   Matrix<double> affineTransformer(6,6,MatrixZero);
00228   affineTransformer[1][1] = affineTransformer[0][0] = jac[0][0];
00229   affineTransformer[3][3] = affineTransformer[2][2] = jac[1][1];
00230   affineTransformer[2][0] = affineTransformer[3][1] = jac[1][0];
00231   affineTransformer[0][2] = affineTransformer[1][3] = jac[0][1];
00232   affineTransformer[4][4] = jac[0][0];
00233   affineTransformer[4][5] = jac[0][1];
00234   affineTransformer[5][4] = jac[1][0];
00235   affineTransformer[5][5] = jac[1][1];
00236 
00237   Vector<double> affineparams(GetAsVector());
00238   //cout<<"affine params are "<< affineparams << endl;
00239 
00240   affineparams = affineTransformer *  affineparams;
00241   // put in correct position
00242   affineparams[4] = x[0];
00243   affineparams[5] = x[1];
00244   //cout<<"affine params after H are "<< affineparams << endl;
00245   //cout<<"H was "<<H<<endl;
00246   Matrix<double> cov(6,6,MatrixZero);
00247   cov = affineTransformer * Cov_ * affineTransformer.Transpose();
00248   result.SetFromVector(affineparams, cov);
00249   return result;
00250 }
00251 
00252 BIAS::LocalAffineFrame LocalAffineFrame::GetLocalSimilarityFrame() const {
00253   LocalAffineFrame Sim;
00254   // a = vec(A_)
00255   Vector<double> a(4);
00256   a[0] = A_[0][0];
00257   a[1] = A_[1][0];
00258   a[2] = A_[0][1];
00259   a[3] = A_[1][1];
00260   // construct mat so that mat * (x,y)^T = a    where  x=l*cos, y=l*sin
00261   Matrix<double> mat(4,2,MatrixZero);
00262   mat[0][0] = 1;
00263   mat[1][1] = -1;
00264   mat[2][1] = 1;
00265   mat[3][0] = 1;
00266   Matrix2x2<double> tammat;
00267   mat.GetSystemMatrix(tammat);
00268   Vector<double> xy = tammat.Invert() * mat.Transpose()*a;
00269   tammat[0][0] = xy[0];
00270   tammat[0][1] = xy[1];
00271   tammat[1][0] = -xy[1];
00272   tammat[1][1] = xy[0];
00273   Sim.SetT(GetT());
00274   Sim.SetA(tammat);
00275   return Sim;
00276 }
 All Classes Functions Variables Typedefs Enumerations Enumerator Friends