[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 |
|
---|
| 28 | using std::string;
|
---|
| 29 | using namespace flair::core;
|
---|
| 30 | using namespace flair::gui;
|
---|
| 31 |
|
---|
[338] | 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);
|
---|
[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 |
|
---|
| 174 | private:/*
|
---|
| 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 |
|
---|
| 232 | namespace flair { namespace filter {
|
---|
| 233 |
|
---|
| 234 | HoughLines::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 |
|
---|
| 239 | HoughLines::~HoughLines(void) {
|
---|
[338] | 240 | delete pimpl_;
|
---|
[122] | 241 | }
|
---|
| 242 |
|
---|
| 243 | void HoughLines::UpdateFrom(const io_data *data) {
|
---|
[338] | 244 | pimpl_->UpdateFrom(data);
|
---|
| 245 | ProcessUpdate(pimpl_->output);
|
---|
[122] | 246 | }
|
---|
| 247 |
|
---|
| 248 | bool HoughLines::isLineDetected() const {
|
---|
[338] | 249 | if(pimpl_->output->Value(3,0)==1) {
|
---|
[122] | 250 | return true;
|
---|
| 251 | } else {
|
---|
| 252 | return false;
|
---|
| 253 | }
|
---|
| 254 | }
|
---|
| 255 |
|
---|
| 256 | float HoughLines::GetOrientation(void) const {
|
---|
[338] | 257 | return pimpl_->output->Value(1,0);
|
---|
[122] | 258 | }
|
---|
| 259 |
|
---|
| 260 | float HoughLines::GetDistance(void) const {
|
---|
[338] | 261 | return pimpl_->output->Value(0,0);
|
---|
[122] | 262 | }
|
---|
| 263 |
|
---|
[214] | 264 | Matrix *HoughLines::Output(void) const {
|
---|
[338] | 265 | return pimpl_->output;
|
---|
[122] | 266 | }
|
---|
| 267 |
|
---|
| 268 | } // end namespace filter
|
---|
| 269 | } // end namespace flair
|
---|