source: flair-src/trunk/lib/FlairVisionFilter/src/HoughLines.cpp

Last change on this file was 338, checked in by Sanahuja Guillaume, 2 years ago

remove opencv dep

File size: 9.3 KB
RevLine 
[122]1//  created:    2015/10/07
2//  filename:   HoughLines.cpp
3//
4//  author:     Guillaume Sanahuja
5//              Copyright Heudiasyc UMR UTC/CNRS 7253
6//
7//  version:    $Id: $
8//
9//  purpose:    HoughLines
10//
11//
12/*********************************************************************/
13
14#include "HoughLines.h"
[338]15#include "VisionFilter.h"
16#include <Image.h>
17#include <OneAxisRotation.h>
[214]18#include <Matrix.h>
[122]19#include <Layout.h>
20#include <GroupBox.h>
21#include <SpinBox.h>
22#include <DoubleSpinBox.h>
23#include <typeinfo>
[338]24#include <math.h>
[122]25
26#define MAX_LINES 100
27
28using std::string;
29using namespace flair::core;
30using namespace flair::gui;
31
[338]32class HoughLines_impl {
33public:
34    HoughLines_impl(flair::filter::HoughLines *self,const LayoutPosition* position,string name,const Vector2Df *inPtRefGlobal,float inThetaRefGlobal) {
35        this->self=self;
36        GroupBox* reglages_groupbox=new GroupBox(position,name);
37        rotation=new OneAxisRotation(reglages_groupbox->NewRow(),"pre rotation",OneAxisRotation::PostRotation);
38        fullRhoStep=new SpinBox(reglages_groupbox->NewRow(),"full rho step:","pixels",0,255,1,1);
39        fullThetaStep=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"full theta step:","degrees",0,90,1,1);
40        trackingRhoStep=new SpinBox(reglages_groupbox->NewRow(),"tracking rho step:","pixels",0,255,1,1);
41        trackingThetaStep=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"tracking theta step:","degrees",0,90,1,1);
42        trackingDeltaTheta=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"tracking delta theta:","degrees",0,90,1,1);
43        nbPoints=new SpinBox(reglages_groupbox->NewRow(),"nb points:",0,10000,10,100);
[122]44
[338]45        isTracking=false;
46        //linesStorage = (CvMat*)AllocFunction(sizeof(CvMat));
47        //linesStorage->data.fl = (float*)AllocFunction(MAX_LINES*2*sizeof(float));//was CV_32FC2, 2 channels
48        //gimg = (IplImage*)AllocFunction(sizeof(IplImage));
[122]49
[338]50        //init output matrix of same size as init
51        MatrixDescriptor* desc=new MatrixDescriptor(4,1);
52        desc->SetElementName(0,0,"distance");
53        desc->SetElementName(1,0,"orientation rad");
54        desc->SetElementName(2,0,"orientation deg");
55        desc->SetElementName(3,0,"line_detected");
56        output=new Matrix(self,desc,floatType,name);
57        delete desc;
[122]58
[338]59        try{
60            Image::Type const &imageType=dynamic_cast<Image::Type const &>(((IODevice*)(self->Parent()))->GetOutputDataType());
61            if(imageType.GetFormat()!=Image::Type::Format::Gray) {
62                self->Err("input image is not gray\n");
63            }
64        } catch(std::bad_cast& bc) {
65            self->Err("io type mismatch\n");
66        }
[122]67
[338]68        thetaRefGlobal=inThetaRefGlobal;
69        if (inPtRefGlobal==NULL) {
70            ptRefGlobal=NULL;
71        } else { //rotation from global coordinates to hough space
72            ptRefGlobal =new Vector2Df(inPtRefGlobal->x,inPtRefGlobal->y);
[122]73        }
[338]74       
[122]75    }
[338]76
77    ~HoughLines_impl() {
78        //FreeFunction((char*)(linesStorage->data.fl));
79        //FreeFunction((char*)linesStorage);
80        //FreeFunction((char*)gimg);
81        if(ptRefGlobal!=NULL) delete ptRefGlobal;
82    }
[157]83   
[338]84    void UpdateFrom(const io_data *data){
85        Image *img=(Image*)data;
86  /*
87        gimg->width=img->GetDataType().GetWidth();
88        gimg->height=img->GetDataType().GetHeight();
89        gimg->imageData=img->buffer;
90        gimg->imageSize=img->GetDataType().GetSize();
91   
92        size_t nbLines;
93        Vector2Df ptRef;
94        float thetaRef;
95
96        if (ptRefGlobal==NULL) {
97          ptRef.x=img->GetDataType().GetWidth()/2;
98          ptRef.y=img->GetDataType().GetHeight()/2;
99        } else { //rotation from global coordinates to hough space
100          Vector3Df ptRef3D(ptRefGlobal->x,ptRefGlobal->y,0);
101          rotation->ComputeRotation(ptRef3D);
102          ptRef.x=ptRef3D.x;
103          ptRef.y=ptRef3D.y;
104        }
105         
106        //orientation in global space is rotated by pi/2 compared to orientation in hough space
107        //eg: vertical line has a 0 orientation in global space (north), but a pi/2 (or -pi/2) orientation in hough space (theta)
108        thetaRef=thetaRefGlobal+CV_PI/2+rotation->GetAngle();
109        if (thetaRef>CV_PI) thetaRef-=CV_PI;
110        if (thetaRef<0) thetaRef+=CV_PI;
111
112        data->GetMutex();
113        if(!isTracking) {
114        nbLines=dspHoughLines2(gimg,linesStorage,CV_HOUGH_STANDARD,
115                                fullRhoStep->Value(),fullThetaStep->Value()*CV_PI/180,
116                                nbPoints->Value());
117        } else {
118        nbLines=dspHoughLinesTracking(gimg,linesStorage,CV_HOUGH_STANDARD,
119                                     trackingRhoStep->Value(),
120                                     theta,trackingDeltaTheta->Value()*CV_PI/180,
121                                     trackingThetaStep->Value()*CV_PI/180,
122                                     nbPoints->Value());
123        //        nbLines=dspHoughLines2_test(gimg,linesStorage,CV_HOUGH_STANDARD,trackingRhoStep->Value(),thetaPrev-trackingDeltaTheta->Value()*CV_PI/180,thetaPrev+trackingDeltaTheta->Value()*CV_PI/180,trackingThetaStep->Value()*CV_PI/180,nbPoints->Value());
124        }
125        data->ReleaseMutex();
126
127        //saturation sur le nb max de ligne, au cas ou le DSP n'aurait pas la meme valeur
128        if(nbLines>MAX_LINES) {
129            self->Warn("erreur nb lines %u>%u\n",nbLines,MAX_LINES);
130            nbLines=MAX_LINES;
131        }
132        float rho;
133        bool noLine=!SelectBestLine(linesStorage,nbLines,rho,theta);
134
135        if (noLine) {
136            isTracking=false;
137        } else {
138            isTracking=true;
139        //        float thetaRef=0;
140
141            //line equation is ax+by+c=0 with a=cos(theta), b=sin(theta) and c=-rho
142            //distance from point xRef,yRef to the line is (a.xRef+b.yRef+c)/sqrt(a*a+b*b)
143            distance=-(cosf(theta)*ptRef.x+sinf(theta)*ptRef.y-rho);
144
145            orientation=theta-thetaRef;
146            if (orientation<-CV_PI/2) {
147              orientation+=CV_PI;
148              distance=-distance;
149            }
150            if (orientation>CV_PI/2) {
151              orientation-=CV_PI;
152              distance=-distance;
153            }
154
155            //printf("=> pour theta=%f et rho=%f, distance au point(%f,%f)=%f\n",theta,rho,xRef,yRef,distance);
156        }
157
158        output->GetMutex();
159        output->SetValueNoMutex(0,0,distance);
160        output->SetValueNoMutex(1,0,orientation);
161        output->SetValueNoMutex(2,0,orientation*180/CV_PI);
162        if(noLine) {
163            output->SetValueNoMutex(3,0,0);
164        } else {
165            output->SetValueNoMutex(3,0,1);
166        }
167        output->ReleaseMutex();
168*/
169        output->SetDataTime(data->DataTime());
170    };
171   
172    Matrix *output;
173   
174private:/*
175    //select best line. Returns false if no line found
176    bool SelectBestLine(CvMat* linesStorage, size_t nbLines, float &rho, float &theta) {
177      if(nbLines==0) {
178        return false;
179      }
180      //one line is found
181      if (nbLines==1) {
182        rho=linesStorage->data.fl[0];
183        theta=linesStorage->data.fl[1];
184        //printf("rho=%f,theta=%f (one line)\n",rho,theta);
185        return true;
186      }
187      //lines are ordered by quality, the first one will be our reference
188      float thetaRef=linesStorage->data.fl[1];
189      float thetaRefErrorSum=0;
190      float rhoSum=linesStorage->data.fl[0];
191      //printf("rho=%f,theta=%f (first of multilines)\n",rhoSum,thetaRef);
192      for(int i=1;i<nbLines;i++) {
193        //printf("rho=%f,theta=%f (multilines)\n",linesStorage->data.fl[2*i],linesStorage->data.fl[2*i+1]);
194        float thetaDiff=linesStorage->data.fl[2*i+1]-thetaRef;
195        float rhoLine=linesStorage->data.fl[2*i];
196        if (thetaDiff>CV_PI/2) {
197          thetaDiff-=CV_PI;
198          rhoLine=-rhoLine;
199        } else if (thetaDiff<-CV_PI/2) {
200          thetaDiff+=CV_PI;
201          rhoLine=-rhoLine;
202        }
203        thetaRefErrorSum += thetaDiff;
204        rhoSum+=rhoLine;
205      }
206      rho=rhoSum/nbLines;
207      theta=thetaRef+thetaRefErrorSum/nbLines;
208      if (theta<0) {
209        theta+=CV_PI;
210        rho=-rho;
211      }
212      if (theta>CV_PI) {
213        theta-=CV_PI;
214        rho=-rho;
215      }
216      return true;
217    }
218*/
219    flair::filter::HoughLines *self;
220    OneAxisRotation* rotation;
221    SpinBox *fullRhoStep,*trackingRhoStep,*nbPoints;
222    DoubleSpinBox *fullThetaStep,*trackingThetaStep,*trackingDeltaTheta;
223    bool isTracking;
224    float theta;
225    float distance,orientation;
226    Vector2Df* ptRefGlobal;
227    float thetaRefGlobal;
228    //CvMat* linesStorage;
229    //IplImage *gimg;
230};
231
232namespace flair { namespace filter {
233
234HoughLines::HoughLines(const IODevice* parent,const LayoutPosition* position,string name,const Vector2Df *inPtRefGlobal,float inThetaRefGlobal) : IODevice(parent,name) {
235  pimpl_=new HoughLines_impl(this,position,name,inPtRefGlobal,inThetaRefGlobal);
236   
[122]237}
238
239HoughLines::~HoughLines(void) {
[338]240   delete  pimpl_;
[122]241}
242
243void HoughLines::UpdateFrom(const io_data *data) {
[338]244    pimpl_->UpdateFrom(data);
245    ProcessUpdate(pimpl_->output);
[122]246}
247
248bool HoughLines::isLineDetected() const {
[338]249    if(pimpl_->output->Value(3,0)==1) {
[122]250        return true;
251    } else {
252        return false;
253    }
254}
255
256float HoughLines::GetOrientation(void) const {
[338]257    return pimpl_->output->Value(1,0);
[122]258}
259
260float HoughLines::GetDistance(void) const {
[338]261    return pimpl_->output->Value(0,0);
[122]262}
263
[214]264Matrix *HoughLines::Output(void) const {
[338]265    return pimpl_->output;
[122]266}
267
268} // end namespace filter
269} // end namespace flair
Note: See TracBrowser for help on using the repository browser.