| 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 |
|
|---|
| 24 | using std::string;
|
|---|
| 25 | using namespace flair::core;
|
|---|
| 26 | using namespace flair::gui;
|
|---|
| 27 |
|
|---|
| 28 | class OpticalFlowSpeed_impl {
|
|---|
| 29 | public:
|
|---|
| 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 |
|
|---|
| 93 | private:
|
|---|
| 94 | flair::filter::OpticalFlowSpeed *self;
|
|---|
| 95 | DoubleSpinBox *quality;
|
|---|
| 96 | CheckBox *weightedAverage;
|
|---|
| 97 | CheckBox *timeMultiplication;
|
|---|
| 98 | };
|
|---|
| 99 |
|
|---|
| 100 | namespace flair { namespace filter {
|
|---|
| 101 |
|
|---|
| 102 | OpticalFlowSpeed::OpticalFlowSpeed(const IODevice* parent, const LayoutPosition* position,string name) : IODevice(parent,name) {
|
|---|
| 103 | pimpl_=new OpticalFlowSpeed_impl(this,position,name);
|
|---|
| 104 | }
|
|---|
| 105 |
|
|---|
| 106 | OpticalFlowSpeed::~OpticalFlowSpeed(void) {
|
|---|
| 107 | delete pimpl_;
|
|---|
| 108 | }
|
|---|
| 109 |
|
|---|
| 110 | void OpticalFlowSpeed::UpdateFrom(const io_data *data) {
|
|---|
| 111 | pimpl_->UpdateFrom(data);
|
|---|
| 112 | ProcessUpdate(pimpl_->output);
|
|---|
| 113 | }
|
|---|
| 114 |
|
|---|
| 115 | float OpticalFlowSpeed::Vx(void) const {
|
|---|
| 116 | return pimpl_->output->Value(0,0);
|
|---|
| 117 | }
|
|---|
| 118 |
|
|---|
| 119 | float OpticalFlowSpeed::Vy(void) const {
|
|---|
| 120 | return pimpl_->output->Value(1,0);
|
|---|
| 121 | }
|
|---|
| 122 |
|
|---|
| 123 | core::Matrix *OpticalFlowSpeed::Output() const {
|
|---|
| 124 | return pimpl_->output;
|
|---|
| 125 | }
|
|---|
| 126 | } // end namespace filter
|
|---|
| 127 | } // end namespace flair
|
|---|
