source: flair-src/trunk/lib/FlairVisionFilter/src/OpticalFlowSpeed.cpp @ 318

Last change on this file since 318 was 318, checked in by Sanahuja Guillaume, 2 years ago
File size: 3.2 KB
RevLine 
[122]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"
[214]16#include <Matrix.h>
[122]17#include <Object.h>
[274]18#include <Layout.h>
19#include <GroupBox.h>
20#include <SpinBox.h>
21#include <CheckBox.h>
22#include <DoubleSpinBox.h>
[122]23
24using std::string;
25using namespace flair::core;
[274]26using namespace flair::gui;
[122]27
[274]28namespace flair { namespace filter {
[122]29
[274]30OpticalFlowSpeed::OpticalFlowSpeed(const IODevice* parent, const LayoutPosition* position,string name) : IODevice(parent,name) {
[318]31  MatrixDescriptor* desc=new MatrixDescriptor(2,1);
[148]32  desc->SetElementName(0,0,"vx");
33  desc->SetElementName(1,0,"vy");
[214]34  output=new Matrix(this,desc,floatType,name);
[148]35  delete desc;
[122]36
[274]37  AddDataToLog(output);
[122]38
[274]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);
[122]43}
44
[274]45OpticalFlowSpeed::~OpticalFlowSpeed(void) { }
46
47void OpticalFlowSpeed::UpdateFrom(const io_data *data) {
[122]48    OpticalFlowData *input=(OpticalFlowData*)data;
49    float deplx,deply;
[274]50    float nb_depl=0;
[122]51
52    deplx=0;
53    deply=0;
54
[274]55    //error is 0 if perfect match and 7x7x255x255 at worst
56    float qualityThreshold=quality->Value()/100.*7*7*255*255;
[122]57    input->GetMutex();
[274]58    int nbUsedPoints=0;
[143]59    for(int i=0;i<input->NbFeatures();i++) {
[274]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;
[122]71        }
72    }
73    input->ReleaseMutex();
[274]74    float deltaT;
75    if (timeMultiplication->Value()) deltaT=(float)(data->DataTime()-output->DataTime())/(1000.*1000.*1000.);
76    else deltaT=1.;
[143]77    output->SetDataTime(data->DataTime());
[122]78
[143]79    if(nb_depl!=0) {
[274]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));
[122]83    }
[274]84//    output->SetDataTime(data->DataTime());
[122]85    ProcessUpdate(output);
86}
87
[274]88float OpticalFlowSpeed::Vx(void) const {
[122]89    return output->Value(0,0);
90}
91
[274]92float OpticalFlowSpeed::Vy(void) const {
[122]93    return output->Value(1,0);
94}
95
[274]96core::Matrix *OpticalFlowSpeed::Output() const {
[122]97    return output;
98}
99} // end namespace filter
100} // end namespace flair
Note: See TracBrowser for help on using the repository browser.