Changeset 274 in flair-src for trunk/lib/FlairVisionFilter/src/OpticalFlowSpeed.cpp

Timestamp:

11/30/18 17:00:46 (5 years ago)

Author:

Sanahuja Guillaume

Message:

opticalflow modif

File:

• trunk/lib/FlairVisionFilter/src/OpticalFlowSpeed.cpp (modified) (2 diffs)

trunk/lib/FlairVisionFilter/src/OpticalFlowSpeed.cpp

@@ -16 +16 @@
 #include <Matrix.h>
 #include <Object.h>
+#include <Layout.h>
+#include <GroupBox.h>
+#include <SpinBox.h>
+#include <CheckBox.h>
+#include <DoubleSpinBox.h>
 
 using std::string;
 using namespace flair::core;
+using namespace flair::gui;
 
-namespace flair
-{
-namespace filter
-{
+namespace flair { namespace filter {
 
-OpticalFlowSpeed::OpticalFlowSpeed(const IODevice* parent,string name) : IODevice(parent,name)
-{
+OpticalFlowSpeed::OpticalFlowSpeed(const IODevice* parent, const LayoutPosition* position,string name) : IODevice(parent,name) {
   cvmatrix_descriptor* desc=new cvmatrix_descriptor(2,1);
   desc->SetElementName(0,0,"vx");
@@ -33 +35 @@
   delete desc;
 
-        AddDataToLog(output);
-  
-  SetIsReady(true);
+  AddDataToLog(output);
+
+  GroupBox* reglages_groupbox=new GroupBox(position,name);
+  quality=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"optical flow quality:",0.,100.,1.,1,5.);
+  weightedAverage=new CheckBox(reglages_groupbox->LastRowLastCol(),"Weighted average", true);
+  timeMultiplication=new CheckBox(reglages_groupbox->LastRowLastCol(),"Time multiplication", true);
 }
 
-OpticalFlowSpeed::~OpticalFlowSpeed(void)
-{
-}
+OpticalFlowSpeed::~OpticalFlowSpeed(void) { }
 
-void OpticalFlowSpeed::UpdateFrom(const io_data *data)
-{
+void OpticalFlowSpeed::UpdateFrom(const io_data *data) {
     OpticalFlowData *input=(OpticalFlowData*)data;
     float deplx,deply;
-    int nb_depl=0;
+    float nb_depl=0;
 
     deplx=0;
     deply=0;
 
+    //error is 0 if perfect match and 7x7x255x255 at worst
+    float qualityThreshold=quality->Value()/100.*7*7*255*255;
     input->GetMutex();
+    int nbUsedPoints=0;
     for(int i=0;i<input->NbFeatures();i++) {
-        if(input->FoundFeature()[i]!=0) {
-            deplx+=input->PointsB()[i].x-input->PointsA()[i].x;
-            deply+=input->PointsB()[i].y-input->PointsA()[i].y;
-            nb_depl++;
+        //if point is found in both images and quality is sufficient
+        if((input->FoundFeature()[i]!=0)&&(input->FeatureError()[i]<qualityThreshold)) {
+          nbUsedPoints++;
+          float qualityFactor=1.0;
+          if (weightedAverage->Value()) {
+            //displacement is weigthed by quality
+            qualityFactor/=(1+input->FeatureError()[i]);
+          } 
+            deplx+=(input->PointsB()[i].x-input->PointsA()[i].x)*qualityFactor;
+            deply+=(input->PointsB()[i].y-input->PointsA()[i].y)*qualityFactor;
+            nb_depl+=qualityFactor;
         }
     }
     input->ReleaseMutex();
-    Time deltaT=data->DataTime()-output->DataTime();
+    float deltaT;
+    if (timeMultiplication->Value()) deltaT=(float)(data->DataTime()-output->DataTime())/(1000.*1000.*1000.);
+    else deltaT=1.;
     output->SetDataTime(data->DataTime());
 
     if(nb_depl!=0) {
-        output->SetValue(0,0,deplx/(nb_depl*deltaT)*1000*1000*1000);
-        output->SetValue(1,0,deply/(nb_depl*deltaT)*1000*1000*1000);
+//Printf("Nombre de points=%d/%d (nb_depl=%f,pondération=%d), deltaT=%f\n",nbUsedPoints,input->NbFeatures(),nb_depl,weightedAverage->Value(),deltaT);
+        output->SetValue(0,0,deplx/(nb_depl*deltaT));
+        output->SetValue(1,0,deply/(nb_depl*deltaT));
     }
-
+//    output->SetDataTime(data->DataTime());
     ProcessUpdate(output);
 }
 
-float OpticalFlowSpeed::Vx(void) const
-{
+float OpticalFlowSpeed::Vx(void) const {
     return output->Value(0,0);
 }
 
-float OpticalFlowSpeed::Vy(void) const
-{
+float OpticalFlowSpeed::Vy(void) const {
     return output->Value(1,0);
 }
 
-core::Matrix *OpticalFlowSpeed::Output() const
-{
+core::Matrix *OpticalFlowSpeed::Output() const {
     return output;
 }