Changeset 325 in flair-src for branches/sanscv/lib/FlairVisionFilter/src/OpticalFlowSpeed.cpp
- Timestamp:
- 08/28/19 16:12:11 (5 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
branches/sanscv/lib/FlairVisionFilter/src/OpticalFlowSpeed.cpp
r324 r325 26 26 using namespace flair::gui; 27 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 28 100 namespace flair { namespace filter { 29 101 30 102 OpticalFlowSpeed::OpticalFlowSpeed(const IODevice* parent, const LayoutPosition* position,string name) : IODevice(parent,name) { 31 MatrixDescriptor* desc=new MatrixDescriptor(2,1); 32 desc->SetElementName(0,0,"vx"); 33 desc->SetElementName(1,0,"vy"); 34 output=new Matrix(this,desc,floatType,name); 35 delete desc; 36 37 AddDataToLog(output); 38 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); 103 pimpl_=new OpticalFlowSpeed_impl(this,position,name); 43 104 } 44 105 45 OpticalFlowSpeed::~OpticalFlowSpeed(void) { } 106 OpticalFlowSpeed::~OpticalFlowSpeed(void) { 107 delete pimpl_; 108 } 46 109 47 110 void OpticalFlowSpeed::UpdateFrom(const io_data *data) { 48 OpticalFlowData *input=(OpticalFlowData*)data; 49 float deplx,deply; 50 float nb_depl=0; 51 52 deplx=0; 53 deply=0; 54 55 //error is 0 if perfect match and 7x7x255x255 at worst 56 float qualityThreshold=quality->Value()/100.*7*7*255*255; 57 input->GetMutex(); 58 int nbUsedPoints=0; 59 for(int i=0;i<input->NbFeatures();i++) { 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; 71 } 72 } 73 input->ReleaseMutex(); 74 float deltaT; 75 if (timeMultiplication->Value()) deltaT=(float)(data->DataTime()-output->DataTime())/(1000.*1000.*1000.); 76 else deltaT=1.; 77 output->SetDataTime(data->DataTime()); 78 79 if(nb_depl!=0) { 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)); 83 } 84 // output->SetDataTime(data->DataTime()); 85 ProcessUpdate(output); 111 pimpl_->UpdateFrom(data); 112 ProcessUpdate(pimpl_->output); 86 113 } 87 114 88 115 float OpticalFlowSpeed::Vx(void) const { 89 return output->Value(0,0);116 return pimpl_->output->Value(0,0); 90 117 } 91 118 92 119 float OpticalFlowSpeed::Vy(void) const { 93 return output->Value(1,0);120 return pimpl_->output->Value(1,0); 94 121 } 95 122 96 123 core::Matrix *OpticalFlowSpeed::Output() const { 97 return output;124 return pimpl_->output; 98 125 } 99 126 } // end namespace filter
Note:
See TracChangeset
for help on using the changeset viewer.