Changeset 147 in flair-src for trunk/lib

Timestamp:

Feb 28, 2017, 12:45:08 PM (8 years ago)

Author:

Sanahuja Guillaume

Message:

modif euler/lowpass

Location:

trunk/lib

Files:

• FlairCore/src/Box.cpp (modified) (1 diff)
• FlairCore/src/OneAxisRotation.cpp (modified) (1 diff)
• FlairCore/src/OneAxisRotation.h (modified) (1 diff)
• FlairCore/src/cvmatrix.h (modified) (2 diffs)
• FlairFilter/src/ButterworthLowPass_impl.cpp (modified) (3 diffs)
• FlairFilter/src/EulerDerivative.h (modified) (2 diffs)
• FlairFilter/src/EulerDerivative_impl.cpp (modified) (3 diffs)
• FlairFilter/src/LowPassFilter.h (modified) (1 diff)
• FlairFilter/src/LowPassFilter_impl.cpp (modified) (4 diffs)
• FlairFilter/src/unexported/ButterworthLowPass_impl.h (modified) (1 diff)
• FlairFilter/src/unexported/LowPassFilter_impl.h (modified) (1 diff)
• FlairMeta/src/MetaVrpnObject.cpp (modified) (2 diffs)
• FlairSimulator/src/SimuCameraGL.cpp (modified) (1 diff)

trunk/lib/FlairCore/src/Box.cpp

@@ -49 +49 @@
 }
 
-void Box::SetValueChanged(void) { value_changed = true; }
+void Box::SetValueChanged(void) {value_changed = true; }
 
 void Box::GetMutex(void) const { ((Layout *)Parent())->mutex->GetMutex(); }

trunk/lib/FlairCore/src/OneAxisRotation.cpp

@@ -57 +57 @@
 
 float OneAxisRotation::GetAngle() const {
-  return pimpl_->GetAngle();
+  return Euler::ToRadian(pimpl_->GetAngle());
 }
 

trunk/lib/FlairCore/src/OneAxisRotation.h

@@ -81 +81 @@
   */
   void ComputeRotation(core::RotationMatrix &matrix) const;
-
+  
+  /*!
+  * \brief Get angle
+  *
+  * \return angle in radians
+  */
   float GetAngle() const;
   int GetAxis() const;

trunk/lib/FlairCore/src/cvmatrix.h

@@ -55 +55 @@
   * Construct an io_data representing a CvMat. \n
   * It uses a cvmatrix_descriptor to get size and elements' names. \n
-  * Names are used for graphs and logs.
+  * Names are used for graphs and logs. \n
+  * All values are initialized to 0.
   *
   * \param parent parent
@@ -71 +72 @@
   *
   * Construct an io_data representing a CvMat. \n
-  * Elements are unamed.
+  * Elements are unamed. \n
+  * All values are initialized to 0.
   *
   * \param parent parent

trunk/lib/FlairFilter/src/ButterworthLowPass_impl.cpp

@@ -47 +47 @@
   f = new PoleFilter(order);
 
-  if (T->Value() != 0)
+  if (T->Value() != 0) {
     f->setup(1. / T->Value(), cutoff->Value());
+  }
+  
   f->reset();
 
   first_update = true;
+  this->order=order;
 }
 
@@ -59 +62 @@
   float result;
   cvmatrix *input = (cvmatrix *)data;
-
-  if (T->ValueChanged() || cutoff->ValueChanged()) {
+  float delta_t;
+  
+  if (T->ValueChanged()) {
     if (T->Value() != 0) {
       f->setup(1. / T->Value(), cutoff->Value());
-    } else {
-      first_update = true;
+      settingsChanged(input->Value(0, 0));
     }
   }
 
+  if (T->Value() == 0) {
+    delta_t = (float)(data->DataTime() - previous_time) / 1000000000.;
+    f->setup(1. / delta_t, cutoff->Value());
+  } else {
+    delta_t=T->Value();
+  }
+  
   // on prend une fois pour toute les mutex et on fait des accès directs
   output->GetMutex();
   input->GetMutex();
 
-  if (T->Value() == 0) {
-    float delta_t = (float)(data->DataTime() - previous_time) / 1000000000.;
+  if (cutoff->ValueChanged()) {
     f->setup(1. / delta_t, cutoff->Value());
+    settingsChanged(input->ValueNoMutex(0, 0));
   }
 
@@ -90 +100 @@
   previous_time = data->DataTime();
 }
+
+//ne gere pas les oscillations (s'arrete des qu'une valeure est bonne a 5%)
+void ButterworthLowPass_impl::settingsChanged(float inputValue) {
+  float result=f->filter(inputValue);
+  
+  while(result<inputValue*0.95 || result>inputValue*1.05) {
+      result=f->filter(inputValue);
+  }
+}

trunk/lib/FlairFilter/src/EulerDerivative.h

@@ -45 +45 @@
   * IODevice::ProcessUpdate is called. \n
   * The optional init_value parameters allow to specify
-  * the size of the input datas and its inital values.
+  * the size of the input datas and output inital values.
   * If unspecified, a 1*1 size is used, and values are
   * initialized with 0.
@@ -52 +52 @@
   * \param position position to display settings
   * \param name name
-  * \param init_value initial value
+  * \param init_value initial value, it is safe to destroy it after calling this constructor
   */
   EulerDerivative(const core::IODevice *parent,

trunk/lib/FlairFilter/src/EulerDerivative_impl.cpp

@@ -33 +33 @@
                                            const cvmatrix *init_value) {
   first_update = true;
+  
+  if (init_value != NULL) {
+    // init output matrix of same size as init
+    cvmatrix_descriptor *desc =new cvmatrix_descriptor(init_value->Rows(), init_value->Cols());
 
-  if (init_value != NULL) {
-    prev_value = (cvmatrix *)init_value;
+    for (int i = 0; i < init_value->Rows(); i++) {
+      for (int j = 0; j < init_value->Cols(); j++) {
+        desc->SetElementName(i, j, init_value->Name(i, j));
+      }
+    }
+    output = new cvmatrix(self, desc,init_value->GetDataType().GetElementDataType(), name);
+    for (int i = 0; i < init_value->Rows(); i++) {
+      for (int j = 0; j < init_value->Cols(); j++) {
+        output->SetValue(i, j, init_value->Value(i,j));
+      }
+    }
   } else {
     // if NULL, assume dimension 1, and init=0
     cvmatrix_descriptor *desc = new cvmatrix_descriptor(1, 1);
     desc->SetElementName(0, 0, "output");
-    prev_value = new cvmatrix(self, desc, floatType, name);
-    prev_value->SetValue(0, 0, 0);
+    output = new cvmatrix(self, desc, floatType, name);
+    output->SetValue(0, 0, 0);
   }
+  
+ 
+  cvmatrix_descriptor *desc = new cvmatrix_descriptor(output->Rows(), output->Cols());
+  prev_value = new cvmatrix(self, desc, output->GetDataType().GetElementDataType(), name);
+
 
   // init UI
@@ -48 +66 @@
   T = new DoubleSpinBox(reglages_groupbox->NewRow(), "period, 0 for auto:",
                         " s", 0, 1, 0.01);
-
-  // init output matrix of same size as init
-  cvmatrix_descriptor *desc =
-      new cvmatrix_descriptor(prev_value->Rows(), prev_value->Cols());
-
-  for (int i = 0; i < prev_value->Rows(); i++) {
-    for (int j = 0; j < prev_value->Cols(); j++) {
-      desc->SetElementName(i, j, prev_value->Name(i, j));
-    }
-  }
-
-  output = new cvmatrix(self, desc,
-                        prev_value->GetDataType().GetElementDataType(), name);
 }
 
@@ -76 +81 @@
     for (int i = 0; i < input->Rows(); i++) {
       for (int j = 0; j < input->Cols(); j++) {
-        output->SetValueNoMutex(i, j, prev_value->ValueNoMutex(i, j));
         prev_value->SetValueNoMutex(i, j, input->ValueNoMutex(i, j));
       }

trunk/lib/FlairFilter/src/LowPassFilter.h

@@ -52 +52 @@
   * \param position position to display settings
   * \param name name
-  * \param init_value initial value
+  * \param init_value initial value, it is safe to destroy it after calling this constructor
   */
   LowPassFilter(const core::IODevice *parent,

trunk/lib/FlairFilter/src/LowPassFilter_impl.cpp

@@ -35 +35 @@
                                        string name,
                                        const cvmatrix *init_value) {
-  first_update = true;
 
   if (init_value != NULL) {
-    prev_value = (cvmatrix *)init_value;
+    // init output matrix of same size as init
+    cvmatrix_descriptor *desc =new cvmatrix_descriptor(init_value->Rows(), init_value->Cols());
+
+    for (int i = 0; i < init_value->Rows(); i++) {
+      for (int j = 0; j < init_value->Cols(); j++) {
+        desc->SetElementName(i, j, init_value->Name(i, j));
+      }
+    }
+    output = new cvmatrix(self, desc,init_value->GetDataType().GetElementDataType(), name);
+    for (int i = 0; i < init_value->Rows(); i++) {
+      for (int j = 0; j < init_value->Cols(); j++) {
+        output->SetValue(i, j, init_value->Value(i,j));
+      }
+    }
   } else {
     // if NULL, assume dimension 1, and init=0
     cvmatrix_descriptor *desc = new cvmatrix_descriptor(1, 1);
     desc->SetElementName(0, 0, "output");
-    prev_value = new cvmatrix(self, desc, floatType, name);
-    prev_value->SetValue(0, 0, 0);
+    output = new cvmatrix(self, desc, floatType, name);
+    output->SetValue(0, 0, 0);
   }
 
@@ -53 +65 @@
   freq = new DoubleSpinBox(reglages_groupbox->NewRow(), "cutoff frequency",
                            " Hz", 0, 10000, 0.1, 2, 1);
-
-  // init output matrix of same size as init
-  cvmatrix_descriptor *desc =
-      new cvmatrix_descriptor(prev_value->Rows(), prev_value->Cols());
-
-  for (int i = 0; i < prev_value->Rows(); i++) {
-    for (int j = 0; j < prev_value->Cols(); j++) {
-      desc->SetElementName(i, j, prev_value->Name(i, j));
-    }
-  }
-
-  output = new cvmatrix(self, desc,
-                        prev_value->GetDataType().GetElementDataType(), name);
-
-  output->SetValue(0, 0, 0);
+                           
+  previous_time=GetTime();
 }
 
@@ -74 +73 @@
 void LowPassFilter_impl::UpdateFrom(const io_data *data) {
   float delta_t;
-  float result;
   cvmatrix *input = (cvmatrix *)data;
 
@@ -81 +79 @@
   input->GetMutex();
 
-  if (first_update == true) {
-    for (int i = 0; i < input->Rows(); i++) {
-      for (int j = 0; j < input->Cols(); j++) {
-        output->SetValueNoMutex(i, j, prev_value->ValueNoMutex(i, j));
-        prev_value->SetValueNoMutex(i, j, input->ValueNoMutex(i, j));
-      }
-    }
-    first_update = false;
+  if (T->Value() == 0) {
+    delta_t = (float)(data->DataTime() - previous_time) / 1000000000.;
   } else {
-    if (T->Value() == 0) {
-      delta_t = (float)(data->DataTime() - previous_time) / 1000000000.;
-    } else {
-      delta_t = T->Value();
-    }
-    for (int i = 0; i < input->Rows(); i++) {
-      for (int j = 0; j < input->Cols(); j++) {
-
-        if (freq->Value() != 0) {
-          output->SetValueNoMutex(i, j, (1 - 2 * PI * freq->Value() * delta_t) *
-                                                prev_value->ValueNoMutex(i, j) +
-                                            2 * PI * freq->Value() * delta_t *
-                                                input->ValueNoMutex(i, j));
-        } else {
-          output->SetValueNoMutex(i, j, input->ValueNoMutex(i, j));
-        }
-        prev_value->SetValueNoMutex(i, j, output->ValueNoMutex(i, j));
+    delta_t = T->Value();
+  }
+  for (int i = 0; i < input->Rows(); i++) {
+    for (int j = 0; j < input->Cols(); j++) {
+      float cutoff=freq->Value();
+      if (cutoff == 0 || freq->ValueChanged()) {
+        output->SetValueNoMutex(i, j, input->ValueNoMutex(i, j));
+      } else {
+        output->SetValueNoMutex(i, j, (1 - 2 * PI * cutoff * delta_t) *
+                                              output->ValueNoMutex(i, j) +
+                                          2 * PI * cutoff * delta_t *
+                                              input->ValueNoMutex(i, j));
       }
     }
   }
+ 
   input->ReleaseMutex();
   output->ReleaseMutex();

trunk/lib/FlairFilter/src/unexported/ButterworthLowPass_impl.h

@@ -128 +128 @@
 
 private:
+  void settingsChanged(float inputValue);
   flair::gui::DoubleSpinBox *cutoff, *T;
   PoleFilter *f;
   bool first_update;
   flair::core::Time previous_time;
+  int order;
 };
 

trunk/lib/FlairFilter/src/unexported/LowPassFilter_impl.h

@@ -43 +43 @@
 private:
   flair::core::Time previous_time;
-  bool first_update;
-  flair::core::cvmatrix *prev_value;
   flair::gui::DoubleSpinBox *freq, *T;
 };

trunk/lib/FlairMeta/src/MetaVrpnObject.cpp

@@ -62 +62 @@
   pbas = new LowPassFilter(this, GetVrpnClient()->GetLayout()->NewRow(),
                            name + " Passe bas", prev_value);
+  delete prev_value;
 
   desc = new cvmatrix_descriptor(7, 1);
@@ -74 +75 @@
   euler = new EulerDerivative(pbas, GetVrpnClient()->GetLayout()->NewRow(),
                               name + "_euler", prev_value);
+  delete prev_value;
 
   vx_opti_plot = new DataPlot1D(GetPlotTab()->NewRow(), "vx", -3, 3);

trunk/lib/FlairSimulator/src/SimuCameraGL.cpp

@@ -191 +191 @@
 
   shmem->Write(buf, width * height * 3);
-Printf("écriture faite!\n");
 }