Classes for tactile sensor calibration

source/RobotAPI/drivers/WeissHapticSensor/CMakeLists.txt

@@ -25,6 +25,8 @@ set(LIB_FILES
     Checksum.cpp
     SerialInterface.cpp
     TactileSensor.cpp
+    CalibrationInfo.cpp
+    CalibrationHelper.cpp
 )
 set(LIB_HEADERS
     WeissHapticSensorsUnit.h
@@ -38,6 +40,8 @@ set(LIB_HEADERS
     TactileSensor.h
     TransmissionException.h
     Types.h
+    CalibrationInfo.h
+    CalibrationHelper.h
 )
 
 armarx_add_library("${LIB_NAME}" "${LIB_VERSION}" "${LIB_SOVERSION}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")

source/RobotAPI/drivers/WeissHapticSensor/CalibrationHelper.cpp

+#include "CalibrationHelper.h"
+
+#include <stdexcept>
+
+using namespace armarx;
+
+CalibrationHelper::CalibrationHelper(int rows, int cols, float noiseThreshold)
+{
+    this->maximumValues = Eigen::MatrixXf::Zero(rows, cols);
+    this->noiseThreshold = noiseThreshold;
+}
+
+void CalibrationHelper::addNoiseSample(Eigen::MatrixXf data)
+{
+    this->noiseSamples.push_back(data);
+}
+
+bool CalibrationHelper::addMaxValueSample(Eigen::MatrixXf data)
+{
+    if(data.maxCoeff() <= noiseThreshold)
+    {
+        this->maximumValues = this->maximumValues.cwiseMax(data);
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+
+CalibrationInfo CalibrationHelper::getCalibrationInfo(float calibratedMinimum, float calibratedMaximum)
+{
+    return CalibrationInfo(getMatrixAverage(noiseSamples), maximumValues, calibratedMinimum, calibratedMaximum);
+}
+
+bool CalibrationHelper::checkMaximumValueThreshold(float threshold)
+{
+    return this->maximumValues.minCoeff() >= threshold;
+}
+
+Eigen::MatrixXf CalibrationHelper::getMaximumValues()
+{
+    return this->maximumValues;
+}
+
+int CalibrationHelper::getNoiseSampleCount()
+{
+    return this->noiseSamples.size();
+}
+
+Eigen::MatrixXf CalibrationHelper::getMatrixAverage(std::vector<Eigen::MatrixXf> samples)
+{
+    if(samples.size() == 0)
+    {
+        throw std::runtime_error("Average of zero samples not possible");
+    }
+
+    Eigen::MatrixXf sum = samples.at(0);
+    for(std::vector<Eigen::MatrixXf>::iterator it = samples.begin() + 1; it != samples.end(); ++it)
+    {
+        sum += *it;
+    }
+    return sum / (float)samples.size();
+}

source/RobotAPI/drivers/WeissHapticSensor/CalibrationHelper.h

+#ifndef CALIBRATIONHELPER_H
+#define CALIBRATIONHELPER_H
+
+#include <vector>
+#include <Eigen/Core>
+
+#include "CalibrationInfo.h"
+
+namespace armarx
+{
+    class CalibrationHelper
+    {
+    public:
+        CalibrationHelper(int rows, int cols, float noiseThreshold);
+
+        void addNoiseSample(Eigen::MatrixXf data);
+        bool addMaxValueSample(Eigen::MatrixXf data);
+
+        CalibrationInfo getCalibrationInfo(float calibratedMinimum, float calibratedMaximum);
+
+        bool checkMaximumValueThreshold(float threshold);
+
+        Eigen::MatrixXf getMaximumValues();
+
+        int getNoiseSampleCount();
+
+    private:
+        std::vector<Eigen::MatrixXf> noiseSamples;
+        Eigen::MatrixXf maximumValues;
+        float noiseThreshold;
+
+        Eigen::MatrixXf getMatrixAverage(std::vector<Eigen::MatrixXf> samples);
+    };
+}
+
+#endif // CALIBRATIONHELPER_H

source/RobotAPI/drivers/WeissHapticSensor/CalibrationInfo.cpp

+#include "CalibrationInfo.h"
+
+using namespace armarx;
+
+CalibrationInfo::CalibrationInfo(Eigen::MatrixXf averageNoiseValues, Eigen::MatrixXf maximumValues, float minValue, float maxValue)
+{
+    this->calibratedMinimum = minValue;
+    this->calibratedMaximum = maxValue;
+    this->averageNoiseValues = averageNoiseValues;
+    this->maximumValues = maximumValues;
+}
+
+Eigen::MatrixXf CalibrationInfo::applyCalibration(Eigen::MatrixXf rawData)
+{
+    return ((rawData - averageNoiseValues).cwiseQuotient(maximumValues - averageNoiseValues).array() * (calibratedMaximum - calibratedMinimum) + calibratedMinimum).matrix();
+}

source/RobotAPI/drivers/WeissHapticSensor/CalibrationInfo.h

+#ifndef CALIBRATIONINFO_H
+#define CALIBRATIONINFO_H
+
+#include <Eigen/Core>
+namespace armarx
+{
+    class CalibrationInfo
+    {
+    public:
+        CalibrationInfo(Eigen::MatrixXf averageNoiseValues, Eigen::MatrixXf maximumValues, float calibratedMinimum, float calibratedMaximum);
+
+        Eigen::MatrixXf applyCalibration(Eigen::MatrixXf rawData);
+
+    private:
+        Eigen::MatrixXf averageNoiseValues;
+        Eigen::MatrixXf maximumValues;
+
+        float calibratedMaximum;
+        float calibratedMinimum;
+    };
+}
+
+#endif // CALIBRATIONINFO_H