diff --git a/SimoxUtility/CMakeLists.txt b/SimoxUtility/CMakeLists.txt
index b39459da876abe4caebf4d4c65af2b4b22101683..dd2a7c433bd57f01791724dba4458e9fb94f9e03 100644
--- a/SimoxUtility/CMakeLists.txt
+++ b/SimoxUtility/CMakeLists.txt
@@ -40,6 +40,8 @@ SET(SOURCES
math/pose/orthogonalize.cpp
math/pose/interpolate.cpp
+ math/regression/linear3d.cpp
+
math/statistics/Histogram1D.cpp
meta/type_name.cpp
@@ -187,6 +189,8 @@ SET(INCLUDES
math/pose/transform.h
math/pose/interpolate.h
+ math/regression/linear3d.h
+
math/similarity/cosine_similarity.h
math/similarity/angular_similarity.h
diff --git a/SimoxUtility/math.h b/SimoxUtility/math.h
index 831dc4ca47cab6ec3d30ed744ca82479c25e5b6a..b57cef0caa0dce7c2474a110f47e0d200eec07a6 100644
--- a/SimoxUtility/math.h
+++ b/SimoxUtility/math.h
@@ -14,6 +14,7 @@
#include "math/periodic_clamp.h"
#include "math/pose.h"
#include "math/project_to_plane.h"
+#include "math/regression.h"
#include "math/rescale.h"
#include "math/scale_value.h"
#include "math/similarity.h"
diff --git a/SimoxUtility/math/regression.h b/SimoxUtility/math/regression.h
new file mode 100644
index 0000000000000000000000000000000000000000..1e6131d854a53b1695f052cb1de44b7f91765313
--- /dev/null
+++ b/SimoxUtility/math/regression.h
@@ -0,0 +1,5 @@
+#pragma once
+
+// This file is generated!
+
+#include "regression/linear3d.h"
diff --git a/SimoxUtility/math/regression/linear3d.cpp b/SimoxUtility/math/regression/linear3d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ce4fa0eb132307fe674cb533f162e6a44d9539ef
--- /dev/null
+++ b/SimoxUtility/math/regression/linear3d.cpp
@@ -0,0 +1,86 @@
+#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
new file mode 100644
index 0000000000000000000000000000000000000000..6807c0bc50a813585cec9f92f18856443b03119b
--- /dev/null
+++ b/SimoxUtility/math/regression/linear3d.h
@@ -0,0 +1,76 @@
+#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/CMakeLists.txt b/SimoxUtility/tests/math/CMakeLists.txt
index 0e9e6d7f60b91ca657eb84535062f981e080e27b..a3558c2192c3a02fd5dc2762b2929d833546f476 100644
--- a/SimoxUtility/tests/math/CMakeLists.txt
+++ b/SimoxUtility/tests/math/CMakeLists.txt
@@ -8,4 +8,4 @@ ADD_SU_TEST( SoftMinMax )
ADD_SU_TEST( statistics )
ADD_SUBDIRECTORY(pose)
-
+ADD_SUBDIRECTORY(regression)
diff --git a/SimoxUtility/tests/math/regression/CMakeLists.txt b/SimoxUtility/tests/math/regression/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..fab80e952c987a730acc2bd292e52736646cd80c
--- /dev/null
+++ b/SimoxUtility/tests/math/regression/CMakeLists.txt
@@ -0,0 +1 @@
+ADD_SU_TEST( linear3d )
diff --git a/SimoxUtility/tests/math/regression/linear3d.cpp b/SimoxUtility/tests/math/regression/linear3d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a235d0a6d5d1a6cd11532df66a52f889ff652dc9
--- /dev/null
+++ b/SimoxUtility/tests/math/regression/linear3d.cpp
@@ -0,0 +1,95 @@
+/**
+* @package SimoxUtility
+* @author Raphael Grimm
+* @copyright 2019 Raphael Grimm
+*/
+
+#define BOOST_TEST_MODULE SimoxUtility_RegressionTest
+
+#include <random>
+#include <iostream>
+
+#include <boost/test/included/unit_test.hpp>
+
+#include <SimoxUtility/math/regression/linear3d.h>
+
+
+
+namespace SimoxMathRegressionTest
+{
+
+struct Fixture
+{
+ const double prec = 1e-10;
+
+ std::function<Eigen::Vector3d(double)> f = [](double x)
+ {
+ Eigen::Vector3d y;
+ for (Eigen::Index i = 0; i < y.rows(); ++i)
+ {
+ y(i) = - (1 + i) + (2 * i) * x;
+ }
+ return y;
+ };
+ const std::vector<double> xs
+ {
+ -1, 0, 2
+ };
+ const std::vector<Eigen::Vector3d> ys
+ {
+ f(xs[0]), f(xs[1]), f(xs[2])
+ };
+
+};
+}
+
+
+BOOST_FIXTURE_TEST_SUITE(SimoxMathRegressionTest, Fixture)
+
+
+BOOST_AUTO_TEST_CASE(test_linear_regression_3d_fit_and_predict)
+{
+ using simox::math::LinearRegression3D;
+
+ // Fit
+
+ const LinearRegression3D regression = LinearRegression3D::Fit(xs, ys);
+
+ BOOST_TEST_MESSAGE("Regression: " << regression);
+
+ BOOST_CHECK_CLOSE(regression.coefficients(0, 0), - (1 + 0), prec);
+ BOOST_CHECK_CLOSE(regression.coefficients(1, 0), - (1 + 1), prec);
+ BOOST_CHECK_CLOSE(regression.coefficients(2, 0), - (1 + 2), prec);
+
+ BOOST_CHECK_CLOSE(regression.coefficients(0, 1), (2 * 0), prec);
+ BOOST_CHECK_CLOSE(regression.coefficients(1, 1), (2 * 1), prec);
+ BOOST_CHECK_CLOSE(regression.coefficients(2, 1), (2 * 2), prec);
+
+
+ // Predict
+
+ BOOST_CHECK_LE((regression.predict(xs[0]) - ys[0]).norm(), prec);
+ BOOST_CHECK_LE((regression.predict(xs[1]) - ys[1]).norm(), prec);
+ BOOST_CHECK_LE((regression.predict(xs[2]) - ys[2]).norm(), prec);
+}
+
+
+BOOST_AUTO_TEST_CASE(test_linear_regression_3d_fit_and_predict_with_input_offset)
+{
+ using simox::math::LinearRegression3D;
+
+ const bool inputOffset = true;
+ const LinearRegression3D regression = LinearRegression3D::Fit(xs, ys, inputOffset);
+
+ BOOST_TEST_MESSAGE("Regression: " << regression);
+ BOOST_CHECK_EQUAL(regression.inputOffset, - xs[0]);
+
+ // Coefficients are different now, but prediction should be the same.
+
+ BOOST_CHECK_LE((regression.predict(xs[0]) - ys[0]).norm(), prec);
+ BOOST_CHECK_LE((regression.predict(xs[1]) - ys[1]).norm(), prec);
+ BOOST_CHECK_LE((regression.predict(xs[2]) - ys[2]).norm(), prec);
+}
+
+
+BOOST_AUTO_TEST_SUITE_END()