From 482083747215afe37a26a79a8308152d9036631c Mon Sep 17 00:00:00 2001
From: Simon Ottenhaus <simon.ottenhaus@kit.edu>
Date: Mon, 7 Jul 2014 18:34:10 +0200
Subject: [PATCH] Classes for tactile sensor calibration

---
 .../drivers/WeissHapticSensor/CMakeLists.txt  |  4 ++
 .../WeissHapticSensor/CalibrationHelper.cpp   | 64 +++++++++++++++++++
 .../WeissHapticSensor/CalibrationHelper.h     | 36 +++++++++++
 .../WeissHapticSensor/CalibrationInfo.cpp     | 16 +++++
 .../WeissHapticSensor/CalibrationInfo.h       | 23 +++++++
 5 files changed, 143 insertions(+)
 create mode 100644 source/RobotAPI/drivers/WeissHapticSensor/CalibrationHelper.cpp
 create mode 100644 source/RobotAPI/drivers/WeissHapticSensor/CalibrationHelper.h
 create mode 100644 source/RobotAPI/drivers/WeissHapticSensor/CalibrationInfo.cpp
 create mode 100644 source/RobotAPI/drivers/WeissHapticSensor/CalibrationInfo.h

diff --git a/source/RobotAPI/drivers/WeissHapticSensor/CMakeLists.txt b/source/RobotAPI/drivers/WeissHapticSensor/CMakeLists.txt
index 60c8611f2..950b6e9c3 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/CMakeLists.txt
+++ b/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}")
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/CalibrationHelper.cpp b/source/RobotAPI/drivers/WeissHapticSensor/CalibrationHelper.cpp
new file mode 100644
index 000000000..244edebed
--- /dev/null
+++ b/source/RobotAPI/drivers/WeissHapticSensor/CalibrationHelper.cpp
@@ -0,0 +1,64 @@
+#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();
+}
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/CalibrationHelper.h b/source/RobotAPI/drivers/WeissHapticSensor/CalibrationHelper.h
new file mode 100644
index 000000000..243d79ff5
--- /dev/null
+++ b/source/RobotAPI/drivers/WeissHapticSensor/CalibrationHelper.h
@@ -0,0 +1,36 @@
+#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
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/CalibrationInfo.cpp b/source/RobotAPI/drivers/WeissHapticSensor/CalibrationInfo.cpp
new file mode 100644
index 000000000..caad13edc
--- /dev/null
+++ b/source/RobotAPI/drivers/WeissHapticSensor/CalibrationInfo.cpp
@@ -0,0 +1,16 @@
+#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();
+}
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/CalibrationInfo.h b/source/RobotAPI/drivers/WeissHapticSensor/CalibrationInfo.h
new file mode 100644
index 000000000..d1ca58a0c
--- /dev/null
+++ b/source/RobotAPI/drivers/WeissHapticSensor/CalibrationInfo.h
@@ -0,0 +1,23 @@
+#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
-- 
GitLab