Index: /trunk/demos/OpticalFlow/uav/src/DemoOpticalFlow.cpp
===================================================================
--- /trunk/demos/OpticalFlow/uav/src/DemoOpticalFlow.cpp	(revision 273)
+++ /trunk/demos/OpticalFlow/uav/src/DemoOpticalFlow.cpp	(revision 274)
@@ -63,5 +63,5 @@
   opticalFlow=new OpticalFlow(greyCameraImage,uav->GetVerticalCamera()->GetLayout()->NewRow(),"flux optique");
   opticalFlowCompensated=new OpticalFlowCompensated(opticalFlow,uav->GetAhrs(),uav->GetVerticalCamera()->GetLayout()->NewRow(),"flux optique compense");
-  opticalFlowSpeedRaw=new OpticalFlowSpeed(opticalFlowCompensated,uav->GetAhrs(),uav->GetVerticalCamera()->GetLayout()->NewRow(),"vitesse du Flux Optique");
+  opticalFlowSpeedRaw=new OpticalFlowSpeed(opticalFlowCompensated,uav->GetVerticalCamera()->GetLayout()->NewRow(),"vitesse du Flux Optique");
   //opticalFlowSpeed=vitesse de déplacement en pixels par seconde (moyenne sur tous les points et division par le delta T)
   Matrix* twoByOneOFS=new Matrix((const Thread*)this,2,1,floatType);
Index: /trunk/lib/FlairVisionFilter/src/OpticalFlowCompensated.cpp
===================================================================
--- /trunk/lib/FlairVisionFilter/src/OpticalFlowCompensated.cpp	(revision 274)
+++ /trunk/lib/FlairVisionFilter/src/OpticalFlowCompensated.cpp	(revision 274)
@@ -0,0 +1,123 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2014/01/14
+//  filename:   OpticalFlowCompensated.cpp
+//
+//  author:     Gildas Bayard
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    Compensate optical flow data for rotations
+//
+//
+/*********************************************************************/
+#include "OpticalFlowCompensated.h"
+#include <io_data.h>
+#include <Ahrs.h>
+#include <AhrsData.h>
+#include <OpticalFlow.h>
+#include <OpticalFlowData.h>
+#include <Euler.h>
+#include <Matrix.h>
+#include <Layout.h>
+#include <GroupBox.h>
+#include <SpinBox.h>
+#include <DoubleSpinBox.h>
+
+using std::string;
+using namespace flair::core;
+using namespace flair::gui;
+
+namespace flair {
+namespace filter {
+
+OpticalFlowCompensated::OpticalFlowCompensated(const OpticalFlow *parent, const Ahrs *ahrs, const LayoutPosition* position, string name) : IODevice(parent, name), ahrs(ahrs), output(NULL) {
+  cvmatrix_descriptor* desc=new cvmatrix_descriptor(3,2);
+  desc->SetElementName(0,0,"raw displacement X");
+  desc->SetElementName(0,1,"raw displacement Y");
+  desc->SetElementName(1,0,"compensation X");
+  desc->SetElementName(1,1,"compensation Y");
+  desc->SetElementName(2,0,"displacement with compensation X");
+  desc->SetElementName(2,0,"displacement with compensation Y");
+  firstPointDisplacement=new Matrix(this,desc,floatType,name);
+  delete desc;
+  previousStepsAngularRates=new Vector3Df*[10];
+  for (int i=0; i<10; i++) previousStepsAngularRates[i]=NULL;
+  previousStepsAngularRatesIndex=0;
+
+  GroupBox* reglages_groupbox=new GroupBox(position,name);
+  //TODO: the gyroDelay is set to compensate for the time difference between image snapshot et gyro measurements
+//it is equal to the time between 2 images (because optical flow always lags 1 frame) + the time to compute optical flow
+//here it is approximated by 2 video frames
+  gyroDelay=new SpinBox(reglages_groupbox->NewRow(),"gyro delay (in video frames):",0,10,1,2);
+  gyroGain=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"gyro gain:",0.,500.,10.,2,300.);
+
+}
+
+OpticalFlowCompensated::~OpticalFlowCompensated() {
+  delete output;
+}
+
+void OpticalFlowCompensated::UpdateFrom(const io_data *data) {
+  OpticalFlowData *input=(OpticalFlowData *)data;
+  if (!output) { //first pass
+    output=new OpticalFlowData(this,input->MaxFeatures(),input->ObjectName()+"_filtered");
+    previousTime=input->DataTime();
+    return;
+  }
+//  float kX=320/Euler::ToRadian(70); //TEST: only for default simuCameraGL. fov=70° and image width=320 => k=320*180/(70*pi)
+//  float kY=240/Euler::ToRadian(70); //TEST: only for default simuCameraGL. fov=70° and image height=240
+  float kX=gyroGain->Value();
+  float kY=gyroGain->Value();
+  float deltaT=(input->DataTime()-previousTime)/(1000*1000*1000.);
+  previousTime=input->DataTime();
+
+  int delayedIndex=previousStepsAngularRatesIndex-gyroDelay->Value(); // Ahhh décalage, esprit canal...
+  if (delayedIndex<0) delayedIndex+=10;
+
+  if (!previousStepsAngularRates[previousStepsAngularRatesIndex]) {
+    previousStepsAngularRates[previousStepsAngularRatesIndex]=new Vector3Df();
+  }
+  *previousStepsAngularRates[previousStepsAngularRatesIndex++]=ahrs->GetDatas()->GetAngularRates();
+
+  if (!previousStepsAngularRates[delayedIndex]) return;
+  float rotationFlowX=previousStepsAngularRates[delayedIndex]->y*deltaT*kY;
+  float rotationFlowY=-previousStepsAngularRates[delayedIndex]->x*deltaT*kX;
+  if (previousStepsAngularRatesIndex==10) previousStepsAngularRatesIndex=0;
+  input->GetMutex();
+  output->GetMutex();
+
+  for (int i=0; i<input->NbFeatures(); i++) {
+    if (!i) {
+      firstPointDisplacement->SetValue(0,0,input->PointsB()[i].x-input->PointsA()[i].x);
+      firstPointDisplacement->SetValue(0,1,input->PointsB()[i].y-input->PointsA()[i].y);
+      firstPointDisplacement->SetValue(1,0,-rotationFlowX);
+      firstPointDisplacement->SetValue(1,1,-rotationFlowY);
+      firstPointDisplacement->SetValue(2,0,input->PointsB()[i].x-input->PointsA()[i].x-rotationFlowX);
+      firstPointDisplacement->SetValue(2,1,input->PointsB()[i].y-input->PointsA()[i].y-rotationFlowY);
+    }
+    output->PointsA()[i].x=input->PointsA()[i].x;
+    output->PointsA()[i].y=input->PointsA()[i].y;
+    output->PointsB()[i].x=input->PointsB()[i].x-rotationFlowX;
+    output->PointsB()[i].y=input->PointsB()[i].y-rotationFlowY;
+  }
+  output->SetNbFeatures(input->NbFeatures());
+  output->SetFoundFeature(input->FoundFeature());
+  output->SetFeatureError(input->FeatureError());
+
+  output->ReleaseMutex();
+  input->ReleaseMutex();
+
+  output->SetDataTime(input->DataTime());
+  ProcessUpdate(output);
+}
+
+  Matrix *OpticalFlowCompensated::GetFirstPointDisplacement() const {
+    return firstPointDisplacement;
+  }
+
+} // end namespace filter
+} // end namespace flair
Index: /trunk/lib/FlairVisionFilter/src/OpticalFlowCompensated.h
===================================================================
--- /trunk/lib/FlairVisionFilter/src/OpticalFlowCompensated.h	(revision 274)
+++ /trunk/lib/FlairVisionFilter/src/OpticalFlowCompensated.h	(revision 274)
@@ -0,0 +1,78 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+/*!
+ * \file OpticalFlowCompensated.h
+ * \brief Compensate optical flow data for rotations
+ * \author Gildas Bayard, Copyright Heudiasyc UMR UTC/CNRS 7253
+ * \date 2017/02/17
+ * \version 1.0
+ */
+
+#ifndef OPTICALFLOWCOMPENSATED_H_
+#define OPTICALFLOWCOMPENSATED_H
+
+#include <IODevice.h>
+#include <Object.h>
+#include <Vector3D.h>
+
+namespace flair {
+  namespace core {
+    class io_data;
+    class Matrix;
+  }
+  namespace gui {
+    class LayoutPosition;
+    class SpinBox;
+    class DoubleSpinBox;
+  }
+  namespace filter {
+    class Ahrs;
+    class OpticalFlow;
+    class OpticalFlowData;
+  }
+}
+
+namespace flair { namespace filter {
+
+/*! \class OpticalFlowCompensated
+*
+* \brief Compensate optical flow data for rotations
+*
+*/
+class OpticalFlowCompensated : public core::IODevice {
+public:
+  /*!
+  * \brief Constructor
+  *
+  * Construct an optical flow compensated filter
+  *
+  * \param parent parent
+  * \param name name
+  * \param ahrs ahrs object used to get rotational speed
+  */
+  OpticalFlowCompensated(const filter::OpticalFlow *parent, const filter::Ahrs *ahrs, const gui::LayoutPosition* position, std::string name);
+
+  /*!
+  * \brief Destructor
+  *
+  */
+  ~OpticalFlowCompensated();
+
+  void UpdateFrom(const core::io_data *data);
+
+  core::Matrix *GetFirstPointDisplacement() const;
+private:
+  OpticalFlowData *output;
+  core::Time previousTime;
+  const Ahrs *ahrs;
+  core::Matrix *firstPointDisplacement;
+  core::Vector3Df **previousStepsAngularRates;
+  unsigned int previousStepsAngularRatesIndex;
+  gui::SpinBox *gyroDelay;
+  gui::DoubleSpinBox *gyroGain;
+};
+
+}} // end namespace flair::filter
+#endif // OPTICALFLOWCOMPENSATED_H
Index: /trunk/lib/FlairVisionFilter/src/OpticalFlowSpeed.cpp
===================================================================
--- /trunk/lib/FlairVisionFilter/src/OpticalFlowSpeed.cpp	(revision 273)
+++ /trunk/lib/FlairVisionFilter/src/OpticalFlowSpeed.cpp	(revision 274)
@@ -16,15 +16,17 @@
 #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,54 +35,64 @@
   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;
 }
Index: /trunk/lib/FlairVisionFilter/src/OpticalFlowSpeed.h
===================================================================
--- /trunk/lib/FlairVisionFilter/src/OpticalFlowSpeed.h	(revision 273)
+++ /trunk/lib/FlairVisionFilter/src/OpticalFlowSpeed.h	(revision 274)
@@ -19,4 +19,10 @@
         class Matrix;
     }
+  namespace gui {
+    class LayoutPosition;
+    class SpinBox;
+    class DoubleSpinBox;
+    class CheckBox;
+  }
 }
 
@@ -31,5 +37,5 @@
     * \brief Optical flow speed calculation
     *
-    * Speed is the mean of all optical flow values divided by the delta time between images.
+    * Speed is the mean of all optical flow values.
     */
     class OpticalFlowSpeed : public core::IODevice
@@ -44,5 +50,5 @@
             * \param name name
             */
-            OpticalFlowSpeed(const core::IODevice* parent,std::string name);
+            OpticalFlowSpeed(const core::IODevice* parent, const gui::LayoutPosition* position,std::string name);
 
             /*!
@@ -67,5 +73,5 @@
             * \brief Output matrix
             *
-            * Matrix is of size (2,1). \n
+            * Matrix is of sze (2,1). \n
             * First line is speed along x axis. \n
             * Second line is speed along y axis. \n
@@ -84,5 +90,9 @@
 
             core::Matrix *output;
-    };
+      gui::DoubleSpinBox *quality;
+      gui::CheckBox *weightedAverage;
+      gui::CheckBox *timeMultiplication;
+};
+
 } // end namespace filter
 } // end namespace flair
