Merge branch 'linear-regression-3d' into 'master'

Linear regression 3d

See merge request Simox/simox!84

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,
+            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;
+}

SimoxUtility/math/regression/linear3d.h

+#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);
+
+}

SimoxUtility/tests/math/CMakeLists.txt

@@ -8,4 +8,4 @@ ADD_SU_TEST( SoftMinMax )
 ADD_SU_TEST( statistics )
 
 ADD_SUBDIRECTORY(pose)
-
+ADD_SUBDIRECTORY(regression)

SimoxUtility/tests/math/regression/CMakeLists.txt

+ADD_SU_TEST( linear3d )

SimoxUtility/tests/math/regression/linear3d.cpp

+/**
+* @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()