source: flair-src/tags/latest/lib/FlairVisionFilter/src/OpticalFlowCompensated.cpp

Last change on this file was 338, checked in by Sanahuja Guillaume, 2 years ago

remove opencv dep

File size: 4.8 KB
Line 
1// %flair:license{
2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
4// %flair:license}
5//  created:    2014/01/14
6//  filename:   OpticalFlowCompensated.cpp
7//
8//  author:     Gildas Bayard
9//              Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11//  version:    $Id: $
12//
13//  purpose:    Compensate optical flow data for rotations
14//
15//
16/*********************************************************************/
17#include "OpticalFlowCompensated.h"
18#include <io_data.h>
19#include <Ahrs.h>
20#include <AhrsData.h>
21#include <OpticalFlow.h>
22#include <OpticalFlowData.h>
23#include <Euler.h>
24#include <Matrix.h>
25#include <Layout.h>
26#include <GroupBox.h>
27#include <SpinBox.h>
28#include <DoubleSpinBox.h>
29
30using std::string;
31using namespace flair::core;
32using namespace flair::gui;
33
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
115namespace flair {
116namespace filter {
117
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);
120
121}
122
123OpticalFlowCompensated::~OpticalFlowCompensated() {
124    delete pimpl_;
125}
126
127void OpticalFlowCompensated::UpdateFrom(const io_data *data) {
128    pimpl_->UpdateFrom(data);
129    ProcessUpdate(pimpl_->output);
130}
131
132
133} // end namespace filter
134} // end namespace flair
Note: See TracBrowser for help on using the repository browser.