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"
|
---|
15 | #include "VisionFilter.h"
|
---|
16 | #include <Image.h>
|
---|
17 | #include <OneAxisRotation.h>
|
---|
18 | #include <Matrix.h>
|
---|
19 | #include <Layout.h>
|
---|
20 | #include <GroupBox.h>
|
---|
21 | #include <SpinBox.h>
|
---|
22 | #include <DoubleSpinBox.h>>
|
---|
23 | #include <typeinfo>
|
---|
24 | #include <math.h>
|
---|
25 |
|
---|
26 | #define MAX_LINES 100
|
---|
27 |
|
---|
28 | using std::string;
|
---|
29 | using namespace flair::core;
|
---|
30 | using namespace flair::gui;
|
---|
31 |
|
---|
32 | class HoughLines_impl {
|
---|
33 | public:
|
---|
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);
|
---|
44 |
|
---|
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));
|
---|
49 |
|
---|
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;
|
---|
58 |
|
---|
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 | }
|
---|
67 |
|
---|
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);
|
---|
73 | }
|
---|
74 |
|
---|
75 | }
|
---|
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 | }
|
---|
83 |
|
---|
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 |
|
---|
91 | size_t nbLines;
|
---|
92 | Vector2Df ptRef;
|
---|
93 | float thetaRef;
|
---|
94 |
|
---|
95 | if (ptRefGlobal==NULL) {
|
---|
96 | ptRef.x=img->GetDataType().GetWidth()/2;
|
---|
97 | ptRef.y=img->GetDataType().GetHeight()/2;
|
---|
98 | } else { //rotation from global coordinates to hough space
|
---|
99 | Vector3Df ptRef3D(ptRefGlobal->x,ptRefGlobal->y,0);
|
---|
100 | rotation->ComputeRotation(ptRef3D);
|
---|
101 | ptRef.x=ptRef3D.x;
|
---|
102 | ptRef.y=ptRef3D.y;
|
---|
103 | }
|
---|
104 |
|
---|
105 | //orientation in global space is rotated by pi/2 compared to orientation in hough space
|
---|
106 | //eg: vertical line has a 0 orientation in global space (north), but a pi/2 (or -pi/2) orientation in hough space (theta)
|
---|
107 | thetaRef=thetaRefGlobal+CV_PI/2+rotation->GetAngle();
|
---|
108 | if (thetaRef>CV_PI) thetaRef-=CV_PI;
|
---|
109 | if (thetaRef<0) thetaRef+=CV_PI;
|
---|
110 |
|
---|
111 | data->GetMutex();
|
---|
112 | if(!isTracking) {
|
---|
113 | nbLines=dspHoughLines2(gimg,linesStorage,CV_HOUGH_STANDARD,
|
---|
114 | fullRhoStep->Value(),fullThetaStep->Value()*CV_PI/180,
|
---|
115 | nbPoints->Value());
|
---|
116 | } else {
|
---|
117 | nbLines=dspHoughLinesTracking(gimg,linesStorage,CV_HOUGH_STANDARD,
|
---|
118 | trackingRhoStep->Value(),
|
---|
119 | theta,trackingDeltaTheta->Value()*CV_PI/180,
|
---|
120 | trackingThetaStep->Value()*CV_PI/180,
|
---|
121 | nbPoints->Value());
|
---|
122 | // 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());
|
---|
123 | }
|
---|
124 | data->ReleaseMutex();
|
---|
125 |
|
---|
126 | //saturation sur le nb max de ligne, au cas ou le DSP n'aurait pas la meme valeur
|
---|
127 | if(nbLines>MAX_LINES) {
|
---|
128 | self->Warn("erreur nb lines %u>%u\n",nbLines,MAX_LINES);
|
---|
129 | nbLines=MAX_LINES;
|
---|
130 | }
|
---|
131 | float rho;
|
---|
132 | bool noLine=!SelectBestLine(linesStorage,nbLines,rho,theta);
|
---|
133 |
|
---|
134 | if (noLine) {
|
---|
135 | isTracking=false;
|
---|
136 | } else {
|
---|
137 | isTracking=true;
|
---|
138 | // float thetaRef=0;
|
---|
139 |
|
---|
140 | //line equation is ax+by+c=0 with a=cos(theta), b=sin(theta) and c=-rho
|
---|
141 | //distance from point xRef,yRef to the line is (a.xRef+b.yRef+c)/sqrt(a*a+b*b)
|
---|
142 | distance=-(cosf(theta)*ptRef.x+sinf(theta)*ptRef.y-rho);
|
---|
143 |
|
---|
144 | orientation=theta-thetaRef;
|
---|
145 | if (orientation<-CV_PI/2) {
|
---|
146 | orientation+=CV_PI;
|
---|
147 | distance=-distance;
|
---|
148 | }
|
---|
149 | if (orientation>CV_PI/2) {
|
---|
150 | orientation-=CV_PI;
|
---|
151 | distance=-distance;
|
---|
152 | }
|
---|
153 |
|
---|
154 | //printf("=> pour theta=%f et rho=%f, distance au point(%f,%f)=%f\n",theta,rho,xRef,yRef,distance);
|
---|
155 | }
|
---|
156 |
|
---|
157 | output->GetMutex();
|
---|
158 | output->SetValueNoMutex(0,0,distance);
|
---|
159 | output->SetValueNoMutex(1,0,orientation);
|
---|
160 | output->SetValueNoMutex(2,0,orientation*180/CV_PI);
|
---|
161 | if(noLine) {
|
---|
162 | output->SetValueNoMutex(3,0,0);
|
---|
163 | } else {
|
---|
164 | output->SetValueNoMutex(3,0,1);
|
---|
165 | }
|
---|
166 | output->ReleaseMutex();
|
---|
167 | */
|
---|
168 | output->SetDataTime(data->DataTime());
|
---|
169 | };
|
---|
170 |
|
---|
171 | Matrix *output;
|
---|
172 |
|
---|
173 | private:/*
|
---|
174 | //select best line. Returns false if no line found
|
---|
175 | bool SelectBestLine(CvMat* linesStorage, size_t nbLines, float &rho, float &theta) {
|
---|
176 | if(nbLines==0) {
|
---|
177 | return false;
|
---|
178 | }
|
---|
179 | //one line is found
|
---|
180 | if (nbLines==1) {
|
---|
181 | rho=linesStorage->data.fl[0];
|
---|
182 | theta=linesStorage->data.fl[1];
|
---|
183 | //printf("rho=%f,theta=%f (one line)\n",rho,theta);
|
---|
184 | return true;
|
---|
185 | }
|
---|
186 | //lines are ordered by quality, the first one will be our reference
|
---|
187 | float thetaRef=linesStorage->data.fl[1];
|
---|
188 | float thetaRefErrorSum=0;
|
---|
189 | float rhoSum=linesStorage->data.fl[0];
|
---|
190 | //printf("rho=%f,theta=%f (first of multilines)\n",rhoSum,thetaRef);
|
---|
191 | for(int i=1;i<nbLines;i++) {
|
---|
192 | //printf("rho=%f,theta=%f (multilines)\n",linesStorage->data.fl[2*i],linesStorage->data.fl[2*i+1]);
|
---|
193 | float thetaDiff=linesStorage->data.fl[2*i+1]-thetaRef;
|
---|
194 | float rhoLine=linesStorage->data.fl[2*i];
|
---|
195 | if (thetaDiff>CV_PI/2) {
|
---|
196 | thetaDiff-=CV_PI;
|
---|
197 | rhoLine=-rhoLine;
|
---|
198 | } else if (thetaDiff<-CV_PI/2) {
|
---|
199 | thetaDiff+=CV_PI;
|
---|
200 | rhoLine=-rhoLine;
|
---|
201 | }
|
---|
202 | thetaRefErrorSum += thetaDiff;
|
---|
203 | rhoSum+=rhoLine;
|
---|
204 | }
|
---|
205 | rho=rhoSum/nbLines;
|
---|
206 | theta=thetaRef+thetaRefErrorSum/nbLines;
|
---|
207 | if (theta<0) {
|
---|
208 | theta+=CV_PI;
|
---|
209 | rho=-rho;
|
---|
210 | }
|
---|
211 | if (theta>CV_PI) {
|
---|
212 | theta-=CV_PI;
|
---|
213 | rho=-rho;
|
---|
214 | }
|
---|
215 | return true;
|
---|
216 | }
|
---|
217 | */
|
---|
218 | flair::filter::HoughLines *self;
|
---|
219 | OneAxisRotation* rotation;
|
---|
220 | SpinBox *fullRhoStep,*trackingRhoStep,*nbPoints;
|
---|
221 | DoubleSpinBox *fullThetaStep,*trackingThetaStep,*trackingDeltaTheta;
|
---|
222 | bool isTracking;
|
---|
223 | float theta;
|
---|
224 | float distance,orientation;
|
---|
225 | Vector2Df* ptRefGlobal;
|
---|
226 | float thetaRefGlobal;
|
---|
227 | //CvMat* linesStorage;
|
---|
228 | //IplImage *gimg;
|
---|
229 | };
|
---|
230 |
|
---|
231 | namespace flair { namespace filter {
|
---|
232 |
|
---|
233 | HoughLines::HoughLines(const IODevice* parent,const LayoutPosition* position,string name,const Vector2Df *inPtRefGlobal,float inThetaRefGlobal) : IODevice(parent,name) {
|
---|
234 | pimpl_=new HoughLines_impl(this,position,name,inPtRefGlobal,inThetaRefGlobal);
|
---|
235 |
|
---|
236 | }
|
---|
237 |
|
---|
238 | HoughLines::~HoughLines(void) {
|
---|
239 | delete pimpl_;
|
---|
240 | }
|
---|
241 |
|
---|
242 | void HoughLines::UpdateFrom(const io_data *data) {
|
---|
243 | pimpl_->UpdateFrom(data);
|
---|
244 | ProcessUpdate(pimpl_->output);
|
---|
245 | }
|
---|
246 |
|
---|
247 | bool HoughLines::isLineDetected() const {
|
---|
248 | if(pimpl_->output->Value(3,0)==1) {
|
---|
249 | return true;
|
---|
250 | } else {
|
---|
251 | return false;
|
---|
252 | }
|
---|
253 | }
|
---|
254 |
|
---|
255 | float HoughLines::GetOrientation(void) const {
|
---|
256 | return pimpl_->output->Value(1,0);
|
---|
257 | }
|
---|
258 |
|
---|
259 | float HoughLines::GetDistance(void) const {
|
---|
260 | return pimpl_->output->Value(0,0);
|
---|
261 | }
|
---|
262 |
|
---|
263 | Matrix *HoughLines::Output(void) const {
|
---|
264 | return pimpl_->output;
|
---|
265 | }
|
---|
266 |
|
---|
267 | } // end namespace filter
|
---|
268 | } // end namespace flair
|
---|