diff --git a/SimoxUtility/CMakeLists.txt b/SimoxUtility/CMakeLists.txt
index dd2a7c433bd57f01791724dba4458e9fb94f9e03..c55fe71a61f88b9023a5dc1e55be4e148719ffda 100644
--- a/SimoxUtility/CMakeLists.txt
+++ b/SimoxUtility/CMakeLists.txt
@@ -40,7 +40,7 @@ SET(SOURCES
math/pose/orthogonalize.cpp
math/pose/interpolate.cpp
- math/regression/linear3d.cpp
+ math/regression/linear.cpp
math/statistics/Histogram1D.cpp
@@ -189,7 +189,8 @@ SET(INCLUDES
math/pose/transform.h
math/pose/interpolate.h
- math/regression/linear3d.h
+ math/regression/linear.h
+ math/regression/linear.hpp
math/similarity/cosine_similarity.h
math/similarity/angular_similarity.h
diff --git a/SimoxUtility/math/regression.h b/SimoxUtility/math/regression.h
index 1e6131d854a53b1695f052cb1de44b7f91765313..78b3bac639ac9999dfca2658e6e074d824a3a8d3 100644
--- a/SimoxUtility/math/regression.h
+++ b/SimoxUtility/math/regression.h
@@ -2,4 +2,4 @@
// This file is generated!
-#include "regression/linear3d.h"
+#include "regression/linear.h"
diff --git a/SimoxUtility/math/regression/linear.cpp b/SimoxUtility/math/regression/linear.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..89944dec533a2e242b118da4a2a82c6a3e9703c0
--- /dev/null
+++ b/SimoxUtility/math/regression/linear.cpp
@@ -0,0 +1,18 @@
+#include "linear.hpp"
+
+
+namespace simox::math
+{
+ template class LinearRegression<1, float>;
+ template class LinearRegression<1, double>;
+
+ template class LinearRegression<2, float>;
+ template class LinearRegression<2, double>;
+
+ template class LinearRegression<3, float>;
+ template class LinearRegression<3, double>;
+
+ template class LinearRegression<4, float>;
+ template class LinearRegression<4, double>;
+
+}
diff --git a/SimoxUtility/math/regression/linear.h b/SimoxUtility/math/regression/linear.h
new file mode 100644
index 0000000000000000000000000000000000000000..e833c4f285dc40345b2a3585326968b595591f05
--- /dev/null
+++ b/SimoxUtility/math/regression/linear.h
@@ -0,0 +1,19 @@
+#pragma once
+
+#include "linear.hpp"
+
+
+namespace simox::math
+{
+
+ using LinearRegression1f = LinearRegression<1, float>;
+ using LinearRegression2f = LinearRegression<2, float>;
+ using LinearRegression3f = LinearRegression<3, float>;
+ using LinearRegression4f = LinearRegression<4, float>;
+
+ using LinearRegression1d = LinearRegression<1, double>;
+ using LinearRegression2d = LinearRegression<2, double>;
+ using LinearRegression3d = LinearRegression<3, double>;
+ using LinearRegression4d = LinearRegression<4, double>;
+
+}
diff --git a/SimoxUtility/math/regression/linear.hpp b/SimoxUtility/math/regression/linear.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..a082d17c543f1a441b7bd15aa092e3e43b8bb442
--- /dev/null
+++ b/SimoxUtility/math/regression/linear.hpp
@@ -0,0 +1,165 @@
+#pragma once
+
+#include <Eigen/Core>
+#include <Eigen/Dense>
+
+
+namespace simox::math
+{
+
+ /**
+ * @brief A linear regression model.
+ *
+ * The linear model has the form
+ *
+ * y = a + b * x,
+ *
+ * or per dimension
+ *
+ * y_i = a_i + b_i * x (i = 1 .. d),
+ *
+ * where
+ *
+ * - x is the scalar input variable (e.g. time)
+ * - y is an n-D vector output variable (e.g. position)
+ * - a is an n-D bias vector
+ * - b is an n-D slope vector
+ *
+ * In matrix notation, the equation system represented by the model is:
+ *
+ * [[ a_1 b_1 ] [ y_1 ]
+ * [ ... ] [ 1 ] = [ ... ]
+ * [ a_i b_i ] * [ x ] = [ y_i ]
+ * [ ... ] = [ ... ]
+ * [ a_d b_d ]] [ y_d ]
+ *
+ * Given data x[j] in R and y[j] \in R^3 (j = 1 .. n),
+ * the regression solves the following equation system(s)
+ * for [ a_i, b_i ] (i = 1 .. d):
+ *
+ * [[ 1 x[1] ] [ y[1]_i ]
+ * [ ... ] [ a_i ] [ ... ]
+ * [ 1 x[j] ] * [ b_i ] = [ y[j]_i ]
+ * [ ... ] [ ... ]
+ * [ 1 x[n] ]] [ y[n]_i ]
+ */
+ template <int _Dim, typename _FloatT = double>
+ class LinearRegression
+ {
+ public:
+
+ static const int Dim = _Dim;
+ using FloatT = _FloatT;
+ using VectorT = typename Eigen::Matrix<FloatT, Dim, 1>;
+ using CoefficientsMatrixT = typename Eigen::Matrix<FloatT, Dim, 2>;
+
+
+ public:
+
+ /**
+ * The coefficients of the bias term (a_i) and input variable x (b_i)
+ * [[ a_1 b_1 ]
+ * [ ... ... ]
+ * [ a_d b_d ]]
+ */
+ CoefficientsMatrixT coefficients = CoefficientsMatrixT::Zero();
+
+ /// The input offset, so the virtual input x' = x + offset.
+ FloatT inputOffset = 0;
+
+
+ /**
+ * @brief Fit a linear regression model to the given data.
+ * @param xs The input variables.
+ * @param ys The output variables.
+ * @param offsetInput If true, the inputs are offset to x' = x - x_0.
+ * @return The regression model.
+ */
+ static LinearRegression
+ Fit(const std::vector<FloatT>& xs,
+ const std::vector<VectorT>& ys,
+ bool offsetInput = false)
+ {
+ using VectorX = typename Eigen::Matrix<FloatT, Eigen::Dynamic, 1>;
+ using MatrixX2 = typename Eigen::Matrix<FloatT, Eigen::Dynamic, 2>;
+ using MatrixDX = typename Eigen::Matrix<FloatT, Dim, Eigen::Dynamic>;
+
+ if (offsetInput and xs.at(0) != 0)
+ {
+ FloatT offset = - xs.at(0); // Move x_0 to 0.
+ std::vector<FloatT> virtualXs = xs;
+ for (FloatT& x : virtualXs)
+ {
+ x = x + offset;
+ }
+ LinearRegression r = LinearRegression::Fit(virtualXs, ys, false);
+ r.inputOffset = offset;
+ return r;
+ }
+
+ MatrixDX ysMatrix(Dim, ys.size());
+ for (long col = 0; col < ysMatrix.cols(); ++col)
+ {
+ ysMatrix.col(col) = ys.at(col);
+ }
+
+ // The matrix of the predictor functions evaluated at the corresponding xs.
+ // Since this is a linear regression, the constant function a(t) = 1 and identity
+ // b(t) = t are used.
+ MatrixX2 linFuncMatrix(xs.size(), 2);
+ linFuncMatrix.col(0) = VectorX::Ones(xs.size());
+ linFuncMatrix.col(1) = Eigen::Map<const VectorX>(xs.data(), xs.size());
+
+ // `linFuncMatrix` is poorly conditioned for xs that are close together
+ // (e.g. time stamps), so the normal equation would loose a lot of precision.
+ auto qrDecomp = linFuncMatrix.colPivHouseholderQr();
+
+ // Each coordinate can be treated individually (general multivariate regression).
+ // `coeffs` contains a_i and b_i in a_0 + b_0 * t = x, a_1 + b_1 * t = y, etc.
+ CoefficientsMatrixT coeffs;
+ for (int dim = 0; dim < 3; ++dim)
+ {
+ VectorX coords = ysMatrix.row(dim).transpose();
+ coeffs.row(dim) = qrDecomp.solve(coords);
+ }
+
+ return LinearRegression { .coefficients = coeffs };
+ }
+
+
+ /**
+ * @brief Predict the output variable of the given input variable.
+ * @param x The input variable.
+ * @return The predicted output variable.
+ */
+ VectorT predict(FloatT x) const
+ {
+ Eigen::Matrix<FloatT, 2, 1> input;
+ input << 1.0, x + inputOffset;
+ return coefficients * input;
+ }
+
+ };
+
+
+ template <int _Dim, typename _FloatT = double>
+ std::ostream&
+ operator<<(std::ostream& os, const LinearRegression<_Dim, _FloatT>& r)
+ {
+ os << "<LinearRegression y = a + b * x with [ ";
+ for (Eigen::Index row = 0; row < r.coefficients.rows(); ++row)
+ {
+ if (row != 0)
+ {
+ os << " | ";
+ }
+ os << "y_" << row
+ << " = " << r.coefficients(row, 0)
+ << " + " << r.coefficients(row, 1) << " * x"
+ ;
+ }
+ os << " ] and input offset " << r.inputOffset << ">";
+ return os;
+ }
+
+}
diff --git a/SimoxUtility/math/regression/linear3d.cpp b/SimoxUtility/math/regression/linear3d.cpp
deleted file mode 100644
index ce4fa0eb132307fe674cb533f162e6a44d9539ef..0000000000000000000000000000000000000000
--- a/SimoxUtility/math/regression/linear3d.cpp
+++ /dev/null
@@ -1,86 +0,0 @@
-#include "linear3d.h"
-
-#include <Eigen/Dense>
-
-
-namespace simox::math
-{
-
- LinearRegression3D
- LinearRegression3D::Fit(
- const std::vector<double>& xs,
- const std::vector<Eigen::Vector3d>& ys,
- bool offsetInput)
- {
- if (offsetInput and xs.at(0) != 0)
- {
- double offset = - xs.at(0); // Move x_0 to 0.
- std::vector<double> virtualXs = xs;
- for (double& x : virtualXs)
- {
- x = x + offset;
- }
- LinearRegression3D r = LinearRegression3D::Fit(virtualXs, ys, false);
- r.inputOffset = offset;
- return r;
- }
-
- Eigen::Matrix3Xd ysMatrix(3, ys.size());
- for (long col = 0; col < ysMatrix.cols(); ++col)
- {
- ysMatrix.col(col) = ys.at(col);
- }
-
- // The matrix of the predictor functions evaluated at the corresponding xs.
- // Since this is a linear regression, the constant function a(t) = 1 and identity
- // b(t) = t are used.
- Eigen::MatrixX2d linFuncMatrix(xs.size(), 2);
- linFuncMatrix.col(0) = Eigen::RowVectorXd::Ones(xs.size());
- linFuncMatrix.col(1) = Eigen::Map<const Eigen::VectorXd>(xs.data(), xs.size());
-
- // `linFuncMatrix` is poorly conditioned for xs that are close together
- // (e.g. time stamps), so the normal equation would loose a lot of precision.
- auto qrDecomp = linFuncMatrix.colPivHouseholderQr();
-
- // Each coordinate can be treated individually (general multivariate regression).
- // `coeffs` contains a_i and b_i in a_0 + b_0 * t = x, a_1 + b_1 * t = y, etc.
- Eigen::Matrix<double, 3, 2> coeffs;
- for (int dim = 0; dim < 3; ++dim)
- {
- Eigen::VectorXd coords = ysMatrix.row(dim).transpose();
- coeffs.row(dim) = qrDecomp.solve(coords);
- }
-
- return LinearRegression3D { .coefficients = coeffs };
- }
-
-
- Eigen::Vector3d
- LinearRegression3D::predict(double x) const
- {
- Eigen::Vector2d input;
- input << 1.0, x + inputOffset;
- return coefficients * input;
- }
-
-}
-
-
-std::ostream&
-simox::math::operator<<(std::ostream& os, const LinearRegression3D& r)
-{
- os << "<LinearRegression3D y = a + b * x with [ ";
- for (Eigen::Index row = 0; row < r.coefficients.rows(); ++row)
- {
- if (row != 0)
- {
- os << " | ";
- }
- os << "y_" << row
- << " = " << r.coefficients(row, 0)
- << " + " << r.coefficients(row, 1) << " * x"
- ;
- }
- os << " ] and input offset " << r.inputOffset << ">";
- return os;
-}
diff --git a/SimoxUtility/math/regression/linear3d.h b/SimoxUtility/math/regression/linear3d.h
deleted file mode 100644
index 6807c0bc50a813585cec9f92f18856443b03119b..0000000000000000000000000000000000000000
--- a/SimoxUtility/math/regression/linear3d.h
+++ /dev/null
@@ -1,76 +0,0 @@
-#pragma once
-
-#include <Eigen/Core>
-
-
-namespace simox::math
-{
-
- /**
- * @brief A linear regression model of the form y = a + b * x,
- * or per dimension, y_i = a_i + b_i * x.
- *
- * - x is the scalar input variable (e.g. time)
- * - y is 3D vector output variable (e.g. position)
- * - a is a 3D bias vector
- * - b is a 3D slope vector
- *
- * In matrix notation, the equation system represented by the model is:
- *
- * [[ a_0 b_0 ] [ 1 ] [ y_0 ]
- * [ a_1 b_1 ] * [ x ] = [ y_1 ]
- * [ a_2 b_2 ]] [ y_2 ]
- *
- * Given data x[j] in R and y[j] \in R^3 (j = 0 .. n-1),
- * the regression solves the following equation system(s)
- * for [ a_i, b_i ] (i = 0 .. (3-1)):
- *
- * [[ 1 x[0] ] [ y[0]_i ]
- * [ ... ] [ a_i ] [ ... ]
- * [ 1 x[j] ] * [ b_i ] = [ y[j]_i ]
- * [ ... ] [ ... ]
- * [ 1 x[n-1] ]] [ y[n-1]_i ]
- */
- class LinearRegression3D
- {
- public:
-
- using CoefficientsMatrix = Eigen::Matrix<double, 3, 2>;
-
- /**
- * The coefficients of the bias term and input variable x
- * [[ a_0 b_0 ]
- * [ a_1 b_1 ]
- * [ a_2 b_2 ]]
- */
- CoefficientsMatrix coefficients = CoefficientsMatrix::Zero();
-
- /// The input offset, so the virtual input x' = x + offset.
- double inputOffset = 0;
-
-
- /**
- * @brief Fit a linear regression model to the given data.
- * @param xs The input variables.
- * @param ys The output variables.
- * @param offsetInput If true, the inputs are offset to x' = x - x_0.
- * @return The regression model.
- */
- static LinearRegression3D
- Fit(const std::vector<double>& xs,
- const std::vector<Eigen::Vector3d>& ys,
- bool offsetInput = false);
-
- /**
- * @brief Predict the output variable of the given input variable.
- * @param x The input variable.
- * @return The predicted output variable.
- */
- Eigen::Vector3d predict(double x) const;
-
- };
-
-
- std::ostream& operator<<(std::ostream& os, const LinearRegression3D& r);
-
-}
diff --git a/SimoxUtility/tests/math/regression/linear3d.cpp b/SimoxUtility/tests/math/regression/linear3d.cpp
index a235d0a6d5d1a6cd11532df66a52f889ff652dc9..a1c1fa1120ba8df74cffd362309eb04c5dde4206 100644
--- a/SimoxUtility/tests/math/regression/linear3d.cpp
+++ b/SimoxUtility/tests/math/regression/linear3d.cpp
@@ -11,8 +11,7 @@
#include <boost/test/included/unit_test.hpp>
-#include <SimoxUtility/math/regression/linear3d.h>
-
+#include <SimoxUtility/math/regression/linear.h>
namespace SimoxMathRegressionTest
@@ -49,11 +48,11 @@ BOOST_FIXTURE_TEST_SUITE(SimoxMathRegressionTest, Fixture)
BOOST_AUTO_TEST_CASE(test_linear_regression_3d_fit_and_predict)
{
- using simox::math::LinearRegression3D;
+ using simox::math::LinearRegression3d;
// Fit
- const LinearRegression3D regression = LinearRegression3D::Fit(xs, ys);
+ const LinearRegression3d regression = LinearRegression3d::Fit(xs, ys);
BOOST_TEST_MESSAGE("Regression: " << regression);
@@ -76,10 +75,10 @@ BOOST_AUTO_TEST_CASE(test_linear_regression_3d_fit_and_predict)
BOOST_AUTO_TEST_CASE(test_linear_regression_3d_fit_and_predict_with_input_offset)
{
- using simox::math::LinearRegression3D;
+ using simox::math::LinearRegression3d;
const bool inputOffset = true;
- const LinearRegression3D regression = LinearRegression3D::Fit(xs, ys, inputOffset);
+ const LinearRegression3d regression = LinearRegression3d::Fit(xs, ys, inputOffset);
BOOST_TEST_MESSAGE("Regression: " << regression);
BOOST_CHECK_EQUAL(regression.inputOffset, - xs[0]);