DiscreteFilter: Multidimesional signal support

robotology · Aug 10, 2017 · d0cd476 · d0cd476
1 parent 57a432d
commit d0cd476
Show file tree

Hide file tree

Showing 2 changed files with 52 additions and 22 deletions.
diff --git a/toolbox/include/DiscreteFilter.h b/toolbox/include/DiscreteFilter.h
@@ -1,5 +1,6 @@
 #include "Block.h"
 #include <string>
+#include <vector>
 
 #ifndef WBT_FILTER_H
 #define WBT_FILTER_H
@@ -22,7 +23,7 @@ namespace yarp {
 
 class wbt::DiscreteFilter : public wbt::Block {
 private:
-    iCub::ctrl::IFilter* filter;
+    std::vector<iCub::ctrl::IFilter*> filters;
     yarp::sig::Vector* num;
     yarp::sig::Vector* den;
 

diff --git a/toolbox/src/DiscreteFilter.cpp b/toolbox/src/DiscreteFilter.cpp
@@ -25,13 +25,15 @@
 #define SIGNAL_DYNAMIC_SIZE -1
 
 using namespace wbt;
+using namespace iCub::ctrl;
+using namespace yarp::sig;
 
 std::string DiscreteFilter::ClassName = "DiscreteFilter";
 
-DiscreteFilter::DiscreteFilter() : filter(nullptr), num(nullptr), den(nullptr)
+DiscreteFilter::DiscreteFilter() : num(nullptr), den(nullptr)
 {
-    num = new yarp::sig::Vector();
-    den = new yarp::sig::Vector();
+    num = new Vector();
+    den = new Vector();
 }
 
 unsigned DiscreteFilter::numberOfParameters()
@@ -116,6 +118,11 @@ bool DiscreteFilter::initialize(BlockInformation* blockInfo, wbt::Error* error)
     // Create the filter object
     // ========================
 
+    // Get the width of the signals
+    unsigned inputSignalWidth  = blockInfo->getInputPortWidth(INPUT_IDX_SIGNAL);
+    unsigned outputSignalWidth = blockInfo->getOutputPortWidth(OUTPUT_IDX_SIGNAL);
+    assert(inputSignalWidth == outputSignalWidth);
+
     // Default
     // -------
     if (filter_type == "Default") {
@@ -125,7 +132,10 @@ bool DiscreteFilter::initialize(BlockInformation* blockInfo, wbt::Error* error)
             }
             return 1;
         }
-        filter = new iCub::ctrl::Filter(*num, *den);
+        for (unsigned i = 0; i < inputSignalWidth; ++i) {
+            IFilter* filter = new Filter(*num, *den);
+            filters.push_back(filter);
+        }
     }
     // FirstOrderLowPassFilter
     // -----------------------
@@ -139,8 +149,11 @@ bool DiscreteFilter::initialize(BlockInformation* blockInfo, wbt::Error* error)
             }
             return false;
         }
-        filter = new iCub::ctrl::FirstOrderLowPassFilter(firstOrderLowPassFilter_fc.floatData(),
-                                                         firstOrderLowPassFilter_ts.floatData());
+        for (unsigned i = 0; i < inputSignalWidth; ++i) {
+            IFilter* filter = new FirstOrderLowPassFilter(firstOrderLowPassFilter_fc.floatData(),
+                                                          firstOrderLowPassFilter_ts.floatData());
+            filters.push_back(filter);
+        }
     }
     // MedianFilter
     // ------------
@@ -153,7 +166,10 @@ bool DiscreteFilter::initialize(BlockInformation* blockInfo, wbt::Error* error)
             }
             return false;
         }
-        filter = new iCub::ctrl::MedianFilter(medianFilter_order.int32Data());
+        for (unsigned i = 0; i < inputSignalWidth; ++i) {
+            IFilter* filter = new MedianFilter(medianFilter_order.int32Data());
+            filters.push_back(filter);
+        }
     }
     else {
         if (error) error->message = ClassName + " Filter type not recognized.";
@@ -165,9 +181,14 @@ bool DiscreteFilter::initialize(BlockInformation* blockInfo, wbt::Error* error)
 
 bool DiscreteFilter::terminate(BlockInformation* blockInfo, wbt::Error* error)
 {
-    if (filter) {
-        delete filter;
-        filter = nullptr;
+    // Deallocate all the memory
+    // -------------------------
+
+    for (auto filter : filters) {
+        if (filter) {
+            delete filter;
+            filter = nullptr;
+        }
     }
 
     if (num) {
@@ -185,25 +206,33 @@ bool DiscreteFilter::terminate(BlockInformation* blockInfo, wbt::Error* error)
 
 bool DiscreteFilter::output(BlockInformation* blockInfo, wbt::Error* error)
 {
-    if (filter == nullptr) return false;
+    // Get the input and output signals
+    Signal inputSignal  = blockInfo->getInputPortSignal(INPUT_IDX_SIGNAL);
+    Signal outputSignal = blockInfo->getOutputPortSignal(OUTPUT_IDX_SIGNAL);
+
+    // Structure that holds the filtered signals
+    Vector outputVector;
 
-    // Get the input signal
-    Signal inputSignal = blockInfo->getInputPortSignal(INPUT_IDX_SIGNAL);
+    unsigned signalComponentIndex = -1;
 
-    unsigned index_element = 0;
-    yarp::sig::Vector inputSignalVector(1, inputSignal.get(index_element).doubleData());
+    for (auto filter : filters) {
+        if (filter == nullptr) return false;
 
-    // Filter the input signal
-    const yarp::sig::Vector& outputVector = filter->filt(inputSignalVector);
+        // Retrieve one component per time
+        signalComponentIndex++;
+        Vector inputSignalVector(1, inputSignal.get(signalComponentIndex).doubleData());
+
+        // Filter the current component of the input signal
+        outputVector.push_back(filter->filt(inputSignalVector)[0]);
+    }
 
-    // Forward the filtered signal to the output port
-    Signal output = blockInfo->getOutputPortSignal(OUTPUT_IDX_SIGNAL);
-    output.setBuffer(outputVector.data(), outputVector.length());
+    // Forward the filtered signals to the output port
+    outputSignal.setBuffer(outputVector.data(), outputVector.length());
 
     return true;
 }
 
-void DiscreteFilter::stringToYarpVector(const std::string str, yarp::sig::Vector* v)
+void DiscreteFilter::stringToYarpVector(const std::string str, Vector* v)
 {
     assert(v != nullptr);