diff --git a/source/RobotAPI/libraries/core/CMakeLists.txt b/source/RobotAPI/libraries/core/CMakeLists.txt
index 8d51330a6010bdaf47f04da8a42d97abb86c7173..53a0b23239a54fb90016c8d0786afd331f3df2a9 100644
--- a/source/RobotAPI/libraries/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/core/CMakeLists.txt
@@ -48,6 +48,12 @@ set(LIB_HEADERS
RobotAPIObjectFactories.h
remoterobot/RobotStateObserver.h
remoterobot/RemoteRobot.h
+ math/SlidingWindowVectorMedian.h
+ math/MathUtils.h
+ math/Trigonometry.h
+ math/SVD.h
+ math/StatUtils.h
+ math/MatrixHelpers.h
)
diff --git a/source/RobotAPI/libraries/core/math/MathUtils.h b/source/RobotAPI/libraries/core/math/MathUtils.h
new file mode 100644
index 0000000000000000000000000000000000000000..1a56170a8d6e123a940c39f82ccd3f89ef7d2a5c
--- /dev/null
+++ b/source/RobotAPI/libraries/core/math/MathUtils.h
@@ -0,0 +1,105 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package ArmarX::Core
+* @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+* @date 2015
+* @copyright http://www.gnu.org/licenses/gpl.txt
+* GNU General Public License
+*/
+
+#ifndef ARMARX_ROBOTAPI_MATH_MATHUTILS_H_
+#define ARMARX_ROBOTAPI_MATH_MATHUTILS_H_
+
+#include <math.h>
+#include <Eigen/Eigen>
+#include <vector>
+
+namespace armarx
+{
+ namespace math
+ {
+ class MathUtils
+ {
+ public:
+ static int LimitMinMax(int min, int max, int value)
+ {
+ return value < min ? min : (value > max ? max : value);
+ }
+ static float LimitMinMax(float min, float max, float value)
+ {
+ return value < min ? min : (value > max ? max : value);
+ }
+ static double LimitMinMax(double min, double max, double value)
+ {
+ return value < min ? min : (value > max ? max : value);
+ }
+ static Eigen::Vector3f LimitMinMax(const Eigen::Vector3f &min, const Eigen::Vector3f &max, const Eigen::Vector3f &value)
+ {
+ return Eigen::Vector3f(LimitMinMax(min(0), max(0), value(0)), LimitMinMax(min(1), max(1), value(1)), LimitMinMax(min(2), max(2), value(2)));
+ }
+
+ static bool CheckMinMax(int min, int max, int value)
+ {
+ return value >= min && value <= max;
+ }
+ static bool CheckMinMax(float min, float max, float value)
+ {
+ return value >= min && value <= max;
+ }
+ static bool CheckMinMax(double min, double max, double value)
+ {
+ return value >= min && value <= max;
+ }
+ static bool CheckMinMax(const Eigen::Vector3f &min, const Eigen::Vector3f &max, const Eigen::Vector3f &value)
+ {
+ return CheckMinMax(min(0), max(0), value(0)) && CheckMinMax(min(1), max(1), value(1)) && CheckMinMax(min(2), max(2), value(2));
+ }
+
+ static std::vector<float> VectorSubtract(const std::vector<float> &v1, const std::vector<float> &v2)
+ {
+ std::vector<float> result;
+ for(size_t i = 0; i < v1.size() && i < v2.size(); i++)
+ {
+ result.push_back(v1.at(i) - v2.at(i));
+ }
+ return result;
+ }
+ static std::vector<float> VectorAbsDiff(const std::vector<float> &v1, const std::vector<float> &v2)
+ {
+ std::vector<float> result;
+ for(size_t i = 0; i < v1.size() && i < v2.size(); i++)
+ {
+ result.push_back(std::fabs(v1.at(i) - v2.at(i)));
+ }
+ return result;
+ }
+
+ static float VectorMax(const std::vector<float> &vec)
+ {
+ float max = vec.at(0);
+ for(size_t i = 1; i < vec.size(); i++)
+ {
+ max = std::max(max, vec.at(i));
+ }
+ return max;
+ }
+
+ };
+ }
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/core/math/MatrixHelpers.h b/source/RobotAPI/libraries/core/math/MatrixHelpers.h
new file mode 100644
index 0000000000000000000000000000000000000000..258a16600462eb0be51dc1acc1c8c96507987ccb
--- /dev/null
+++ b/source/RobotAPI/libraries/core/math/MatrixHelpers.h
@@ -0,0 +1,69 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package ArmarX::Core
+* @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+* @date 2015
+* @copyright http://www.gnu.org/licenses/gpl.txt
+* GNU General Public License
+*/
+
+#ifndef ARMARX_ROBOTAPI_MATH_MATRIXHELPERS_H_
+#define ARMARX_ROBOTAPI_MATH_MATRIXHELPERS_H_
+
+#include <math.h>
+#include <Eigen/Eigen>
+
+namespace armarx
+{
+ namespace math
+ {
+ class MatrixHelpers
+ {
+ public:
+ static void SetRowToValue(Eigen::MatrixXf &matrix, int rowNr, float value)
+ {
+ for(int i = 0; i < matrix.cols(); i++)
+ {
+ matrix(rowNr, i) = value;
+ }
+ }
+
+ static Eigen::Vector3f CalculateCog3D(const Eigen::MatrixXf &points)
+ {
+ Eigen::Vector3f sum(0, 0, 0);
+ for(int i = 0; i < points.cols(); i++)
+ {
+ sum += points.block<3,1>(0,i);
+ }
+ return sum / points.cols();
+ }
+
+ static Eigen::MatrixXf SubtractVectorFromAllColumns3D(const Eigen::MatrixXf &points, const Eigen::Vector3f &vec)
+ {
+ Eigen::MatrixXf matrix(3, points.cols());
+ for(int i = 0; i < points.cols(); i++)
+ {
+ matrix.block<3,1>(0,i) = points.block<3,1>(0,i) - vec;
+ }
+ return matrix;
+ }
+
+ };
+ }
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/core/math/SVD.h b/source/RobotAPI/libraries/core/math/SVD.h
new file mode 100644
index 0000000000000000000000000000000000000000..09f6ad0ffa4557d4647d20b28bda1a508010f23a
--- /dev/null
+++ b/source/RobotAPI/libraries/core/math/SVD.h
@@ -0,0 +1,59 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package ArmarX::Core
+* @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+* @date 2015
+* @copyright http://www.gnu.org/licenses/gpl.txt
+* GNU General Public License
+*/
+
+#ifndef ARMARX_ROBOTAPI_MATH_SVD_H_
+#define ARMARX_ROBOTAPI_MATH_SVD_H_
+
+#include <math.h>
+#include <Eigen/Eigen>
+
+namespace armarx
+{
+ namespace math
+ {
+ class SVD
+ {
+ private:
+ Eigen::JacobiSVD<Eigen::MatrixXf> svd;
+ public:
+ Eigen::MatrixXf matrixU;
+ Eigen::MatrixXf matrixV;
+ Eigen::VectorXf singularValues;
+ SVD(Eigen::MatrixXf matrix)
+ : svd(matrix, Eigen::ComputeThinU | Eigen::ComputeThinV)
+ {
+ matrixU = svd.matrixU();
+ matrixV = svd.matrixV();
+ singularValues = svd.singularValues();
+ }
+
+ Eigen::Vector3f getLeftSingularVector3D(int nr)
+ {
+ return matrixU.block<3,1>(0,nr);
+ }
+
+ };
+ }
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/core/math/SlidingWindowVectorMedian.h b/source/RobotAPI/libraries/core/math/SlidingWindowVectorMedian.h
new file mode 100644
index 0000000000000000000000000000000000000000..dd0caa836098a446d3b9c0b181453e7fc064cf50
--- /dev/null
+++ b/source/RobotAPI/libraries/core/math/SlidingWindowVectorMedian.h
@@ -0,0 +1,97 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package ArmarX::Core
+* @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+* @date 2015
+* @copyright http://www.gnu.org/licenses/gpl.txt
+* GNU General Public License
+*/
+
+#ifndef ARMARX_ROBOTAPI_MATH_SLIDINGWINDOWVECTORMEDIAN_H_
+#define ARMARX_ROBOTAPI_MATH_SLIDINGWINDOWVECTORMEDIAN_H_
+
+#include "StatUtils.h"
+
+#include <math.h>
+#include <vector>
+
+#include <Core/core/exceptions/Exception.h>
+#include <boost/shared_ptr.hpp>
+
+namespace armarx
+{
+ namespace math
+ {
+ class SlidingWindowVectorMedian;
+ typedef boost::shared_ptr<SlidingWindowVectorMedian> SlidingWindowVectorMedianPtr;
+
+ class SlidingWindowVectorMedian
+ {
+ private:
+ size_t windowSize;
+ size_t vectorSize;
+ std::vector<float> data;
+ size_t currentIndex;
+ bool fullCycle;
+
+ public:
+ SlidingWindowVectorMedian(size_t vectorSize, size_t windowSize)
+ : windowSize(windowSize),
+ vectorSize(vectorSize),
+ data(vectorSize * windowSize, 0), // initialize all data to 0
+ currentIndex(0),
+ fullCycle(false)
+ {
+ }
+
+ void addEntry(const std::vector<float> &entry)
+ {
+ if(entry.size() != vectorSize)
+ {
+ throw LocalException("Vector of wrong size added. Execting: ") << vectorSize << "; Actual: " << entry.size();
+ }
+
+ for(size_t i = 0; i < entry.size(); i++)
+ {
+ data.at(i + currentIndex * vectorSize) = entry.at(i);
+ }
+
+ currentIndex = (currentIndex + 1) % windowSize;
+ fullCycle = fullCycle || currentIndex == 0;
+ }
+
+ std::vector<float> getMedian()
+ {
+ std::vector<float> median;
+ for(size_t i = 0; i < vectorSize; i++)
+ {
+ std::vector<float> samples;
+ for(size_t n = 0; n < windowSize; n++)
+ {
+ samples.push_back(data.at(i + n * vectorSize));
+ }
+ std::sort(samples.begin(), samples.end());
+ median.push_back(StatUtils::GetMedian(samples));
+ }
+ return median;
+ }
+
+ };
+ }
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/core/math/StatUtils.h b/source/RobotAPI/libraries/core/math/StatUtils.h
new file mode 100644
index 0000000000000000000000000000000000000000..f63f00ad86b3088b0f6146154fcfcf32f865c565
--- /dev/null
+++ b/source/RobotAPI/libraries/core/math/StatUtils.h
@@ -0,0 +1,72 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package ArmarX::Core
+* @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+* @date 2015
+* @copyright http://www.gnu.org/licenses/gpl.txt
+* GNU General Public License
+*/
+
+#ifndef ARMARX_ROBOTAPI_MATH_STATUTILS_H_
+#define ARMARX_ROBOTAPI_MATH_STATUTILS_H_
+
+#include <math.h>
+#include <vector>
+
+namespace armarx
+{
+ namespace math
+ {
+ class StatUtils
+ {
+ public:
+ static float GetPercentile(const std::vector<float> &sortedData, float percentile)
+ {
+ if(sortedData.size() == 0)
+ {
+ throw LocalException("GetPercentile not possible for empty vector");
+ }
+ float indexf = (sortedData.size() - 1) * percentile;
+ indexf = std::max(0.f, std::min(sortedData.size() - 1.f, indexf));
+ int index = (int)indexf;
+ float f = indexf - index;
+ if(index == (int)sortedData.size() - 1)
+ {
+ return sortedData.at(sortedData.size() - 1);
+ }
+ return sortedData.at(index) * (1 - f) + sortedData.at(index + 1) * f;
+ }
+ static std::vector<float> GetPercentiles(const std::vector<float> &sortedData, int percentiles)
+ {
+ std::vector<float> result;
+ result.push_back(sortedData.at(0));
+ for(int i = 1; i < percentiles; i++)
+ {
+ result.push_back(GetPercentile(sortedData, 1.f / percentiles * i));
+ }
+ result.push_back(sortedData.at(sortedData.size() - 1));
+ return result;
+ }
+ static float GetMedian(const std::vector<float> &sortedData)
+ {
+ return GetPercentile(sortedData, 0.5f);
+ }
+ };
+ }
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/core/math/Trigonometry.h b/source/RobotAPI/libraries/core/math/Trigonometry.h
new file mode 100644
index 0000000000000000000000000000000000000000..4520e674d438992d4f770fad0bcc23455066f08c
--- /dev/null
+++ b/source/RobotAPI/libraries/core/math/Trigonometry.h
@@ -0,0 +1,61 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package ArmarX::Core
+* @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+* @date 2015
+* @copyright http://www.gnu.org/licenses/gpl.txt
+* GNU General Public License
+*/
+
+#ifndef ARMARX_ROBOTAPI_MATH_TRIGONOMETRY_H_
+#define ARMARX_ROBOTAPI_MATH_TRIGONOMETRY_H_
+
+#include <math.h>
+
+namespace armarx
+{
+ namespace math
+ {
+ class Trigonometry
+ {
+ public:
+ static float Deg2RadF(const float angle)
+ {
+ return angle / 180.0f * M_PI;
+ }
+ static double Deg2RadD(const double angle)
+ {
+ return angle / 180.0d * M_PI;
+ }
+ static float Rad2DegF(const float rad)
+ {
+ return rad / M_PI * 180.0f;
+ }
+ static double Rad2DegD(const float rad)
+ {
+ return rad / M_PI * 180.0d;
+ }
+
+ static double GetAngleFromVectorXY(const Eigen::Vector3f &vector)
+ {
+ return atan2(vector(1), vector(0));
+ }
+ };
+ }
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticSensor.h b/source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticSensor.h
index 884c946e7ee9632bc4f8f8f4a63d13eb3bdb70f9..897f104364008ebbb264f6f31e808ad2b077ebf4 100644
--- a/source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticSensor.h
+++ b/source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticSensor.h
@@ -10,7 +10,7 @@
//#include <Core/util/variants/eigen3/Eigen3LibRegistry.h>
#include "TextWriter.h"
#include <Core/observers/variant/TimestampVariant.h>
-#include <Core/util/math/SlidingWindowVectorMedian.h>
+#include <RobotAPI/libraries/core/math/SlidingWindowVectorMedian.h>
#include <Core/util/variants/eigen3/MatrixVariant.h>
#include <boost/thread/mutex.hpp>