ellipse_refinement.cpp
Go to the documentation of this file.
00001 
00008 #include "v4r_utils/ellipse_refinement.h"
00009 #include <Eigen/Dense>
00010 #include <iostream>
00011 //#include <opencv2/highgui/highgui.hpp>
00012 
00013 
00014 namespace V4R
00015 {
00016 
00017 template<typename T>
00018 inline bool isZero(const T d)
00019 {
00020     return fabs(d) <= std::numeric_limits<T>::epsilon();
00021 }
00022 
00026 inline double scaleAngle_0_2pi(double a)
00027 {
00028     while(a >= 2.*M_PI) a -= 2.*M_PI;
00029     while(a < 0.) a += 2.*M_PI;
00030     return a;
00031 }
00032 
00033 using namespace std;
00034 
00038 EllipseRefinement::EllipseRefinement(const Parameter &_param)
00039     : param(_param)
00040 {
00041 
00042 }
00043 
00044 EllipseRefinement::~EllipseRefinement()
00045 {
00046 
00047 }
00048 
00052 bool EllipseRefinement::refine(const cv::Mat_<short> &im_dx, const cv::Mat_<short> &im_dy,
00053                                    const std::vector<cv::Point2f> &points, Ellipse &ellipse)
00054 {
00055     // get region around the ellipse edgels where to compute the dual ellipse
00056     // that means, pixels where the gradient is higher than the given threshold and the distance
00057     // to the ellipse borders along the line joining the pixel and the ellipse center
00058     // is less than a certain radius (3px for example)
00059 
00060     double minx=10000, maxx=0, miny=10000, maxy=0;
00061     double minGradPoints=10000,maxGradPoints=0;
00062 
00063     for(unsigned i=0; i<points.size(); i++)
00064     {
00065         const cv::Point2f &pt = points[i];
00066 
00067         if(pt.x < minx) minx=pt.x;
00068         if(pt.y < miny) miny=pt.y;
00069         if(pt.x > maxx) maxx=pt.x;
00070         if(pt.y > maxy) maxy=pt.y;
00071         float a =   im_dx(pt.y,pt.x);
00072         float b =   im_dy(pt.y,pt.x);
00073         float grad = sqrt(a*a + b*b);
00074         if(minGradPoints > grad) minGradPoints = grad;
00075         if(grad > maxGradPoints) maxGradPoints = grad;
00076     }
00077 
00078     double gradThreshold = (minGradPoints + (maxGradPoints - minGradPoints)/2)/2;
00079     gradThreshold = param.min_gradient; // minimum gradient to use a pixel
00080     // it does not make sense to use different values for ellipseRand and rand
00081     int ellipseRand=param.border; // maximum distance to given hypothesis
00082     int rand=param.border; // window border size to search for border points (hypothesis parameter A+rand X B+rand)
00083     pointsToUse.clear();
00084     double sumx=0,sumy=0;
00085     // searching within the window for points to use
00086     short min_y = (short)(miny-rand);
00087     short max_y = (short)(maxy+rand);
00088     short min_x = (short)(minx-rand);
00089     short max_x = (short)(maxx+rand);
00090     if (min_y < 0) min_y = 0;
00091     if (max_y >= im_dx.rows) max_y = (short)im_dx.rows-1;
00092     if (min_x < 0) min_x = 0;
00093     if (max_x >= im_dx.cols) max_x = (short)im_dx.cols-1;
00094 
00095     for(short j=min_y; j < max_y; j++)
00096     {
00097         for(short i=min_x; i < max_x; i++)
00098         {
00099             float a =   im_dx(j,i);
00100             float b =   im_dy(j,i);
00101             float grad = sqrt(a*a + b*b);
00102             if(gradThreshold < grad &&
00103                     ellipse.insideEllipse(ellipse.a+ellipseRand,ellipse.b+ellipseRand,ellipse.x,ellipse.y, ellipse.phi,i,j) &&
00104                     !ellipse.insideEllipse(ellipse.a-ellipseRand,ellipse.b-ellipseRand,ellipse.x,ellipse.y, ellipse.phi,i,j))
00105             {
00106                 pointsToUse.push_back(cv::Point2d(i,j));
00107                 sumx+=i;
00108                 sumy+=j;
00109             }
00110         }
00111     }
00112 
00113     // now the pointsToUse array exists (points with a distance smaller than ellipseRand from the hypothesis)
00114     // normalisation (to ensure numeric stability)
00115     double mx = sumx/pointsToUse.size();
00116     double my = sumy/pointsToUse.size();
00117 
00118     // compute scaling factors
00119     double lengths=0;
00120     for(unsigned i=0; i<pointsToUse.size(); i++)
00121     {
00122         double a = pointsToUse[i].x-mx;
00123         double b = pointsToUse[i].y-my;
00124 
00125         lengths+= sqrt(a*a + b*b);
00126     }
00127 
00128     double ss = sqrt(2.) / (lengths/pointsToUse.size());
00129     Eigen::Matrix3d H;
00130     H << ss, 0, -ss*mx, 0, ss, -ss*my, 0, 0, 1;
00131     Eigen::Matrix3d Hinv = H.inverse();
00132     Eigen::Matrix3d HinvT = Hinv.transpose();
00133 
00134 
00135     // transformation finished
00136     /*
00137     Establish the least-square-system.
00138 
00139     In the following, [] means the definition of a vector or matrix and [x]^T its
00140     the transpose of [x]
00141 
00142     A point xi = [ui,vi,1]^T (in homogenous coordinates) lies on the conic C iff
00143     it satisfies xi^T*C*xi=0
00144     C is:
00145     A           B/2             D/2
00146     B/2         C               E/2
00147     D/2         E/2              F
00148     The ConicParameters = [A,B,C,D,E,F] of the conic C can be obtained by least square techniques
00149     by minimizing f(ConicParameters of C):
00150 
00151     f(ConicParameters of C) = sum_{iterate points}(w_i*(x_i^T*C*x_i)^2)
00152 
00153     Following part is based in the duality relationship of projective geometry where the role of
00154     homogenous points and lines can be interchanged.
00155 
00156     In the dual space, a line l_i = [a_i,b_i,c_i]^T is tangent to the dual of the conic C*
00157     iff it satisfies l_i^T*(C*)*l_i=0 where C* = inv(C)
00158     As in the point space, the DualConicParameters = [A*,B*,C*,D*,E*,F*] of the dual conic C*
00159     can be obtained similarly by finding f where f(DualConicParameters of C) reaches a minimum:
00160 
00161     f(DualConicParameters of C) = sum_{iterate lines}(w_i*(l_i^T*(C*)*l_i)^2) (Eq 1)
00162 
00163     li = [a_i,b_i,c_i]^T are obtained directly from the image gradient as follows:
00164     a_i = dx_i
00165     b_i = dy_i
00166     c_i = -[dx_i,dy_i]^T*[x_i,y_i]
00167 
00168     when the image gradient at [x_i,y_i] is not null, it defines the normal orientation of a line
00169     passing through [x_i,y_i]
00170 
00171     Operating in Eq 1, we obtain the following normal equations:
00172     [sum{iterate lines}(sqr(w_i)*Ki*Ki^T)][DualConicParameters] = 0
00173 
00174     where Ki = [sqr(a_i),a_i*b_i, sqr(b_i), a_i*c_i, b_i*c_i, c_i*2]^T
00175 
00176     By using the constraint F* = 1 to solve the system and adding the ellipse discriminant 4*A*C - sqr(B) = 1,
00177     we obtain the final system to find DualConicParameters' = [A'*,B'*,C'*,D'*,E'*,F'*]:
00178 
00179     sum{iterate lines}(-sqr(w_i)*K'i*sqr(c_i)) = 0
00180 
00181     which is the final system solved in the following code.
00182     Further references to the method can be found in:
00183     http://vision.gel.ulaval.ca/~hebert/pdf/ouellet07Ellipses.pdf
00184     */
00185 
00186     Eigen::MatrixXd M = Eigen::MatrixXd::Zero(5,5);
00187     Eigen::VectorXd E = Eigen::VectorXd::Zero(5);
00188     Eigen::VectorXd Kprima(5);
00189     Eigen::MatrixXd res1(5,5);
00190     Eigen::Vector3d li;
00191 
00192     std::vector<Eigen::Vector3d> tangentLines((unsigned)pointsToUse.size());
00193 
00194     for (unsigned i=0; i<pointsToUse.size(); i++)
00195     {
00196         const cv::Point2d &pt = pointsToUse[i];
00197         Kprima.setZero();
00198         res1.setZero();
00199         float a =   im_dx((short)pt.y,(short)pt.x);
00200         float b =   im_dy((short)pt.y,(short)pt.x);
00201         float grad = sqrt(a*a + b*b);
00202 
00203         // compute tangent lines to the dual conic directly from the image gradient
00204         li << double(im_dx((short)pt.y,(short)pt.x)), double(im_dy((short)pt.y,(short)pt.x)),
00205            -( double(im_dx((short)pt.y,(short)pt.x))*pt.x + double(im_dy((short)pt.y,(short)pt.x))*pt.y );
00206 
00207         // scale line such norm(a_i,b_i)=1
00208         li /= grad;
00209 
00210         // normalize lines like L'=H^-T * L (-T is the inverse transposed)
00211         li = HinvT * li;
00212 
00213         //save tangent line
00214         tangentLines[i] = li;
00215 
00216         double weight = grad*grad;
00217         Kprima[0] = li[0]*li[0];
00218         Kprima[1] = li[0]*li[1];
00219         Kprima[2] = li[1]*li[1];
00220         Kprima[3] = li[0]*li[2];
00221         Kprima[4] = li[1]*li[2];
00222 
00223         res1 = weight*Kprima*Kprima.transpose();
00224         M += res1;
00225 
00226         Kprima[0] *= -weight*li[2]*li[2];
00227         Kprima[1] *= -weight*li[2]*li[2];
00228         Kprima[2] *= -weight*li[2]*li[2];
00229         Kprima[3] *= -weight*li[2]*li[2];
00230         Kprima[4] *= -weight*li[2]*li[2];
00231 
00232         E += Kprima;
00233     }
00234 
00235     // solve the least square system using the pseudo inverse technique
00236     // params = inv(M^T*M)*M^T*E
00237 
00238     Eigen::MatrixXd MT(5,5);
00239     MT = M.transpose();
00240 
00241     Eigen::MatrixXd MPerMT(5,5), MPerMTinv(5,5);
00242 
00243     MPerMT = MT*M;
00244     MPerMTinv = MPerMT.inverse();
00245     MPerMT = MPerMTinv*MT;
00246 
00247     Eigen::VectorXd params(5);
00248     params = MPerMT*E;
00249 
00250     Eigen::Matrix3d CNorm;
00251     CNorm << params[0],    params[1]/2., params[3]/2.,
00252           params[1]/2., params[2],    params[4]/2.,
00253           params[3]/2., params[4]/2., 1;
00254 
00255     //unnormalize params
00256     Eigen::Matrix3d CNormMalHInv;
00257     CNormMalHInv = Hinv*CNorm*HinvT;
00258 
00259     //compute error in the dual space
00260     //the error is proportional to distance between a tangent li and its pole xi=(C*)*li
00261 
00262     double dSumDistance=0;
00263     Eigen::Vector3d pole;
00264 
00265     for (unsigned i=0; i<pointsToUse.size(); i++)
00266     {
00267         const Eigen::Vector3d &l = tangentLines[i];
00268         //distance between li and pole
00269         pole = CNorm*l;
00270 
00271         double x0 = pole[0]/pole[2];
00272         double y0 = pole[1]/pole[2];
00273         double a = l[0];
00274         double b = l[1];
00275         double d = abs(l[0] * x0 + l[1] * y0 + l[2]) / sqrt(a*a+b*b);
00276         dSumDistance += d;
00277     }
00278 
00279     ellipse.fit_error = dSumDistance / double(pointsToUse.size());
00280 
00281     // by inverting the dual conic matrix C* we obtain the original
00282     // conic parameters we were seeking for
00283 
00284     Eigen::Matrix3d C;
00285     C = CNormMalHInv.inverse();
00286 
00287     double Ac,Bc,Cc,Dc,Ec,Fc;
00288     Ac = C(0,0);
00289     Bc = C(0,1);
00290     Cc = C(1,1);
00291     Dc = C(2,0);
00292     Ec = C(1,2);
00293     Fc = C(2,2);
00294 
00295     // calculate ellipse parameters x/y/A/B/phi from conic equation Ax^2+Bxy+Cy^2....+F=0
00296     bool ell_ok = ellipse.computeAndSetGeomFromConic(Ac,Bc,Cc,Dc,Ec,Fc);
00297 
00298     return ell_ok;
00299 }
00300 
00301 
00302 
00306 EllipseRefinement::Ellipse::Ellipse()
00307     : id(0), x(0), y(0), a(0), b(0), phi(0), fit_error(0.), support(0.), a2(0), a4(0), b2(0), b4(0), x2(0), y2(0)
00308 {}
00309 
00310 EllipseRefinement::Ellipse::Ellipse(const Ellipse &e)
00311     : id(e.id), x(e.x), y(e.y), a(e.a), b(e.b), phi(e.phi), fit_error(e.fit_error), support(e.support), a2(e.a2), a4(e.a4), b2(e.b2), b4(e.b4), x2(e.x2), y2(e.y2){
00312 }
00313 
00314 void EllipseRefinement::Ellipse::setEllipse(const double &_x, const double &_y, const double &_a, const double &_b, const double &_phi) {
00315     x = _x, y = _y, a = _a, b = _b, phi = _phi;
00316     a2 = a*a;
00317     a4 = a2*a2;
00318     b2 = b*b;
00319     b4 = b2*b2;
00320     x2 = 0;
00321     y2 = 0;
00322 }
00323 
00327 void EllipseRefinement::Ellipse::setEllipse(const cv::RotatedRect& rect) {
00328     x = rect.center.x;
00329     y = rect.center.y;
00330     // box size is double the axis lengths
00331     a = double(rect.size.width)/2.;
00332     b = double(rect.size.height)/2.;
00333     // note: the angle returned is in degrees!
00334     phi = scaleAngle_0_2pi(rect.angle*M_PI/180.);
00335     // note: for unknown reasons sometimes a < b!
00336     if(a < b)
00337     {
00338         swap(a, b);
00339         phi = scaleAngle_0_2pi(phi + M_PI_2);
00340     }
00341     setEllipse(x, y, a, b, phi);
00342 }
00343 
00347 unsigned EllipseRefinement::Ellipse::ellipseSupport(const std::vector<cv::Point2f> &points, double inl_dist, std::vector<bool> &inl_idx)
00348 {
00349     double co = cos(-phi), si = sin(-phi), n, d, dist;
00350     unsigned z;
00351     unsigned nbInl=0;
00352 
00353     cv::Point2f p, q;
00354 
00355     fit_error = 0.;
00356     inl_idx.resize(points.size());
00357 
00358     for(z=0; z<points.size(); z++)
00359     {
00360         // transform point in image coords to ellipse coords
00361         // Note that this piece of code is called often.
00362         // Implementing this explicitely here is faster than using
00363         // TransformToEllipse(), as cos and sin only need to be evaluated once.
00364         //p = Vector2(points[z].x,points[z].y);
00365         p=points[z];
00366         p.x -= x;
00367         p.y -= y;
00368         q.x = co*p.x - si*p.y;
00369         q.y = si*p.x + co*p.y;
00370         // calculate absolute distance to ellipse
00371         if(isZero(q.x) && isZero(q.y))
00372             dist = b;
00373         else
00374         {
00375             x2 = q.x * q.x;
00376             y2 = q.y * q.y;
00377             n = fabs(x2/a2 + y2/b2 - 1.);
00378             d = 2.*sqrt(x2/a4 + y2/b4);
00379             dist = n/d;
00380         }
00381         if(dist <= inl_dist)
00382         {
00383             inl_idx[z]=true;
00384             nbInl++;
00385             fit_error += dist;
00386         }
00387         else
00388             inl_idx[z]=false;
00389     }
00390 
00391     double circumference = ellipseCircumference(a, b);
00392 
00393     if (nbInl>0 && !isZero(circumference))
00394     {
00395         fit_error /= (double)nbInl;
00396         support = (double)nbInl / circumference;
00397     }
00398     else
00399     {
00400         fit_error = DBL_MAX;
00401         support=0;
00402     }
00403 
00404     return nbInl;
00405 }
00406 
00407 
00408 void EllipseRefinement::Ellipse::get(cv::RotatedRect &r) {
00409     r.center.x = x, r.center.y = y;
00410     r.size.width = a*2., r.size.height = b*2.;
00411     r.angle = phi*180./M_PI;
00412 }
00413 
00414 
00418 bool EllipseRefinement::Ellipse::computeAndSetGeomFromConic(double Ac, double Bc, double Cc, double Dc, double Ec, double Fc) {
00419 
00420     double BcBcAcCc = (Bc*Bc - Ac*Cc);
00421     if (BcBcAcCc == 0) {
00422         return false;
00423     }
00424     double x0 = (Cc*Dc - Bc*Ec) / BcBcAcCc;
00425     double y0 = (Ac*Ec - Bc*Dc) / BcBcAcCc;
00426     double a0, a0_2, b0, b0_2, phi0;
00427     double d = Ac-Cc;
00428     double SqrAcCc4BcBc = d*d + 4*Bc*Bc;
00429     if (SqrAcCc4BcBc < 0) {
00430         return false;
00431     }
00432     a0_2 = (2*(Ac*Ec*Ec + Cc*Dc*Dc + Fc*Bc*Bc - 2*Bc*Dc*Ec - Ac*Cc*Fc)) / ((Bc*Bc - Ac*Cc)*((sqrt(SqrAcCc4BcBc)) - (Ac+Cc)));
00433     b0_2 = (2*(Ac*Ec*Ec + Cc*Dc*Dc + Fc*Bc*Bc - 2*Bc*Dc*Ec - Ac*Cc*Fc)) / ((Bc*Bc - Ac*Cc)*(-(sqrt(SqrAcCc4BcBc)) - (Ac+Cc)));
00434     if ((a0_2 < 0) || (b0_2 < 0)) {
00435         return false;
00436     }
00437     a0 = sqrt(a0_2);
00438     b0 = sqrt(b0_2);
00439 
00440     //   double tempPhi=phi;
00441     phi0=0;
00442     if (Bc == 0) {
00443         if (Ac < Cc) {
00444             //cout << "1" << endl;
00445             phi0=M_PI_2;
00446         }
00447     } else {
00448         double z = (Ac-Cc) / (2*Bc);
00449         if (Ac < Cc) {
00450             phi0=0.5*(atan(1./z));
00451             //cout << "m2" << endl;
00452         }
00453         if (Ac > Cc) {
00454             phi0=M_PI_2 + 0.5*(atan(1./z));
00455             //cout << "m3" << endl;
00456         }
00457     }
00458 
00459     x = x0;
00460     y = y0;
00461     a = a0;
00462     b = b0;
00463 
00464     //phi = scaleAngle_0_2pi(phi0);
00465     if (a < b) {
00466         swap(a, b);
00467         //phi = scaleAngle_0_2pi(phi + M_PI_2);
00468     }
00469 
00470     //phi=tempPhi;
00471 
00472     return true;
00473 }
00474 
00479 double EllipseRefinement::Ellipse::ellipseCircumference(double a, double b)
00480 {
00481     return M_PI*(1.5*(a + b) - sqrt(a*b));
00482 }
00483 
00484 
00488 bool EllipseRefinement::Ellipse::insideEllipse(double _a, double _b, double _x0, double _y0, double _phi, double _x, double _y) const
00489 {
00490     double dx = ((_x - _x0)*cos(_phi) + (_y-_y0)*sin(_phi)) / _a;
00491     double dy = (-(_x - _x0)*sin(_phi) + (_y-_y0)*cos(_phi)) / _b;
00492     double distance = dx * dx + dy * dy;
00493     return (distance < 1.0) ? 1 : 0;
00494 
00495 }
00496 }  // -- THE END --
00497 


v4r_ellipses
Author(s):
autogenerated on Wed Aug 26 2015 16:41:40