source: flair-src/branches/sanscv/lib/FlairVisionFilter/src/HoughLines.cpp@ 326

Last change on this file since 326 was 326, checked in by Sanahuja Guillaume, 3 years ago

using pimpl

File size: 9.3 KB
Line 
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
28using std::string;
29using namespace flair::core;
30using namespace flair::gui;
31
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);
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
173private:/*
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
231namespace flair { namespace filter {
232
233HoughLines::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
238HoughLines::~HoughLines(void) {
239 delete pimpl_;
240}
241
242void HoughLines::UpdateFrom(const io_data *data) {
243 pimpl_->UpdateFrom(data);
244 ProcessUpdate(pimpl_->output);
245}
246
247bool HoughLines::isLineDetected() const {
248 if(pimpl_->output->Value(3,0)==1) {
249 return true;
250 } else {
251 return false;
252 }
253}
254
255float HoughLines::GetOrientation(void) const {
256 return pimpl_->output->Value(1,0);
257}
258
259float HoughLines::GetDistance(void) const {
260 return pimpl_->output->Value(0,0);
261}
262
263Matrix *HoughLines::Output(void) const {
264 return pimpl_->output;
265}
266
267} // end namespace filter
268} // end namespace flair
Note: See TracBrowser for help on using the repository browser.