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


Ignore:
Timestamp:
08/28/19 16:12:11 (5 years ago)
Author:
Sanahuja Guillaume
Message:

using pimpl

Location:
branches/sanscv/lib/FlairVisionFilter/src
Files:
16 edited

Legend:

Unmodified
Added
Removed

  • branches/sanscv/lib/FlairVisionFilter/src/CvtColor.cpp

    r324 r325  
    1313
    1414#include "CvtColor.h"
     15#include "VisionFilter.h"
    1516#include <Image.h>
     17//#include <dspcv_gpp.h>
    1618#include <typeinfo>
    1719
    … …  
    1921using namespace flair::core;
    2022
     23class CvtColor_impl {
     24public:
     25    CvtColor_impl(flair::filter::CvtColor *self,flair::filter::CvtColor::Conversion_t conversion):output(0) {
     26        this->conversion=conversion;
     27        this->self=self;
     28
     29        switch(conversion) {
     30        case flair::filter::CvtColor::Conversion_t::ToGray:
     31            try{
     32                Image::Type const &imageType=dynamic_cast<Image::Type const &>(((IODevice*)(self->Parent()))->GetOutputDataType());
     33                output=new Image(self,imageType.GetWidth(),imageType.GetHeight(),Image::Type::Format::Gray,"conversion",true,2);
     34                //inIplImage = (IplImage*)AllocFunction(sizeof(IplImage));
     35                //outIplImage = (IplImage*)AllocFunction(sizeof(IplImage));
     36            } catch(std::bad_cast& bc) {
     37                self->Err("io type mismatch\n");
     38            }
     39            break;
     40        default:
     41            self->Err("conversion not supported\n");
     42        }
     43    }
     44
     45    ~CvtColor_impl() {
     46        //FreeFunction((char*)(inIplImage));
     47        //FreeFunction((char*)(outIplImage));
     48    }
     49   
     50    void UpdateFrom(const io_data *data){
     51        Image *img=(Image*)data;
     52
     53        if(output!=NULL) {
     54            data->GetMutex();/*
     55            inIplImage->width=img->GetDataType().GetWidth();
     56            inIplImage->height=img->GetDataType().GetHeight();
     57            inIplImage->imageData=img->buffer;
     58            */
     59            output->GetMutex();/*
     60            outIplImage->width=output->GetDataType().GetWidth();
     61            outIplImage->height=output->GetDataType().GetHeight();
     62            outIplImage->imageData=output->buffer;
     63*/
     64            switch(conversion) {
     65            case flair::filter::CvtColor::Conversion_t::ToGray:
     66                switch(((Image*)data)->GetDataType().GetFormat()) {
     67                case Image::Type::Format::YUYV:
     68                    //dspCvtColor(inIplImage,outIplImage,DSP_YUYV2GRAY);
     69                    break;
     70                case Image::Type::Format::UYVY:
     71                    //dspCvtColor(inIplImage,outIplImage,DSP_UYVY2GRAY);
     72                    break;
     73                case Image::Type::Format::BGR:
     74                    //dspCvtColor(inIplImage,outIplImage,DSP_BGR2GRAY);
     75                    break;
     76                default:
     77                    self->Err("input format not supported\n");
     78                }
     79                break;
     80            default:
     81                self->Err("conversion not supported\n");
     82            }
     83
     84            output->ReleaseMutex();
     85            data->ReleaseMutex();
     86
     87            output->SetDataTime(data->DataTime());
     88        }
     89    };
     90   
     91    Image *output;
     92
     93private:
     94    flair::filter::CvtColor::Conversion_t conversion;
     95    //IplImage *inIplImage,*outIplImage;
     96    flair::filter::CvtColor *self;
     97};
     98
     99
    21100namespace flair { namespace filter {
    22101
    23 CvtColor::CvtColor(const core::IODevice* parent,std::string name,Conversion_t conversion) : IODevice(parent,name),output(0) {
    24     this->conversion=conversion;
    25 
    26     switch(conversion) {
    27     case Conversion_t::ToGray:
    28         try{
    29             Image::Type const &imageType=dynamic_cast<Image::Type const &>(parent->GetOutputDataType());
    30             output=new Image(this,imageType.GetWidth(),imageType.GetHeight(),Image::Type::Format::Gray,"conversion",true,2);
    31 
    32         } catch(std::bad_cast& bc) {
    33             Err("io type mismatch\n");
    34         }
    35         break;
    36     default:
    37         Err("conversion not supported\n");
    38     }
    39    
    40     SetIsReady(true);
     102CvtColor::CvtColor(const core::IODevice* parent,std::string name,Conversion_t conversion) : IODevice(parent,name) {
     103    pimpl_=new CvtColor_impl(this,conversion);
    41104}
    42105
    43 CvtColor::~CvtColor(void) {}
     106CvtColor::~CvtColor(void) {
     107    delete pimpl_;
     108}
    44109
    45110Image* CvtColor::Output(void) {
    46     return output;
     111    return pimpl_->output;
    47112
    48113}
    49114
    50115DataType const &CvtColor::GetOutputDataType() const {
    51     if(output!=NULL) {
    52         return output->GetDataType();
     116    if(pimpl_->output!=NULL) {
     117        return pimpl_->output->GetDataType();
    53118    } else {
    54119        return dummyType;
    … …  
    57122
    58123void CvtColor::UpdateFrom(const io_data *data) {
    59     Image *img=(Image*)data;
    60 
    61     data->GetMutex();
    62     output->GetMutex();
    63 
    64     switch(conversion) {
    65     case Conversion_t::ToGray:
    66         switch(img->GetDataType().GetFormat()) {
    67         case Image::Type::Format::YUYV:
    68             //dspCvtColor(img,output->img,DSP_YUYV2GRAY);
    69             break;
    70         case Image::Type::Format::UYVY:
    71             //dspCvtColor(img,output->img,DSP_UYVY2GRAY);
    72             break;
    73         case Image::Type::Format::BGR:
    74             //dspCvtColor(img,output->img,DSP_BGR2GRAY);
    75             break;
    76         default:
    77             Err("input format not supported\n");
    78         }
    79         break;
    80     default:
    81         Err("conversion not supported\n");
    82     }
    83 
    84     output->ReleaseMutex();
    85     data->ReleaseMutex();
    86 
    87     output->SetDataTime(data->DataTime());
    88     ProcessUpdate(output);
     124    pimpl_->UpdateFrom(data);
     125    if(pimpl_->output!=NULL) ProcessUpdate(pimpl_->output);
     126   
    89127}
    90128

  • branches/sanscv/lib/FlairVisionFilter/src/CvtColor.h

    r324 r325  
    1313#include <IODevice.h>
    1414
    15 namespace flair
    16 {
    17     namespace core
    18     {
     15namespace flair {
     16    namespace core {
    1917        class Image;
    2018    }
    2119}
    2220
    23 namespace flair
    24 {
     21class CvtColor_impl;
     22
     23namespace flair {
    2524namespace filter
    2625{
    … …  
    8281            void UpdateFrom(const core::io_data *data);
    8382
    84             core::Image *output;
    85             Conversion_t conversion;
     83            class CvtColor_impl *pimpl_;
    8684    };
    8785} // end namespace filter

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

    r324 r325  
    1313
    1414#include "HoughLines.h"
     15#include "VisionFilter.h"
    1516#include <Image.h>
     17#include <OneAxisRotation.h>
    1618#include <Matrix.h>
    1719#include <Layout.h>
    … …  
    1921#include <SpinBox.h>
    2022#include <DoubleSpinBox.h>
     23//#include <dspcv_gpp.h>
    2124#include <typeinfo>
     25#include <math.h>
    2226
    2327#define MAX_LINES 100
    … …  
    2731using namespace flair::gui;
    2832
     33class HoughLines_impl {
     34public:
     35    HoughLines_impl(flair::filter::HoughLines *self,const LayoutPosition* position,string name,const Vector2Df *inPtRefGlobal,float inThetaRefGlobal) {
     36        this->self=self;
     37        GroupBox* reglages_groupbox=new GroupBox(position,name);
     38        rotation=new OneAxisRotation(reglages_groupbox->NewRow(),"pre rotation",OneAxisRotation::PostRotation);
     39        fullRhoStep=new SpinBox(reglages_groupbox->NewRow(),"full rho step:","pixels",0,255,1,1);
     40        fullThetaStep=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"full theta step:","degrees",0,90,1,1);
     41        trackingRhoStep=new SpinBox(reglages_groupbox->NewRow(),"tracking rho step:","pixels",0,255,1,1);
     42        trackingThetaStep=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"tracking theta step:","degrees",0,90,1,1);
     43        trackingDeltaTheta=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"tracking delta theta:","degrees",0,90,1,1);
     44        nbPoints=new SpinBox(reglages_groupbox->NewRow(),"nb points:",0,10000,10,100);
     45
     46        isTracking=false;
     47        //linesStorage = (CvMat*)AllocFunction(sizeof(CvMat));
     48        //linesStorage->data.fl = (float*)AllocFunction(MAX_LINES*2*sizeof(float));//was CV_32FC2, 2 channels
     49        //gimg = (IplImage*)AllocFunction(sizeof(IplImage));
     50
     51        //init output matrix of same size as init
     52        MatrixDescriptor* desc=new MatrixDescriptor(4,1);
     53        desc->SetElementName(0,0,"distance");
     54        desc->SetElementName(1,0,"orientation rad");
     55        desc->SetElementName(2,0,"orientation deg");
     56        desc->SetElementName(3,0,"line_detected");
     57        output=new Matrix(self,desc,floatType,name);
     58        delete desc;
     59
     60        try{
     61            Image::Type const &imageType=dynamic_cast<Image::Type const &>(((IODevice*)(self->Parent()))->GetOutputDataType());
     62            if(imageType.GetFormat()!=Image::Type::Format::Gray) {
     63                self->Err("input image is not gray\n");
     64            }
     65        } catch(std::bad_cast& bc) {
     66            self->Err("io type mismatch\n");
     67        }
     68
     69        thetaRefGlobal=inThetaRefGlobal;
     70        if (inPtRefGlobal==NULL) {
     71            ptRefGlobal=NULL;
     72        } else { //rotation from global coordinates to hough space
     73            ptRefGlobal =new Vector2Df(inPtRefGlobal->x,inPtRefGlobal->y);
     74        }
     75       
     76    }
     77
     78    ~HoughLines_impl() {
     79        //FreeFunction((char*)(linesStorage->data.fl));
     80        //FreeFunction((char*)linesStorage);
     81        //FreeFunction((char*)gimg);
     82        if(ptRefGlobal!=NULL) delete ptRefGlobal;
     83    }
     84   
     85    void UpdateFrom(const io_data *data){
     86        Image *img=(Image*)data;
     87  /*
     88        gimg->width=img->GetDataType().GetWidth();
     89        gimg->height=img->GetDataType().GetHeight();
     90        gimg->imageData=img->buffer;
     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
    29232namespace flair { namespace filter {
    30233
    31 HoughLines::HoughLines(const IODevice* parent,const LayoutPosition* position,string name) : IODevice(parent,name) {
    32     GroupBox* reglages_groupbox=new GroupBox(position,name);
    33     fullRhoStep=new SpinBox(reglages_groupbox->NewRow(),"full rho step:","pixels",0,255,1,1);
    34     fullThetaStep=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"full theta step:","degrees",0,90,1,1);
    35     trackingRhoStep=new SpinBox(reglages_groupbox->NewRow(),"tracking rho step:","pixels",0,255,1,1);
    36     trackingThetaStep=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"tracking theta step:","degrees",0,90,1,1);
    37     trackingDeltaTheta=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"tracking delta theta:","degrees",0,90,1,1);
    38     nbPoints=new SpinBox(reglages_groupbox->NewRow(),"nb points:",0,10000,10,100);
    39 
    40     isTracking=false;
    41     lostLine=false;
    42     initLine=false;
    43     linesStorage = (CvMat*)malloc(sizeof(CvMat));
    44     linesStorage->data.fl = (float*)malloc(MAX_LINES*2*sizeof(float));//was CV_32FC2, 2 channels
    45 
    46 
    47     //init output matrix of same size as init
    48     MatrixDescriptor* desc=new MatrixDescriptor(4,1);
    49     desc->SetElementName(0,0,"distance");
    50     desc->SetElementName(1,0,"orientation rad");
    51     desc->SetElementName(2,0,"orientation deg");
    52     desc->SetElementName(3,0,"line_detected");
    53     output=new Matrix(this,desc,floatType,name);
    54     delete desc;
    55 
    56     try{
    57         Image::Type const &imageType=dynamic_cast<Image::Type const &>(parent->GetOutputDataType());
    58         if(imageType.GetFormat()!=Image::Type::Format::Gray) {
    59             Err("input image is not gray\n");
    60             return;
    61         }
    62     } catch(std::bad_cast& bc) {
    63         Err("io type mismatch\n");
    64         return;
    65     }
    66    
    67     SetIsReady(true);
     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   
    68237}
    69238
    70239HoughLines::~HoughLines(void) {
    71     free((char*)(linesStorage->data.fl));
    72     free((char*)linesStorage);
     240   delete  pimpl_;
    73241}
    74242
    75243void HoughLines::UpdateFrom(const io_data *data) {
    76  
     244    pimpl_->UpdateFrom(data);
     245    ProcessUpdate(pimpl_->output);
    77246}
    78247
    79248bool HoughLines::isLineDetected() const {
    80     if(output->Value(3,0)==1) {
     249    if(pimpl_->output->Value(3,0)==1) {
    81250        return true;
    82251    } else {
    … …  
    86255
    87256float HoughLines::GetOrientation(void) const {
    88     return output->Value(1,0);
     257    return pimpl_->output->Value(1,0);
    89258}
    90259
    91260float HoughLines::GetDistance(void) const {
    92     return output->Value(0,0);
     261    return pimpl_->output->Value(0,0);
    93262}
    94263
    95264Matrix *HoughLines::Output(void) const {
    96     return output;
     265    return pimpl_->output;
    97266}
    98267

  • branches/sanscv/lib/FlairVisionFilter/src/HoughLines.h

    r324 r325  
    1212
    1313#include <IODevice.h>
    14 #include <cvtypes.h>
     14#include <Vector2D.h>
    1515
    1616namespace flair {
    1717    namespace core {
    18         class Image;
    1918        class Matrix;
    2019    }
    2120    namespace gui {
    2221        class LayoutPosition;
    23         class SpinBox;
    24         class DoubleSpinBox;
    2522    }
    2623}
     24
     25class HoughLines_impl;
    2726
    2827namespace flair { namespace filter {
    … …  
    4443            * \param position position
    4544            * \param name name
     45            * \param ptRefGlobal reference point used to compute distance to the line
     46            * \param thetaRefGlobal reference angle used to compute orientation in range [-pi/2,pi/2]. Default value is 0 which defines a vertical line
    4647            */
    47             HoughLines(const core::IODevice* parent,const gui::LayoutPosition* position,std::string name);
     48            HoughLines(const core::IODevice* parent,const gui::LayoutPosition* position,std::string name,const core::Vector2Df *ptRefGlobal=NULL,float thetaRefGlobal=0);
    4849
    4950            /*!
    … …  
    6061        private:
    6162            void UpdateFrom(const core::io_data *data);
    62             gui::SpinBox *fullRhoStep,*trackingRhoStep,*nbPoints;
    63             gui::DoubleSpinBox *fullThetaStep,*trackingThetaStep,*trackingDeltaTheta;
    64             bool isTracking,lostLine,initLine,noLine;
    65             int nbLines;
    66             float yawMean,prevYawMean;
    67             float prevRhoMean,rhoMean;
    68             float prevThetaMean,thetaMean;
    69             float distance,orientation;
    70             CvMat* linesStorage;
    71             core::Matrix *output;
     63            class HoughLines_impl *pimpl_;
     64           
     65           
    7266    };
    7367} // end namespace filter

  • branches/sanscv/lib/FlairVisionFilter/src/ImgThreshold.cpp

    r324 r325  
    1313
    1414#include "ImgThreshold.h"
     15#include "VisionFilter.h"
    1516#include <Image.h>
    1617#include <Layout.h>
    1718#include <GroupBox.h>
    1819#include <SpinBox.h>
     20//#include <dspcv_gpp.h>
    1921#include <typeinfo>
    2022
    … …  
    2325using namespace flair::gui;
    2426
     27class ImgThreshold_impl {
     28public:
     29    ImgThreshold_impl(flair::filter::ImgThreshold *self,const LayoutPosition* position,string name):output(0) {
     30        this->self=self;
     31       
     32        GroupBox* reglages_groupbox=new GroupBox(position,name);
     33        threshold=new SpinBox(reglages_groupbox->NewRow(),"threshold:",0,255,1,127);
     34
     35        Printf("todo: pouvoir reutiliser la meme image en sortie\n");
     36        try{
     37            Image::Type const &imageType=dynamic_cast<Image::Type const &>(((IODevice*)(self->Parent()))->GetOutputDataType());
     38            if(imageType.GetFormat()==Image::Type::Format::Gray) {
     39                output=new Image(self,imageType.GetWidth(),imageType.GetHeight(),imageType.GetFormat(),"threshold");
     40                //inIplImage = (IplImage*)AllocFunction(sizeof(IplImage));
     41                //outIplImage = (IplImage*)AllocFunction(sizeof(IplImage));
     42            } else {
     43                self->Err("input image is not gray\n");
     44            }
     45        } catch(std::bad_cast& bc) {
     46            self->Err("io type mismatch\n");
     47        }
     48
     49       
     50    }
     51
     52    ~ImgThreshold_impl() {
     53        //FreeFunction((char*)(inIplImage));
     54        //FreeFunction((char*)(outIplImage));
     55    }
     56   
     57    void UpdateFrom(const io_data *data){
     58        Image *img=(Image*)data;
     59/*
     60        data->GetMutex();
     61        inIplImage->width=img->GetDataType().GetWidth();
     62        inIplImage->height=img->GetDataType().GetHeight();
     63        inIplImage->imageData=img->buffer;
     64       
     65        output->GetMutex();
     66        outIplImage->width=output->GetDataType().GetWidth();
     67        outIplImage->height=output->GetDataType().GetHeight();
     68        outIplImage->imageData=output->buffer;
     69               
     70        dspThreshold(inIplImage,outIplImage, threshold->Value(), 255, CV_THRESH_BINARY);
     71        output->ReleaseMutex();
     72        data->ReleaseMutex();
     73*/
     74        output->SetDataTime(data->DataTime());
     75    };
     76   
     77    Image *output;
     78
     79private:
     80    flair::filter::ImgThreshold *self;
     81    SpinBox *threshold;
     82    //IplImage *inIplImage,*outIplImage;
     83};
     84
    2585namespace flair { namespace filter {
    2686
    27 ImgThreshold::ImgThreshold(const IODevice* parent,const LayoutPosition* position,string name) : IODevice(parent,name),output(0) {
    28     GroupBox* reglages_groupbox=new GroupBox(position,name);
    29     threshold=new SpinBox(reglages_groupbox->NewRow(),"threshold:",0,255,1,127);
    30 
    31     Printf("todo: pouvoir reutiliser la meme image en sortie\n");
    32     try{
    33         Image::Type const &imageType=dynamic_cast<Image::Type const &>(parent->GetOutputDataType());
    34         if(imageType.GetFormat()==Image::Type::Format::Gray) {
    35             output=new Image(this,imageType.GetWidth(),imageType.GetHeight(),imageType.GetFormat(),"threshold");
    36         } else {
    37             Err("input image is not gray\n");
    38             return;
    39         }
    40     } catch(std::bad_cast& bc) {
    41         Err("io type mismatch\n");
    42         return;
    43     }
    44     SetIsReady(true);
     87ImgThreshold::ImgThreshold(const IODevice* parent,const LayoutPosition* position,string name) : IODevice(parent,name) {
     88    pimpl_=new ImgThreshold_impl(this,position,name);
    4589}
    4690
    4791ImgThreshold::~ImgThreshold(void) {
     92    delete pimpl_;
    4893}
    4994
    5095Image* ImgThreshold::Output(void) {
    51     return output;
     96    return pimpl_->output;
    5297}
    5398
    5499void ImgThreshold::UpdateFrom(const io_data *data) {
    55  /*   Image *cvImage=(Image*)data;
    56     IplImage *gimg=cvImage->img;
    57 
    58     data->GetMutex();
    59     output->GetMutex();
    60     dspThreshold(gimg, output->img, threshold->Value(), 255, CV_THRESH_BINARY);
    61     output->ReleaseMutex();
    62     data->ReleaseMutex();
    63 */
    64     output->SetDataTime(data->DataTime());
    65     ProcessUpdate(output);
     100    pimpl_->UpdateFrom(data);
     101    ProcessUpdate(pimpl_->output);
    66102}
    67103
    68104DataType const &ImgThreshold::GetOutputDataType() const {
    69     if(output!=NULL) {
    70         return output->GetDataType();
     105    if(pimpl_->output!=NULL) {
     106        return pimpl_->output->GetDataType();
    71107    } else {
    72108        return dummyType;

  • branches/sanscv/lib/FlairVisionFilter/src/ImgThreshold.h

    r324 r325  
    1919    namespace gui {
    2020        class LayoutPosition;
    21         class SpinBox;
    2221    }
    2322}
     23
     24class ImgThreshold_impl;
    2425
    2526namespace flair { namespace filter {
    … …  
    6364        private:
    6465            void UpdateFrom(const core::io_data *data);
    65             core::Image *output;
    66             gui::SpinBox *threshold;
     66            class ImgThreshold_impl *pimpl_;
    6767    };
    6868} // end namespace filter

  • branches/sanscv/lib/FlairVisionFilter/src/OpticalFlow.cpp

    r324 r325  
    1414#include "OpticalFlow.h"
    1515#include "OpticalFlowData.h"
     16#include "VisionFilter.h"
    1617#include <Image.h>
    1718#include <Layout.h>
    1819#include <GroupBox.h>
    1920#include <SpinBox.h>
    20 //#include <algorithm>
     21//#include <dspcv_gpp.h>
     22#include <algorithm>
    2123#include <OneAxisRotation.h>
    2224#include <Vector3D.h>
    … …  
    3032using namespace flair::gui;
    3133
     34class OpticalFlow_impl {
     35public:
     36    OpticalFlow_impl(flair::filter::OpticalFlow *self,const LayoutPosition* position,string name) {
     37        this->self=self;
     38        is_init=false;
     39
     40        GroupBox* reglages_groupbox=new GroupBox(position,name);
     41        rotation=new OneAxisRotation(reglages_groupbox->NewRow(),"post rotation",OneAxisRotation::PostRotation);
     42        max_features=new SpinBox(reglages_groupbox->NewRow(),"max features:",1,65535,1,1);
     43       
     44        try{
     45            Image::Type const &imageType=dynamic_cast<Image::Type const &>(((IODevice*)(self->Parent()))->GetOutputDataType());
     46            pyr=new Image(self,imageType.GetWidth(),imageType.GetHeight(),Image::Type::Format::Gray);
     47            pyr_old=new Image(self,imageType.GetWidth(),imageType.GetHeight(),Image::Type::Format::Gray);
     48            output=new flair::filter::OpticalFlowData(self->Parent(),max_features->Value());
     49/*
     50            pointsA=(CvPoint*)AllocFunction(max_features->Value()*sizeof(CvPoint));
     51            pointsB=(CvPoint*)AllocFunction(max_features->Value()*sizeof(CvPoint));
     52
     53            found_feature=(char*)AllocFunction(max_features->Value()*sizeof(char));
     54            feature_error=(unsigned int*)AllocFunction(max_features->Value()*sizeof(unsigned int));
     55           
     56            iplgimg = (IplImage*)AllocFunction(sizeof(IplImage));
     57            iplgimg_old = (IplImage*)AllocFunction(sizeof(IplImage));
     58            iplpyr = (IplImage*)AllocFunction(sizeof(IplImage));
     59            iplpyr_old = (IplImage*)AllocFunction(sizeof(IplImage));
     60           
     61            iplpyr->width=imageType.GetWidth();
     62            iplpyr->height=imageType.GetHeight();
     63            iplpyr_old->width=imageType.GetWidth();
     64            iplpyr_old->height=imageType.GetHeight();
     65*/ 
     66      } catch(std::bad_cast& bc) {
     67            self->Err("io type mismatch\n");
     68            pyr=NULL;
     69            pyr_old=NULL;
     70        }
     71     
     72    }
     73
     74    ~OpticalFlow_impl() {
     75        /*
     76        FreeFunction((char*)pointsA);
     77        FreeFunction((char*)pointsB);
     78        FreeFunction((char*)found_feature);
     79        FreeFunction((char*)feature_error);
     80        FreeFunction((char*)iplgimg);
     81        FreeFunction((char*)iplgimg_old);
     82        FreeFunction((char*)iplpyr);
     83        FreeFunction((char*)iplpyr_old);*/
     84    }
     85   
     86    void UpdateFrom(const io_data *data){
     87        //Time tStart=GetTime();
     88        Image *img=(Image*)data;
     89        Image *img_old=((Image*)data->Prev(1));
     90/*
     91        iplgimg->width=img->GetDataType().GetWidth();
     92        iplgimg->height=img->GetDataType().GetHeight();
     93        iplgimg->imageData=img->buffer;
     94       
     95        iplgimg_old->width=img_old->GetDataType().GetWidth();
     96        iplgimg_old->height=img_old->GetDataType().GetHeight();
     97        iplgimg_old->imageData=img_old->buffer;
     98       
     99        iplpyr->imageData=pyr->buffer;
     100        iplpyr_old->imageData=pyr_old->buffer;
     101
     102        unsigned int count;
     103        CvSize window = {3,3};
     104        CvTermCriteria termination_criteria ={CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, .3 };
     105        unsigned int i;
     106
     107        if(max_features->ValueChanged()==true) {
     108            FreeFunction((char*)pointsA);
     109            FreeFunction((char*)pointsB);
     110            FreeFunction((char*)found_feature);
     111            FreeFunction((char*)feature_error);
     112            pointsA=(CvPoint*)AllocFunction(max_features->Value()*sizeof(CvPoint));
     113            pointsB=(CvPoint*)AllocFunction(max_features->Value()*sizeof(CvPoint));
     114            found_feature=(char*)AllocFunction(max_features->Value()*sizeof(char));
     115            feature_error=(unsigned int*)AllocFunction(max_features->Value()*sizeof(unsigned int));
     116
     117            output->Resize(max_features->Value());
     118        }
     119
     120        if(is_init==false) {
     121            data->GetMutex();
     122            //init image old
     123            dspPyrDown(iplgimg,iplpyr_old,LEVEL_PYR);
     124            data->ReleaseMutex();
     125            is_init=true;
     126            printf("ajouter mise a 0 des points\n");
     127            return;
     128        }
     129
     130        data->GetMutex();
     131        data->Prev(1)->GetMutex();
     132
     133        //select good features
     134        count=max_features->Value();
     135        dspGoodFeaturesToTrack(iplgimg_old,pointsA,&count,0.08,5);
     136        //pyramide
     137        dspPyrDown(iplgimg,iplpyr,LEVEL_PYR);
     138        //lk
     139        dspCalcOpticalFlowPyrLK(iplgimg_old,iplgimg,iplpyr_old,iplpyr,pointsA,pointsB,count,window,LEVEL_PYR,found_feature,feature_error,termination_criteria,0) ;
     140
     141        data->Prev(1)->ReleaseMutex();
     142        data->ReleaseMutex();
     143
     144        //apply rotation
     145        for(i=0;i<count;i++) {
     146            Vector3Df tmp;
     147            tmp.x=pointsA[i].x;
     148            tmp.y=pointsA[i].y;
     149            tmp.z=0;
     150            rotation->ComputeRotation(tmp);
     151            pointsA[i].x=tmp.x;
     152            pointsA[i].y=tmp.y;
     153
     154            tmp.x=pointsB[i].x;
     155            tmp.y=pointsB[i].y;
     156            tmp.z=0;
     157            rotation->ComputeRotation(tmp);
     158            pointsB[i].x=tmp.x;
     159            pointsB[i].y=tmp.y;
     160        }
     161
     162        output->GetMutex();
     163        CvPoint2D32f* pointsBf= output->PointsB();
     164        for(i=0;i<count;i++) {
     165            pointsBf[i].x=pointsA[i].x+((float)pointsB[i].x)/256;
     166            pointsBf[i].y=pointsA[i].y+((float)pointsB[i].y)/256;
     167        }
     168        output->ReleaseMutex();
     169
     170        output->SetPointsA(pointsA);
     171        output->SetFoundFeature(found_feature);
     172        output->SetFeatureError(feature_error);
     173        output->SetNbFeatures(count);
     174
     175        //rotation
     176        swap(pyr,pyr_old);
     177*/
     178        output->SetDataTime(data->DataTime());
     179        //Printf("Optical flow computation time=%f\n",(GetTime()-tStart)/(1000.*1000));
     180    };
     181   
     182    flair::filter::OpticalFlowData *output;
     183
     184private:
     185    flair::filter::OpticalFlow *self;
     186    SpinBox *max_features;
     187    OneAxisRotation* rotation;
     188
     189    //CvPoint* pointsA;
     190    //CvPoint* pointsB;
     191    char *found_feature;
     192    unsigned int *feature_error;
     193    Image *pyr,*pyr_old;
     194    //IplImage *iplgimg,*iplgimg_old;
     195    //IplImage *iplpyr,*iplpyr_old;
     196
     197    bool is_init;
     198};
     199
     200
    32201namespace flair
    33202{
    … …  
    35204{
    36205
    37 OpticalFlow::OpticalFlow(const IODevice* parent,const LayoutPosition* position,string name) : IODevice(parent,name)
    38 {
     206OpticalFlow::OpticalFlow(const IODevice* parent,const LayoutPosition* position,string name) : IODevice(parent,name) {
    39207    Printf("optical flow: voir pour faire du multiple output\n");//pour pts A et B, found et error
    40 
    41     try{
    42         Image::Type const &imageType=dynamic_cast<Image::Type const &>(parent->GetOutputDataType());
    43         pyr=new Image(this,imageType.GetWidth(),imageType.GetHeight(),Image::Type::Format::Gray);
    44         pyr_old=new Image(this,imageType.GetWidth(),imageType.GetHeight(),Image::Type::Format::Gray);
    45     } catch(std::bad_cast& bc) {
    46         Err("io type mismatch\n");
    47         pyr=NULL;
    48         pyr_old=NULL;
    49         return;
     208    pimpl_=new OpticalFlow_impl(this,position,name);
     209}
     210
     211OpticalFlow::~OpticalFlow(void) {
     212    delete pimpl_;
     213}
     214
     215void OpticalFlow::UpdateFrom(const io_data *data) {
     216    pimpl_->UpdateFrom(data);
     217    ProcessUpdate(pimpl_->output);
     218}
     219
     220OpticalFlowData* OpticalFlow::Output(void) {
     221    return pimpl_->output;
     222
     223}
     224
     225DataType const &OpticalFlow::GetOutputDataType() const {
     226    if(pimpl_->output!=NULL) {
     227        return pimpl_->output->GetDataType();
     228    } else {
     229        return dummyType;
    50230    }
    51 
    52     is_init=false;
    53 
    54     GroupBox* reglages_groupbox=new GroupBox(position,name);
    55     rotation=new OneAxisRotation(reglages_groupbox->NewRow(),"post rotation",OneAxisRotation::PostRotation);
    56     max_features=new SpinBox(reglages_groupbox->NewRow(),"max features:",1,65535,1,1);
    57 
    58     output=new OpticalFlowData(parent,max_features->Value());
    59 
    60     pointsA=(CvPoint*)malloc(max_features->Value()*sizeof(CvPoint));
    61     pointsB=(CvPoint*)malloc(max_features->Value()*sizeof(CvPoint));
    62 
    63     found_feature=(char*)malloc(max_features->Value()*sizeof(char));
    64     feature_error=(unsigned int*)malloc(max_features->Value()*sizeof(unsigned int));
    65    
    66     SetIsReady(true);
    67 }
    68 
    69 OpticalFlow::~OpticalFlow(void) {
    70     free((char*)pointsA);
    71     free((char*)pointsB);
    72     free((char*)found_feature);
    73     free((char*)feature_error);
    74 }
    75 
    76 void OpticalFlow::UpdateFrom(const io_data *data) {/*
    77     IplImage *gimg=((Image*)data)->img;
    78     IplImage *gimg_old=((Image*)data->Prev(1))->img;
    79 
    80     unsigned int count;
    81     CvSize window = cvSize(3,3);
    82     CvTermCriteria termination_criteria = cvTermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, .3 );
    83     unsigned int i;
    84 
    85     if(max_features->ValueChanged()==true) {
    86         cvFree(&pointsA);
    87         cvFree(&pointsB);
    88         cvFree(&found_feature);
    89         cvFree(&feature_error);
    90         pointsA=(CvPoint *)cvAlloc(max_features->Value()*sizeof(CvPoint));
    91         pointsB=(CvPoint *)cvAlloc(max_features->Value()*sizeof(CvPoint));
    92         found_feature=(char *)cvAlloc(max_features->Value()*sizeof(char));
    93         feature_error=(unsigned int *)cvAlloc(max_features->Value()*sizeof(unsigned int));
    94 
    95         output->Resize(max_features->Value());
    96     }
    97 
    98     if(is_init==false)
    99     {
    100         data->GetMutex();
    101         //init image old
    102         dspPyrDown(gimg,pyr_old,LEVEL_PYR);
    103         data->ReleaseMutex();
    104         is_init=true;
    105         printf("ajouter mise a 0 des points\n");
    106         return;
    107     }
    108 
    109     data->GetMutex();
    110     data->Prev(1)->GetMutex();
    111 
    112     //select good features
    113     count=max_features->Value();
    114     dspGoodFeaturesToTrack(gimg_old,pointsA,&count,0.08,5);
    115     //pyramide
    116     dspPyrDown(gimg,pyr,LEVEL_PYR);
    117     //lk
    118     dspCalcOpticalFlowPyrLK(gimg_old,gimg,pyr_old,pyr,pointsA,pointsB,count,window,LEVEL_PYR,found_feature,feature_error,termination_criteria,0) ;
    119 
    120     data->Prev(1)->ReleaseMutex();
    121     data->ReleaseMutex();
    122 
    123     //apply rotation
    124     for(i=0;i<count;i++) {
    125         Vector3Df tmp;
    126         tmp.x=pointsA[i].x;
    127         tmp.y=pointsA[i].y;
    128         tmp.z=0;
    129         rotation->ComputeRotation(tmp);
    130         pointsA[i].x=tmp.x;
    131         pointsA[i].y=tmp.y;
    132 
    133         tmp.x=pointsB[i].x;
    134         tmp.y=pointsB[i].y;
    135         tmp.z=0;
    136         rotation->ComputeRotation(tmp);
    137         pointsB[i].x=tmp.x;
    138         pointsB[i].y=tmp.y;
    139     }
    140 
    141     output->GetMutex();
    142     CvPoint2D32f* pointsBf= output->PointsB();
    143     for(i=0;i<count;i++)
    144     {
    145         pointsBf[i].x=pointsA[i].x+((float)pointsB[i].x)/256;
    146         pointsBf[i].y=pointsA[i].y+((float)pointsB[i].y)/256;
    147     }
    148     output->ReleaseMutex();
    149 
    150     output->SetPointsA(pointsA);
    151     output->SetFoundFeature(found_feature);
    152     output->SetFeatureError(feature_error);
    153     output->SetNbFeatures(count);
    154 
    155     //rotation
    156     swap(pyr,pyr_old);
    157 */
    158     output->SetDataTime(data->DataTime());
    159     ProcessUpdate(output);
    160231}
    161232

  • branches/sanscv/lib/FlairVisionFilter/src/OpticalFlow.h

    r324 r325  
    1212
    1313#include <IODevice.h>
    14 #include <cvtypes.h>
    1514
    1615namespace flair {
    17     namespace core {
    18         class Image;
    19         class OneAxisRotation;
    20     }
    2116    namespace gui {
    2217        class LayoutPosition;
    23         class SpinBox;
    2418    }
    2519    namespace filter {
    … …  
    2721    }
    2822}
     23
     24class OpticalFlow_impl;
    2925
    3026namespace flair {
    … …  
    5753            */
    5854            ~OpticalFlow();
     55           
     56            filter::OpticalFlowData* Output(void);
     57
     58            core::DataType const &GetOutputDataType() const;
    5959
    6060        private:
    6161            void UpdateFrom(const core::io_data *data);
    62             OpticalFlowData *output;
    63             gui::SpinBox *max_features;
    64             core::OneAxisRotation* rotation;
    65 
    66             CvPoint* pointsA;
    67             CvPoint* pointsB;
    68             char *found_feature;
    69             unsigned int *feature_error;
    70             core::Image *pyr,*pyr_old;
    71 
    72             bool is_init;
     62           
     63            class OpticalFlow_impl *pimpl_;
     64           
    7365    };
    7466} // end namespace filter

  • branches/sanscv/lib/FlairVisionFilter/src/OpticalFlowCompensated.cpp

    r324 r325  
    3232using namespace flair::gui;
    3333
     34class OpticalFlowCompensated_impl {
     35public:
     36    OpticalFlowCompensated_impl(flair::filter::OpticalFlowCompensated *self,const flair::filter::Ahrs *ahrs, const LayoutPosition* position, string name): ahrs(ahrs), output(NULL) {
     37        this->self=self;
     38        previousStepsAngularRates=new Vector3Df*[10];
     39        for (int i=0; i<10; i++) previousStepsAngularRates[i]=NULL;
     40        previousStepsAngularRatesIndex=0;
     41
     42        GroupBox* reglages_groupbox=new GroupBox(position,name);
     43        //TODO: the gyroDelay is set to compensate for the time difference between image snapshot et gyro measurements
     44        //it is equal to the time between 2 images (because optical flow always lags 1 frame) + the time to compute optical flow
     45        //here it is approximated by 2 video frames
     46        gyroDelay=new SpinBox(reglages_groupbox->NewRow(),"gyro delay (in video frames):",0,10,1,2);
     47        gyroGain=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"gyro gain:",0.,500.,10.,2,300.);
     48
     49       
     50    }
     51
     52    ~OpticalFlowCompensated_impl() {
     53        delete output;
     54    }
     55   
     56    void UpdateFrom(const io_data *data){
     57        flair::filter::OpticalFlowData *input=(flair::filter::OpticalFlowData *)data;
     58        if (!output) { //first pass
     59            output=new flair::filter::OpticalFlowData(self,input->MaxFeatures(),input->ObjectName()+"_filtered");
     60            previousTime=input->DataTime();
     61            return;
     62        }
     63        //  float kX=320/Euler::ToRadian(70); //TEST: only for default simuCameraGL. fov=70° and image width=320 => k=320*180/(70*pi)
     64        //  float kY=240/Euler::ToRadian(70); //TEST: only for default simuCameraGL. fov=70° and image height=240
     65        float kX=gyroGain->Value();
     66        float kY=gyroGain->Value();
     67        float deltaT=(input->DataTime()-previousTime)/(1000*1000*1000.);
     68        previousTime=input->DataTime();
     69
     70        int delayedIndex=previousStepsAngularRatesIndex-gyroDelay->Value(); // Ahhh décalage, esprit canal...
     71        if (delayedIndex<0) delayedIndex+=10;
     72
     73        if (!previousStepsAngularRates[previousStepsAngularRatesIndex]) {
     74            previousStepsAngularRates[previousStepsAngularRatesIndex]=new Vector3Df();
     75        }
     76        *previousStepsAngularRates[previousStepsAngularRatesIndex++]=ahrs->GetDatas()->GetAngularRates();
     77
     78        if (!previousStepsAngularRates[delayedIndex]) return;
     79        float rotationFlowX=previousStepsAngularRates[delayedIndex]->y*deltaT*kY;
     80        float rotationFlowY=-previousStepsAngularRates[delayedIndex]->x*deltaT*kX;
     81        if (previousStepsAngularRatesIndex==10) previousStepsAngularRatesIndex=0;
     82        input->GetMutex();
     83        output->GetMutex();
     84
     85        for (int i=0; i<input->NbFeatures(); i++) {
     86            output->PointsA()[i].x=input->PointsA()[i].x;
     87            output->PointsA()[i].y=input->PointsA()[i].y;
     88            output->PointsB()[i].x=input->PointsB()[i].x-rotationFlowX;
     89            output->PointsB()[i].y=input->PointsB()[i].y-rotationFlowY;
     90        }
     91        output->SetNbFeatures(input->NbFeatures());
     92        output->SetFoundFeature(input->FoundFeature());
     93        output->SetFeatureError(input->FeatureError());
     94
     95        output->ReleaseMutex();
     96        input->ReleaseMutex();
     97
     98        output->SetDataTime(input->DataTime());
     99           
     100    };
     101   
     102    flair::filter::OpticalFlowData *output;
     103
     104private:
     105    flair::filter::OpticalFlowCompensated *self;
     106    Time previousTime;
     107    const flair::filter::Ahrs *ahrs;
     108    Vector3Df **previousStepsAngularRates;
     109    unsigned int previousStepsAngularRatesIndex;
     110    SpinBox *gyroDelay;
     111    DoubleSpinBox *gyroGain;
     112};
     113
     114
    34115namespace flair {
    35116namespace filter {
    36117
    37 OpticalFlowCompensated::OpticalFlowCompensated(const OpticalFlow *parent, const Ahrs *ahrs, const LayoutPosition* position, string name) : IODevice(parent, name), ahrs(ahrs), output(NULL) {
    38   MatrixDescriptor* desc=new MatrixDescriptor(3,2);
    39   desc->SetElementName(0,0,"raw displacement X");
    40   desc->SetElementName(0,1,"raw displacement Y");
    41   desc->SetElementName(1,0,"compensation X");
    42   desc->SetElementName(1,1,"compensation Y");
    43   desc->SetElementName(2,0,"displacement with compensation X");
    44   desc->SetElementName(2,0,"displacement with compensation Y");
    45   firstPointDisplacement=new Matrix(this,desc,floatType,name);
    46   delete desc;
    47   previousStepsAngularRates=new Vector3Df*[10];
    48   for (int i=0; i<10; i++) previousStepsAngularRates[i]=NULL;
    49   previousStepsAngularRatesIndex=0;
    50 
    51   GroupBox* reglages_groupbox=new GroupBox(position,name);
    52   //TODO: the gyroDelay is set to compensate for the time difference between image snapshot et gyro measurements
    53 //it is equal to the time between 2 images (because optical flow always lags 1 frame) + the time to compute optical flow
    54 //here it is approximated by 2 video frames
    55   gyroDelay=new SpinBox(reglages_groupbox->NewRow(),"gyro delay (in video frames):",0,10,1,2);
    56   gyroGain=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"gyro gain:",0.,500.,10.,2,300.);
     118OpticalFlowCompensated::OpticalFlowCompensated(const OpticalFlow *parent, const Ahrs *ahrs, const LayoutPosition* position, string name) : IODevice(parent, name) {
     119    pimpl_=new OpticalFlowCompensated_impl(this,ahrs,position,name);
    57120
    58121}
    59122
    60123OpticalFlowCompensated::~OpticalFlowCompensated() {
    61   delete output;
     124    delete pimpl_;
    62125}
    63126
    64127void OpticalFlowCompensated::UpdateFrom(const io_data *data) {
    65   OpticalFlowData *input=(OpticalFlowData *)data;
    66   if (!output) { //first pass
    67     output=new OpticalFlowData(this,input->MaxFeatures(),input->ObjectName()+"_filtered");
    68     previousTime=input->DataTime();
    69     return;
    70   }
    71 //  float kX=320/Euler::ToRadian(70); //TEST: only for default simuCameraGL. fov=70° and image width=320 => k=320*180/(70*pi)
    72 //  float kY=240/Euler::ToRadian(70); //TEST: only for default simuCameraGL. fov=70° and image height=240
    73   float kX=gyroGain->Value();
    74   float kY=gyroGain->Value();
    75   float deltaT=(input->DataTime()-previousTime)/(1000*1000*1000.);
    76   previousTime=input->DataTime();
    77 
    78   int delayedIndex=previousStepsAngularRatesIndex-gyroDelay->Value(); // Ahhh décalage, esprit canal...
    79   if (delayedIndex<0) delayedIndex+=10;
    80 
    81   if (!previousStepsAngularRates[previousStepsAngularRatesIndex]) {
    82     previousStepsAngularRates[previousStepsAngularRatesIndex]=new Vector3Df();
    83   }
    84   *previousStepsAngularRates[previousStepsAngularRatesIndex++]=ahrs->GetDatas()->GetAngularRates();
    85 
    86   if (!previousStepsAngularRates[delayedIndex]) return;
    87   float rotationFlowX=previousStepsAngularRates[delayedIndex]->y*deltaT*kY;
    88   float rotationFlowY=-previousStepsAngularRates[delayedIndex]->x*deltaT*kX;
    89   if (previousStepsAngularRatesIndex==10) previousStepsAngularRatesIndex=0;
    90   input->GetMutex();
    91   output->GetMutex();
    92 
    93   for (int i=0; i<input->NbFeatures(); i++) {
    94     if (!i) {
    95       firstPointDisplacement->SetValue(0,0,input->PointsB()[i].x-input->PointsA()[i].x);
    96       firstPointDisplacement->SetValue(0,1,input->PointsB()[i].y-input->PointsA()[i].y);
    97       firstPointDisplacement->SetValue(1,0,-rotationFlowX);
    98       firstPointDisplacement->SetValue(1,1,-rotationFlowY);
    99       firstPointDisplacement->SetValue(2,0,input->PointsB()[i].x-input->PointsA()[i].x-rotationFlowX);
    100       firstPointDisplacement->SetValue(2,1,input->PointsB()[i].y-input->PointsA()[i].y-rotationFlowY);
    101     }
    102     output->PointsA()[i].x=input->PointsA()[i].x;
    103     output->PointsA()[i].y=input->PointsA()[i].y;
    104     output->PointsB()[i].x=input->PointsB()[i].x-rotationFlowX;
    105     output->PointsB()[i].y=input->PointsB()[i].y-rotationFlowY;
    106   }
    107   output->SetNbFeatures(input->NbFeatures());
    108   output->SetFoundFeature(input->FoundFeature());
    109   output->SetFeatureError(input->FeatureError());
    110 
    111   output->ReleaseMutex();
    112   input->ReleaseMutex();
    113 
    114   output->SetDataTime(input->DataTime());
    115   ProcessUpdate(output);
     128    pimpl_->UpdateFrom(data);
     129    ProcessUpdate(pimpl_->output);
    116130}
    117131
    118   Matrix *OpticalFlowCompensated::GetFirstPointDisplacement() const {
    119     return firstPointDisplacement;
    120   }
    121132
    122133} // end namespace filter

  • branches/sanscv/lib/FlairVisionFilter/src/OpticalFlowCompensated.h

    r324 r325  
    1616#include <IODevice.h>
    1717#include <Object.h>
    18 #include <Vector3D.h>
    1918
    2019namespace flair {
    21   namespace core {
    22     class io_data;
    23     class Matrix;
    24   }
    2520  namespace gui {
    2621    class LayoutPosition;
    27     class SpinBox;
    28     class DoubleSpinBox;
    2922  }
    3023  namespace filter {
    3124    class Ahrs;
    3225    class OpticalFlow;
    33     class OpticalFlowData;
    3426  }
    3527}
     28
     29class OpticalFlowCompensated_impl;
    3630
    3731namespace flair { namespace filter {
    … …  
    6155  ~OpticalFlowCompensated();
    6256
    63   void UpdateFrom(const core::io_data *data);
    64 
    65   core::Matrix *GetFirstPointDisplacement() const;
    6657private:
    67   OpticalFlowData *output;
    68   core::Time previousTime;
    69   const Ahrs *ahrs;
    70   core::Matrix *firstPointDisplacement;
    71   core::Vector3Df **previousStepsAngularRates;
    72   unsigned int previousStepsAngularRatesIndex;
    73   gui::SpinBox *gyroDelay;
    74   gui::DoubleSpinBox *gyroGain;
     58    void UpdateFrom(const core::io_data *data);
     59    OpticalFlowCompensated_impl *pimpl_;
     60 
    7561};
    7662

  • branches/sanscv/lib/FlairVisionFilter/src/OpticalFlowSpeed.cpp

    r324 r325  
    2626using namespace flair::gui;
    2727
     28class OpticalFlowSpeed_impl {
     29public:
     30    OpticalFlowSpeed_impl(flair::filter::OpticalFlowSpeed *self,const LayoutPosition* position,string name):output(0) {
     31        this->self=self;
     32        MatrixDescriptor* desc=new MatrixDescriptor(2,1);
     33        desc->SetElementName(0,0,"vx");
     34        desc->SetElementName(1,0,"vy");
     35        output=new Matrix(self,desc,floatType,name);
     36        delete desc;
     37
     38        self->AddDataToLog(output);
     39
     40        GroupBox* reglages_groupbox=new GroupBox(position,name);
     41        quality=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"optical flow quality:",0.,100.,1.,1,5.);
     42        weightedAverage=new CheckBox(reglages_groupbox->LastRowLastCol(),"Weighted average", true);
     43        timeMultiplication=new CheckBox(reglages_groupbox->LastRowLastCol(),"Time multiplication", true);
     44
     45       
     46    }
     47
     48    ~OpticalFlowSpeed_impl() {
     49       
     50    }
     51   
     52    void UpdateFrom(const io_data *data){
     53        flair::filter::OpticalFlowData *input=(flair::filter::OpticalFlowData*)data;
     54        float deplx,deply;
     55        float nb_depl=0;
     56
     57        deplx=0;
     58        deply=0;
     59
     60        //error is 0 if perfect match and 7x7x255x255 at worst
     61        float qualityThreshold=quality->Value()/100.*7*7*255*255;
     62        input->GetMutex();
     63        int nbUsedPoints=0;
     64        for(int i=0;i<input->NbFeatures();i++) {
     65            //if point is found in both images and quality is sufficient
     66            if((input->FoundFeature()[i]!=0)&&(input->FeatureError()[i]<qualityThreshold)) {
     67              nbUsedPoints++;
     68              float qualityFactor=1.0;
     69              if (weightedAverage->Value()) {
     70                //displacement is weigthed by quality
     71                qualityFactor/=(1+input->FeatureError()[i]);
     72              }
     73                deplx+=(input->PointsB()[i].x-input->PointsA()[i].x)*qualityFactor;
     74                deply+=(input->PointsB()[i].y-input->PointsA()[i].y)*qualityFactor;
     75                nb_depl+=qualityFactor;
     76            }
     77        }
     78        input->ReleaseMutex();
     79        float deltaT;
     80        if (timeMultiplication->Value()) deltaT=(float)(data->DataTime()-output->DataTime())/(1000.*1000.*1000.);
     81        else deltaT=1.;
     82        output->SetDataTime(data->DataTime());
     83
     84        if(nb_depl!=0) {
     85    //Printf("Nombre de points=%d/%d (nb_depl=%f,pondération=%d), deltaT=%f\n",nbUsedPoints,input->NbFeatures(),nb_depl,weightedAverage->Value(),deltaT);
     86            output->SetValue(0,0,deplx/(nb_depl*deltaT));
     87            output->SetValue(1,0,deply/(nb_depl*deltaT));
     88        }
     89    };
     90   
     91    Matrix *output;
     92   
     93private:
     94    flair::filter::OpticalFlowSpeed *self;   
     95    DoubleSpinBox *quality;
     96    CheckBox *weightedAverage;
     97    CheckBox *timeMultiplication;
     98};
     99
    28100namespace flair { namespace filter {
    29101
    30102OpticalFlowSpeed::OpticalFlowSpeed(const IODevice* parent, const LayoutPosition* position,string name) : IODevice(parent,name) {
    31   MatrixDescriptor* desc=new MatrixDescriptor(2,1);
    32   desc->SetElementName(0,0,"vx");
    33   desc->SetElementName(1,0,"vy");
    34   output=new Matrix(this,desc,floatType,name);
    35   delete desc;
    36 
    37   AddDataToLog(output);
    38 
    39   GroupBox* reglages_groupbox=new GroupBox(position,name);
    40   quality=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"optical flow quality:",0.,100.,1.,1,5.);
    41   weightedAverage=new CheckBox(reglages_groupbox->LastRowLastCol(),"Weighted average", true);
    42   timeMultiplication=new CheckBox(reglages_groupbox->LastRowLastCol(),"Time multiplication", true);
     103    pimpl_=new OpticalFlowSpeed_impl(this,position,name);
    43104}
    44105
    45 OpticalFlowSpeed::~OpticalFlowSpeed(void) { }
     106OpticalFlowSpeed::~OpticalFlowSpeed(void) {
     107    delete pimpl_;
     108}
    46109
    47110void OpticalFlowSpeed::UpdateFrom(const io_data *data) {
    48     OpticalFlowData *input=(OpticalFlowData*)data;
    49     float deplx,deply;
    50     float nb_depl=0;
    51 
    52     deplx=0;
    53     deply=0;
    54 
    55     //error is 0 if perfect match and 7x7x255x255 at worst
    56     float qualityThreshold=quality->Value()/100.*7*7*255*255;
    57     input->GetMutex();
    58     int nbUsedPoints=0;
    59     for(int i=0;i<input->NbFeatures();i++) {
    60         //if point is found in both images and quality is sufficient
    61         if((input->FoundFeature()[i]!=0)&&(input->FeatureError()[i]<qualityThreshold)) {
    62           nbUsedPoints++;
    63           float qualityFactor=1.0;
    64           if (weightedAverage->Value()) {
    65             //displacement is weigthed by quality
    66             qualityFactor/=(1+input->FeatureError()[i]);
    67           }
    68             deplx+=(input->PointsB()[i].x-input->PointsA()[i].x)*qualityFactor;
    69             deply+=(input->PointsB()[i].y-input->PointsA()[i].y)*qualityFactor;
    70             nb_depl+=qualityFactor;
    71         }
    72     }
    73     input->ReleaseMutex();
    74     float deltaT;
    75     if (timeMultiplication->Value()) deltaT=(float)(data->DataTime()-output->DataTime())/(1000.*1000.*1000.);
    76     else deltaT=1.;
    77     output->SetDataTime(data->DataTime());
    78 
    79     if(nb_depl!=0) {
    80 //Printf("Nombre de points=%d/%d (nb_depl=%f,pondération=%d), deltaT=%f\n",nbUsedPoints,input->NbFeatures(),nb_depl,weightedAverage->Value(),deltaT);
    81         output->SetValue(0,0,deplx/(nb_depl*deltaT));
    82         output->SetValue(1,0,deply/(nb_depl*deltaT));
    83     }
    84 //    output->SetDataTime(data->DataTime());
    85     ProcessUpdate(output);
     111    pimpl_->UpdateFrom(data);
     112    ProcessUpdate(pimpl_->output);
    86113}
    87114
    88115float OpticalFlowSpeed::Vx(void) const {
    89     return output->Value(0,0);
     116    return pimpl_->output->Value(0,0);
    90117}
    91118
    92119float OpticalFlowSpeed::Vy(void) const {
    93     return output->Value(1,0);
     120    return pimpl_->output->Value(1,0);
    94121}
    95122
    96123core::Matrix *OpticalFlowSpeed::Output() const {
    97     return output;
     124    return pimpl_->output;
    98125}
    99126} // end namespace filter

  • branches/sanscv/lib/FlairVisionFilter/src/OpticalFlowSpeed.h

    r324 r325  
    2121  namespace gui {
    2222    class LayoutPosition;
    23     class SpinBox;
    24     class DoubleSpinBox;
    25     class CheckBox;
    2623  }
    2724}
     25
     26class OpticalFlowSpeed_impl;
    2827
    2928namespace flair
    … …  
    8887            */
    8988            void UpdateFrom(const core::io_data *data);
    90 
    91             core::Matrix *output;
    92       gui::DoubleSpinBox *quality;
    93       gui::CheckBox *weightedAverage;
    94       gui::CheckBox *timeMultiplication;
     89           
     90            OpticalFlowSpeed_impl *pimpl_;
    9591};
    9692

  • branches/sanscv/lib/FlairVisionFilter/src/Sobel.cpp

    r324 r325  
    1313
    1414#include "Sobel.h"
     15#include "VisionFilter.h"
    1516#include <Image.h>
    1617#include <Layout.h>
    1718#include <GroupBox.h>
    1819#include <SpinBox.h>
     20//#include <dspcv_gpp.h>
    1921#include <typeinfo>
    2022
    … …  
    2325using namespace flair::gui;
    2426
     27class Sobel_impl {
     28public:
     29    Sobel_impl(flair::filter::Sobel *self,const LayoutPosition* position,string name):output(0) {
     30        this->self=self;
     31       
     32        GroupBox* reglages_groupbox=new GroupBox(position,name);
     33        dx=new SpinBox(reglages_groupbox->NewRow(),"dx:",0,1,1,1);
     34        dy=new SpinBox(reglages_groupbox->NewRow(),"dy:",0,1,1,1);
     35
     36        Printf("TODO: IODevice doit faire un check de GetInputDataType et GetOutputDataType\n");
     37        //Image devrait accepter un type dans son constructeur pour construire un type identique
     38        try{
     39            Image::Type const &imageType=dynamic_cast<Image::Type const &>(((IODevice*)(self->Parent()))->GetOutputDataType());
     40            if(imageType.GetFormat()==Image::Type::Format::Gray) {
     41                output=new Image(self,imageType.GetWidth(),imageType.GetHeight(),imageType.GetFormat(),"sobel");
     42                //inIplImage = (IplImage*)AllocFunction(sizeof(IplImage));
     43                //outIplImage = (IplImage*)AllocFunction(sizeof(IplImage));
     44            } else {
     45                self->Err("input image is not gray\n");
     46            }
     47
     48        } catch(std::bad_cast& bc) {
     49            self->Err("io type mismatch\n");
     50        }
     51    }
     52
     53    ~Sobel_impl() {
     54        //FreeFunction((char*)(inIplImage));
     55        //FreeFunction((char*)(outIplImage));
     56    }
     57   
     58    void UpdateFrom(const io_data *data){
     59        Image *image=(Image*)data;
     60/*
     61        data->GetMutex();
     62        inIplImage->width=image->GetDataType().GetWidth();
     63        inIplImage->height=image->GetDataType().GetHeight();
     64        inIplImage->imageData=image->buffer;
     65       
     66        output->GetMutex();
     67        outIplImage->width=output->GetDataType().GetWidth();
     68        outIplImage->height=output->GetDataType().GetHeight();
     69        outIplImage->imageData=output->buffer;
     70               
     71        dspSobel(inIplImage,outIplImage,dx->Value(),dy->Value());
     72        output->ReleaseMutex();
     73        data->ReleaseMutex();
     74*/
     75        output->SetDataTime(data->DataTime());
     76
     77    };
     78   
     79    Image *output;
     80
     81private:
     82    flair::filter::Sobel *self;
     83    SpinBox *dx;
     84    SpinBox *dy;
     85    //IplImage *inIplImage,*outIplImage;
     86};
     87
    2588namespace flair { namespace filter {
    2689
    27 Sobel::Sobel(const IODevice* parent,const LayoutPosition* position,string name) : IODevice(parent,name),output(0) {
    28     GroupBox* reglages_groupbox=new GroupBox(position,name);
    29     dx=new SpinBox(reglages_groupbox->NewRow(),"dx:",0,1,1,1);
    30     dy=new SpinBox(reglages_groupbox->NewRow(),"dy:",0,1,1,1);
    31 
    32     Printf("TODO: IODevice doit faire un check de GetInputDataType et GetOutputDataType\n");
    33     //Image devrait accepter un type dans son constructeur pour construire un type identique
    34     try{
    35         Image::Type const &imageType=dynamic_cast<Image::Type const &>(parent->GetOutputDataType());
    36         if(imageType.GetFormat()==Image::Type::Format::Gray) {
    37             output=new Image(this,imageType.GetWidth(),imageType.GetHeight(),imageType.GetFormat(),"sobel");
    38         } else {
    39             Err("input image is not gray\n");
    40             return;
    41         }
    42 
    43     } catch(std::bad_cast& bc) {
    44         Err("io type mismatch\n");
    45         return;
    46     }
    47     SetIsReady(true);
     90Sobel::Sobel(const IODevice* parent,const LayoutPosition* position,string name) : IODevice(parent,name) {
     91   pimpl_=new Sobel_impl(this,position,name);
    4892}
    4993
    5094Sobel::~Sobel(void) {
     95    delete pimpl_;
    5196}
    5297
    5398Image* Sobel::Output(void) {
    54     return output;
     99    return pimpl_->output;
    55100}
    56101
    57 void Sobel::UpdateFrom(const io_data *data) {/*
    58     Image *cvImage=(Image*)data;
    59     IplImage *gimg=cvImage->img;
    60 
    61     data->GetMutex();
    62     output->GetMutex();
    63     dspSobel(gimg,output->img,dx->Value(),dy->Value());
    64     output->ReleaseMutex();
    65     data->ReleaseMutex();
    66 */
    67     output->SetDataTime(data->DataTime());
    68     ProcessUpdate(output);
     102void Sobel::UpdateFrom(const io_data *data) {
     103    pimpl_->UpdateFrom(data);
     104    ProcessUpdate(pimpl_->output);
    69105}
    70106
    71107DataType const &Sobel::GetOutputDataType() const {
    72     if(output!=NULL) {
    73         return output->GetDataType();
     108    if(pimpl_->output!=NULL) {
     109        return pimpl_->output->GetDataType();
    74110    } else {
    75111        return dummyType;

  • branches/sanscv/lib/FlairVisionFilter/src/Sobel.h

    r324 r325  
    1212
    1313#include <IODevice.h>
    14 #include <cvtypes.h>
     14
    1515
    1616namespace flair {
    … …  
    2020    namespace gui {
    2121        class LayoutPosition;
    22         class SpinBox;
    2322    }
    2423}
     24
     25 class Sobel_impl;
    2526
    2627namespace flair { namespace filter {
    … …  
    6465        private:
    6566            void UpdateFrom(const core::io_data *data);
    66             core::Image *output;
    67             gui::SpinBox *dx;
    68             gui::SpinBox *dy;
     67            Sobel_impl *pimpl_;
    6968    };
    7069} // end namespace filter

  • branches/sanscv/lib/FlairVisionFilter/src/VisionFilter.cpp

    r324 r325  
    88}
    99
    10 void saveToJpeg(IplImage* src_img,std::string filename,PictureFormat_t input_format,PictureFormat_t output_format,unsigned char compression_level) {
     10void saveToJpeg(flair::core::Image* src_img,std::string filename,PictureFormat_t input_format,PictureFormat_t output_format,unsigned char compression_level) {
    1111        printf("saveToJpeg todo\n");
    1212    //if(!cvSaveImage(filename.c_str(),src_img)) printf("Could not save.\n");

  • branches/sanscv/lib/FlairVisionFilter/src/VisionFilter.h

    r324 r325  
    11#include <string>
    2 #include <cvtypes.h>
     2
     3namespace flair {
     4    namespace core {
     5        class Image;
     6    }
     7}
     8
    39
    410/*!
    … …  
    1925                } ;
    2026                                                               
    21 void saveToJpeg(IplImage* src_img,std::string filename,PictureFormat_t input_format,PictureFormat_t output_format,unsigned char compression_level=95);
     27void saveToJpeg(flair::core::Image* src_img,std::string filename,PictureFormat_t input_format,PictureFormat_t output_format,unsigned char compression_level=95);
Note: See TracChangeset for help on using the changeset viewer.