Ignore:
Timestamp:
Oct 17, 2019, 2:49:35 PM (2 years ago)
Author:
Sanahuja Guillaume
Message:

remove opencv dep

Location:
trunk
Files:
2 edited

Legend:

Unmodified
Added
Removed
  • trunk

  • trunk/lib/FlairVisionFilter/src/OpticalFlowCompensated.cpp

    r318 r338  
    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
Note: See TracChangeset for help on using the changeset viewer.