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

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

remove opencv dep

File size: 3.9 KB
Line 
1//  created:    2012/04/12
2//  filename:   OpticalFlowSpeed.cpp
3//
4//  author:     Guillaume Sanahuja
5//              Copyright Heudiasyc UMR UTC/CNRS 7253
6//
7//  version:    $Id: $
8//
9//  purpose:    calcul de la vitesse à partir du flux optique
10//
11//
12/*********************************************************************/
13
14#include "OpticalFlowSpeed.h"
15#include "OpticalFlowData.h"
16#include <Matrix.h>
17#include <Object.h>
18#include <Layout.h>
19#include <GroupBox.h>
20#include <SpinBox.h>
21#include <CheckBox.h>
22#include <DoubleSpinBox.h>
23
24using std::string;
25using namespace flair::core;
26using namespace flair::gui;
27
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
100namespace flair { namespace filter {
101
102OpticalFlowSpeed::OpticalFlowSpeed(const IODevice* parent, const LayoutPosition* position,string name) : IODevice(parent,name) {
103    pimpl_=new OpticalFlowSpeed_impl(this,position,name);
104}
105
106OpticalFlowSpeed::~OpticalFlowSpeed(void) {
107    delete pimpl_;
108}
109
110void OpticalFlowSpeed::UpdateFrom(const io_data *data) {
111    pimpl_->UpdateFrom(data);
112    ProcessUpdate(pimpl_->output);
113}
114
115float OpticalFlowSpeed::Vx(void) const {
116    return pimpl_->output->Value(0,0);
117}
118
119float OpticalFlowSpeed::Vy(void) const {
120    return pimpl_->output->Value(1,0);
121}
122
123core::Matrix *OpticalFlowSpeed::Output() const {
124    return pimpl_->output;
125}
126} // end namespace filter
127} // end namespace flair
Note: See TracBrowser for help on using the repository browser.