V2.C

Go to the documentation of this file.
00001 /*!@file plugins/SceneUnderstanding/V2.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/V2.C $
00035 // $Id: V2.C 14683 2011-04-05 01:30:59Z lior $
00036 //
00037 
00038 #ifndef V2_C_DEFINED
00039 #define V2_C_DEFINED
00040 
00041 #include "plugins/SceneUnderstanding/V2.H"
00042 
00043 #include "Image/DrawOps.H"
00044 #include "Image/MathOps.H"
00045 //#include "Image/OpenCVUtil.H"
00046 #include "Image/Kernels.H"
00047 #include "Image/FilterOps.H"
00048 #include "Image/Transforms.H"
00049 #include "Image/fancynorm.H"
00050 #include "Image/Convolutions.H"
00051 #include "Raster/Raster.H"
00052 #include "Simulation/SimEventQueue.H"
00053 #include "Util/CpuTimer.H"
00054 #include "Util/JobServer.H"
00055 #include "Util/JobWithSemaphore.H"
00056 #include "GUI/DebugWin.H"
00057 #include <math.h>
00058 #include <fcntl.h>
00059 #include <limits>
00060 #include <string>
00061 #include <stdio.h>
00062 
00063 const ModelOptionCateg MOC_V2 = {
00064   MOC_SORTPRI_3,   "V2-Related Options" };
00065 
00066 // Used by: SimulationViewerEyeMvt
00067 const ModelOptionDef OPT_V2ShowDebug =
00068   { MODOPT_ARG(bool), "V2ShowDebug", &MOC_V2, OPTEXP_CORE,
00069     "Show debug img",
00070     "v2-debug", '\0', "<true|false>", "false" };
00071 
00072 const ModelOptionDef OPT_V2TrainNFA =
00073   { MODOPT_ARG(bool), "V2TrainNFA", &MOC_V2, OPTEXP_CORE,
00074     "Train the Number Of False Alarms (NFA)",
00075     "v2-trainNFA", '\0', "<true|false>", "false" };
00076 
00077 const ModelOptionDef OPT_V2TrainNFAFile =
00078   { MODOPT_ARG(std::string), "V2TrainNFAFile", &MOC_V2, OPTEXP_CORE,
00079     "Train the Number Of False Alarms (NFA) output file",
00080     "v2-trainNFAFile", '\0', "<string>", "NFATrain.dat" };
00081 
00082 //Define the inst function name
00083 SIMMODULEINSTFUNC(V2);
00084 
00085 namespace
00086 {
00087 
00088   typedef std::map<size_t,size_t> BinPDFMap;
00089 
00090   void writePDF(std::vector< std::vector<NFAInfo> > pdf)
00091   {
00092     LINFO("Dump Data");
00093     FILE *fp = fopen("BinPDFMap.dat", "w");
00094     for(uint i=0; i<pdf.size(); i++)
00095     {
00096       for(uint j=0; j<pdf[i].size(); j++)
00097       {
00098         fprintf(fp, "%i %i %i %i %i %i %i\n",  i,
00099             pdf[i][j].ori,
00100             pdf[i][j].xLoc,
00101             pdf[i][j].yLoc,
00102             pdf[i][j].length,
00103             pdf[i][j].pts,
00104             pdf[i][j].alg);
00105       }
00106     }
00107     fclose(fp);
00108   }
00109 
00110   void writePDF(std::vector<NFAPDFMap>& pdf,const char* filename)
00111   {
00112     LINFO("Dump Data");
00113     FILE *fp = fopen(filename, "w");
00114     for(uint i=0; i<pdf.size(); i++)
00115     {
00116       NFAPDFMap::iterator itr1;
00117       for(itr1=pdf[i].begin(); itr1 != pdf[i].end(); itr1++)
00118       {
00119         std::map<short int, //xLoc
00120           std::map<short int, //yLoc,
00121           std::map<short int, //length, 
00122           std::map<short int, //alg, 
00123           NumEntries > > > >::iterator itr2;
00124         for(itr2=itr1->second.begin(); itr2 != itr1->second.end(); itr2++)
00125         {
00126           std::map<short int, //yLoc,
00127             std::map<short int, //length, 
00128             std::map<short int, //alg, 
00129             NumEntries > > >::iterator itr3;
00130           for(itr3=itr2->second.begin(); itr3 != itr2->second.end(); itr3++)
00131           {
00132             std::map<short int, //length, 
00133               std::map<short int, //alg, 
00134               NumEntries > >::iterator itr4;
00135             for(itr4=itr3->second.begin(); itr4 != itr3->second.end(); itr4++)
00136             {
00137               std::map<short int, //alg, 
00138                 NumEntries >::iterator itr5;
00139               for(itr5=itr4->second.begin(); itr5 != itr4->second.end(); itr5++)
00140               {
00141                 fprintf(fp, "%i %i %i %i %i %i %i %zu\n", 
00142                     i, 
00143                     itr1->first, itr2->first, itr3->first, itr4->first, itr5->first,
00144                     itr5->second.pts, itr5->second.num);
00145               }
00146 
00147             }
00148           }
00149 
00150         }
00151 
00152       }
00153     }
00154     fclose(fp);
00155   }
00156 
00157   class TrainNFAJob : public JobWithSemaphore
00158   {
00159     public:
00160 
00161       TrainNFAJob(V2* v2,Image<float> angles,
00162           const std::vector<Point2D<int> >& pos,
00163           int rectWidth, int startWidth, 
00164           NFAPDFMap& pdf) :
00165         itsV2(v2),
00166         itsAngles(angles),
00167         itsPositions(pos),
00168         itsRectWidth(rectWidth),
00169         itsStartWidth(startWidth),
00170         itsPDF(pdf)
00171     {}
00172 
00173       virtual ~TrainNFAJob() {}
00174 
00175       virtual void run()
00176       {
00177         int width = itsAngles.getWidth();
00178         int height = itsAngles.getHeight();
00179         int maxLength = sqrt(width*width + height*height);
00180         LINFO("Start thread for %i", itsRectWidth);
00181 
00182         for(int ori  = -180; ori < 180; ori += 4)
00183         {
00184           for(uint locIdx=0; locIdx<(itsPositions.size()/250); locIdx++)
00185           {
00186             for(int length=4; length<maxLength; length+=4)
00187             {
00188               Dims size(length, itsRectWidth);
00189               Rect rect(itsPositions[locIdx], size, (float)ori*M_PI/180.0);
00190 
00191               int pts=0;
00192               int alg=0;
00193               int x1=0,x2=0,y1=0;
00194 
00195               for(rect.initIter(); !rect.iterEnd(); rect.incIter(x1,x2,y1))
00196               {
00197                 for(int x = x1; x <= x2; x++)
00198                 {
00199                   ++pts;
00200                   if( x>=0 && y1>=0 &&
00201                       x<width && y1<height )
00202                   {
00203                     if( itsV2->isaligned(Point2D<int>(x,y1),
00204                           itsAngles,ori,M_PI/20) )
00205                       ++alg;
00206                   }
00207                 }
00208               }
00209 
00210               if (alg > 0)
00211               {
00212                 const short int xPos = itsPositions[locIdx].i;
00213                 const short int yPos = itsPositions[locIdx].j;
00214                 NumEntries& info = itsPDF[ori][xPos][yPos][length][alg];
00215                 info.num++;
00216                 info.pts = pts;
00217               }
00218             }
00219           }
00220           LINFO("Ori %i", ori);
00221         }
00222         LINFO("Done %i: size %zu", itsRectWidth, itsPDF.size());
00223         this->markFinished();
00224       }
00225 
00226       virtual const char* jobType() const { return "TrainNFAJob"; }
00227       V2* itsV2;
00228       Image<float> itsAngles;
00229       const std::vector<Point2D<int> >& itsPositions;
00230       int itsRectWidth;
00231       int itsStartWidth;
00232       NFAPDFMap& itsPDF; 
00233   };
00234 }
00235 
00236 
00237 
00238 // ######################################################################
00239 V2::V2(OptionManager& mgr, const std::string& descrName,
00240     const std::string& tagName) :
00241   SimModule(mgr, descrName, tagName),
00242   SIMCALLBACK_INIT(SimEventV1Output),
00243   SIMCALLBACK_INIT(SimEventSaveOutput),
00244   SIMCALLBACK_INIT(SimEventUserInput),
00245   itsShowDebug(&OPT_V2ShowDebug, this),
00246   itsTrainNFA(&OPT_V2TrainNFA, this),
00247   itsTrainNFAFile(&OPT_V2TrainNFAFile, this),
00248   itsLSDVerbose(false),
00249   itsQuantError(2.0),
00250   itsAngleTolerance(20.0),
00251   itsEPS(0)
00252   //itsStorage(cvCreateMemStorage(0))
00253 {
00254   //itsThreadServer.reset(new WorkThreadServer("V2", 20));
00255   itsPDF.resize(20);
00256  // itsStoredFrames = "true";
00257  
00258   //Pick random locations to sample
00259   int width = 640;
00260   int height = 480;
00261   for(int y=0; y<height; y++)
00262     for(int x=0; x<width; x++)
00263       itsLocations.push_back(Point2D<int>(x,y));
00264   randShuffle(&itsLocations[0], itsLocations.size());
00265 
00266 
00267 }
00268 
00269 // ######################################################################
00270 V2::~V2()
00271 {
00272   //cvReleaseMemStorage(&itsStorage);
00273 }
00274 
00275 // ######################################################################
00276 void V2::init(Dims numCells)
00277 {
00278 
00279 }
00280 
00281 // ######################################################################
00282 void V2::onSimEventV1Output(SimEventQueue& q,
00283                                   rutz::shared_ptr<SimEventV1Output>& e)
00284 {
00285   itsV1EdgesState = e->getEdgesState();
00286 
00287   if (SeC<SimEventLGNOutput> lgn = q.check<SimEventLGNOutput>(this))
00288     itsLGNInput = lgn->getCells();
00289   
00290   evolve(q);
00291 
00292 
00293 }
00294 
00295 // ######################################################################
00296 void V2::onSimEventSaveOutput(SimEventQueue& q, rutz::shared_ptr<SimEventSaveOutput>& e)
00297 {
00298   if (itsShowDebug.getVal())
00299     {
00300       // get the OFS to save to, assuming sinfo is of type
00301       // SimModuleSaveInfo (will throw a fatal exception otherwise):
00302       nub::ref<FrameOstream> ofs =
00303         dynamic_cast<const SimModuleSaveInfo&>(e->sinfo()).ofs;
00304       Layout<PixRGB<byte> > disp = getDebugImage();
00305       ofs->writeRgbLayout(disp, "V2", FrameInfo("V2", SRC_POS));
00306     }
00307 }
00308 
00309 // ######################################################################
00310 void V2::onSimEventUserInput(SimEventQueue& q, rutz::shared_ptr<SimEventUserInput>& e)
00311 {
00312 
00313   LINFO("Got event %s %ix%i key=%i",
00314       e->getWinName(),
00315       e->getMouseClick().i,
00316       e->getMouseClick().j,
00317       e->getKey());
00318 
00319   if (strcmp(e->getWinName(), "V2"))
00320     return;
00321 
00322   switch(e->getKey())
00323   {
00324     case 10: //1
00325       itsQuantError += 1;
00326       break;
00327     case 24: //q
00328       itsQuantError -= 1;
00329       break;
00330     case 11: //1
00331       itsAngleTolerance += 1;
00332       break;
00333     case 25: //q
00334       itsAngleTolerance -= 1;
00335       break;
00336     case 12: //1
00337       itsEPS += 1;
00338       break;
00339     case 26: //q
00340       itsEPS -= 1;
00341       break;
00342     default:
00343       break;
00344   }
00345 
00346   LINFO("Q %f A %f EPS %f",
00347       itsQuantError, itsAngleTolerance,
00348       itsEPS);
00349 
00350   //itsQuantError(2.0),
00351   //itsAngleTolerance(20.0),
00352   //itsEPS(0),
00353   
00354   
00355   if (e->getMouseClick().isValid())
00356   {
00357   }
00358 
00359   evolve(q);
00360 
00361 }
00362 
00363 void V2::evolve(SimEventQueue& q)
00364 {
00365   evolveLines();
00366 
00367   //Find corners and symetries
00368   //cornerSymetryDetection(itsLines);
00369   //evolveContours();
00370   //evolveBorderOwner();
00371   
00372   q.post(rutz::make_shared(new SimEventV2Output(this, itsLines,
00373           itsCornersState, itsCornersProposal, itsEdges, itsTensorFields, itsEdges.getDims())));
00374 }
00375 
00376 void V2::evolveBorderOwner()
00377 {
00378   std::vector<ContourState> contours = proposeContoursBO(itsLines);
00379   itsContours = contours;
00380 }
00381 
00382 std::vector<V2::ContourState> V2::proposeContoursBO(std::vector<LineSegment>& lines)
00383 {
00384 
00385   std::vector<ContourState> contours;
00386 
00387   //Propose countors based on border ownership
00388 
00389 
00390   //Group all lines that have the same color together
00391   std::list<LineSegment> currentLines;
00392   for(uint i=0; i<lines.size(); i++)
00393     currentLines.push_back(lines[i]);
00394 
00395   //Pick a line and find all matches
00396 
00397   while(!currentLines.empty())
00398   {
00399     ContourState contour;
00400     LineSegment ls = currentLines.front();
00401     currentLines.pop_front();
00402     contour.lines.push_back(ls);
00403 
00404     std::list<LineSegment>::iterator iter;
00405     for(iter = currentLines.begin(); iter != currentLines.end(); )
00406     {
00407       if (ls.colorDist(*iter) < 50.0F)
00408       {
00409         contour.lines.push_back(*iter);
00410         currentLines.erase(iter++); //increment the iter after erasing
00411       } else {
00412         iter++;
00413       }
00414     }
00415 
00416     contours.push_back(contour);
00417   }
00418 
00419   return contours;
00420 }
00421 
00422 void V2::evolveContours()
00423 {
00424 
00425   //Link lines
00426 
00427   itsContours.clear();
00428 
00429   std::vector<ContourState> contours = proposeContours(itsLines);
00430 
00431   //Take the input as the perception for now
00432   itsContours = contours;
00433 }
00434 
00435 std::vector<V2::ContourState> V2::proposeContours(std::vector<LineSegment>& lines)
00436 {
00437 
00438   std::vector<ContourState> contours;
00439 
00440   std::list<LineSegment> currentLines;
00441 
00442   for(uint i=0; i<lines.size(); i++)
00443     currentLines.push_back(lines[i]);
00444 
00445   while(!currentLines.empty())
00446   {
00447     LineSegment ls = currentLines.front();
00448     currentLines.pop_front();
00449 
00450     Point2D<float> p1 = ls.p1;
00451     Point2D<float> p2 = ls.p2;
00452 
00453     LINFO("End Points %0.2fx%0.2f %0.2fx%0.2f",
00454         p1.i, p1.j,
00455         p2.i, p2.j);
00456 
00457     ContourState contour;
00458     contour.lines.push_back(ls);
00459 
00460     std::stack<Point2D<float> > endPoints;
00461     endPoints.push(p1);
00462     endPoints.push(p2);
00463 
00464     Image<PixRGB<byte> > contoursImg = itsLGNInput[0];
00465     drawLine(contoursImg, (Point2D<int>)p1, (Point2D<int>)p2,
00466         PixRGB<byte>(255,0,0), 1);
00467 
00468     //SHOWIMG(contoursImg);
00469 
00470     //std::vector<LineSegment> nerestLines = findNerestLines(currentLines, endPoints);
00471     //LINFO("Found %lu closest lines", nerestLines.size());
00472 
00473     //while(nerestLines.size() > 0)
00474     //{
00475     //  for(uint i=0; i<nerestLines.size(); i++)
00476     //  {
00477     //    contour.lines.push_back(nerestLines[i]);
00478 
00479     //    LineSegment& ls = nerestLines[i];
00480     //    drawLine(contoursImg, (Point2D<int>)ls.p1,
00481     //        (Point2D<int>)ls.p2, PixRGB<byte>(0,255,0), 1);
00482     //  }
00483     //  SHOWIMG(contoursImg);
00484     //  nerestLines = findNerestLines(currentLines, endPoints);
00485     //  LINFO("Found Again %lu closest lines", nerestLines.size());
00486 
00487     //}
00488 
00489     //if (contour.lines.size() > 1)
00490     //  contours.push_back(contour);
00491 
00492     LINFO("Loop\n");
00493   }
00494 
00495 
00496   return contours;
00497 
00498 }
00499 
00500 std::vector<V2::LineSegment> V2::findNerestLines(std::list<LineSegment>& lines,
00501     std::stack<Point2D<float> >& endPoints)
00502 {
00503 
00504   std::vector<LineSegment> nerestLines;
00505 
00506   Point2D<float> p = endPoints.top();
00507   endPoints.pop();
00508 
00509   //Find the closest line to the endpoint
00510   //float thresh = 5.0;
00511   //std::list<LineSegment>::iterator iter;
00512 
00513   //for(iter = lines.begin(); iter != lines.end(); iter++)
00514   //{
00515   //  if (p.distance(iter->p1) < thresh)
00516   //  {
00517   //    endPoints.push(iter->p2);
00518   //    nerestLines.push_back(*iter);
00519   //    lines.erase(iter);
00520   //  } else if (p.distance(iter->p2) < thresh)
00521   //  {
00522   //    nerestLines.push_back(*iter);
00523   //    endPoints.push(iter->p1);
00524   //    lines.erase(iter);
00525   //  }
00526   //}
00527 
00528   return nerestLines;
00529 
00530 }
00531 
00532 void V2::evolveLines()
00533 {
00534 
00535 
00536   std::vector<LineSegment> lines;
00537 
00538   if (itsTrainNFA.getVal())
00539   {
00540     double logNT = 0;
00541     double p = 1.0/20.0;
00542     //print out the raguler NFA computations
00543     for(uint n=1; n<18000; n++)
00544     {
00545       for(uint k=1; k<n && k<3000; k++)
00546       {
00547         double nfaValue = nfa(n, k, p, logNT);
00548         if (!std::isinf(nfaValue) || nfaValue != 0)
00549           printf("%i %i %f\n", k, n, nfaValue);
00550 
00551       }
00552     }
00553     //trainNFA(itsV1EdgesState);
00554 
00555   } else {
00556     //Use the LGN Input
00557     //std::vector<LineSegment> lines = proposeLineSegments(itsLGNInput);
00558 
00559     //Use the V1 Input
00560     lines = proposeLineSegments(itsV1EdgesState);
00561 
00562   }
00563 
00564 
00565   ////For each proposed line find the most Likely line
00566   ////segment in our current perception
00567   //for(uint i=0; i<lines.size(); i++)
00568   //{
00569   //  float prob;
00570   //  int lineIdx = findMostProbableLine(lines[i], prob);
00571 
00572   //  if (lineIdx >= 0)
00573   //  {
00574   //    //Line Found, update it
00575   //    itsLines[lineIdx] = lines[i];
00576 
00577 
00578   //    //Update the strength
00579   //    itsLines[lineIdx].strength += 0.5;
00580   //    if ( itsLines[lineIdx].strength > 1)
00581   //      itsLines[lineIdx].strength = 1;
00582   //  } else {
00583   //    //Add the New line to the current perception
00584   //    lines[i].strength = 0.5;
00585 
00586   //    itsLines.push_back(lines[i]);
00587   //  }
00588   //}
00589 
00590   ////Leaky itegrator line
00591   ////Slowly kill off any lines that are in the current perception but not in the data
00592   //for(uint i =0; i<itsLines.size(); i++)
00593   //{
00594   //  itsLines[i].strength -= 0.10;
00595 
00596   //  if (itsLines[i].strength < 0)
00597   //  {
00598   //    //Remove the line for the perception
00599   //  }
00600   //}
00601 
00602 
00603   itsLines = lines;
00604 
00605 }
00606 
00607 
00608 int V2::findMostProbableLine(const LineSegment& line, float& prob)
00609 {
00610 
00611   for(uint i=0; i<itsLines.size(); i++)
00612   {
00613     if (itsLines[i].distance(line) < 5)
00614     {
00615       prob = itsLines[i].distance(line);
00616       return i;
00617     }
00618   }
00619 
00620   return -1;
00621 }
00622 
00623 std::vector<V2::LineSegment> V2::proposeLineSegments(ImageSet<float> &LGNInput)
00624 {
00625 
00626   std::vector<LineSegment> lines;
00627 
00628   LINFO("Evolve tensors");
00629   std::vector<LineSegment> linesLum = lineSegmentDetection(LGNInput[0]);
00630   std::vector<LineSegment> linesRG = lineSegmentDetection(LGNInput[1]);
00631   std::vector<LineSegment> linesBY = lineSegmentDetection(LGNInput[2]);
00632   LINFO("Done");
00633 
00634   for(uint i=0; i<linesLum.size(); i++)
00635   {
00636     linesLum[i].color=0;
00637     lines.push_back(linesLum[i]);
00638   }
00639   for(uint i=0; i<linesRG.size(); i++)
00640   {
00641     linesRG[i].color=1;
00642     lines.push_back(linesRG[i]);
00643   }
00644   for(uint i=0; i<linesBY.size(); i++)
00645   {
00646     linesBY[i].color=2;
00647     lines.push_back(linesBY[i]);
00648   }
00649 
00650 
00651   return lines;
00652 
00653 }
00654 
00655 std::vector<V2::LineSegment> V2::proposeLineSegments(V1::EdgesState& edgesState)
00656 {
00657 
00658   //Preform TensorVoting
00659 
00660   //Preform nonMax surpression if true
00661   TensorField lumTensorField = itsLumTV.evolve(edgesState.lumTensorField, false);
00662   TensorField rgTensorField  = itsRGTV.evolve(edgesState.rgTensorField, false);
00663   TensorField byTensorField  = itsBYTV.evolve(edgesState.byTensorField, false);
00664 
00665   itsTensorFields.clear();
00666   itsTensorFields.push_back(itsLumTV);
00667   itsTensorFields.push_back(itsRGTV);
00668   itsTensorFields.push_back(itsBYTV);
00669 
00670 
00671   EigenSpace eigen = getTensorEigen(lumTensorField);
00672   itsEdges = eigen.l1-eigen.l2;
00673   itsCornersProposal = eigen.l2;
00674 
00675   //Get the max edge from all color spaces
00676   itsMaxTF = lumTensorField;
00677   //itsMaxTF.setMax(rgTensorField);
00678   //itsMaxTF.setMax(byTensorField);
00679 
00680 
00681   std::vector<LineSegment> lines = lineSegmentDetection(itsMaxTF, itsQuantError, itsAngleTolerance, itsEPS);
00682   //std::vector<LineSegment> linesRG = lineSegmentDetection(rgTensorField, itsQuantError, itsAngleTolerance, itsEPS);
00683   //std::vector<LineSegment> linesBY = lineSegmentDetection(byTensorField, itsQuantError, itsAngleTolerance, itsEPS);
00684 
00685 
00686   //for(uint i=0; i<linesLum.size(); i++)
00687   //{
00688   //  linesLum[i].color=0;
00689   //  lines.push_back(linesLum[i]);
00690   //}
00691   //for(uint i=0; i<linesRG.size(); i++)
00692   //{
00693   //  linesRG[i].color=0;
00694   //  lines.push_back(linesRG[i]);
00695   //}
00696   //for(uint i=0; i<linesBY.size(); i++)
00697   //{
00698   //  linesBY[i].color=0;
00699   //  lines.push_back(linesBY[i]);
00700   //}
00701 
00702 
00703   return lines;
00704 
00705 }
00706 
00707 
00708 V2::rect V2::getRect(const Point2D<int> p1, const Point2D<int> p2, int width)
00709 {
00710 
00711   rect rec;
00712   // set the first and second point of the line segment
00713   if (p1.j > p2.j)
00714   {
00715     rec.x1 = p2.i*2; rec.y1 = p2.j*2;
00716     rec.x2 = p1.i*2; rec.y2 = p1.j*2;
00717   } else {
00718     rec.x1 = p1.i*2; rec.y1 = p1.j*2;
00719     rec.x2 = p2.i*2; rec.y2 = p2.j*2;
00720   }
00721 
00722   double theta = atan2(p2.j-p1.j,p2.i-p1.i);
00723   if (theta<0)
00724     theta += M_PI;
00725 
00726   rec.theta = theta;
00727   rec.dx = (float) cos( (double) rec.theta );
00728   rec.dy = (float) sin( (double) rec.theta );
00729   rec.width=width;
00730   rec.prec = M_PI / 20; /* tolerance angle */
00731   rec.p = 1.0 / (double) 20;
00732   return rec;
00733 }
00734 
00735 void V2::trainNFA(V1::EdgesState& edgesState)
00736 {
00737 
00738   //Preform TensorVoting
00739 
00740   //Preform nonMax surpression if true
00741   TensorField lumTensorField = itsLumTV.evolve(edgesState.lumTensorField, false);
00742   //TensorField rgTensorField  = itsRGTV.evolve(edgesState.rgTensorField, false);
00743   //TensorField byTensorField  = itsBYTV.evolve(edgesState.byTensorField, false);
00744 
00745   itsTensorFields.clear();
00746   itsTensorFields.push_back(itsLumTV);
00747   //itsTensorFields.push_back(itsRGTV);
00748   //itsTensorFields.push_back(itsBYTV);
00749 
00750   //std::vector<LineSegment> linesRG = lineSegmentDetection(rgTensorField, itsQuantError, itsAngleTolerance, itsEPS);
00751   //std::vector<LineSegment> linesBY = lineSegmentDetection(byTensorField, itsQuantError, itsAngleTolerance, itsEPS);
00752 
00753 
00754   
00755 
00756   std::vector<Point2D<int> > list_p;
00757   Image<float> modgrad;
00758 
00759   int max_grad = 260100;
00760   int n_bins = 16256;
00761   Image<float> angles = ll_angle(lumTensorField,max_grad*0.10,list_p,modgrad,n_bins,max_grad);
00762 
00763 
00764   CpuTimer timer;
00765   timer.reset();
00766 
00767   WorkThreadServer threadServer("V2", 20);
00768 
00769   std::vector<rutz::shared_ptr<TrainNFAJob> > jobs;
00770   for(int rw=0; rw<20; rw += 1)
00771   {
00772     jobs.push_back(rutz::make_shared(new TrainNFAJob(this, angles,
00773             itsLocations, rw+3,
00774             0, itsPDF[rw])));
00775     threadServer.enqueueJob(jobs.back());
00776   }
00777 
00778   LINFO("Waiting for jobs to finish");
00779   while((uint)threadServer.getNumRun() < jobs.size())
00780   {
00781     //LINFO("%i/%zu jobs completed", threadServer.getNumRun(),jobs.size());
00782     sleep(1);
00783   }
00784   timer.mark();
00785   LINFO("Total time for image %0.2f sec", timer.real_secs());
00786 
00787   writePDF(itsPDF, itsTrainNFAFile.getVal().c_str());
00788 }
00789 
00790 void V2::cornerSymetryDetection(std::vector<LineSegment>& lines)
00791 {
00792   itsCornersState.clear();
00793   itsSymmetries.clear();
00794 
00795   for(uint ls1Idx=0; ls1Idx<itsLines.size(); ls1Idx++)
00796   {
00797     for(uint ls2Idx=ls1Idx+1; ls2Idx<itsLines.size(); ls2Idx++)
00798     {
00799       //find parallel lines
00800       /* This code is based on the solution of these two input equations:
00801        *  Pa = P1 + ua (P2-P1)
00802        *  Pb = P3 + ub (P4-P3)
00803        *
00804        * Where line one is composed of points P1 and P2 and line two is composed
00805        *  of points P3 and P4.
00806        *
00807        * ua/b is the fractional value you can multiple the x and y legs of the
00808        *  triangle formed by each line to find a point on the line.
00809        *
00810        * The two equations can be expanded to their x/y components:
00811        *  Pa.x = p1.x + ua(p2.x - p1.x)
00812        *  Pa.y = p1.y + ua(p2.y - p1.y)
00813        *
00814        *  Pb.x = p3.x + ub(p4.x - p3.x)
00815        *  Pb.y = p3.y + ub(p4.y - p3.y)
00816        *
00817        * When Pa.x == Pb.x and Pa.y == Pb.y the lines intersect so you can come
00818        *  up with two equations (one for x and one for y):
00819        *
00820        * p1.x + ua(p2.x - p1.x) = p3.x + ub(p4.x - p3.x)
00821        * p1.y + ua(p2.y - p1.y) = p3.y + ub(p4.y - p3.y)
00822        *
00823        * ua and ub can then be individually solved for.  This results in the
00824        *  equations used in the following code.
00825        */
00826       //Finding Intersection of the two lines
00827       LineSegment& lineSeg1 = itsLines[ls1Idx];
00828       LineSegment& lineSeg2 = itsLines[ls2Idx];
00829 
00830       double x1 = lineSeg1.p1.i, y1 = lineSeg1.p1.j;
00831       double x2 = lineSeg1.p2.i, y2 = lineSeg1.p2.j;
00832       double x3 = lineSeg2.p1.i, y3 = lineSeg2.p1.j;
00833       double x4 = lineSeg2.p2.i, y4 = lineSeg2.p2.j;
00834 
00835       /*
00836        * If the denominator for the equations for ua and ub is 0 then the two lines are parallel.
00837        * If the denominator and numerator for the equations for ua and ub are 0 then the two
00838        * lines are coincident.
00839        */
00840       double den = ((y4 - y3)*(x2 - x1)) - ((x4 - x3)*(y2 - y1) );
00841       Point2D<float> intersectionPoint;
00842       if(den == 0.0F)
00843       {
00844         ////LINFO("Parallel lines");
00845         ////LINFO("%0.2fx%0.2f %0.2fx%0.2f     %0.2fx%0.2f %0.2fx%0.2f",
00846         ////    x1, y1, x2, y2,
00847         ////    x3, y3, x4, y4);
00848 
00849         ////LINFO("%f %f %f %f",
00850         ////    x1-x3, y1-y3, x2-x4, y2-y4);
00851 
00852         //float L1 = lineSeg1.length;
00853         //float L2 = lineSeg2.length;
00854 
00855         ////float xVL = ( (lineSeg1.center.i * L1) + (lineSeg2.center.i * L2) ) / (L1 + L2);
00856         ////float yVL = ( (lineSeg1.center.j * L1) + (lineSeg2.center.j * L2) ) / (L1 + L2);
00857 
00858         //float xG = ((L1 *(x1 + x2) ) + (L2*(x3 + x4)))/(2*(L1+L2));
00859         //float yG = ((L1 *(y1 + y2) ) + (L2*(y3 + y4)))/(2*(L1+L2));
00860 
00861         //float ori1 = lineSeg1.ori;
00862         //float ori2 = lineSeg2.ori;
00863 
00864         //float ang = 0;
00865         //if ( fabs(ori1 - ori2) <= M_PI/2)
00866         //  ang = ( (ori1 * L1) + (ori2 * L2) ) / (L1 + L2);
00867         //else
00868         //  ang = ( (ori1 * L1) + ((ori2 - (M_PI*ori2/fabs(ori2))) * L2) ) / (L1 + L2);
00869 
00870         //Point2D<int> center((int)xG, (int)yG);
00871 
00872         //Image<PixRGB<byte> > tmp(itsV1EdgesDims, ZEROS);
00873         //drawCircle(tmp, center, 3, PixRGB<byte>(0,255,255), 2);
00874         //drawLine(tmp, center, ang, 3000.0F, PixRGB<byte>(255,0,0));
00875         //drawLine(tmp, (Point2D<int>)lineSeg1.p1, (Point2D<int>)lineSeg1.p2, PixRGB<byte>(255,0,0));
00876         //drawLine(tmp, (Point2D<int>)lineSeg2.p1, (Point2D<int>)lineSeg2.p2, PixRGB<byte>(0,255,0));
00877         //SHOWIMG(tmp);
00878 
00879         //drawLine(tmp, (Point2D<int>)lineSeg1.center, lineSeg1.ori, 3000.0F, PixRGB<byte>(255,0,0));
00880         //drawLine(tmp, (Point2D<int>)lineSeg2.center, lineSeg2.ori, 3000.0F, PixRGB<byte>(0,255,0));
00881         //SHOWIMG(tmp);
00882       } else {
00883         //Find the intersection
00884         double u_a = (((x4 - x3)*(y1 - y3) ) - (( y4 - y3)*(x1 - x3))) / den;
00885         double u_b = (((x2 - x1)*(y1 - y3) ) - (( y2 - y1)*(x1 - x3))) / den;
00886 
00887         intersectionPoint.i = x1 + (u_a * (x2 - x1));
00888         intersectionPoint.j = y1 + (u_a * (y2 - y1));
00889 
00890         if (u_a >= -0.1 && u_a <= 1.1 &&
00891             u_b >= -0.1 && u_b <= 1.1)
00892         {
00893 
00894           //LINFO("Line1 Ori %0.2f line 2 Ori %0.2f",
00895           //    lineSeg1.ori*180/M_PI, lineSeg2.ori*180/M_PI);
00896 
00897           float dotProduct =
00898             ((lineSeg1.center.i - intersectionPoint.i)*(lineSeg2.center.i - intersectionPoint.i)) +
00899             ((lineSeg1.center.j - intersectionPoint.j)*(lineSeg2.center.j - intersectionPoint.j));
00900           float crossProduct =
00901             ((lineSeg1.center.i - intersectionPoint.i)*(lineSeg2.center.j - intersectionPoint.j)) +
00902             ((lineSeg1.center.j - intersectionPoint.j)*(lineSeg2.center.i - intersectionPoint.i));
00903 
00904           float ang = atan2(crossProduct, dotProduct);
00905 
00906           float ori1 = atan2((intersectionPoint.j - lineSeg1.center.j),
00907                             (lineSeg1.center.i - intersectionPoint.i));
00908           float ori2 = atan2((intersectionPoint.j - lineSeg2.center.j),
00909                             (lineSeg2.center.i - intersectionPoint.i));
00910 
00911           //Find width direction the angle bisector is facing and set that as the orinetation
00912           if (ori1 < 0 )
00913             ori1 += 2*M_PI;
00914           if (ori2 < 0 )
00915             ori2 += 2*M_PI;
00916 
00917           float ori = (ori2 - ori1);
00918 
00919           if ( (fabs(ori) > M_PI && ori < 0 ) ||
00920                (fabs(ori) < M_PI && ori > 0 ) )
00921             ori = ori1 + fabs(ang/2);
00922           else
00923             ori = ori1 - fabs(ang/2);
00924 
00925           //Dont add any sharp or flat corners
00926           if (fabs(ang) < 170*M_PI/180 && fabs(ang) > 5*M_PI/180)
00927           {
00928             CornerState corner(intersectionPoint,
00929                 ori,
00930                 ang);
00931             corner.lineSeg1 = ls1Idx;
00932             corner.lineSeg2 = ls2Idx;
00933 
00934 
00935             //Find which end points are furthest from the corner
00936             if (lineSeg1.p1.distance(intersectionPoint) < lineSeg1.p2.distance(intersectionPoint))
00937               corner.endPoint1 = lineSeg1.p2;
00938             else
00939               corner.endPoint1 = lineSeg1.p1;
00940 
00941             if (lineSeg2.p1.distance(intersectionPoint) < lineSeg2.p2.distance(intersectionPoint))
00942               corner.endPoint2 = lineSeg2.p2;
00943             else
00944               corner.endPoint2 = lineSeg2.p1;
00945 
00946             itsCornersState.push_back(corner);
00947           }
00948 
00949         } else {
00950           Image<PixRGB<byte> > tmp(320,240, ZEROS);
00951           if (!tmp.coordsOk(intersectionPoint))
00952           {
00953             float L1 = lineSeg1.length;
00954             float L2 = lineSeg2.length;
00955 
00956             //float xVL = ( (lineSeg1.center.i * L1) + (lineSeg2.center.i * L2) ) / (L1 + L2);
00957             //float yVL = ( (lineSeg1.center.j * L1) + (lineSeg2.center.j * L2) ) / (L1 + L2);
00958 
00959             float xG = ((L1 *(x1 + x2) ) + (L2*(x3 + x4)))/(2*(L1+L2));
00960             float yG = ((L1 *(y1 + y2) ) + (L2*(y3 + y4)))/(2*(L1+L2));
00961 
00962             float ori1 = lineSeg1.ori;
00963             float ori2 = lineSeg2.ori;
00964 
00965             float ang = 0;
00966             if ( fabs(ori1 - ori2) <= M_PI/2)
00967               ang = ( (ori1 * L1) + (ori2 * L2) ) / (L1 + L2);
00968             else
00969               ang = ( (ori1 * L1) + ((ori2 - (M_PI*ori2/fabs(ori2))) * L2) ) / (L1 + L2);
00970 
00971 
00972             Point2D<float> p1Proj((((y1-yG)*sin(ang)) + ( (x1-xG)*cos(ang))),
00973                                   (((y1-yG)*cos(ang)) - ( (x1-xG)*sin(ang))));
00974             //Point2D<float> p1Proj( (x1-xG), (y1-yG));
00975 
00976             //Convert P1Proj back to 0 0 origin
00977             //Point2D<float> p1Disp((((p1Proj.j+yG)*sin(M_PI+ang)) + ( (p1Proj.i+xG)*cos(M_PI+ang))),
00978             //                      (((p1Proj.j+yG)*cos(M_PI+ang)) - ( (p1Proj.i+xG)*sin(M_PI+ang))));
00979             //LINFO("POint %fx%f proj %fx%f \n point=(%f,%f)",
00980             //    x1, y1,
00981             //    p1Proj.i, p1Proj.j,
00982             //    p1Disp.i, p1Disp.j);
00983 
00984             Point2D<float> p2Proj(((y2-yG)*sin(ang)) + ( (x2-xG)*cos(ang)),
00985                                   ((y2-yG)*cos(ang)) - ( (x2-xG)*sin(ang)));
00986             Point2D<float> p3Proj(((y3-yG)*sin(ang)) + ( (x3-xG)*cos(ang)),
00987                                   ((y3-yG)*cos(ang)) - ( (x3-xG)*sin(ang)));
00988             Point2D<float> p4Proj(((y4-yG)*sin(ang)) + ( (x4-xG)*cos(ang)),
00989                                   ((y4-yG)*cos(ang)) - ( (x4-xG)*sin(ang)));
00990 
00991             float points[4] = {p1Proj.i, p2Proj.i, p3Proj.i, p4Proj.i};
00992 
00993             float min = points[0];
00994             float max = points[0];
00995 
00996             for(int i=1; i<4; i++)
00997             {
00998               if (points[i] > max)
00999                 max = points[i];
01000               if (points[i] < min)
01001                 min = points[i];
01002             }
01003 
01004             //LINFO("POints %f %f %f %f",
01005             //    points[0], points[1], points[2], points[3]);
01006             //LINFO("Max %f min %f", max, min);
01007 
01008             float totalLength = fabs(max-min);
01009             //LINFO("Distance %f  totalLength %f",
01010             //    totalLength, L1+L2);
01011 
01012             //Do we have overlap with some noise
01013             if (totalLength < L1+L2 + 10)
01014             {
01015               SymmetryState  symState;
01016               symState.lineSeg1 = ls1Idx;
01017               symState.lineSeg2 = ls2Idx;
01018               symState.center = Point2D<float>(xG,yG);
01019               symState.ang = ang;
01020               symState.length = totalLength;
01021 
01022               itsSymmetries.push_back(symState);
01023 
01024 
01025              // Point2D<int> center((int)xG, (int)yG);
01026 
01027              // //drawCircle(tmp, (Point2D<int>)p1Disp, 3, PixRGB<byte>(0,255,0), 2);
01028              // //drawCircle(tmp, (Point2D<int>)p2Proj, 3, PixRGB<byte>(0,255,0), 2);
01029              // //drawCircle(tmp, (Point2D<int>)p3Proj, 3, PixRGB<byte>(0,255,0), 2);
01030              // //drawCircle(tmp, (Point2D<int>)p4Proj, 3, PixRGB<byte>(0,255,0), 2);
01031 
01032              // drawCircle(tmp, center, 3, PixRGB<byte>(0,255,255), 2);
01033              // drawLine(tmp, center, ang, 3000.0F, PixRGB<byte>(255,0,0));
01034              // drawLine(tmp, (Point2D<int>)lineSeg1.p1, (Point2D<int>)lineSeg1.p2, PixRGB<byte>(255,0,0));
01035              // drawLine(tmp, (Point2D<int>)lineSeg2.p1, (Point2D<int>)lineSeg2.p2, PixRGB<byte>(0,255,0));
01036              // SHOWIMG(tmp);
01037 
01038              // drawLine(tmp, (Point2D<int>)lineSeg1.center, lineSeg1.ori, 3000.0F, PixRGB<byte>(255,0,0));
01039              // drawLine(tmp, (Point2D<int>)lineSeg2.center, lineSeg2.ori, 3000.0F, PixRGB<byte>(0,255,0));
01040              // SHOWIMG(tmp);
01041             }
01042           }
01043         }
01044       }
01045 
01046 
01047 
01048       //LINFO("Intersection %fx%f", x, y);
01049 
01050 
01051       //if (!tmpImg.coordsOk(intersectionPoint) &&
01052       //    fabs(lineSeg1.length - lineSeg2.length) < 10.0F && false)
01053       //{
01054       //  Image<PixRGB<byte> > tmp = tmpImg;
01055       //  drawLine(tmp, (Point2D<int>)lineSeg1.p1, (Point2D<int>)lineSeg1.p2, PixRGB<byte>(255,0,0));
01056       //  drawLine(tmp, (Point2D<int>)lineSeg2.p1, (Point2D<int>)lineSeg2.p2, PixRGB<byte>(0,255,0));
01057       //  SHOWIMG(tmp);
01058 
01059       //  drawLine(tmp, (Point2D<int>)lineSeg1.center, lineSeg1.ori, 3000.0F, PixRGB<byte>(255,0,0));
01060       //  drawLine(tmp, (Point2D<int>)lineSeg2.center, lineSeg2.ori, 3000.0F, PixRGB<byte>(0,255,0));
01061       //  drawCircle(tmp, (Point2D<int>)intersectionPoint, 6, PixRGB<byte>(0,255,255));
01062       //  SHOWIMG(tmp);
01063       //}
01064 
01065 
01066     }
01067   }
01068 
01069 }
01070 
01071 
01072 
01073 
01074 
01075 void V2::findLines(const Image<float> &mag, const Image<float>& ori, Image<float>& retMag, Image<float>& retOri)
01076 {
01077   retMag = Image<float>(mag.getDims()/2, ZEROS);
01078   retOri = Image<float>(ori.getDims()/2, ZEROS);
01079 
01080   Dims win(3,3);
01081   for(int y=0; y<mag.getHeight()-win.h()-1; y += 2)
01082     for(int x=0; x<mag.getWidth()-win.w()-1; x += 2)
01083     {
01084       double sumSin = 0, sumCos = 0;
01085       float numElem = 0;
01086       float totalWeight = 0;
01087 
01088       double meanLocX = 0, meanLocY = 0;
01089       Image<float> tt(win, ZEROS);
01090 
01091       //Add the angles within the window into the histogram
01092       float hist[360];
01093       for(int wy=0; wy<win.h(); wy++)
01094         for(int wx=0; wx<win.w(); wx++)
01095         {
01096           float val = mag.getVal(x+wx,y+wy);
01097 
01098           if (val > 0)
01099           {
01100             float ang = ori.getVal(x+wx,y+wy) + M_PI/2;
01101             meanLocX += wx;
01102             meanLocY += wy;
01103             sumSin += sin(ang); //*mag;
01104             sumCos += cos(ang); //*mag;
01105             numElem++;
01106             totalWeight += 1; //mag;
01107 
01108             //Add to histogram
01109             int angI = (int)(ang*180.0/M_PI);
01110             if (angI < 0) angI += 360;
01111             if (angI > 360) angI -= 360;
01112             hist[angI] += 1; //mag;
01113             //tt.setVal(wx,wy,mag);
01114           }
01115         }
01116       if (numElem > 0)
01117       {
01118         double xMean = sumSin/totalWeight;
01119         double yMean = sumCos/totalWeight;
01120         double sum = sqrt( xMean*xMean + yMean*yMean);
01121         float ang = atan2(xMean, yMean);
01122 
01123         if (sum > 0.99)
01124         {
01125           //This must be an edge, calculate the angle from the mean
01126           //drawLine(LTmp, Point2D<int>(x+(int)(meanLocX/numElem), y+(int)(meanLocY/numElem)), (float)ang, win.w(), 255.0F); // (float)ang);
01127           //V1::EdgeState edgeState;
01128           //edgeState.pos = Point2D<int>(x+(int)(meanLocX/numElem), y+(int)(meanLocY/numElem));
01129           //edgeState.ori = ang;
01130           //edgeState.var = (10*M_PI/180)*(10*M_PI/180); //10
01131           //edgeState.prob = sum;
01132           //itsEdgesState.push_back(edgeState);
01133 
01134           retMag.setVal(Point2D<int>(x/2+(int)(meanLocX/numElem), y/2+(int)(meanLocY/numElem)), 255.0);
01135           retOri.setVal(Point2D<int>(x/2+(int)(meanLocX/numElem), y/2+(int)(meanLocY/numElem)), ang-M_PI/2);
01136           //nLines++;
01137         } else {
01138           //We have a corner
01139         }
01140       } else {
01141         //Stmp.setVal(x,y,0.0);
01142       }
01143     }
01144 
01145 }
01146 
01147 void V2::evolve(Image<PixRGB<byte> >& img)
01148 {
01149 
01150   Image<float> lum = luminance(img);
01151   SHOWIMG(lum);
01152   inplaceNormalize(lum, 0.0F, 255.0F);
01153 
01154   std::vector<LineSegment> lines = lineSegmentDetection(lum);
01155 
01156   Image<PixRGB<byte> > edgesImg(lum.getDims(), ZEROS);
01157   for(uint i=0; i<lines.size(); i++)
01158   {
01159     LineSegment& ls = lines[i];
01160     drawLine(edgesImg, (Point2D<int>)ls.p1, (Point2D<int>)ls.p2, PixRGB<byte>(255,0,0), 1); //(int)ls.width);
01161   }
01162 
01163   SHOWIMG(edgesImg);
01164 
01165 }
01166 
01167 void V2::evolve(Image<float>& img)
01168 {
01169 
01170   //SHOWIMG(img);
01171   inplaceNormalize(img, 0.0F, 255.0F);
01172 
01173   itsLines = lineSegmentDetection(img);
01174 
01175   //Image<PixRGB<byte> > edgesImg(.getDims(), ZEROS);
01176   //for(uint i=0; i<lines.size(); i++)
01177   //{
01178   //  LineSegment& ls = lines[i];
01179   //  drawLine(edgesImg, (Point2D<int>)ls.p1, (Point2D<int>)ls.p2, PixRGB<byte>(255,0,0), 1); //(int)ls.width);
01180   //}
01181 
01182   //SHOWIMG(edgesImg);
01183 
01184 }
01185 
01186 
01187 void V2::evolve(TensorField& tensorField)
01188 {
01189   nonMaxSurp(tensorField);
01190 
01191   LINFO("Get LInes");
01192   std::vector<LineSegment> lines = lineSegmentDetection(tensorField);
01193   LINFO("Show %i Lines", (uint)lines.size());
01194 
01195   Image<PixRGB<byte> > edgesImg(tensorField.t1.getDims(), ZEROS);
01196   for(uint i=0; i<lines.size(); i++)
01197   {
01198     LineSegment& ls = lines[i];
01199     drawLine(edgesImg, (Point2D<int>)ls.p1, (Point2D<int>)ls.p2, PixRGB<byte>(255,0,0), 1); //(int)ls.width);
01200   }
01201 
01202   //SHOWIMG(edgesImg);
01203 
01204 }
01205 
01206 Layout<PixRGB<byte> > V2::getDebugImage()
01207 {
01208   Layout<PixRGB<byte> > outDisp;
01209 
01210   Image<float> input = itsLGNInput[0];
01211   //input = rescale(input, 320, 240);
01212   Image<PixRGB<byte> > linesLumImg = input;
01213   Image<PixRGB<byte> > linesImg(input.getDims(), ZEROS);
01214   for(uint i=0; i<itsLines.size(); i++)
01215   {
01216     LineSegment& ls = itsLines[i];
01217     if (ls.strength > 0)
01218     {
01219       PixRGB<byte> color;
01220       switch(ls.color)
01221       {
01222         case 0: color = PixRGB<byte>(255,0,0); break;
01223         case 1: color = PixRGB<byte>(0,255,0); break;
01224         case 2: color = PixRGB<byte>(0,0,255); break;
01225       }
01226 
01227       drawLine(linesLumImg, (Point2D<int>)ls.p1, (Point2D<int>)ls.p2,color, 1); //(int)ls.width);
01228       drawLine(linesImg, (Point2D<int>)ls.p1, (Point2D<int>)ls.p2, PixRGB<byte>(255,0,0)); 
01229     }
01230   }
01231   outDisp = hcat(linesLumImg, linesImg);
01232   EigenSpace eigen = getTensorEigen(itsMaxTF);
01233   Image<float> maxTf = eigen.l1-eigen.l2;
01234   inplaceNormalize(maxTf, 0.0F, 255.0F);
01235   Image<PixRGB<byte> > maxEdges = toRGB(maxTf);
01236   outDisp = hcat(outDisp, maxEdges);
01237 
01238   Layout<PixRGB<byte> > tensorDisp;
01239   Image<PixRGB<byte> > lumEdges = toRGB(itsLumTV.getTokensMag(true));
01240   Image<PixRGB<byte> > rgEdges = toRGB(itsRGTV.getTokensMag(true));
01241   Image<PixRGB<byte> > byEdges = toRGB(itsBYTV.getTokensMag(true));
01242 
01243   tensorDisp = hcat(lumEdges, rgEdges);
01244   tensorDisp = hcat(tensorDisp, byEdges);
01245 
01246   outDisp = vcat(outDisp, tensorDisp);
01247 
01248   return outDisp;
01249 
01250 }
01251 
01252 
01253 
01254 ///////////////////////////////////////////////////////////////////////// LSD computations ////////////////////////////////////////////////////
01255 /*----------------------------------------------------------------------------*/
01256 /*------------------------------ Gradient Angle ------------------------------*/
01257 /*----------------------------------------------------------------------------*/
01258 
01259 /*----------------------------------------------------------------------------*/
01260 /*
01261    compute the direction of the level line at each point.
01262    it returns:
01263 
01264    - an image_float with the angle at each pixel or NOTDEF.
01265    - the image_float 'modgrad' (a pointer is passed as argument)
01266      with the gradient magnitude at each point.
01267    - a list of pixels 'list_p' roughly ordered by gradient magnitude.
01268      (the order is made by classing points into bins by gradient magnitude.
01269       the parameters 'n_bins' and 'max_grad' specify the number of
01270       bins and the gradient modulus at the highest bin.)
01271    - a pointer 'mem_p' to the memory used by 'list_p' to be able to
01272      free the memory.
01273  */
01274 Image<float> V2::ll_angle(Image<float>& in, float threshold,
01275                              std::vector<Point2D<int> > &list_p, void ** mem_p,
01276                              Image<float>& modgrad, int n_bins, int max_grad )
01277 {
01278   Image<float> grad(in.getWidth(), in.getHeight(), ZEROS);
01279   int n,p,adr,i;
01280   float com1,com2,gx,gy,norm2;
01281   /* variables used in pseudo-ordering of gradient magnitude */
01282   float f_n_bins = (float) n_bins;
01283   float f_max_grad = (float) max_grad;
01284   int list_count = 0;
01285   struct coorlist * list;
01286   struct coorlist ** range_l_s;
01287   struct coorlist ** range_l_e;
01288   struct coorlist * start;
01289   struct coorlist * end;
01290 
01291 
01292   threshold *= 4.0 * threshold;
01293 
01294   n = in.getHeight();
01295   p = in.getWidth();
01296 
01297   /* get memory for the image of gradient modulus */
01298   modgrad = Image<float>(in.getWidth(),in.getHeight(), ZEROS);
01299 
01300   /* get memory for "ordered" coordinate list */
01301   list = (struct coorlist *) calloc(n*p,sizeof(struct coorlist));
01302   *mem_p = (void *) list;
01303   range_l_s = (struct coorlist **) calloc(n_bins,sizeof(struct coorlist *));
01304   range_l_e = (struct coorlist **) calloc(n_bins,sizeof(struct coorlist *));
01305   if( !list || !range_l_s || !range_l_e ) LFATAL("Not enough memory.");
01306   for(i=0;i<n_bins;i++) range_l_s[i] = range_l_e[i] = NULL;
01307 
01308   /* undefined on downright boundary */
01309   for(int x=0;x<p;x++) grad[(n-1)*p+x] = NOTDEF;
01310   for(int y=0;y<n;y++) grad[p*y+p-1]   = NOTDEF;
01311 
01312   /*** remaining part ***/
01313   for(int x=0;x<p-1;x++)
01314     for(int y=0;y<n-1;y++)
01315       {
01316         adr = y*p+x;
01317 
01318         /* norm 2 computation */
01319         com1 = in[adr+p+1] - in[adr];
01320         com2 = in[adr+1]   - in[adr+p];
01321         gx = com1+com2;
01322         gy = com1-com2;
01323         norm2 = gx*gx+gy*gy;
01324 
01325         modgrad[adr] = (float) sqrt( (double) norm2 / 4.0 );
01326 
01327         if(norm2 <= threshold) /* norm too small, gradient no defined */
01328           grad[adr] = NOTDEF;
01329         else
01330           {
01331             /* angle computation */
01332             grad[adr] = (float) atan2(gx,-gy);
01333 
01334             /* store the point in the right bin,
01335                according to its norm */
01336             i = (int) (norm2 * f_n_bins / f_max_grad);
01337             if(i>=n_bins) i = n_bins-1;
01338             if( range_l_e[i]==NULL )
01339               range_l_s[i] = range_l_e[i] = list+list_count++;
01340             else
01341               {
01342                 range_l_e[i]->next = list+list_count;
01343                 range_l_e[i] = list+list_count++;
01344               }
01345             range_l_e[i]->x = x;
01346             range_l_e[i]->y = y;
01347             range_l_e[i]->next = NULL;
01348           }
01349       }
01350 
01351   /* make the list of points "ordered" by norm value */
01352   for(i=n_bins-1; i>0 && range_l_s[i]==NULL; i--);
01353   start = range_l_s[i];
01354   end = range_l_e[i];
01355   if(start!=NULL)
01356     for(i--;i>0; i--)
01357       if( range_l_s[i] != NULL )
01358         {
01359           end->next = range_l_s[i];
01360           end = range_l_e[i];
01361         }
01362 
01363   struct coorlist * lp = start;
01364 
01365   for(;lp; lp = lp->next )
01366     list_p.push_back(Point2D<int>(lp->x, lp->y));
01367 
01368 
01369   /* free memory */
01370   free(range_l_s);
01371   free(range_l_e);
01372 
01373   return grad;
01374 }
01375 
01376 /*----------------------------------------------------------------------------*/
01377 /*------------------------------ Gradient Angle ------------------------------*/
01378 /*----------------------------------------------------------------------------*/
01379 
01380 /*----------------------------------------------------------------------------*/
01381 /*
01382    compute the direction of the level line at each point.
01383    it returns:
01384 
01385    - an image_float with the angle at each pixel or NOTDEF.
01386    - the image_float 'modgrad' (a pointer is passed as argument)
01387      with the gradient magnitude at each point.
01388    - a list of pixels 'list_p' roughly ordered by gradient magnitude.
01389      (the order is made by classing points into bins by gradient magnitude.
01390       the parameters 'n_bins' and 'max_grad' specify the number of
01391       bins and the gradient modulus at the highest bin.)
01392    - a pointer 'mem_p' to the memory used by 'list_p' to be able to
01393      free the memory.
01394  */
01395 Image<float> V2::ll_angle(const TensorField& tensorField, float threshold,
01396                              std::vector<Point2D<int> > &list_p,
01397                              Image<float>& modgrad, int n_bins, int max_grad )
01398 {
01399   Image<float> grad(tensorField.t1.getWidth(), tensorField.t1.getHeight(), ZEROS);
01400   int n,p,i;
01401   //float com1,com2,gx,gy,norm2;
01402   /* variables used in pseudo-ordering of gradient magnitude */
01403   float f_n_bins = (float) n_bins;
01404   float f_max_grad = (float) max_grad;
01405   int list_count = 0;
01406   struct coorlist * list;
01407   struct coorlist ** range_l_s;
01408   struct coorlist ** range_l_e;
01409   struct coorlist * start;
01410   struct coorlist * end;
01411 
01412 
01413   n = tensorField.t1.getHeight();
01414   p = tensorField.t1.getWidth();
01415 
01416   /* get memory for the image of gradient modulus */
01417   modgrad = Image<float>(tensorField.t1.getWidth(),tensorField.t1.getHeight(), ZEROS);
01418 
01419   /* get memory for "ordered" coordinate list */
01420   list = (struct coorlist *) calloc(n*p,sizeof(struct coorlist));
01421   range_l_s = (struct coorlist **) calloc(n_bins,sizeof(struct coorlist *));
01422   range_l_e = (struct coorlist **) calloc(n_bins,sizeof(struct coorlist *));
01423   if( !list || !range_l_s || !range_l_e ) LFATAL("Not enough memory.");
01424 
01425   for(i=0;i<n_bins;i++)
01426     range_l_s[i] = range_l_e[i] = NULL;
01427 
01428   EigenSpace eigen = getTensorEigen(tensorField);
01429   Image<float> features = eigen.l1-eigen.l2;
01430   inplaceNormalize(features, 0.0F, f_max_grad);
01431 
01432   for(int y=0; y<features.getHeight(); y++)
01433     for(int x=0; x<features.getWidth(); x++)
01434     {
01435       /* norm 2 computation */
01436       //double norm2 = tensorField.t1.getVal(x,y) + tensorField.t4.getVal(x,y);
01437       //modgrad.setVal(x,y, (float) sqrt( (double) norm2 / 4.0 ));
01438       //float norm2 = eigen.l1.getVal(x,y) - eigen.l2.getVal(x,y);
01439       float val = features.getVal(x,y);
01440       modgrad.setVal(x,y, val);
01441 
01442       if(val > threshold) /* norm too small, gradient no defined */
01443       {
01444         /* angle computation */
01445 
01446         //Get the direction of the vote from e1, while the weight is l1-l2
01447         float u = eigen.e1[1].getVal(x,y);
01448         float v = eigen.e1[0].getVal(x,y);
01449         grad.setVal(x,y, atan(-u/v));
01450 
01451         /* store the point in the right bin,
01452            according to its norm */
01453         i = (int) (val * f_n_bins / f_max_grad);
01454         if(i>=n_bins) i = n_bins-1;
01455         if( range_l_e[i]==NULL )
01456           range_l_s[i] = range_l_e[i] = list+list_count++;
01457         else
01458         {
01459           range_l_e[i]->next = list+list_count;
01460           range_l_e[i] = list+list_count++;
01461         }
01462         range_l_e[i]->x = x;
01463         range_l_e[i]->y = y;
01464         range_l_e[i]->next = NULL;
01465       } else {
01466         grad.setVal(x,y,NOTDEF);
01467       }
01468   }
01469 
01470   /* make the list of points "ordered" by norm value */
01471   for(i=n_bins-1; i>0 && range_l_s[i]==NULL; i--);
01472   start = range_l_s[i];
01473   end = range_l_e[i];
01474   if(start!=NULL)
01475     for(i--;i>0; i--)
01476       if( range_l_s[i] != NULL )
01477         {
01478           end->next = range_l_s[i];
01479           end = range_l_e[i];
01480         }
01481 
01482   struct coorlist * lp = start;
01483 
01484   for(;lp; lp = lp->next )
01485     list_p.push_back(Point2D<int>(lp->x, lp->y));
01486 
01487   /* free memory */
01488   free(range_l_s);
01489   free(range_l_e);
01490   free(list);
01491 
01492   return grad;
01493 }
01494 
01495 /*----------------------------------------------------------------------------*/
01496 /*
01497    find if the point x,y in angles have angle theta up to precision prec
01498  */
01499 bool V2::isaligned(Point2D<int> loc,const Image<float>& angles,
01500                    float theta, float prec)
01501 {
01502   if (!angles.coordsOk(loc)) return false;
01503 
01504   float a = angles.getVal(loc);
01505 
01506   if( a == NOTDEF ) return false;
01507 
01508   /* it is assumed that theta and a are in the range [-pi,pi] */
01509   theta -= a;
01510   if( theta < 0.0 ) theta = -theta;
01511   if( theta > M_3_2_PI )
01512     {
01513       theta -= M_2__PI;
01514       if( theta < 0.0 ) theta = -theta;
01515     }
01516 
01517   //LINFO("Theta %f < %f", theta, prec);
01518   return theta < prec;
01519 }
01520 
01521 /*----------------------------------------------------------------------------*/
01522 float V2::angle_diff(float a, float b)
01523 {
01524   a -= b;
01525   while( a <= -M_PI ) a += 2.0*M_PI;
01526   while( a >   M_PI ) a -= 2.0*M_PI;
01527   if( a < 0.0 ) a = -a;
01528   return a;
01529 }
01530 
01531 
01532 
01533 /*----------------------------------------------------------------------------*/
01534 /*----------------------------- NFA computation ------------------------------*/
01535 /*----------------------------------------------------------------------------*/
01536 
01537 /*----------------------------------------------------------------------------*/
01538 /*
01539    Calculates the natural logarithm of the absolute value of
01540    the gamma function of x using the Lanczos approximation,
01541    see http://www.rskey.org/gamma.htm.
01542 
01543    The formula used is
01544      \Gamma(x) = \frac{ \sum_{n=0}^{N} q_n x^n }{ \Pi_{n=0}^{N} (x+n) }
01545                  (x+5.5)^(x+0.5) e^{-(x+5.5)}
01546    so
01547      \log\Gamma(x) = \log( \sum_{n=0}^{N} q_n x^n ) + (x+0.5) \log(x+5.5)
01548                      - (x+5.5) - \sum_{n=0}^{N} \log(x+n)
01549    and
01550      q0 = 75122.6331530
01551      q1 = 80916.6278952
01552      q2 = 36308.2951477
01553      q3 = 8687.24529705
01554      q4 = 1168.92649479
01555      q5 = 83.8676043424
01556      q6 = 2.50662827511
01557  */
01558 double V2::log_gamma_lanczos(double x)
01559 {
01560   static double q[7] = { 75122.6331530, 80916.6278952, 36308.2951477,
01561                          8687.24529705, 1168.92649479, 83.8676043424,
01562                          2.50662827511 };
01563   double a = (x+0.5) * log(x+5.5) - (x+5.5);
01564   double b = 0.0;
01565   int n;
01566 
01567   for(n=0;n<7;n++)
01568     {
01569       a -= log( x + (double) n );
01570       b += q[n] * pow( x, (double) n );
01571     }
01572   return a + log(b);
01573 }
01574 
01575 /*----------------------------------------------------------------------------*/
01576 /*
01577    Calculates the natural logarithm of the absolute value of
01578    the gamma function of x using Robert H. Windschitl method,
01579    see http://www.rskey.org/gamma.htm.
01580 
01581    The formula used is
01582      \Gamma(x) = \sqrt(\frac{2\pi}{x}) ( \frac{x}{e}
01583                    \sqrt{ x\sinh(1/x) + \frac{1}{810x^6} } )^x
01584    so
01585      \log\Gamma(x) = 0.5\log(2\pi) + (x-0.5)\log(x) - x
01586                      + 0.5x\log( x\sinh(1/x) + \frac{1}{810x^6} ).
01587 
01588    This formula is good approximation when x > 15.
01589  */
01590 double V2::log_gamma_windschitl(double x)
01591 {
01592   return 0.918938533204673 + (x-0.5)*log(x) - x
01593          + 0.5*x*log( x*sinh(1/x) + 1/(810.0*pow(x,6.0)) );
01594 }
01595 
01596 /*----------------------------------------------------------------------------*/
01597 /*
01598    Calculates the natural logarithm of the absolute value of
01599    the gamma function of x. When x>15 use log_gamma_windschitl(),
01600    otherwise use log_gamma_lanczos().
01601  */
01602 #define log_gamma(x) ((x)>15.0?log_gamma_windschitl(x):log_gamma_lanczos(x))
01603 
01604 /*----------------------------------------------------------------------------*/
01605 /*
01606    Computes the logarithm of NFA to base 10.
01607 
01608    NFA = NT.b(n,k,p)
01609    the return value is log10(NFA)
01610 
01611    n,k,p - binomial parameters.
01612    logNT - logarithm of Number of Tests
01613  */
01614 #define TABSIZE 100000
01615 double V2::nfa(int n, int k, double p, double logNT)
01616 {
01617   static double inv[TABSIZE];   /* table to keep computed inverse values */
01618   double tolerance = 0.1;       /* an error of 10% in the result is accepted */
01619   double log1term,term,bin_term,mult_term,bin_tail,err;
01620   double p_term = p / (1.0-p);
01621   int i;
01622 
01623   if( n<0 || k<0 || k>n || p<0.0 || p>1.0 )
01624     LFATAL("Wrong n=%i, k=%i or p=%f values in nfa()", n, k, p);
01625 
01626   if( n==0 || k==0 ) return -logNT;
01627   if( n==k ) return -logNT - (double) n * log10(p);
01628 
01629   log1term = log_gamma((double)n+1.0) - log_gamma((double)k+1.0)
01630            - log_gamma((double)(n-k)+1.0)
01631            + (double) k * log(p) + (double) (n-k) * log(1.0-p);
01632 
01633   term = exp(log1term);
01634   if( double_equal(term,0.0) )              /* the first term is almost zero */
01635     {
01636       if( (double) k > (double) n * p )    /* at begin or end of the tail? */
01637         return -log1term / M_LN10 - logNT; /* end: use just the first term */
01638       else
01639         return -logNT;                     /* begin: the tail is roughly 1 */
01640     }
01641 
01642   bin_tail = term;
01643   for(i=k+1;i<=n;i++)
01644     {
01645       bin_term = (double) (n-i+1) * ( i<TABSIZE ?
01646                    ( inv[i] != 0.0 ? inv[i] : (inv[i]=1.0/(double)i))
01647                    : 1.0/(double)i );
01648       mult_term = bin_term * p_term;
01649       term *= mult_term;
01650       bin_tail += term;
01651       if(bin_term<1.0)
01652         {
01653           /* when bin_term<1 then mult_term_j<mult_term_i for j>i.
01654              then, the error on the binomial tail when truncated at
01655              the i term can be bounded by a geometric series of form
01656              term_i * sum mult_term_i^j.                            */
01657           err = term * ( ( 1.0 - pow( mult_term, (double) (n-i+1) ) ) /
01658                          (1.0-mult_term) - 1.0 );
01659 
01660           /* one wants an error at most of tolerance*final_result, or:
01661              tolerance * abs(-log10(bin_tail)-logNT).
01662              now, the error that can be accepted on bin_tail is
01663              given by tolerance*final_result divided by the derivative
01664              of -log10(x) when x=bin_tail. that is:
01665              tolerance * abs(-log10(bin_tail)-logNT) / (1/bin_tail)
01666              finally, we truncate the tail if the error is less than:
01667              tolerance * abs(-log10(bin_tail)-logNT) * bin_tail        */
01668           if( err < tolerance * fabs(-log10(bin_tail)-logNT) * bin_tail ) break;
01669         }
01670     }
01671   return -log10(bin_tail) - logNT;
01672 }
01673 
01674 
01675 
01676 //double V2::nfa(int n, int k, double p, double logNT)
01677 //{
01678 //  double N1[2] = {1.0337965773779e-06,   -0.00215083485137987};
01679 //  double N2[2] = {-8.53870284101796e-06,  0.0273185078224109};
01680 //  double N3[2] = {0.000230591970586356,  -0.828902881766958};
01681 //  double N4[2] = {-0.000233656573018064,  0.719832739194112};
01682 //
01683 //  double F1 = N1[1]*n + N1[0];
01684 //  double F2 = N2[1]*n + N2[0];
01685 //  double F3 = N3[1]*n + N3[0];
01686 //  double F4 = N4[1]*n + N4[0];
01687 //
01688 //
01689 //  double prob = F1*k*k*k + F2*k*k + F3*k + F4;
01690 //
01691 //  return -log10(exp(prob)) - logNT;
01692 //}
01693 
01694 
01695 
01696 /*----------------------------------------------------------------------------*/
01697 void V2::rect_copy(struct rect * in, struct rect * out)
01698 {
01699   out->x1 = in->x1;
01700   out->y1 = in->y1;
01701   out->x2 = in->x2;
01702   out->y2 = in->y2;
01703   out->width = in->width;
01704   out->x = in->x;
01705   out->y = in->y;
01706   out->theta = in->theta;
01707   out->dx = in->dx;
01708   out->dy = in->dy;
01709   out->prec = in->prec;
01710   out->p = in->p;
01711 }
01712 
01713 
01714 
01715 /*----------------------------------------------------------------------------*/
01716 float V2::inter_low(float x, float x1, float y1, float x2, float y2)
01717 {
01718   if( x1 > x2 || x < x1 || x > x2 )
01719     {
01720       LFATAL("inter_low: x %g x1 %g x2 %g.\n",x,x1,x2);
01721       LFATAL("Impossible situation.");
01722     }
01723   if( x1 == x2 && y1<y2 ) return y1;
01724   if( x1 == x2 && y1>y2 ) return y2;
01725   return y1 + (x-x1) * (y2-y1) / (x2-x1);
01726 }
01727 
01728 /*----------------------------------------------------------------------------*/
01729 float V2::inter_hi(float x, float x1, float y1, float x2, float y2)
01730 {
01731   if( x1 > x2 || x < x1 || x > x2 )
01732     {
01733       LFATAL("inter_hi: x %g x1 %g x2 %g.\n",x,x1,x2);
01734       LFATAL("Impossible situation.");
01735     }
01736   if( x1 == x2 && y1<y2 ) return y2;
01737   if( x1 == x2 && y1>y2 ) return y1;
01738   return y1 + (x-x1) * (y2-y1) / (x2-x1);
01739 }
01740 
01741 /*----------------------------------------------------------------------------*/
01742 void V2::ri_del(rect_iter * iter)
01743 {
01744   free(iter);
01745 }
01746 
01747 /*----------------------------------------------------------------------------*/
01748 int V2::ri_end(rect_iter * i)
01749 {
01750   return (float)(i->x) > i->vx[2];
01751 }
01752 
01753 int V2::ri_end(rect_iter& itr)
01754 {
01755   return (float)(itr.x) > itr.vx[2];
01756 }
01757 
01758 /*----------------------------------------------------------------------------*/
01759 void V2::ri_inc(rect_iter * i)
01760 {
01761   if( (float) (i->x) <= i->vx[2] ) i->y++;
01762 
01763   while( (float) (i->y) > i->ye && (float) (i->x) <= i->vx[2] )
01764     {
01765       /* new x */
01766       i->x++;
01767 
01768       if( (float) (i->x) > i->vx[2] ) return; /* end of iteration */
01769 
01770       /* update lower y limit for the line */
01771       if( (float) i->x < i->vx[3] )
01772         i->ys = inter_low((float)i->x,i->vx[0],i->vy[0],i->vx[3],i->vy[3]);
01773       else i->ys = inter_low((float)i->x,i->vx[3],i->vy[3],i->vx[2],i->vy[2]);
01774 
01775       /* update upper y limit for the line */
01776       if( (float)i->x < i->vx[1] )
01777         i->ye = inter_hi((float)i->x,i->vx[0],i->vy[0],i->vx[1],i->vy[1]);
01778       else i->ye = inter_hi( (float)i->x,i->vx[1],i->vy[1],i->vx[2],i->vy[2]);
01779 
01780       /* new y */
01781       i->y = (int)((float) ceil( (double) i->ys ));
01782     }
01783 }
01784 
01785 /*----------------------------------------------------------------------------*/
01786 void V2::ri_inc(rect_iter& itr)
01787 {
01788   if( (float) (itr.x) <= itr.vx[2] ) itr.y++;
01789 
01790   while( (float) (itr.y) > itr.ye && (float) (itr.x) <= itr.vx[2] )
01791     {
01792       /* new x */
01793       itr.x++;
01794 
01795       if( (float) (itr.x) > itr.vx[2] ) return; /* end of iteration */
01796 
01797       /* update lower y limit for the line */
01798       if( (float) itr.x < itr.vx[3] )
01799         itr.ys = inter_low((float)itr.x,itr.vx[0],itr.vy[0],itr.vx[3],itr.vy[3]);
01800       else itr.ys = inter_low((float)itr.x,itr.vx[3],itr.vy[3],itr.vx[2],itr.vy[2]);
01801 
01802       /* update upper y limit for the line */
01803       if( (float)itr.x < itr.vx[1] )
01804         itr.ye = inter_hi((float)itr.x,itr.vx[0],itr.vy[0],itr.vx[1],itr.vy[1]);
01805       else itr.ye = inter_hi( (float)itr.x,itr.vx[1],itr.vy[1],itr.vx[2],itr.vy[2]);
01806 
01807       /* new y */
01808       itr.y = (int)((float) ceil( (double) itr.ys ));
01809     }
01810 }
01811 
01812 
01813 /*----------------------------------------------------------------------------*/
01814 V2::rect_iter * V2::ri_ini(struct rect * r)
01815 {
01816   float vx[4],vy[4];
01817   int n,offset;
01818   rect_iter * i;
01819 
01820   i = (rect_iter *) malloc(sizeof(rect_iter));
01821   if(!i) LFATAL("ri_ini: Not enough memory.");
01822 
01823   vx[0] = r->x1 - r->dy * r->width / 2.0;
01824   vy[0] = r->y1 + r->dx * r->width / 2.0;
01825   vx[1] = r->x2 - r->dy * r->width / 2.0;
01826   vy[1] = r->y2 + r->dx * r->width / 2.0;
01827   vx[2] = r->x2 + r->dy * r->width / 2.0;
01828   vy[2] = r->y2 - r->dx * r->width / 2.0;
01829   vx[3] = r->x1 + r->dy * r->width / 2.0;
01830   vy[3] = r->y1 - r->dx * r->width / 2.0;
01831 
01832   if( r->x1 < r->x2 && r->y1 <= r->y2 ) offset = 0;
01833   else if( r->x1 >= r->x2 && r->y1 < r->y2 ) offset = 1;
01834   else if( r->x1 > r->x2 && r->y1 >= r->y2 ) offset = 2;
01835   else offset = 3;
01836   /* else if( r->x1 <= r->x2 && r->y1 > r->y2 ) offset = 3; */
01837 
01838   for(n=0; n<4; n++)
01839     {
01840       i->vx[n] = vx[(offset+n)%4];
01841       i->vy[n] = vy[(offset+n)%4];
01842     }
01843 
01844   /* starting point */
01845   i->x = (int)(ceil( (double) (i->vx[0]) ) - 1);
01846   i->y = (int)(ceil( (double) (i->vy[0]) ));
01847   i->ys = i->ye = -BIG_NUMBER;
01848 
01849   /* advance to the first point */
01850   ri_inc(i);
01851 
01852   return i;
01853 }
01854 
01855 /*----------------------------------------------------------------------------*/
01856 void V2::ri_ini(struct rect * r, rect_iter& itr)
01857 {
01858   float vx[4],vy[4];
01859   int n,offset;
01860 
01861   vx[0] = r->x1 - r->dy * r->width / 2.0;
01862   vy[0] = r->y1 + r->dx * r->width / 2.0;
01863   vx[1] = r->x2 - r->dy * r->width / 2.0;
01864   vy[1] = r->y2 + r->dx * r->width / 2.0;
01865   vx[2] = r->x2 + r->dy * r->width / 2.0;
01866   vy[2] = r->y2 - r->dx * r->width / 2.0;
01867   vx[3] = r->x1 + r->dy * r->width / 2.0;
01868   vy[3] = r->y1 - r->dx * r->width / 2.0;
01869 
01870   if( r->x1 < r->x2 && r->y1 <= r->y2 ) offset = 0;
01871   else if( r->x1 >= r->x2 && r->y1 < r->y2 ) offset = 1;
01872   else if( r->x1 > r->x2 && r->y1 >= r->y2 ) offset = 2;
01873   else offset = 3;
01874   /* else if( r->x1 <= r->x2 && r->y1 > r->y2 ) offset = 3; */
01875 
01876   for(n=0; n<4; n++)
01877     {
01878       itr.vx[n] = vx[(offset+n)%4];
01879       itr.vy[n] = vy[(offset+n)%4];
01880     }
01881 
01882   /* starting point */
01883   itr.x = (int)(ceil( (double) (itr.vx[0]) ) - 1);
01884   itr.y = (int)(ceil( (double) (itr.vy[0]) ));
01885   itr.ys = itr.ye = -BIG_NUMBER;
01886 
01887   /* advance to the first point */
01888   ri_inc(itr);
01889 }
01890 
01891 /*----------------------------------------------------------------------------*/
01892 double V2::rect_nfa(struct rect * rec, Image<float>& angles, double logNT)
01893 {
01894   rect_iter * i;
01895   int pts = 0;
01896   int alg = 0;
01897   double nfa_val;
01898 
01899   for(i=ri_ini(rec); !ri_end(i); ri_inc(i))
01900     if( i->x>=0 && i->y>=0 && i->x<angles.getWidth() && i->y<angles.getHeight() )
01901     {
01902       if(itsLSDVerbose) LINFO("| %d %d ",i->x,i->y);
01903       ++pts;
01904       if( isaligned(Point2D<int>(i->x,i->y),angles,rec->theta,rec->prec) )
01905         ++alg;
01906     }
01907   ri_del(i);
01908 
01909   nfa_val = nfa(pts,alg,rec->p,logNT);
01910   if(itsLSDVerbose) LINFO("\npts %d alg %d p %g nfa %g\n",
01911                       pts,alg,rec->p,nfa_val);
01912 
01913   return nfa_val;
01914 }
01915 
01916 
01917 /*----------------------------------------------------------------------------*/
01918 /*---------------------------------- REGIONS ---------------------------------*/
01919 /*----------------------------------------------------------------------------*/
01920 
01921 /*----------------------------------------------------------------------------*/
01922 float V2::get_theta( struct point * reg, int reg_size, float x, float y,
01923                         Image<float>& modgrad, float reg_angle, float prec,
01924                         float * elongation )
01925 {
01926   int i;
01927   float Ixx = 0.0;
01928   float Iyy = 0.0;
01929   float Ixy = 0.0;
01930   float lambda1,lambda2,tmp;
01931   float theta;
01932   float weight,sum;
01933 
01934   if(reg_size <= 1) LFATAL("get_theta: region size <= 1.");
01935 
01936 
01937   /*----------- theta ---------------------------------------------------*/
01938   /*
01939       Region inertia matrix A:
01940          Ixx Ixy
01941          Ixy Iyy
01942       where
01943         Ixx = \sum_i y_i^2
01944         Iyy = \sum_i x_i^2
01945         Ixy = -\sum_i x_i y_i
01946 
01947       lambda1 and lambda2 are the eigenvalues, with lambda1 >= lambda2.
01948       They are found by solving the characteristic polynomial
01949       det(\lambda I - A) = 0.
01950 
01951       To get the line segment direction we want to get the eigenvector of
01952       the smaller eigenvalue. We have to solve a,b in:
01953         a.Ixx + b.Ixy = a.lambda2
01954         a.Ixy + b.Iyy = b.lambda2
01955       We want the angle theta = atan(b/a). I can be computed with
01956       any of the two equations:
01957         theta = atan( (lambda2-Ixx) / Ixy )
01958       or
01959         theta = atan( Ixy / (lambda2-Iyy) )
01960 
01961       When |Ixx| > |Iyy| we use the first, otherwise the second
01962       (just to get better numeric precision).
01963    */
01964   sum = 0.0;
01965   for(i=0; i<reg_size; i++)
01966     {
01967       weight = modgrad[ reg[i].x + reg[i].y * modgrad.getWidth() ];
01968       Ixx += ((float)reg[i].y - y) * ((float)reg[i].y - y) * weight;
01969       Iyy += ((float)reg[i].x - x) * ((float)reg[i].x - x) * weight;
01970       Ixy -= ((float)reg[i].x - x) * ((float)reg[i].y - y) * weight;
01971       sum += weight;
01972     }
01973   if( sum <= 0.0 ) LFATAL("get_theta: weights sum less or equal to zero.");
01974   Ixx /= sum;
01975   Iyy /= sum;
01976   Ixy /= sum;
01977   lambda1 = ( Ixx + Iyy + (float) sqrt( (double) (Ixx - Iyy) * (Ixx - Iyy)
01978                                         + 4.0 * Ixy * Ixy ) ) / 2.0;
01979   lambda2 = ( Ixx + Iyy - (float) sqrt( (double) (Ixx - Iyy) * (Ixx - Iyy)
01980                                         + 4.0 * Ixy * Ixy ) ) / 2.0;
01981   if( fabs(lambda1) < fabs(lambda2) )
01982     {
01983       fprintf(stderr,"Ixx %g Iyy %g Ixy %g lamb1 %g lamb2 %g - lamb1 < lamb2\n",
01984                       Ixx,Iyy,Ixy,lambda1,lambda2);
01985       tmp = lambda1;
01986       lambda1 = lambda2;
01987       lambda2 = tmp;
01988     }
01989   if(itsLSDVerbose) LINFO("Ixx %g Iyy %g Ixy %g lamb1 %g lamb2 %g\n",
01990                       Ixx,Iyy,Ixy,lambda1,lambda2);
01991 
01992   *elongation = lambda1/lambda2;
01993 
01994   if( fabs(Ixx) > fabs(Iyy) )
01995     theta = (float) atan2( (double) lambda2 - Ixx, (double) Ixy );
01996   else
01997     theta = (float) atan2( (double) Ixy, (double) lambda2 - Iyy );
01998 
01999   /* the previous procedure don't cares orientations,
02000      so it could be wrong by 180 degrees.
02001      here is corrected if necessary */
02002   if( angle_diff(theta,reg_angle) > prec ) theta += M_PI;
02003 
02004   return theta;
02005 }
02006 
02007 /*----------------------------------------------------------------------------*/
02008 float V2::region2rect( struct point * reg, int reg_size,
02009                           Image<float>& modgrad, float reg_angle,
02010                           float prec, double p, struct rect * rec,
02011                           float* sum_l, float* sum_w, int sum_offset, int sum_res)
02012 {
02013   float x,y,dx,dy,l,w,lf,lb,wl,wr,theta,weight,sum,sum_th,s,elongation;
02014   int i,n;
02015   int l_min,l_max,w_min,w_max;
02016 
02017   /* center */
02018   x = y = sum = 0.0;
02019   for(i=0; i<reg_size; i++)
02020     {
02021       weight = modgrad[ reg[i].x + reg[i].y * modgrad.getWidth() ];
02022       x += (float) reg[i].x * weight;
02023       y += (float) reg[i].y * weight;
02024       sum += weight;
02025     }
02026   if( sum <= 0.0 ) LFATAL("region2rect: weights sum equal to zero.");
02027   x /= sum;
02028   y /= sum;
02029   if(itsLSDVerbose) LINFO("center x %g y %g\n",x,y);
02030 
02031   /* theta */
02032   theta = get_theta(reg,reg_size,x,y,modgrad,reg_angle,prec,&elongation);
02033   if(itsLSDVerbose) LINFO("theta %g\n",theta);
02034 
02035   /* length and width */
02036   lf = lb = wl = wr = 0.5;
02037   l_min = l_max = w_min = w_max = 0;
02038   dx = (float) cos( (double) theta );
02039   dy = (float) sin( (double) theta );
02040   for(i=0; i<reg_size; i++)
02041     {
02042       l =  ((float)reg[i].x - x) * dx + ((float)reg[i].y - y) * dy;
02043       w = -((float)reg[i].x - x) * dy + ((float)reg[i].y - y) * dx;
02044       weight = modgrad[ reg[i].x + reg[i].y * modgrad.getWidth() ];
02045 
02046       n = (int) MY_ROUND( l * (float) sum_res );
02047       if(n<l_min) l_min = n;
02048       if(n>l_max) l_max = n;
02049       sum_l[sum_offset + n] += weight;
02050 
02051       n = (int) MY_ROUND( w * (float) sum_res );
02052       if(n<w_min) w_min = n;
02053       if(n>w_max) w_max = n;
02054       sum_w[sum_offset + n] += weight;
02055     }
02056 
02057   sum_th = 0.01 * sum; /* weight threshold for selecting region */
02058   for(s=0.0,i=l_min; s<sum_th && i<=l_max; i++) s += sum_l[sum_offset + i];
02059   lb = ( (float) (i-1) - 0.5 ) / (float) sum_res;
02060   for(s=0.0,i=l_max; s<sum_th && i>=l_min; i--) s += sum_l[sum_offset + i];
02061   lf = ( (float) (i+1) + 0.5 ) / (float) sum_res;
02062 
02063   sum_th = 0.01 * sum; /* weight threshold for selecting region */
02064   for(s=0.0,i=w_min; s<sum_th && i<=w_max; i++) s += sum_w[sum_offset + i];
02065   wr = ( (float) (i-1) - 0.5 ) / (float) sum_res;
02066   for(s=0.0,i=w_max; s<sum_th && i>=w_min; i--) s += sum_w[sum_offset + i];
02067   wl = ( (float) (i+1) + 0.5 ) / (float) sum_res;
02068 
02069   if(itsLSDVerbose) LINFO("lb %g lf %g wr %g wl %g\n",lb,lf,wr,wl);
02070 
02071   /* free vector */
02072   for(i=l_min; i<=l_max; i++) sum_l[sum_offset + i] = 0.0;
02073   for(i=w_min; i<=w_max; i++) sum_w[sum_offset + i] = 0.0;
02074 
02075   rec->x1 = x + lb * dx;
02076   rec->y1 = y + lb * dy;
02077   rec->x2 = x + lf * dx;
02078   rec->y2 = y + lf * dy;
02079   rec->width = wl - wr;
02080   rec->x = x;
02081   rec->y = y;
02082   rec->theta = theta;
02083   rec->dx = dx;
02084   rec->dy = dy;
02085   rec->prec = prec;
02086   rec->p = p;
02087   rec->sum = sum;
02088 
02089   if( rec->width < 1.0 ) rec->width = 1.0;
02090 
02091   return elongation;
02092 }
02093 
02094 /*----------------------------------------------------------------------------*/
02095 void V2::region_grow(Point2D<int> loc, Image<float>& angles, struct point * reg,
02096                          int * reg_size, float * reg_angle, Image<byte>& used,
02097                          float prec, int radius,
02098                          Image<float> modgrad, double p, int min_reg_size )
02099 {
02100   float sumdx,sumdy;
02101   int xx,yy,i;
02102 
02103   /* first point of the region */
02104   *reg_size = 1;
02105   reg[0].x = loc.i;
02106   reg[0].y = loc.j;
02107   *reg_angle = angles.getVal(loc);
02108   sumdx = (float) cos( (double) (*reg_angle) );
02109   sumdy = (float) sin( (double) (*reg_angle) );
02110   used.setVal(loc, USED);
02111 
02112   /* try neighbors as new region points */
02113   //LINFO("Grow point %ix%i", loc.i, loc.j);
02114   //Image<PixRGB<byte> > tmp = modgrad;
02115 
02116   for(i=0; i<*reg_size; i++)
02117   {
02118     for(xx=reg[i].x-radius; xx<=reg[i].x+radius; xx++)
02119       for(yy=reg[i].y-radius; yy<=reg[i].y+radius; yy++)
02120       {
02121         if( xx>=0 && yy>=0 && xx<used.getWidth() && yy<used.getHeight() &&
02122             used[xx+yy*used.getWidth()] != USED)
02123         {
02124           //LINFO("Aligned %f %f = %i", *reg_angle, prec,
02125           //    isaligned(Point2D<int>(xx,yy),angles,*reg_angle,prec) );
02126           if ( isaligned(Point2D<int>(xx,yy),angles,*reg_angle,prec) )
02127           {
02128             /* add point */
02129 
02130             //tmp.setVal(xx,yy, PixRGB<byte>(255, 0,0));
02131             //SHOWIMG(tmp);
02132 
02133             used[xx+yy*used.getWidth()] = USED;
02134             reg[*reg_size].x = xx;
02135             reg[*reg_size].y = yy;
02136             ++(*reg_size);
02137 
02138             /* update reg_angle */
02139             sumdx += (float) cos( (double) angles[xx+yy*angles.getWidth()] );
02140             sumdy += (float) sin( (double) angles[xx+yy*angles.getWidth()] );
02141             *reg_angle = (float) atan2( (double) sumdy, (double) sumdx );
02142           }
02143         }
02144       }
02145   }
02146   //if (*reg_size > 5)
02147   //  SHOWIMG(tmp);
02148 
02149   if(itsLSDVerbose) /* print region points */
02150     {
02151       LINFO("region found: %d points\n",*reg_size);
02152       for(i=0; i<*reg_size; i++) fprintf(stderr,"| %d %d ",reg[i].x,reg[i].y);
02153       fprintf(stderr,"\n");
02154     }
02155 }
02156 
02157 /*----------------------------------------------------------------------------*/
02158 double V2::rect_improve( struct rect * rec, Image<float>& angles,
02159                             double logNT, double eps )
02160 {
02161   struct rect r;
02162   double nfa_val,nfa_new;
02163   float delta = 0.5;
02164   float delta_2 = delta / 2.0;
02165   int n;
02166 
02167   nfa_val = rect_nfa(rec,angles,logNT);
02168 
02169   if( nfa_val > eps ) return nfa_val;
02170 
02171   rect_copy(rec,&r);
02172   for(n=0; n<5; n++)
02173     {
02174       r.p /= 2.0;
02175       r.prec = M_PI * r.p;
02176       nfa_new = rect_nfa(&r,angles,logNT);
02177       if( nfa_new > nfa_val )
02178         {
02179           nfa_val = nfa_new;
02180           rect_copy(&r,rec);
02181         }
02182     }
02183 
02184   if( nfa_val > eps ) return nfa_val;
02185 
02186   rect_copy(rec,&r);
02187   for(n=0; n<5; n++)
02188     {
02189       if( (r.width - delta) >= 0.5 )
02190         {
02191           r.width -= delta;
02192           nfa_new = rect_nfa(&r,angles,logNT);
02193           if( nfa_new > nfa_val )
02194             {
02195               rect_copy(&r,rec);
02196               nfa_val = nfa_new;
02197             }
02198         }
02199     }
02200 
02201   if( nfa_val > eps ) return nfa_val;
02202 
02203   rect_copy(rec,&r);
02204   for(n=0; n<5; n++)
02205     {
02206       if( (r.width - delta) >= 0.5 )
02207         {
02208           r.x1 += -r.dy * delta_2;
02209           r.y1 +=  r.dx * delta_2;
02210           r.x2 += -r.dy * delta_2;
02211           r.y2 +=  r.dx * delta_2;
02212           r.width -= delta;
02213           nfa_new = rect_nfa(&r,angles,logNT);
02214           if( nfa_new > nfa_val )
02215             {
02216               rect_copy(&r,rec);
02217               nfa_val = nfa_new;
02218             }
02219         }
02220     }
02221 
02222   if( nfa_val > eps ) return nfa_val;
02223 
02224   rect_copy(rec,&r);
02225   for(n=0; n<5; n++)
02226     {
02227       if( (r.width - delta) >= 0.5 )
02228         {
02229           r.x1 -= -r.dy * delta_2;
02230           r.y1 -=  r.dx * delta_2;
02231           r.x2 -= -r.dy * delta_2;
02232           r.y2 -=  r.dx * delta_2;
02233           r.width -= delta;
02234           nfa_new = rect_nfa(&r,angles,logNT);
02235           if( nfa_new > nfa_val )
02236             {
02237               rect_copy(&r,rec);
02238               nfa_val = nfa_new;
02239             }
02240         }
02241     }
02242 
02243   if( nfa_val > eps ) return nfa_val;
02244 
02245   rect_copy(rec,&r);
02246   for(n=0; n<5; n++)
02247     {
02248       r.p /= 2.0;
02249       r.prec = M_PI * r.p;
02250       nfa_new = rect_nfa(&r,angles,logNT);
02251       if( nfa_new > nfa_val )
02252         {
02253           nfa_val = nfa_new;
02254           rect_copy(&r,rec);
02255         }
02256     }
02257 
02258   return nfa_val;
02259 }
02260 
02261 
02262 /*----------------------------------------------------------------------------*/
02263 /*-------------------------- LINE SEGMENT DETECTION --------------------------*/
02264 /*----------------------------------------------------------------------------*/
02265 
02266 /*----------------------------------------------------------------------------*/
02267 std::vector<V2::LineSegment> V2::lineSegmentDetection(Image<float>& img, float q, float d, double eps,
02268                                   int n_bins, int max_grad, float scale, float sigma_scale )
02269 {
02270 
02271   float rho = q / (float) sin( M_PI / (double) d );   /* gradient threshold */
02272   std::vector<LineSegment> lines;
02273 
02274   /* scale (if necesary) and angles computation */
02275   Image<float> in = img;
02276   //if( scale != 1.0 )
02277   //  in = gaussian_sampler( img, scale, sigma_scale );
02278 
02279   //struct coorlist * list_p;
02280   std::vector<Point2D<int> > list_p;
02281   void * mem_p;
02282   Image<float> modgrad;
02283   if (itsLSDVerbose) LINFO("Get Angles");
02284   Image<float> angles = ll_angle(in,rho,list_p,&mem_p,modgrad,n_bins,max_grad);
02285 
02286 
02287   int xsize = in.getWidth();
02288   int ysize = in.getHeight();
02289 
02290   double logNT = 5.0 * ( log10( (double) xsize ) + log10( (double) ysize ) ) / 2.0;
02291   double p = 1.0 / (double) d;
02292   int min_reg_size = 5; //(int)(-logNT/log10(p)); /* minimal number of point that can
02293                           //                      give a meaningful event */
02294 
02295   Image<byte> used(xsize, ysize, ZEROS);
02296   struct point * reg = (struct point *) calloc(xsize * ysize, sizeof(struct point));
02297 
02298   int sum_res = 1;
02299   int sum_offset =(int)( sum_res * ceil( sqrt( (double) xsize * xsize + (double) ysize * ysize) ) + 2);
02300   float* sum_l = (float *) calloc(2*sum_offset,sizeof(float));
02301   float* sum_w = (float *) calloc(2*sum_offset,sizeof(float));
02302   if( !reg || !sum_l || !sum_w ) LFATAL("Not enough memory!\n");
02303   for(int i=0;i<2*sum_offset;i++) sum_l[i] = sum_w[i] = 0;
02304 
02305   /* just start at point x,y option */
02306   //if( x && y && *x > 0 && *y > 0 && *x < (xsize-1) && *y < (ysize-1) )
02307   //  {
02308   //    if(itsLSDVerbose) fprintf(stderr,"starting only at point %d,%d.\n",*x,*y);
02309   //    list_p = (struct coorlist *) mem_p;
02310   //    list_p->x = *x;
02311   //    list_p->y = *y;
02312   //    list_p->next = NULL;
02313   //  }
02314 
02315   if (itsLSDVerbose) LINFO("Search for line segments");
02316   /* search for line segments */
02317   int segnum = 0;
02318   for(uint pIdx = 0; pIdx < list_p.size(); pIdx++)
02319     if( ( used[ list_p[pIdx].i + list_p[pIdx].j * used.getWidth() ] == NOTUSED &&
02320           angles[ list_p[pIdx].i + list_p[pIdx].j * angles.getWidth() ] != NOTDEF)  )
02321       {
02322           if (itsLSDVerbose) LINFO("try to find a line segment starting on %d,%d.\n",
02323                   list_p[pIdx].i,list_p[pIdx].j);
02324 
02325           /* find the region of connected point and ~equal angle */
02326           int reg_size;
02327           float reg_angle;
02328           float prec = M_PI / d;
02329           int radius = 1;
02330           region_grow( list_p[pIdx], angles, reg, &reg_size,
02331               &reg_angle, used, prec, radius,
02332               modgrad, p, min_reg_size );
02333 
02334         /* just process regions with a minimum number of points */
02335         if( reg_size < min_reg_size )
02336           {
02337            // LINFO("Region too small %i %i", reg_size, min_reg_size);
02338             if(itsLSDVerbose) LINFO("region too small, discarded.\n");
02339             for(int i=0; i<reg_size; i++)
02340               used[reg[i].x+reg[i].y*used.getWidth()] = NOTINI;
02341             continue;
02342           }
02343 
02344         if(itsLSDVerbose) LINFO("rectangular approximation of region.\n");
02345         struct rect rec;
02346         region2rect(reg,reg_size,modgrad,reg_angle,prec,p,&rec, sum_l, sum_w, sum_offset, sum_res);
02347 
02348         if(itsLSDVerbose) LINFO("improve rectangular approximation.\n");
02349         double nfa_val = rect_improve(&rec,angles,logNT,eps);
02350 
02351         LINFO("nfa_val %f", nfa_val);
02352         if( nfa_val > eps )
02353         {
02354           if(itsLSDVerbose) LINFO("line segment found! num %d nfa %g\n",
02355               segnum,nfa_val);
02356 
02357           /*
02358              0.5 must be added to compensate that the gradient was
02359              computed using a 2x2 window, so the computed gradient
02360              is valid at a point with offset (0.5,0.5). The coordinates
02361              origin is at the center of pixel 0,0.
02362              */
02363           rec.x1 += 0.5; rec.y1 += 0.5;
02364           rec.x2 += 0.5; rec.y2 += 0.5;
02365 
02366           if( scale != 1.0 )
02367           {
02368             rec.x1 /= scale;
02369             rec.y1 /= scale;
02370             rec.x2 /= scale;
02371             rec.y2 /= scale;
02372             rec.width /= scale;
02373           }
02374 
02375           LineSegment ls;
02376           ls.p1.i = rec.x1;
02377           ls.p1.j = rec.y1;
02378           ls.p2.i = rec.x2;
02379           ls.p2.j = rec.y2;
02380           ls.width = rec.width;
02381           ls.length = sqrt( squareOf(rec.x1 - rec.x2) + squareOf(rec.y1 - rec.y2));
02382           ls.center.i = rec.x;
02383           ls.center.j = rec.y;
02384           ls.ori = atan2(rec.y1-rec.y2,rec.x2-rec.x1);
02385           ls.strength = 1;
02386 
02387           //Find the color on etiher size of the line
02388           //float len = 7;
02389           //int x1 = int(cos(ls.ori-(M_PI/2))*len/2);
02390           //int y1 = int(sin(ls.ori-(M_PI/2))*len/2);
02391 
02392           //Point2D<int> p1((int)ls.center.i-x1, (int)ls.center.j+y1);
02393           //Point2D<int> p2((int)ls.center.i+x1, (int)ls.center.j-y1);
02394           //if (img.coordsOk(p1))
02395           //{
02396           //  ls.side1Color.clear();
02397           //  ls.side1Color.push_back(itsLGNInput[0].getVal(p1));
02398           //  ls.side1Color.push_back(itsLGNInput[1].getVal(p1));
02399           //  ls.side1Color.push_back(itsLGNInput[2].getVal(p1));
02400           //}
02401 
02402           //if (img.coordsOk(p2))
02403           //{
02404           //  ls.side2Color.clear();
02405           //  ls.side2Color.push_back(itsLGNInput[0].getVal(p2));
02406           //  ls.side2Color.push_back(itsLGNInput[1].getVal(p2));
02407           //  ls.side2Color.push_back(itsLGNInput[2].getVal(p2));
02408           //}
02409 
02410           lines.push_back(ls);
02411 
02412 
02413           //Show regions
02414 
02415           //fprintf(arout,"region %d:",segnum);
02416           //for(i=0; i<reg_size; i++)
02417           //  fprintf(arout," %d,%d;",reg[i].x,reg[i].y);
02418           //fprintf(arout,"\n");
02419 
02420           ++segnum;
02421         }
02422         else
02423           for(int i=0; i<reg_size; i++)
02424             used[reg[i].x+reg[i].y*used.getWidth()] = NOTINI;
02425       }
02426 
02427 
02428 //  getchar();
02429 
02430   return lines;
02431 }
02432 
02433 std::vector<V2::LineSegment> V2::lineSegmentDetection(const TensorField& tensorField,
02434     float q, float d, double eps,
02435     int n_bins, int max_grad )
02436 {
02437   //float rho = q / (float) sin( M_PI / (double) d );   /* gradient threshold */
02438   std::vector<LineSegment> lines;
02439 
02440   //struct coorlist * list_p;
02441   std::vector<Point2D<int> > list_p;
02442   Image<float> modgrad;
02443   if (itsLSDVerbose) LINFO("Get Angles");
02444   Image<float> angles = ll_angle(tensorField,max_grad*0.10,list_p,modgrad,n_bins,max_grad);
02445 
02446   int xsize = tensorField.t1.getWidth();
02447   int ysize = tensorField.t1.getHeight();
02448 
02449   double logNT = 5.0 * ( log10( (double) xsize ) + log10( (double) ysize ) ) / 2.0;
02450   double p = 1.0 / (double) d;
02451   int min_reg_size = 5; //(int)(-logNT/log10(p)); /* minimal number of point that can
02452                          //                       give a meaningful event */
02453 
02454   Image<byte> used(xsize, ysize, ZEROS);
02455   struct point * reg = (struct point *) calloc(xsize * ysize, sizeof(struct point));
02456 
02457   int sum_res = 1;
02458   int sum_offset =(int)( sum_res * ceil( sqrt( (double) xsize * xsize + (double) ysize * ysize) ) + 2);
02459   float* sum_l = (float *) calloc(2*sum_offset,sizeof(float));
02460   float* sum_w = (float *) calloc(2*sum_offset,sizeof(float));
02461   if( !reg || !sum_l || !sum_w ) LFATAL("Not enough memory!\n");
02462   for(int i=0;i<2*sum_offset;i++) sum_l[i] = sum_w[i] = 0;
02463 
02464   if (itsLSDVerbose) LINFO("Search for line segments");
02465   /* search for line segments */
02466   int segnum = 0;
02467   for(uint pIdx = 0; pIdx < list_p.size(); pIdx++)
02468     if( ( used[ list_p[pIdx].i + list_p[pIdx].j * used.getWidth() ] == NOTUSED &&
02469           angles[ list_p[pIdx].i + list_p[pIdx].j * angles.getWidth() ] != NOTDEF)  )
02470       {
02471           if (itsLSDVerbose) LINFO("try to find a line segment starting on %d,%d.\n",
02472                   list_p[pIdx].i,list_p[pIdx].j);
02473 
02474           /* find the region of connected point and ~equal angle */
02475           int reg_size;
02476           float reg_angle;
02477           float prec = M_PI / d;
02478           int radius = 2;
02479           region_grow( list_p[pIdx], angles, reg, &reg_size,
02480               &reg_angle, used,
02481               prec, radius,
02482               modgrad, p, min_reg_size );
02483 
02484         /* just process regions with a minimum number of points */
02485         if( reg_size < min_reg_size )
02486           {
02487             if(itsLSDVerbose) LINFO("region too small, discarded.\n");
02488             for(int i=0; i<reg_size; i++)
02489               used[reg[i].x+reg[i].y*used.getWidth()] = NOTINI;
02490             continue;
02491           }
02492 
02493         if(itsLSDVerbose) LINFO("rectangular approximation of region.\n");
02494         struct rect rec;
02495         region2rect(reg,reg_size,modgrad,reg_angle,prec,p,&rec, sum_l, sum_w, sum_offset, sum_res);
02496 
02497         if(itsLSDVerbose) LINFO("improve rectangular approximation.\n");
02498         double nfa_val = rect_improve(&rec,angles,logNT,eps);
02499 
02500         if ( fabs(nfa_val) > eps ) 
02501         {
02502           if(itsLSDVerbose) LINFO("line segment found! num %d nfa %g\n",
02503               segnum,nfa_val);
02504 
02505           /*
02506              0.5 must be added to compensate that the gradient was
02507              computed using a 2x2 window, so the computed gradient
02508              is valid at a point with offset (0.5,0.5). The coordinates
02509              origin is at the center of pixel 0,0.
02510              */
02511           rec.x1 += 0.5; rec.y1 += 0.5;
02512           rec.x2 += 0.5; rec.y2 += 0.5;
02513 
02514           if (rec.x1 >= xsize) rec.x1 = xsize-1;
02515           if (rec.y1 >= ysize) rec.y1 = ysize-1;
02516 
02517           if (rec.x2 >= xsize) rec.x2 = xsize-1;
02518           if (rec.y2 >= ysize) rec.y2 = ysize-1;
02519 
02520           LineSegment ls;
02521           ls.p1.i = rec.x1;
02522           ls.p1.j = rec.y1;
02523           ls.p2.i = rec.x2;
02524           ls.p2.j = rec.y2;
02525           ls.width = rec.width;
02526           ls.length = sqrt( squareOf(rec.x1 - rec.x2) + squareOf(rec.y1 - rec.y2));
02527           ls.center.i = rec.x;
02528           ls.center.j = rec.y;
02529           ls.ori = atan2(rec.y1-rec.y2,rec.x2-rec.x1);
02530           ls.prob = rec.sum;
02531 
02532           if (ls.ori < 0) ls.ori += M_PI;
02533           if (ls.ori >= M_PI) ls.ori -= M_PI;
02534 
02535           ls.strength = 1;
02536 
02537           //Find the color on etiher size of the line
02538           //float len = 7;
02539           //int x1 = int(cos(ls.ori-(M_PI/2))*len/2);
02540           //int y1 = int(sin(ls.ori-(M_PI/2))*len/2);
02541 
02542           //Point2D<int> p1((int)ls.center.i-x1, (int)ls.center.j+y1);
02543           //Point2D<int> p2((int)ls.center.i+x1, (int)ls.center.j-y1);
02544           //if (img.coordsOk(p1))
02545           //{
02546           //  ls.side1Color.clear();
02547           //  ls.side1Color.push_back(itsLGNInput[0].getVal(p1));
02548           //  ls.side1Color.push_back(itsLGNInput[1].getVal(p1));
02549           //  ls.side1Color.push_back(itsLGNInput[2].getVal(p1));
02550           //}
02551 
02552           //if (img.coordsOk(p2))
02553           //{
02554           //  ls.side2Color.clear();
02555           //  ls.side2Color.push_back(itsLGNInput[0].getVal(p2));
02556           //  ls.side2Color.push_back(itsLGNInput[1].getVal(p2));
02557           //  ls.side2Color.push_back(itsLGNInput[2].getVal(p2));
02558           //}
02559 
02560           lines.push_back(ls);
02561 
02562 
02563           //Show regions
02564 
02565           //fprintf(arout,"region %d:",segnum);
02566           //for(i=0; i<reg_size; i++)
02567           //  fprintf(arout," %d,%d;",reg[i].x,reg[i].y);
02568           //fprintf(arout,"\n");
02569 
02570           ++segnum;
02571         }
02572         else
02573           for(int i=0; i<reg_size; i++)
02574             used[reg[i].x+reg[i].y*used.getWidth()] = NOTINI;
02575       }
02576 
02577 
02578 //  getchar();
02579 
02580   free(reg);
02581   free(sum_l);
02582   free(sum_w);
02583 
02584   return lines;
02585 }
02586 
02587 
02588 // ######################################################################
02589 /* So things look consistent in everyone's emacs... */
02590 /* Local Variables: */
02591 /* indent-tabs-mode: nil */
02592 /* End: */
02593 
02594 #endif
02595 
Generated on Sun May 8 08:41:12 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3