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

Last change on this file since 214 was 214, checked in by Sanahuja Guillaume, 3 years ago

matrix

File size: 1.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
19using std::string;
20using namespace flair::core;
21
22namespace flair
23{
24namespace filter
25{
26
27OpticalFlowSpeed::OpticalFlowSpeed(const IODevice* parent,string name) : IODevice(parent,name)
28{
29  cvmatrix_descriptor* desc=new cvmatrix_descriptor(2,1);
30  desc->SetElementName(0,0,"vx");
31  desc->SetElementName(1,0,"vy");
32  output=new Matrix(this,desc,floatType,name);
33  delete desc;
34
35        AddDataToLog(output);
36 
37  SetIsReady(true);
38}
39
40OpticalFlowSpeed::~OpticalFlowSpeed(void)
41{
42}
43
44void OpticalFlowSpeed::UpdateFrom(const io_data *data)
45{
46    OpticalFlowData *input=(OpticalFlowData*)data;
47    float deplx,deply;
48    int nb_depl=0;
49
50    deplx=0;
51    deply=0;
52
53    input->GetMutex();
54    for(int i=0;i<input->NbFeatures();i++) {
55        if(input->FoundFeature()[i]!=0) {
56            deplx+=input->PointsB()[i].x-input->PointsA()[i].x;
57            deply+=input->PointsB()[i].y-input->PointsA()[i].y;
58            nb_depl++;
59        }
60    }
61    input->ReleaseMutex();
62    Time deltaT=data->DataTime()-output->DataTime();
63    output->SetDataTime(data->DataTime());
64
65    if(nb_depl!=0) {
66        output->SetValue(0,0,deplx/(nb_depl*deltaT)*1000*1000*1000);
67        output->SetValue(1,0,deply/(nb_depl*deltaT)*1000*1000*1000);
68    }
69
70    ProcessUpdate(output);
71}
72
73float OpticalFlowSpeed::Vx(void) const
74{
75    return output->Value(0,0);
76}
77
78float OpticalFlowSpeed::Vy(void) const
79{
80    return output->Value(1,0);
81}
82
83core::Matrix *OpticalFlowSpeed::Output() const
84{
85    return output;
86}
87} // end namespace filter
88} // end namespace flair
Note: See TracBrowser for help on using the repository browser.