TwoHalfDSketch.C

Go to the documentation of this file.
00001 /*!@file SceneUnderstanding/TwoHalfDSketch.C  */
00002 
00003 // //////////////////////////////////////////////////////////////////// //
00004 // The iLab Neuromorphic Vision C++ Toolkit - Copyright (C) 2000-2005   //
00005 // by the University of Southern California (USC) and the iLab at USC.  //
00006 // See http://iLab.usc.edu for information about this project.          //
00007 // //////////////////////////////////////////////////////////////////// //
00008 // Major portions of the iLab Neuromorphic Vision Toolkit are protected //
00009 // under the U.S. patent ``Computation of Intrinsic Perceptual Saliency //
00010 // in Visual Environments, and Applications'' by Christof Koch and      //
00011 // Laurent Itti, California Institute of Technology, 2001 (patent       //
00012 // pending; application number 09/912,225 filed July 23, 2001; see      //
00013 // http://pair.uspto.gov/cgi-bin/final/home.pl for current status).     //
00014 // //////////////////////////////////////////////////////////////////// //
00015 // This file is part of the iLab Neuromorphic Vision C++ Toolkit.       //
00016 //                                                                      //
00017 // The iLab Neuromorphic Vision C++ Toolkit is free software; you can   //
00018 // redistribute it and/or modify it under the terms of the GNU General  //
00019 // Public License as published by the Free Software Foundation; either  //
00020 // version 2 of the License, or (at your option) any later version.     //
00021 //                                                                      //
00022 // The iLab Neuromorphic Vision C++ Toolkit is distributed in the hope  //
00023 // that it will be useful, but WITHOUT ANY WARRANTY; without even the   //
00024 // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR      //
00025 // PURPOSE.  See the GNU General Public License for more details.       //
00026 //                                                                      //
00027 // You should have received a copy of the GNU General Public License    //
00028 // along with the iLab Neuromorphic Vision C++ Toolkit; if not, write   //
00029 // to the Free Software Foundation, Inc., 59 Temple Place, Suite 330,   //
00030 // Boston, MA 02111-1307 USA.                                           //
00031 // //////////////////////////////////////////////////////////////////// //
00032 //
00033 // Primary maintainer for this file: Lior Elazary <elazary@usc.edu>
00034 // $HeadURL: svn://isvn.usc.edu/software/invt/trunk/saliency/src/plugins/SceneUnderstanding/TwoHalfDSketch.C $
00035 // $Id: TwoHalfDSketch.C 14181 2010-10-28 22:46:20Z lior $
00036 //
00037 
00038 #ifndef TwoHalfDSketch_C_DEFINED
00039 #define TwoHalfDSketch_C_DEFINED
00040 
00041 #include "plugins/SceneUnderstanding/TwoHalfDSketch.H"
00042 #include "Image/DrawOps.H"
00043 #include "Image/MathOps.H"
00044 #include "Image/Kernels.H"
00045 #include "Image/FilterOps.H"
00046 #include "Image/Transforms.H"
00047 #include "Image/fancynorm.H"
00048 #include "Image/Convolutions.H"
00049 #include "Image/MatrixOps.H"
00050 #include "Simulation/SimEventQueue.H"
00051 #include "GUI/DebugWin.H"
00052 #include <math.h>
00053 #include <fcntl.h>
00054 #include <limits>
00055 #include <string>
00056 
00057 const ModelOptionCateg MOC_TwoHalfDSketch = {
00058   MOC_SORTPRI_3,   "TwoHalfDSketch-Related Options" };
00059 
00060 // Used by: SimulationViewerEyeMvt
00061 const ModelOptionDef OPT_TwoHalfDSketchShowDebug =
00062   { MODOPT_ARG(bool), "TwoHalfDSketchShowDebug", &MOC_TwoHalfDSketch, OPTEXP_CORE,
00063     "Show debug img",
00064     "twohalfdsketch-debug", '\0', "<true|false>", "false" };
00065 
00066 //Define the inst function name
00067 SIMMODULEINSTFUNC(TwoHalfDSketch);
00068 
00069 
00070 // ######################################################################
00071 TwoHalfDSketch::TwoHalfDSketch(OptionManager& mgr, const std::string& descrName,
00072     const std::string& tagName) :
00073   SimModule(mgr, descrName, tagName),
00074   SIMCALLBACK_INIT(SimEventV2Output),
00075   SIMCALLBACK_INIT(SimEventContoursOutput),
00076   SIMCALLBACK_INIT(SimEventSaveOutput),
00077   SIMCALLBACK_INIT(SimEventUserInput),
00078   itsShowDebug(&OPT_TwoHalfDSketchShowDebug, this),
00079   itsProposalThreshold(-1500),
00080   itsAcceptedThreshold(-50)
00081   
00082 {
00083   itsCurrentProb = -1e10;
00084   itsCurrentIdx = 0;
00085   itsBiasMode = false;
00086   itsBias = -1;
00087   itsBiasId = -1;
00088   
00089   initRandomNumbers();
00090 
00091   itsUserProposal.e=0.8;
00092   itsUserProposal.a=10; //25;
00093   itsUserProposal.b=11; //23;
00094   itsUserProposal.k1=0; //23;
00095   itsUserProposal.k2=0; //23;
00096   itsUserProposal.rot = 0;
00097   itsUserProposal.pos = Point2D<float>(257, 141);
00098   itsUserProposal.start = -M_PI;
00099   itsUserProposal.end = M_PI;
00100   itsUserProposal.gibs=0;
00101   
00102   //AppleLogo
00103   itsModel.addLine(Line(7, 71, 17, 88));
00104   itsModel.addLine(Line(19, 90, 27, 95)); 
00105   itsModel.addLine(Line(29, 95, 38, 91));
00106   itsModel.addLine(Line(39, 91, 55, 95));
00107   itsModel.addLine(Line(56, 95, 61, 93));
00108   itsModel.addLine(Line(62, 93, 74, 73));
00109   itsModel.addLine(Line(74, 72, 65, 62));
00110   itsModel.addLine(Line(65, 61, 63, 57));
00111   itsModel.addLine(Line(63, 56, 65, 42));
00112   itsModel.addLine(Line(66, 42, 71, 36));
00113   itsModel.addLine(Line(70, 35, 68, 33));
00114   itsModel.addLine(Line(67, 32, 53, 29));
00115   itsModel.addLine(Line(52, 30, 45, 32));
00116   itsModel.addLine(Line(44, 27, 56, 15));
00117   itsModel.addLine(Line(56, 14, 57, 7));
00118   itsModel.addLine(Line(56, 6, 53, 7));
00119   itsModel.addLine(Line(52, 7, 40, 19));
00120   itsModel.addLine(Line(40, 20, 42, 26));
00121   itsModel.addLine(Line(44, 33, 25, 29));
00122   itsModel.addLine(Line(24, 29, 17, 31));
00123   itsModel.addLine(Line(15, 32, 6, 43));
00124   itsModel.addLine(Line(5, 45, 5, 64));
00125   itsModel.addLine(Line(6, 65, 7, 70));
00126 
00127   //Giraff
00128   //itsModel.lines.push_back(Line(39,44,14,7));
00129   //itsModel.lines.push_back(Line(26,63,16,26));
00130   //itsModel.lines.push_back(Line(61,64,87,70));
00131   //itsModel.lines.push_back(Line(22,80,31,104));
00132   //itsModel.lines.push_back(Line(74,94,53,94));
00133   //itsModel.lines.push_back(Line(93,77,96,94));
00134   //itsModel.lines.push_back(Line(47,49,58,62));
00135   //itsModel.lines.push_back(Line(1,15,12,3));
00136   //itsModel.lines.push_back(Line(89,104, 81, 92));
00137   //itsModel.lines.push_back(Line(29,68,22,79));
00138   //itsModel.lines.push_back(Line(96,95,99,106));
00139   //itsModel.lines.push_back(Line(40,91,51,93));
00140   //itsModel.lines.push_back(Line(3, 18, 13, 20));
00141   //itsModel.lines.push_back(Line(39,93,38,103));
00142   //itsModel.lines.push_back(Line(40,45,46,49));
00143   //itsModel.lines.push_back(Line(92,76,88,71));
00144   //itsModel.lines.push_back(Line(80,92,75,93));
00145   //itsModel.lines.push_back(Line(15,25,14,21));
00146   //itsModel.lines.push_back(Line(28,67,27,64));
00147   //itsModel.lines.push_back(Line(1, 16, 2, 18));
00148   //itsModel.lines.push_back(Line(14, 6, 13, 5));
00149   //itsModel.lines.push_back(Line(11, 2, 10, 3));
00150   //itsModel.lines.push_back(Line(88, 106, 89, 105));
00151   //itsModel.lines.push_back(Line(39, 104, 38, 105));
00152   //itsModel.lines.push_back(Line(60, 63, 59, 63));
00153 
00154 
00155   itsModel.setCOM(); //Center to COM
00156   itsModel.quantize(60); //number of directions in the grid
00157 
00158 }
00159 
00160 // ######################################################################
00161 TwoHalfDSketch::~TwoHalfDSketch()
00162 {
00163 }
00164 
00165 // ######################################################################
00166 void TwoHalfDSketch::onSimEventUserInput(SimEventQueue& q, rutz::shared_ptr<SimEventUserInput>& e)
00167 {
00168 
00169   LINFO("Got event --%s-- %ix%i key=%i",
00170       e->getWinName(),
00171       e->getMouseClick().i,
00172       e->getMouseClick().j,
00173       e->getKey());
00174 
00175   if (strcmp(e->getWinName(), "TwoHalfDSketch"))
00176     return;
00177   SurfaceState& surface = itsUserProposal;
00178 
00179   switch(e->getKey())
00180   {
00181     case 111: //98: //111: //up
00182       surface.pos.i -= 1;
00183       break;
00184     case 116: //104: //116: //down
00185       surface.pos.i += 1;
00186       break;
00187     case 113: //100: //113: //left
00188       surface.pos.j -= 1;
00189       break;
00190     case 114: //102: //114: //right
00191       surface.pos.j += 1;
00192       break;
00193     case 21: //=
00194       break;
00195     case 20: //-
00196       break;
00197     case 38: //a
00198       break;
00199     case 52: //z
00200       break;
00201     case 39: //s
00202       break;
00203     case 53: //x
00204       break;
00205     case 40: //d
00206       break;
00207     case 54: //c
00208       break;
00209     case 10: //1
00210       surface.a += 1.0;
00211       break;
00212     case 24: //q
00213       surface.a -= 1.0;
00214       break;
00215     case 11: //2
00216       surface.b += 1.0;
00217       break;
00218     case 25: //w
00219       surface.b -= 1.0;
00220       break;
00221     case 12: //3
00222       surface.e += 0.1;
00223       break;
00224     case 26: //e
00225       surface.e -= 0.1;
00226       break;
00227     case 13: //4
00228       surface.k1 += 0.1;
00229       break;
00230     case 27: //r
00231       surface.k1 -= 0.1;
00232       break;
00233     case 14: //5
00234       surface.k2 += 0.1;
00235       break;
00236     case 28: //t
00237       surface.k2 -= 0.1;
00238       break;
00239     case 15: //6
00240       surface.rot += 1*M_PI/180;
00241       break;
00242     case 29: //y
00243       surface.rot -= 1*M_PI/180;
00244       break;
00245   }
00246 
00247   LINFO("Pos(%f,%f), param((%0.2f,%0.2f,%0.2f)",
00248       surface.pos.i, surface.pos.j,
00249       surface.a, surface.b, surface.e);
00250 
00251 
00252   evolve(q);
00253 
00254 }
00255 
00256 
00257 // ######################################################################
00258 void TwoHalfDSketch::onSimEventV2Output(SimEventQueue& q, rutz::shared_ptr<SimEventV2Output>& e)
00259 {
00260   //Check if we have the smap
00261   //if (SeC<SimEventSMapOutput> smap = q.check<SimEventSMapOutput>(this))
00262   //  itsSMap = smap->getSMap();
00263 
00264   //Check if we have the corners
00265   itsLines = e->getLines();
00266   Dims dims = e->getDims();
00267 
00268   itsLinesMag = Image<float>(dims, ZEROS);
00269   itsLinesOri = Image<float>(dims, NO_INIT);
00270   for(uint i=0; i<itsLinesOri.size(); i++)
00271     itsLinesOri[i] = NOTDEF;
00272 
00273   std::vector<Line> lines;
00274   for(uint i=0; i<itsLines.size(); i++)
00275   {
00276     V2::LineSegment& ls = itsLines[i];
00277     drawLine(itsLinesMag, Point2D<int>(ls.p1), Point2D<int>(ls.p2), 255.0F); //ls.length);
00278     drawLine(itsLinesOri, Point2D<int>(ls.p1), Point2D<int>(ls.p2), ls.ori);
00279     lines.push_back(Line(ls.p1, ls.p2));
00280   }
00281   
00282   //Build the FDCM
00283   itsOriChamferMatcher.setLines(lines, 
00284     60, //Num Orientation to quantize
00285     5, //Direction cost
00286     itsLinesMag.getDims());
00287 
00288   evolve(q);
00289 
00290 }
00291 
00292 void TwoHalfDSketch::onSimEventContoursOutput(SimEventQueue& q, rutz::shared_ptr<SimEventContoursOutput>& e)
00293 {
00294   LINFO("Contours output");
00295 
00296   itsContours = e->getContours();
00297 
00298   itsLinesMag = Image<float>(e->getImg().getDims(), ZEROS);
00299   itsLinesOri = Image<float>(e->getImg().getDims(), NO_INIT);
00300   for(uint i=0; i<itsLinesOri.size(); i++)
00301     itsLinesOri[i] = NOTDEF;
00302 
00303   std::vector<Line> lines;
00304 
00305   for(uint i=0; i<itsContours.size(); i++)
00306   {
00307     std::vector<Point2D<int> > polygon = approxPolyDP(itsContours[i].points, 2);
00308     for(uint j=0; j<polygon.size()-1; j++)
00309     {
00310       drawLine(itsLinesMag, polygon[j], polygon[(j+1)], 255.0F);
00311 
00312       float ori = atan2(polygon[j].j-polygon[(j+1)].j, polygon[(j+1)].i - polygon[j].i);
00313       if (ori < 0) ori += M_PI;
00314       if (ori >= M_PI) ori -= M_PI;
00315       drawLine(itsLinesOri, polygon[j], polygon[(j+1)], ori); 
00316 
00317       lines.push_back(Line(polygon[j], polygon[(j+1)]));
00318     }
00319   }
00320 
00321   //Build the FDCM
00322   itsOriChamferMatcher.setLines(lines, 
00323     60, //Num Orientation to quantize
00324     0.5, //Direction cost
00325     itsLinesMag.getDims());
00326 
00327   evolve(q);
00328 
00329 }
00330 
00331 double TwoHalfDSketch::getCost(OriChamferMatching& cm,
00332     Polygon& poly, Point2D<float> loc, bool biasMode)
00333 {
00334 
00335   //Image<PixRGB<byte> > tmp = itsLinesMag;
00336   //LINFO("Loc %f %f", loc.i, loc.j);
00337   ////We have a polygon
00338   //for(uint j=0; j<poly.getNumLines(); j++)
00339   //{
00340   //  Line l = poly.getLine(j); //Get the line up to scale and pos
00341   //  l.trans(loc);
00342   //  Point2D<int> p1 = (Point2D<int>)l.getP1();
00343   //  Point2D<int> p2 = (Point2D<int>)l.getP2();
00344   //  drawLine(tmp, p1,p2, PixRGB<byte>(255,0,0));
00345   //}
00346 
00347 
00348 
00349   double totalProb = 0;                         
00350   //double totalHalLength = 0; //The total length we have been hallucinating
00351   //double factor = 1.0/poly.getLength();
00352   for (uint i=0 ; i<poly.getNumLines(); i++)
00353   {
00354     Line l = poly.getLine(i); //Make a copy so that we dont change the position of the original
00355     l.trans(loc); //Move the line to the correct location
00356 
00357     Point2D<int> p1 = (Point2D<int>)l.getP1();
00358     Point2D<int> p2 = (Point2D<int>)l.getP2();
00359     double sum = cm.getCost(l.getDirectionIdx(), p1, p2);
00360 
00361     double prob = exp(-sum/double(2*l.getLength()));
00362     if (sum < 0) 
00363     {
00364       LINFO("Invalid sum %f", sum);
00365       prob = 0;
00366     }
00367     if (!biasMode)
00368     {
00369       if (prob < exp(-500/10))  //If we are very far off, then there might be an edge there, we just dont see it
00370       {
00371         //prob = 0.10;
00372         //totalHalLength+=l.getLength();
00373       }
00374     } else {
00375       //calcNFA(l);
00376     }
00377     //Weight the line based on how much it contributes to the overall length
00378     prob = pow(prob, 1-l.getWeight());
00379     //LINFO("%i sum=%f l=%f w=%f p=%f", i, sum, l.getLength(), l.getWeight(), prob );
00380     //Image<PixRGB<byte> > tmp = itsLinesMag;
00381     //drawLine(tmp, p1, p2, PixRGB<byte>(255,0,0));
00382     //SHOWIMG(tmp);
00383 
00384     totalProb += log(prob);
00385   }
00386 
00387   //We need at least 10% of the contour to exist in order to hallucinate
00388   //TODO this should be a continues function change the totalProb as the totalHalLength decreases
00389   //if (totalHalLength/poly.getLength() > 0.9)
00390   //  totalProb = -1000;
00391   //LINFO("Hal %f %f %f %f", poly.getLength(), totalHalLength, totalHalLength/poly.getLength(), totalProb);
00392   //LINFO("TOtal prob %f", totalProb);
00393 
00394   //SHOWIMG(tmp);
00395   return totalProb;
00396 }
00397 
00398 
00399 double TwoHalfDSketch::calcNFA(Line& line)
00400 {
00401   //Image<PixRGB<byte> > tmp = itsLinesMag;
00402 
00403 
00404   //double theta = line.getOri();
00405   //double prec = M_PI/8.0;
00406   //double p = 1.0 / (double) 8;
00407 
00408   //int w = itsLinesMag.getWidth();
00409   //int h = itsLinesMag.getHeight();
00410   //double logNT = 5.0 * ( log10( (double) w ) +
00411   //                       log10( (double) h ) ) / 2.0;
00412 
00413 
00414   //int pts = 0;
00415   //int alg = 0;
00416   //double nfa_val;
00417 
00418 
00419   //rect rec;
00420   //if (line.p1.j > line.p2.j)
00421   //{
00422   //  rec.x1 = line.p2.i*2;
00423   //  rec.y1 = line.p2.j*2;
00424   //  rec.x2 = line.p1.i*2;
00425   //  rec.y2 = line.p1.j*2;
00426   //} else {
00427   //  rec.x1 = line.p1.i*2;
00428   //  rec.y1 = line.p1.j*2;
00429   //  rec.x2 = line.p2.i*2;
00430   //  rec.y2 = line.p2.j*2;
00431   //}
00432 
00433   //rec.theta = theta; //ori*M_PI/180;
00434   //rec.dx = (float) cos( (double) rec.theta );
00435   //rec.dy = (float) sin( (double) rec.theta );
00436   //rec.width=15;
00437 
00438   //rect_iter * i;
00439 
00440   //for(i=ri_ini(&rec); !ri_end(i); ri_inc(i))
00441   //  if( i->x>=0 && i->y>=0 && i->x < itsLinesMag.getWidth() && i->y < itsLinesMag.getHeight() )
00442   //  {
00443   //    Point2D<int> loc(i->x, i->y);
00444   //    if (itsLinesMag.getVal(loc) > 0)
00445   //    {
00446   //      pts++;
00447   //      if( isaligned(loc,itsLinesOri,theta,prec) )
00448   //      {
00449   //        tmp.setVal(loc, PixRGB<byte>(0,255,0));
00450   //        ++alg;
00451   //      }
00452   //    }
00453   //  }
00454   //ri_del(i);
00455 
00456   //nfa_val = nfa(pts,alg,p,logNT);
00457   //LINFO("pts=%i alg=%i NFA %f",pts, alg, nfa_val);
00458 
00459   //SHOWIMG(tmp);
00460 
00461   return 0;
00462 }
00463 
00464 /*----------------------------------------------------------------------------*/
00465 float TwoHalfDSketch::inter_low(float x, float x1, float y1, float x2, float y2)
00466 {
00467   if( x1 > x2 || x < x1 || x > x2 )
00468     {
00469       LFATAL("inter_low: x %g x1 %g x2 %g.\n",x,x1,x2);
00470       LFATAL("Impossible situation.");
00471     }
00472   if( x1 == x2 && y1<y2 ) return y1;
00473   if( x1 == x2 && y1>y2 ) return y2;
00474   return y1 + (x-x1) * (y2-y1) / (x2-x1);
00475 }
00476 
00477 /*----------------------------------------------------------------------------*/
00478 float TwoHalfDSketch::inter_hi(float x, float x1, float y1, float x2, float y2)
00479 {
00480   if( x1 > x2 || x < x1 || x > x2 )
00481     {
00482       LFATAL("inter_hi: x %g x1 %g x2 %g.\n",x,x1,x2);
00483       LFATAL("Impossible situation.");
00484     }
00485   if( x1 == x2 && y1<y2 ) return y2;
00486   if( x1 == x2 && y1>y2 ) return y1;
00487   return y1 + (x-x1) * (y2-y1) / (x2-x1);
00488 }
00489 
00490 /*----------------------------------------------------------------------------*/
00491 void TwoHalfDSketch::ri_del(rect_iter * iter)
00492 {
00493   free(iter);
00494 }
00495 
00496 /*----------------------------------------------------------------------------*/
00497 int TwoHalfDSketch::ri_end(rect_iter * i)
00498 {
00499   return (float)(i->x) > i->vx[2];
00500 }
00501 
00502 /*----------------------------------------------------------------------------*/
00503 void TwoHalfDSketch::ri_inc(rect_iter * i)
00504 {
00505   if( (float) (i->x) <= i->vx[2] ) i->y++;
00506 
00507   while( (float) (i->y) > i->ye && (float) (i->x) <= i->vx[2] )
00508     {
00509       /* new x */
00510       i->x++;
00511 
00512       if( (float) (i->x) > i->vx[2] ) return; /* end of iteration */
00513 
00514       /* update lower y limit for the line */
00515       if( (float) i->x < i->vx[3] )
00516         i->ys = inter_low((float)i->x,i->vx[0],i->vy[0],i->vx[3],i->vy[3]);
00517       else i->ys = inter_low((float)i->x,i->vx[3],i->vy[3],i->vx[2],i->vy[2]);
00518 
00519       /* update upper y limit for the line */
00520       if( (float)i->x < i->vx[1] )
00521         i->ye = inter_hi((float)i->x,i->vx[0],i->vy[0],i->vx[1],i->vy[1]);
00522       else i->ye = inter_hi( (float)i->x,i->vx[1],i->vy[1],i->vx[2],i->vy[2]);
00523 
00524       /* new y */
00525       i->y = (int)((float) ceil( (double) i->ys ));
00526     }
00527 }
00528 
00529 /*----------------------------------------------------------------------------*/
00530 TwoHalfDSketch::rect_iter * TwoHalfDSketch::ri_ini(struct rect * r)
00531 {
00532   float vx[4],vy[4];
00533   int n,offset;
00534   rect_iter * i;
00535 
00536   i = (rect_iter *) malloc(sizeof(rect_iter));
00537   if(!i) LFATAL("ri_ini: Not enough memory.");
00538 
00539   vx[0] = r->x1 - r->dy * r->width / 2.0;
00540   vy[0] = r->y1 + r->dx * r->width / 2.0;
00541   vx[1] = r->x2 - r->dy * r->width / 2.0;
00542   vy[1] = r->y2 + r->dx * r->width / 2.0;
00543   vx[2] = r->x2 + r->dy * r->width / 2.0;
00544   vy[2] = r->y2 - r->dx * r->width / 2.0;
00545   vx[3] = r->x1 + r->dy * r->width / 2.0;
00546   vy[3] = r->y1 - r->dx * r->width / 2.0;
00547 
00548   if( r->x1 < r->x2 && r->y1 <= r->y2 ) offset = 0;
00549   else if( r->x1 >= r->x2 && r->y1 < r->y2 ) offset = 1;
00550   else if( r->x1 > r->x2 && r->y1 >= r->y2 ) offset = 2;
00551   else offset = 3;
00552   /* else if( r->x1 <= r->x2 && r->y1 > r->y2 ) offset = 3; */
00553 
00554   for(n=0; n<4; n++)
00555     {
00556       i->vx[n] = vx[(offset+n)%4];
00557       i->vy[n] = vy[(offset+n)%4];
00558     }
00559 
00560 #define BIG_NUMBER 1.0e+300
00561   
00562   /* starting point */
00563   i->x = (int)(ceil( (double) (i->vx[0]) ) - 1);
00564   i->y = (int)(ceil( (double) (i->vy[0]) ));
00565   i->ys = i->ye = -BIG_NUMBER;
00566 
00567   /* advance to the first point */
00568   ri_inc(i);
00569 
00570   return i;
00571 }
00572 
00573 
00574 
00575 int TwoHalfDSketch::isaligned(Point2D<int> loc, Image<float>& angles, float theta, float prec)
00576 {
00577   float a = angles.getVal(loc);
00578 
00579   printf("IsAligned %f %f %f ", a*180/M_PI, theta*180/M_PI, prec*180/M_PI);
00580   if( a == NOTDEF ) return false;
00581 
00582   /* it is assumed that theta and a are in the range [-pi,pi] */
00583   theta -= a;
00584   if( theta < 0.0 ) theta = -theta;
00585   if( theta > M_3_2_PI )
00586     {
00587       theta -= M_2__PI;
00588       if( theta < 0.0 ) theta = -theta;
00589     }
00590 
00591   printf(" %f < %f \n", theta*180/M_PI, prec*180/M_PI);
00592   return theta < prec;
00593 }
00594 
00595 /*----------------------------------------------------------------------------*/
00596 /*----------------------------- NFA computation ------------------------------*/
00597 /*----------------------------------------------------------------------------*/
00598 
00599 /*----------------------------------------------------------------------------*/
00600 /*
00601    Calculates the natural logarithm of the absolute value of
00602    the gamma function of x using the Lanczos approximation,
00603    see http://www.rskey.org/gamma.htm.
00604 
00605    The formula used is
00606      \Gamma(x) = \frac{ \sum_{n=0}^{N} q_n x^n }{ \Pi_{n=0}^{N} (x+n) }
00607                  (x+5.5)^(x+0.5) e^{-(x+5.5)}
00608    so
00609      \log\Gamma(x) = \log( \sum_{n=0}^{N} q_n x^n ) + (x+0.5) \log(x+5.5)
00610                      - (x+5.5) - \sum_{n=0}^{N} \log(x+n)
00611    and
00612      q0 = 75122.6331530
00613      q1 = 80916.6278952
00614      q2 = 36308.2951477
00615      q3 = 8687.24529705
00616      q4 = 1168.92649479
00617      q5 = 83.8676043424
00618      q6 = 2.50662827511
00619  */
00620 double TwoHalfDSketch::log_gamma_lanczos(double x)
00621 {
00622   static double q[7] = { 75122.6331530, 80916.6278952, 36308.2951477,
00623                          8687.24529705, 1168.92649479, 83.8676043424,
00624                          2.50662827511 };
00625   double a = (x+0.5) * log(x+5.5) - (x+5.5);
00626   double b = 0.0;
00627   int n;
00628 
00629   for(n=0;n<7;n++)
00630     {
00631       a -= log( x + (double) n );
00632       b += q[n] * pow( x, (double) n );
00633     }
00634   return a + log(b);
00635 }
00636 
00637 /*----------------------------------------------------------------------------*/
00638 /*
00639    Calculates the natural logarithm of the absolute value of
00640    the gamma function of x using Robert H. Windschitl method,
00641    see http://www.rskey.org/gamma.htm.
00642 
00643    The formula used is
00644      \Gamma(x) = \sqrt(\frac{2\pi}{x}) ( \frac{x}{e}
00645                    \sqrt{ x\sinh(1/x) + \frac{1}{810x^6} } )^x
00646    so
00647      \log\Gamma(x) = 0.5\log(2\pi) + (x-0.5)\log(x) - x
00648                      + 0.5x\log( x\sinh(1/x) + \frac{1}{810x^6} ).
00649 
00650    This formula is good approximation when x > 15.
00651  */
00652 double TwoHalfDSketch::log_gamma_windschitl(double x)
00653 {
00654   return 0.918938533204673 + (x-0.5)*log(x) - x
00655          + 0.5*x*log( x*sinh(1/x) + 1/(810.0*pow(x,6.0)) );
00656 }
00657 
00658 /*----------------------------------------------------------------------------*/
00659 /*
00660    Calculates the natural logarithm of the absolute value of
00661    the gamma function of x. When x>15 use log_gamma_windschitl(),
00662    otherwise use log_gamma_lanczos().
00663  */
00664 //#define log_gamma(x) ((x)>15.0?log_gamma_windschitl(x):log_gamma_lanczos(x))
00665 
00666 /*----------------------------------------------------------------------------*/
00667 /*
00668    Computes the logarithm of NFA to base 10.
00669 
00670    NFA = NT.b(n,k,p)
00671    the return value is log10(NFA)
00672 
00673    n,k,p - binomial parameters.
00674    logNT - logarithm of Number of Tests
00675  */
00676 //#define TABSIZE 100000
00677 
00678 
00679 double TwoHalfDSketch::nfa(int n, int k, double p, double logNT)
00680 {
00681   static double inv[TABSIZE];   /* table to keep computed inverse values */
00682   double tolerance = 0.1;       /* an error of 10% in the result is accepted */
00683   double log1term,term,bin_term,mult_term,bin_tail,err;
00684   double p_term = p / (1.0-p);
00685   int i;
00686 
00687   if( n<0 || k<0 || k>n || p<0.0 || p>1.0 )
00688     LFATAL("Wrong n, k or p values in nfa()");
00689 
00690   if( n==0 || k==0 ) return -logNT;
00691   if( n==k ) return -logNT - (double) n * log10(p);
00692 
00693   log1term = log_gamma((double)n+1.0) - log_gamma((double)k+1.0)
00694            - log_gamma((double)(n-k)+1.0)
00695            + (double) k * log(p) + (double) (n-k) * log(1.0-p);
00696 
00697   term = exp(log1term);
00698   if( term == 0.0 )              /* the first term is almost zero */
00699     {
00700       if( (double) k > (double) n * p )    /* at begin or end of the tail? */
00701         return -log1term / M_LN10 - logNT; /* end: use just the first term */
00702       else
00703         return -logNT;                     /* begin: the tail is roughly 1 */
00704     }
00705 
00706   bin_tail = term;
00707   for(i=k+1;i<=n;i++)
00708     {
00709       bin_term = (double) (n-i+1) * ( i<TABSIZE ?
00710                    ( inv[i] == 0.0 ? inv[i] : (inv[i]=1.0/(double)i))
00711                    : 1.0/(double)i );
00712       mult_term = bin_term * p_term;
00713       term *= mult_term;
00714       bin_tail += term;
00715       if(bin_term<1.0)
00716         {
00717           /* when bin_term<1 then mult_term_j<mult_term_i for j>i.
00718              then, the error on the binomial tail when truncated at
00719              the i term can be bounded by a geometric series of form
00720              term_i * sum mult_term_i^j.                            */
00721           err = term * ( ( 1.0 - pow( mult_term, (double) (n-i+1) ) ) /
00722                          (1.0-mult_term) - 1.0 );
00723 
00724           /* one wants an error at most of tolerance*final_result, or:
00725              tolerance * abs(-log10(bin_tail)-logNT).
00726              now, the error that can be accepted on bin_tail is
00727              given by tolerance*final_result divided by the derivative
00728              of -log10(x) when x=bin_tail. that is:
00729              tolerance * abs(-log10(bin_tail)-logNT) / (1/bin_tail)
00730              finally, we truncate the tail if the error is less than:
00731              tolerance * abs(-log10(bin_tail)-logNT) * bin_tail        */
00732           if( err < tolerance * fabs(-log10(bin_tail)-logNT) * bin_tail ) break;
00733         }
00734     }
00735   return -log10(bin_tail) - logNT;
00736 }
00737 
00738 
00739 
00740 
00741 // ######################################################################
00742 void TwoHalfDSketch::onSimEventSaveOutput(SimEventQueue& q, rutz::shared_ptr<SimEventSaveOutput>& e)
00743 {
00744   if (itsShowDebug.getVal())
00745     {
00746       // get the OFS to save to, assuming sinfo is of type
00747       // SimModuleSaveInfo (will throw a fatal exception otherwise):
00748       nub::ref<FrameOstream> ofs =
00749         dynamic_cast<const SimModuleSaveInfo&>(e->sinfo()).ofs;
00750       Layout<PixRGB<byte> > disp = getDebugImage(q);
00751       if (disp.initialized())
00752         ofs->writeRgbLayout(disp, "TwoHalfDSketch", FrameInfo("TwoHalfDSketch", SRC_POS));
00753     }
00754 }
00755 
00756 
00757 // ######################################################################
00758 void TwoHalfDSketch::evolve(SimEventQueue& q)
00759 {
00760 
00761   if (!itsBiasMode)
00762   {
00763     Image<SurfaceState> surfaces = proposeSurfaces(false);
00764 
00765     itsProposals.clear();
00766     for(uint i=0; i<50; i++)
00767     {
00768       Point2D<int> maxLoc; SurfaceState maxSurface;
00769       findMax(surfaces, maxLoc, maxSurface);
00770 
00771       if (maxSurface.prob == INVALID_PROB)
00772         break; //No more valid surfaces
00773 
00774       itsProposals.push_back(maxSurface);
00775 
00776       //Apply IOR 3/4 the size of the object
00777       double radius = maxSurface.polygon.getRadius()*0.75;
00778       drawDisk(surfaces, maxLoc, (int)radius);
00779     }
00780   } else {
00781     optimizePolygon(itsProposals[itsBiasId]);
00782     LINFO("Optimized match %f\n", itsProposals[itsBiasId].prob);
00783   }
00784 
00785   printResults(itsBias);
00786 
00787   //sort by prob
00788   //Update the surfaces
00789   itsSurfaces.clear();
00790   for(uint i=0; i<itsProposals.size(); i++)
00791   {
00792     if (itsProposals[i].prob > itsAcceptedThreshold)
00793       itsSurfaces.push_back(itsProposals[i]);
00794   }
00795 
00796   //Layout<PixRGB<byte> > layout = getDebugImage(q);
00797   //Image<PixRGB<byte> > tmp = layout.render();
00798   //SHOWIMG(tmp);
00799 
00800   if (itsSurfaces.size() > 0)
00801     q.post(rutz::make_shared(new SimEventTwoHalfDSketchOutput(this, itsSurfaces)));
00802 
00803   if (itsProposals.size() > 0 && !itsBiasMode) //Check to see if we can bias
00804   {
00805     itsBiasMode = true;
00806     LINFO("Biasing");
00807 
00808     for(float bias=0; bias<0.30; bias += 0.10)
00809     {
00810       for(uint sid=0; sid<itsProposals.size(); sid++)
00811       {
00812         itsBias = bias;
00813         itsBiasId = sid;
00814         std::vector<V1::SpatialBias> v1Bias;
00815 
00816         Polygon& polygon = itsProposals[sid].polygon;
00817         for(uint j=0; j<polygon.getNumLines(); j++)
00818         {
00819           Line l = itsProposals[sid].getLine(j); //Get the line up to scale and pos
00820 
00821           Point2D<int> p1 = (Point2D<int>)l.getP1();
00822           Point2D<int> p2 = (Point2D<int>)l.getP2();
00823 
00824           Point2D<int> bP1 = p1;
00825           Point2D<int> bP2 = (p1+p2)/2;
00826           Point2D<int> bP3 = (p1+bP2)/2;
00827           Point2D<int> bP4 = (p2+bP2)/2;
00828           
00829           //Bias V1
00830           {
00831           V1::SpatialBias sb;
00832           sb.loc = bP1;
00833           sb.threshold = bias ;
00834           sb.dims = Dims(15,15);
00835           v1Bias.push_back(sb);
00836           }
00837 
00838           {
00839           V1::SpatialBias sb;
00840           sb.loc = bP2;
00841           sb.threshold = bias ;
00842           sb.dims = Dims(15,15);
00843           v1Bias.push_back(sb);
00844           }
00845 
00846           {
00847           V1::SpatialBias sb;
00848           sb.loc = bP3;
00849           sb.threshold = bias ;
00850           sb.dims = Dims(15,15);
00851           v1Bias.push_back(sb);
00852           }
00853 
00854           {
00855           V1::SpatialBias sb;
00856           sb.loc = bP4;
00857           sb.threshold = bias ;
00858           sb.dims = Dims(15,15);
00859           v1Bias.push_back(sb);
00860           }
00861         }
00862         LINFO("Bias %f for %i\n", itsBias, sid);
00863         q.post(rutz::make_shared(new SimEventV1Bias(this, v1Bias)));
00864       }
00865 
00866     }
00867   }
00868 
00869 }
00870 
00871 
00872 Image<float> TwoHalfDSketch::getSurfaceProbImage(Image<SurfaceState>& surfaceState)
00873 {
00874 
00875   Image<float> probImg(surfaceState.getDims(), NO_INIT);
00876   for(uint i=0; i<probImg.size(); i++)
00877   {
00878     if (surfaceState[i].prob < -1000)
00879       probImg[i] = 0;
00880     else
00881       probImg[i] = surfaceState[i].prob;
00882   }
00883 
00884   return probImg;
00885 }
00886 
00887 void TwoHalfDSketch::optimizePolygon(TwoHalfDSketch::SurfaceState& surfaceState)
00888 {
00889 
00890   float matchingScale = 1; //0.5;
00891 
00892   //int x = 285;
00893   //int y = 328;
00894   //double scale = 0.9;
00895   ////float aspectx = 1.9;
00896   //float aspecty = 1.11;
00897   //float rot = 2.0;
00898 
00899 
00900   double maxProb = getCost(itsOriChamferMatcher, surfaceState.polygon,surfaceState.pos, true);
00901   if (maxProb > -200 || maxProb < -400)
00902   {
00903     surfaceState.prob = maxProb;
00904     return;
00905   }
00906   LINFO("Optimize Polygon %f", maxProb);
00907 
00908   for(float aspectx = surfaceState.aspect.i; aspectx < surfaceState.aspect.i+0.5; aspectx +=0.2)
00909   {
00910     for(float aspecty = surfaceState.aspect.j; aspecty < surfaceState.aspect.j+0.5; aspecty +=0.2)
00911     {
00912       LINFO("Aspectx %f %f", aspectx, aspecty);
00913       if (aspectx <= 0.5 || aspecty <= 0.5)
00914         continue;
00915       for(double scale = surfaceState.scale-0.2  ; scale< surfaceState.scale+0.2 ; scale += 0.01)       
00916       { 
00917         for(float rot =  surfaceState.rot-5 ; rot< surfaceState.rot+5 ; rot+= 1)        
00918         {       
00919           Polygon model = surfaceState.polygon;
00920           //LINFO("Rot %f Scale %f aspect %f %f\n", rot, scale, aspectx, aspecty);
00921 
00922           model.rotate(rot*M_PI/180.0);
00923           model.scale(scale*matchingScale*aspectx, scale*matchingScale*aspecty); 
00924 
00925           model.setWeights();
00926 
00927           for (int x=surfaceState.pos.i-5 ; x<surfaceState.pos.i+5; x++)
00928           {
00929             for (int y=surfaceState.pos.j-5 ; y<surfaceState.pos.j+5; y++)
00930             {
00931               double prob = getCost(itsOriChamferMatcher, model,Point2D<float>(x,y), true);
00932 
00933               if (prob>maxProb)
00934               {
00935                 maxProb = prob;
00936                 surfaceState.pos = Point2D<float>(x,y);
00937                 surfaceState.aspect = Point2D<float>(aspectx, aspecty);
00938                 surfaceState.scale = scale;
00939                 surfaceState.rot = rot;
00940                 surfaceState.prob = prob;
00941                 surfaceState.polygon = model;
00942                 surfaceState.matchingScale = matchingScale;
00943               }
00944             }
00945           }
00946         }
00947       }
00948     }
00949 
00950   }
00951 }
00952 
00953 
00954 Image<TwoHalfDSketch::SurfaceState> TwoHalfDSketch::proposeSurfaces(bool biasMode)
00955 {
00956 
00957   LINFO("Propose surfaces");
00958 
00959   float matchingScale = 1; //0.5;
00960 
00961   Image<SurfaceState>  surfacesImg(itsLinesMag.getDims(), NO_INIT);
00962 
00963   //int aspectx =0;
00964   //int aspecty = 2;
00965   //double scale = 1.1;
00966   //int x = 283;
00967   //int y = 325;
00968 
00969   for(int aspectx = 0; aspectx < 4; aspectx++)
00970   {
00971     for(int aspecty = 0; aspecty < 4; aspecty++)
00972     {
00973       for(double scale = 0.2  ; scale< 6.0 ; scale+= 0.1)       
00974       { 
00975         float rot = 0;
00976         //for(float rot =  -60 ; rot< 60 ; rot+= 5)     
00977         {       
00978           Polygon model = itsModel;
00979           double ax = pow(1.1, aspectx);
00980           double ay = pow(1.1, aspecty);
00981           LINFO("Rot %f Scale %f aspect %f %f\n", rot, scale, ax, ay);
00982 
00983           model.rotate(rot*M_PI/180.0);
00984           model.scale(scale*matchingScale*ax, scale*matchingScale*ay); 
00985 
00986           model.setWeights();
00987 
00988           double minx, miny, maxx, maxy;
00989           model.getBoundary(minx, miny, maxx, maxy);
00990 
00991           int width = surfacesImg.getWidth(); 
00992           int height = surfacesImg.getHeight();
00993 
00994           //Minx/miny is neg since our template is from -w/-h untill w/h
00995           //Skip the first 4*2 pixels
00996           for (int x=-(int)(minx-4) ; x<width-((int)maxx+4) ; x += 4)
00997           {
00998             for (int y=-(int)(miny-4); y<height-((int)maxy+4); y += 4)
00999             {
01000 
01001               double prob = getCost(itsOriChamferMatcher, model,Point2D<float>(x,y), biasMode);
01002 
01003               if (prob>itsProposalThreshold)
01004               {
01005                 if (surfacesImg.coordsOk(x,y))
01006                   if (prob > surfacesImg.getVal(x,y).prob)
01007                   {
01008                     SurfaceState ss;
01009                     ss.pos = Point2D<float>(x,y);
01010                     ss.aspect = Point2D<float>(ax, ay);
01011                     ss.scale = scale;
01012                     ss.rot = rot;
01013                     ss.prob = prob;
01014                     ss.polygon = model;
01015                     ss.matchingScale = matchingScale;
01016                     surfacesImg.setVal(x,y, ss);
01017 
01018                   }
01019               }
01020             }
01021           }
01022         }
01023       }
01024 
01025     }
01026 
01027   }
01028 
01029   return surfacesImg;
01030 }
01031 
01032 void TwoHalfDSketch::calcSurfaceLikelihood(SurfaceState& surface)
01033 {
01034   Image<float> edges;
01035   Image<float> lumSurface;
01036   double edgeProb = calcSurfaceEdgeLikelihood(surface, edges, lumSurface);
01037   //double surfaceProb = calcSurfaceLumLikelihood(surface, edges, lumSurface);
01038   surface.prob = edgeProb; // * surfaceProb;
01039 }
01040 
01041 double TwoHalfDSketch::calcSurfaceLumLikelihood(SurfaceState& surface, Image<float>& edges, Image<float>& surfaceLum)
01042 {
01043   //Remove the edges from the surface
01044   for(uint i=0; i<surfaceLum.size(); i++)
01045     if (edges[i] > 0)
01046       surfaceLum[i] = 0;
01047 
01048   return getSurfaceLumProb(itsEdgesDT,surfaceLum);
01049 
01050 }
01051 
01052 double TwoHalfDSketch::getSurfaceLumProb(Image<float>& data, Image<float>& model)
01053 {
01054 
01055   double prob = 0;
01056   int pixCount = 0;
01057 
01058   for(int y=0; y < model.getHeight(); y++)
01059     for(int x=0; x < model.getWidth();  x++)
01060       if (model.getVal(x,y) > 0)
01061       {
01062         prob += (10-data.getVal(x,y));
01063         pixCount++;
01064       }
01065   prob /= (10*pixCount);
01066 
01067   return exp(-prob);
01068 }
01069 
01070 
01071 
01072 
01073 
01074 double TwoHalfDSketch::calcSurfaceEdgeLikelihood(SurfaceState& surface, Image<float>& edges, Image<float>& surfaceLum)
01075 {
01076   int pixCount = 0;
01077   double prob  = 0;
01078 
01079   if (surface.polygon.getNumLines() > 0)
01080   {
01081 
01082     for(uint j=0; j<surface.polygon.getNumLines(); j++)
01083     {
01084       Point2D<int> p1; // = (Point2D<int>)(surface.polygon[j] + surface.pos);
01085       Point2D<int> p2; // = (Point2D<int>)(surface.polygon[(j+1)%surface.polygon.size()] + surface.pos);
01086 
01087       //Point2D<float> center = (p1 + p2)/2;
01088       float ori = atan2(p1.j-p2.j, p2.i - p1.i);
01089       if (ori < 0) ori += M_PI;
01090       if (ori >= M_PI) ori -= M_PI;
01091 
01092       prob += getLineProb(p1, p2, ori, pixCount);
01093 
01094       //prob += getEdgeProb(Point2D<int>(p1), ori);
01095       //prob += getEdgeProb(Point2D<int>(center), ori);
01096     }
01097     pixCount *= 2;
01098 
01099   } else {
01100 
01101     float nSeg = 20;
01102     const float dTheta = 2*M_PI / (float)nSeg;
01103 
01104     float a = surface.a;
01105     float b = surface.b;
01106     float e = surface.e;
01107     float k1 = surface.k1;
01108     float k2 = surface.k2;
01109     float rot = surface.rot;
01110 
01111     Point2D<float> p = surface.pos;
01112 
01113     for (float theta=surface.start; theta < surface.end; theta += dTheta)
01114     {
01115       Point2D<float> p1 = ellipsoid(a,b, e, theta);
01116       Point2D<float> p2 = ellipsoid(a,b, e, theta + dTheta);
01117 
01118       Point2D<float> tmpPos1;
01119       Point2D<float> tmpPos2;
01120 
01121       //Sheer
01122       tmpPos1.i = p1.i + p1.j*k1;
01123       tmpPos1.j = p1.i*k2 + p1.j;
01124 
01125       tmpPos2.i = p2.i + p2.j*k1;
01126       tmpPos2.j = p2.i*k2 + p2.j;
01127 
01128       //Rotate and move to p
01129       p1.i = (cos(rot)*tmpPos1.i - sin(rot)*tmpPos1.j) + p.i;
01130       p1.j = (sin(rot)*tmpPos1.i + cos(rot)*tmpPos1.j) + p.j;
01131 
01132       p2.i = (cos(rot)*tmpPos2.i - sin(rot)*tmpPos2.j) + p.i;
01133       p2.j = (sin(rot)*tmpPos2.i + cos(rot)*tmpPos2.j) + p.j;
01134 
01135 
01136       Point2D<float> center = (p1 + p2)/2;
01137       float ori = atan2(p1.j-p2.j, p2.i - p1.i);
01138       if (ori < 0) ori += M_PI;
01139       if (ori >= M_PI) ori -= M_PI;
01140 
01141       prob += getEdgeProb(Point2D<int>(p1), ori);
01142       prob += getEdgeProb(Point2D<int>(center), ori);
01143       pixCount += 2;
01144     }
01145     pixCount *= 1;
01146   }
01147 
01148   //double pr =  exp(-prob/ double(pixCount));
01149   //LINFO("%i: PRob %f pixCount %i = %f", (uint)surface.polygon.size(), prob, pixCount*2, pr);
01150 
01151   return exp(-prob/ double(pixCount))*2;
01152   //return prob/ double(pixCount);
01153 
01154 
01155 }
01156 
01157 double TwoHalfDSketch::getEdgeProb(Point2D<int> loc, float ori)
01158 {
01159 
01160   int numOfEntries = itsOriEdgesDT.size();
01161 
01162   double lambda = (1/6)*M_PI/180; //6 degrees error =  1 pixel distance
01163 
01164   double D = M_PI/numOfEntries;
01165   int oriIdx = (int)floor(ori/D);
01166 
01167   float minDist = 10000;
01168   if (itsOriEdgesDT[0].coordsOk(loc))
01169   {
01170     for(int i=0; i<numOfEntries; i++)
01171     {
01172       float v1 = itsOriEdgesDT[i].getVal(loc) + lambda*angDiff(oriIdx*D, i*D);
01173       if (v1 < minDist)
01174         minDist = v1;
01175     }
01176   }
01177 
01178   return minDist;
01179 }
01180 
01181 double TwoHalfDSketch::getLineProb(Point2D<int> p1, Point2D<int> p2, float ori, int& pixCount)
01182 {
01183 
01184   int numOfEntries = itsOriEdgesDT.size();
01185 
01186   double lambda = (1/2)*M_PI/180; //6 degrees error =  1 pixel distance
01187 
01188   double D = M_PI/numOfEntries;
01189   int oriIdx = (int)floor(ori/D);
01190 
01191   int dx = p2.i - p1.i, ax = abs(dx) << 1, sx = signOf(dx);
01192   int dy = p2.j - p1.j, ay = abs(dy) << 1, sy = signOf(dy);
01193   int x = p1.i, y = p1.j;
01194 
01195   double prob = 0;
01196   int wSize = 1;
01197   if (ax > ay)
01198   {
01199     int d = ay - (ax >> 1);
01200     for (;;)
01201     {
01202       //search for a max edge prob in a window
01203       for(int yy = y-wSize; yy < y+wSize; yy++)
01204         for(int xx = x-wSize; xx < x+wSize; xx++)
01205         {
01206           float minDist = 10000;
01207           if (itsOriEdgesDT[0].coordsOk(xx,yy))
01208           {
01209             for(int i=0; i<numOfEntries; i++)
01210             {
01211               float v1 = itsOriEdgesDT[i].getVal(xx,yy) + lambda*angDiff(oriIdx*D, i*D);
01212               if (v1 < minDist)
01213                 minDist = v1;
01214             }
01215           }
01216           prob += minDist;
01217           pixCount++;
01218         }
01219 
01220       if (x == p2.i) return prob;
01221       if (d >= 0) { y += sy; d -= ax; }
01222       x += sx; d += ay;
01223     }
01224   } else {
01225     int d = ax - (ay >> 1);
01226     for (;;)
01227     {
01228       for(int yy = y-wSize; yy < y+wSize; yy++)
01229         for(int xx = x-wSize; xx < x+wSize; xx++)
01230         {
01231           float minDist = 10000;
01232           if (itsOriEdgesDT[0].coordsOk(xx,yy))
01233           {
01234             for(int i=0; i<numOfEntries; i++)
01235             {
01236               float v1 = itsOriEdgesDT[i].getVal(xx,yy) + lambda*angDiff(oriIdx*D, i*D);
01237               if (v1 < minDist)
01238                 minDist = v1;
01239             }
01240           }
01241           prob += minDist;
01242           pixCount++;
01243         }
01244       if (y == p2.j) return prob;
01245 
01246       if (d >= 0) { x += sx; d -= ay; }
01247       y += sy; d += ax;
01248     }
01249   }
01250 
01251   return prob;
01252   
01253 }
01254 
01255 
01256 void TwoHalfDSketch::printResults(float bias)
01257 {
01258   for(uint i=0; i<itsProposals.size(); i++)
01259   {
01260     Rectangle bb = itsProposals[i].getBB();
01261 
01262     if (i == (uint)itsBiasId)
01263       bias = itsBias;
01264     else
01265       bias = -1;
01266 
01267     printf("Result: %i %i %f %i %i %i %i %f %f %f\n",
01268         i, (uint)itsBiasId,
01269         bias,
01270         bb.topLeft().i,
01271         bb.topLeft().j,
01272         bb.bottomRight().i,
01273         bb.bottomRight().j,
01274         itsProposals[i].prob,
01275         itsProposals[i].pos.i,
01276         itsProposals[i].pos.j
01277 
01278         );
01279   }
01280 
01281 }
01282 
01283 
01284 Layout<PixRGB<byte> > TwoHalfDSketch::getDebugImage(SimEventQueue& q)
01285 {
01286   Layout<PixRGB<byte> > outDisp;
01287 
01288   Image<float> input = itsLinesMag;
01289   inplaceNormalize(input, 0.0F, 255.0F);
01290 
01291   Image<PixRGB<byte> > worldFrame = input;
01292   for(uint i=0; i<itsSurfaces.size(); i++)
01293   {
01294     if (itsSurfaces[i].polygon.getNumLines()> 0)
01295     {
01296       Polygon& polygon = itsSurfaces[i].polygon;
01297       //We have a polygon
01298       for(uint j=0; j<polygon.getNumLines(); j++)
01299       {
01300         Line l = itsProposals[i].getLine(j); //Get the line up to scale and pos
01301         Point2D<int> p1 = (Point2D<int>)l.getP1();
01302         Point2D<int> p2 = (Point2D<int>)l.getP2();
01303         drawLine(worldFrame, p1, p2, PixRGB<byte>(0,255,0));
01304       }
01305 
01306     } else {
01307       drawSuperquadric(worldFrame,
01308           Point2D<int>(itsSurfaces[i].pos),
01309           itsSurfaces[i].a, itsSurfaces[i].b, itsSurfaces[i].e, 
01310           PixRGB<byte>(0,255,0),
01311           itsSurfaces[i].rot, itsSurfaces[i].k1, itsSurfaces[i].k2,
01312           itsSurfaces[i].start,itsSurfaces[i].end);
01313     }
01314     //char msg[255];
01315     //sprintf(msg, "%0.2f", itsSurfaces[i].prob);
01316     //writeText(worldFrame, (Point2D<int>)itsSurfaces[i].getPos(), msg,
01317     //    PixRGB<byte>(255,255,255),
01318     //    PixRGB<byte>(0,0,0));
01319 
01320   }
01321 
01322   Image<PixRGB<byte> > proposalsFrame = input;
01323   float maxProb = -100000;
01324   for(uint i=0; i<itsProposals.size(); i++)
01325   {
01326     LINFO("Propsal %i p:%fx%f s:%f a:%fx%f r:%f p:%f", i,
01327         itsProposals[i].pos.i, itsProposals[i].pos.j,
01328         itsProposals[i].scale,
01329         itsProposals[i].aspect.i, itsProposals[i].aspect.j,
01330         itsProposals[i].rot,
01331         itsProposals[i].prob);
01332     //if (itsProposals[i].prob > 0.00)
01333     {
01334       if (itsProposals[i].prob > maxProb)
01335         maxProb = itsProposals[i].prob;
01336 
01337       if (itsProposals[i].polygon.getNumLines() > 0)
01338       {
01339         Polygon& polygon = itsProposals[i].polygon;
01340         //We have a polygon
01341         for(uint j=0; j<polygon.getNumLines(); j++)
01342         {
01343           Line l = itsProposals[i].getLine(j); //Get the line up to scale and pos
01344           Point2D<int> p1 = (Point2D<int>)l.getP1();
01345           Point2D<int> p2 = (Point2D<int>)l.getP2();
01346           drawLine(proposalsFrame, p1,p2, PixRGB<byte>(255,0,0));
01347         }
01348       } else {
01349 
01350         float nSeg = 20;
01351         const float dTheta = 2*M_PI / (float)nSeg;
01352 
01353         float a = itsProposals[i].a;
01354         float b = itsProposals[i].b;
01355         float e = itsProposals[i].e;
01356         float k1 = itsProposals[i].k1;
01357         float k2 = itsProposals[i].k2;
01358         float rot = itsProposals[i].rot;
01359         float start = itsProposals[i].start;
01360         float end = itsProposals[i].end;
01361 
01362         Point2D<float> p = itsProposals[i].pos;
01363 
01364         for (float theta=start; theta < end; theta += dTheta)
01365         {
01366           Point2D<float> p1 = ellipsoid(a,b, e, theta);
01367           Point2D<float> tmpPos;
01368 
01369           //Sheer
01370           tmpPos.i = p1.i + p1.j*k1;
01371           tmpPos.j = p1.i*k2 + p1.j;
01372 
01373           //Rotate and move to p
01374           p1.i = (cos(rot)*tmpPos.i - sin(rot)*tmpPos.j) + p.i;
01375           p1.j = (sin(rot)*tmpPos.i + cos(rot)*tmpPos.j) + p.j;
01376 
01377           drawCircle(proposalsFrame, (Point2D<int>)p1, 3, PixRGB<byte>(255,0,0));
01378         }
01379       }
01380 
01381       char msg[255];
01382       sprintf(msg, "%0.2f", itsProposals[i].prob);
01383       writeText(proposalsFrame, (Point2D<int>)itsProposals[i].getPos(), msg,
01384           PixRGB<byte>(255,255,255),
01385           PixRGB<byte>(0,0,0));
01386     }
01387 
01388   }
01389   char msg[255];
01390   sprintf(msg, "Max: %0.2f", maxProb);
01391   writeText(proposalsFrame, Point2D<int>(0,proposalsFrame.getHeight()-20), msg,
01392       PixRGB<byte>(255,255,255),
01393       PixRGB<byte>(0,0,0));
01394 
01395 
01396   outDisp = proposalsFrame; //hcat(toRGB(Image<byte>(input)), proposalsFrame);
01397   outDisp = hcat(outDisp, worldFrame);
01398 
01399   return outDisp;
01400 
01401 }
01402 
01403 
01404 // ######################################################################
01405 /* So things look consistent in everyone's emacs... */
01406 /* Local Variables: */
01407 /* indent-tabs-mode: nil */
01408 /* End: */
01409 
01410 #endif
01411 
Generated on Sun May 8 08:41:09 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3