Changeset 325 in flair-src for branches/sanscv/lib/FlairVisionFilter/src/HoughLines.cpp

Timestamp:

08/28/19 16:12:11 (5 years ago)

Author:

Sanahuja Guillaume

Message:

using pimpl

File:

• branches/sanscv/lib/FlairVisionFilter/src/HoughLines.cpp (modified) (4 diffs)

branches/sanscv/lib/FlairVisionFilter/src/HoughLines.cpp

@@ -13 +13 @@
 
 #include "HoughLines.h"
+#include "VisionFilter.h"
 #include <Image.h>
+#include <OneAxisRotation.h>
 #include <Matrix.h>
 #include <Layout.h>
@@ -19 +21 @@
 #include <SpinBox.h>
 #include <DoubleSpinBox.h>
+//#include <dspcv_gpp.h>
 #include <typeinfo>
+#include <math.h>
 
 #define MAX_LINES 100
@@ -27 +31 @@
 using namespace flair::gui;
 
+class HoughLines_impl {
+public:
+    HoughLines_impl(flair::filter::HoughLines *self,const LayoutPosition* position,string name,const Vector2Df *inPtRefGlobal,float inThetaRefGlobal) {
+        this->self=self;
+        GroupBox* reglages_groupbox=new GroupBox(position,name);
+        rotation=new OneAxisRotation(reglages_groupbox->NewRow(),"pre rotation",OneAxisRotation::PostRotation);
+        fullRhoStep=new SpinBox(reglages_groupbox->NewRow(),"full rho step:","pixels",0,255,1,1);
+        fullThetaStep=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"full theta step:","degrees",0,90,1,1);
+        trackingRhoStep=new SpinBox(reglages_groupbox->NewRow(),"tracking rho step:","pixels",0,255,1,1);
+        trackingThetaStep=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"tracking theta step:","degrees",0,90,1,1);
+        trackingDeltaTheta=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"tracking delta theta:","degrees",0,90,1,1);
+        nbPoints=new SpinBox(reglages_groupbox->NewRow(),"nb points:",0,10000,10,100);
+
+        isTracking=false;
+        //linesStorage = (CvMat*)AllocFunction(sizeof(CvMat));
+        //linesStorage->data.fl = (float*)AllocFunction(MAX_LINES*2*sizeof(float));//was CV_32FC2, 2 channels
+        //gimg = (IplImage*)AllocFunction(sizeof(IplImage));
+
+        //init output matrix of same size as init
+        MatrixDescriptor* desc=new MatrixDescriptor(4,1);
+        desc->SetElementName(0,0,"distance");
+        desc->SetElementName(1,0,"orientation rad");
+        desc->SetElementName(2,0,"orientation deg");
+        desc->SetElementName(3,0,"line_detected");
+        output=new Matrix(self,desc,floatType,name);
+        delete desc;
+
+        try{
+            Image::Type const &imageType=dynamic_cast<Image::Type const &>(((IODevice*)(self->Parent()))->GetOutputDataType());
+            if(imageType.GetFormat()!=Image::Type::Format::Gray) {
+                self->Err("input image is not gray\n");
+            }
+        } catch(std::bad_cast& bc) {
+            self->Err("io type mismatch\n");
+        }
+
+        thetaRefGlobal=inThetaRefGlobal;
+        if (inPtRefGlobal==NULL) {
+            ptRefGlobal=NULL;
+        } else { //rotation from global coordinates to hough space
+            ptRefGlobal =new Vector2Df(inPtRefGlobal->x,inPtRefGlobal->y);
+        }
+        
+    }
+
+    ~HoughLines_impl() {
+        //FreeFunction((char*)(linesStorage->data.fl));
+        //FreeFunction((char*)linesStorage);
+        //FreeFunction((char*)gimg);
+        if(ptRefGlobal!=NULL) delete ptRefGlobal;
+    }
+    
+    void UpdateFrom(const io_data *data){
+        Image *img=(Image*)data;
+  /*
+        gimg->width=img->GetDataType().GetWidth();
+        gimg->height=img->GetDataType().GetHeight();
+        gimg->imageData=img->buffer;
+    
+        size_t nbLines;
+        Vector2Df ptRef;
+        float thetaRef;
+
+        if (ptRefGlobal==NULL) {
+          ptRef.x=img->GetDataType().GetWidth()/2;
+          ptRef.y=img->GetDataType().GetHeight()/2;
+        } else { //rotation from global coordinates to hough space
+          Vector3Df ptRef3D(ptRefGlobal->x,ptRefGlobal->y,0);
+          rotation->ComputeRotation(ptRef3D);
+          ptRef.x=ptRef3D.x;
+          ptRef.y=ptRef3D.y;
+        }
+          
+        //orientation in global space is rotated by pi/2 compared to orientation in hough space
+        //eg: vertical line has a 0 orientation in global space (north), but a pi/2 (or -pi/2) orientation in hough space (theta)
+        thetaRef=thetaRefGlobal+CV_PI/2+rotation->GetAngle();
+        if (thetaRef>CV_PI) thetaRef-=CV_PI;
+        if (thetaRef<0) thetaRef+=CV_PI;
+
+        data->GetMutex();
+        if(!isTracking) {
+        nbLines=dspHoughLines2(gimg,linesStorage,CV_HOUGH_STANDARD,
+                                fullRhoStep->Value(),fullThetaStep->Value()*CV_PI/180,
+                                nbPoints->Value());
+        } else {
+        nbLines=dspHoughLinesTracking(gimg,linesStorage,CV_HOUGH_STANDARD,
+                                     trackingRhoStep->Value(),
+                                     theta,trackingDeltaTheta->Value()*CV_PI/180,
+                                     trackingThetaStep->Value()*CV_PI/180,
+                                     nbPoints->Value());
+        //        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());
+        }
+        data->ReleaseMutex();
+
+        //saturation sur le nb max de ligne, au cas ou le DSP n'aurait pas la meme valeur
+        if(nbLines>MAX_LINES) {
+            self->Warn("erreur nb lines %u>%u\n",nbLines,MAX_LINES);
+            nbLines=MAX_LINES;
+        }
+        float rho;
+        bool noLine=!SelectBestLine(linesStorage,nbLines,rho,theta);
+
+        if (noLine) {
+            isTracking=false;
+        } else {
+            isTracking=true;
+        //        float thetaRef=0;
+
+            //line equation is ax+by+c=0 with a=cos(theta), b=sin(theta) and c=-rho
+            //distance from point xRef,yRef to the line is (a.xRef+b.yRef+c)/sqrt(a*a+b*b)
+            distance=-(cosf(theta)*ptRef.x+sinf(theta)*ptRef.y-rho);
+
+            orientation=theta-thetaRef;
+            if (orientation<-CV_PI/2) {
+              orientation+=CV_PI;
+              distance=-distance;
+            }
+            if (orientation>CV_PI/2) {
+              orientation-=CV_PI;
+              distance=-distance;
+            }
+
+            //printf("=> pour theta=%f et rho=%f, distance au point(%f,%f)=%f\n",theta,rho,xRef,yRef,distance);
+        }
+
+        output->GetMutex();
+        output->SetValueNoMutex(0,0,distance);
+        output->SetValueNoMutex(1,0,orientation);
+        output->SetValueNoMutex(2,0,orientation*180/CV_PI);
+        if(noLine) {
+            output->SetValueNoMutex(3,0,0);
+        } else {
+            output->SetValueNoMutex(3,0,1);
+        }
+        output->ReleaseMutex();
+*/
+        output->SetDataTime(data->DataTime());
+    };
+    
+    Matrix *output;
+    
+private:/*
+    //select best line. Returns false if no line found
+    bool SelectBestLine(CvMat* linesStorage, size_t nbLines, float &rho, float &theta) {
+      if(nbLines==0) {
+        return false;
+      }
+      //one line is found
+      if (nbLines==1) {
+        rho=linesStorage->data.fl[0];
+        theta=linesStorage->data.fl[1];
+        //printf("rho=%f,theta=%f (one line)\n",rho,theta);
+        return true;
+      }
+      //lines are ordered by quality, the first one will be our reference
+      float thetaRef=linesStorage->data.fl[1];
+      float thetaRefErrorSum=0;
+      float rhoSum=linesStorage->data.fl[0];
+      //printf("rho=%f,theta=%f (first of multilines)\n",rhoSum,thetaRef);
+      for(int i=1;i<nbLines;i++) {
+        //printf("rho=%f,theta=%f (multilines)\n",linesStorage->data.fl[2*i],linesStorage->data.fl[2*i+1]);
+        float thetaDiff=linesStorage->data.fl[2*i+1]-thetaRef;
+        float rhoLine=linesStorage->data.fl[2*i];
+        if (thetaDiff>CV_PI/2) {
+          thetaDiff-=CV_PI;
+          rhoLine=-rhoLine;
+        } else if (thetaDiff<-CV_PI/2) {
+          thetaDiff+=CV_PI;
+          rhoLine=-rhoLine;
+        }
+        thetaRefErrorSum += thetaDiff;
+        rhoSum+=rhoLine;
+      }
+      rho=rhoSum/nbLines;
+      theta=thetaRef+thetaRefErrorSum/nbLines;
+      if (theta<0) {
+        theta+=CV_PI;
+        rho=-rho;
+      }
+      if (theta>CV_PI) {
+        theta-=CV_PI;
+        rho=-rho;
+      }
+      return true;
+    }
+*/
+    flair::filter::HoughLines *self;
+    OneAxisRotation* rotation;
+    SpinBox *fullRhoStep,*trackingRhoStep,*nbPoints;
+    DoubleSpinBox *fullThetaStep,*trackingThetaStep,*trackingDeltaTheta;
+    bool isTracking;
+    float theta;
+    float distance,orientation;
+    Vector2Df* ptRefGlobal;
+    float thetaRefGlobal;
+    //CvMat* linesStorage;
+    //IplImage *gimg;
+};
+
 namespace flair { namespace filter {
 
-HoughLines::HoughLines(const IODevice* parent,const LayoutPosition* position,string name) : IODevice(parent,name) {
-    GroupBox* reglages_groupbox=new GroupBox(position,name);
-    fullRhoStep=new SpinBox(reglages_groupbox->NewRow(),"full rho step:","pixels",0,255,1,1);
-    fullThetaStep=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"full theta step:","degrees",0,90,1,1);
-    trackingRhoStep=new SpinBox(reglages_groupbox->NewRow(),"tracking rho step:","pixels",0,255,1,1);
-    trackingThetaStep=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"tracking theta step:","degrees",0,90,1,1);
-    trackingDeltaTheta=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"tracking delta theta:","degrees",0,90,1,1);
-    nbPoints=new SpinBox(reglages_groupbox->NewRow(),"nb points:",0,10000,10,100);
-
-    isTracking=false;
-    lostLine=false;
-    initLine=false;
-    linesStorage = (CvMat*)malloc(sizeof(CvMat));
-    linesStorage->data.fl = (float*)malloc(MAX_LINES*2*sizeof(float));//was CV_32FC2, 2 channels
-
-
-    //init output matrix of same size as init
-    MatrixDescriptor* desc=new MatrixDescriptor(4,1);
-    desc->SetElementName(0,0,"distance");
-    desc->SetElementName(1,0,"orientation rad");
-    desc->SetElementName(2,0,"orientation deg");
-    desc->SetElementName(3,0,"line_detected");
-    output=new Matrix(this,desc,floatType,name);
-    delete desc;
-
-    try{
-        Image::Type const &imageType=dynamic_cast<Image::Type const &>(parent->GetOutputDataType());
-        if(imageType.GetFormat()!=Image::Type::Format::Gray) {
-            Err("input image is not gray\n");
-            return;
-        }
-    } catch(std::bad_cast& bc) {
-        Err("io type mismatch\n");
-        return;
-    }
-    
-    SetIsReady(true);
+HoughLines::HoughLines(const IODevice* parent,const LayoutPosition* position,string name,const Vector2Df *inPtRefGlobal,float inThetaRefGlobal) : IODevice(parent,name) {
+  pimpl_=new HoughLines_impl(this,position,name,inPtRefGlobal,inThetaRefGlobal);
+   
 }
 
 HoughLines::~HoughLines(void) {
-    free((char*)(linesStorage->data.fl));
-    free((char*)linesStorage);
+   delete  pimpl_;
 }
 
 void HoughLines::UpdateFrom(const io_data *data) {
- 
+    pimpl_->UpdateFrom(data);
+    ProcessUpdate(pimpl_->output);
 }
 
 bool HoughLines::isLineDetected() const {
-    if(output->Value(3,0)==1) {
+    if(pimpl_->output->Value(3,0)==1) {
         return true;
     } else {
@@ -86 +255 @@
 
 float HoughLines::GetOrientation(void) const {
-    return output->Value(1,0);
+    return pimpl_->output->Value(1,0);
 }
 
 float HoughLines::GetDistance(void) const {
-    return output->Value(0,0);
+    return pimpl_->output->Value(0,0);
 }
 
 Matrix *HoughLines::Output(void) const {
-    return output;
+    return pimpl_->output;
 }