Add simox::math::LinearRegression3D

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
 

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"

SimoxUtility/math/regression.h

+#pragma once
+
+// This file is generated!
+
+#include "regression/linear3d.h"

SimoxUtility/math/regression/linear3d.cpp

+#include "linear3d.h"
+
+#include <Eigen/Dense>
+
+
+namespace simox::math
+{
+
+    LinearRegression3D
+    LinearRegression3D::Fit(const std::vector<double>& xs, const std::vector<Eigen::Vector3d>& ys)
+    {
+        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;
+        return coefficients * input;
+    }
+
+}
+
+
+std::ostream&
+simox::math::operator<<(std::ostream& os, const LinearRegression3D& r)
+{
+    os << "<LinearRegression3D a + b * x = y [ ";
+    for (Eigen::Index row = 0; row < r.coefficients.rows(); ++row)
+    {
+        if (row != 0)
+        {
+            os << ", ";
+        }
+        os << r.coefficients(row, 0)
+           << " + " << r.coefficients(row, 1) << " * x_" << row
+           << " = y_" << row;
+    }
+    os << " ]";
+    return os;
+}

SimoxUtility/math/regression/linear3d.h

+#pragma once
+
+#include <Eigen/Core>
+
+
+namespace simox::math
+{
+
+    /**
+     * @brief A linear regression model of the form a + b * x = y,
+     * or per dimension, a_i + b_i * x_i = y_i.
+     *
+     * - 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 coefficient vector
+     *
+     * In matrix notation, the underlying equation system is:
+     *
+     * [[ a_0  b_0 ]    [  1    1    1  ]   [ y_0 ]
+     *  [ a_1  b_1 ]  * [ x_0  x_1  x_2 ] = [ y_1 ]
+     *  [ a_2  b_2 ]]                       [ y_2 ]
+     */
+    class LinearRegression3D
+    {
+    public:
+
+        /**
+         * The coefficients of the bias term and input variable x
+         * [[ a_0  b_0 ]
+         *  [ a_1  b_1 ]
+         *  [ a_2  b_2 ]]
+         */
+        Eigen::Matrix<double, 3, 2> coefficients;
+
+
+        /**
+         * @brief Fit a linear regression model to the given data.
+         * @param xs The input variables.
+         * @param ys The output variables.
+         * @return The regression model.
+         */
+        static LinearRegression3D
+        Fit(const std::vector<double>& xs, const std::vector<Eigen::Vector3d>& ys);
+
+        /**
+         * @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);
+
+}