diff --git a/SimoxUtility/math/regression/linear3d.cpp b/SimoxUtility/math/regression/linear3d.cpp index f1a37111185b3752a549b1f9f29c770f8c0e1e46..6403512c00db60a53c7cf4d7a6fd8aa550bdb5e5 100644 --- a/SimoxUtility/math/regression/linear3d.cpp +++ b/SimoxUtility/math/regression/linear3d.cpp @@ -7,8 +7,24 @@ namespace simox::math { LinearRegression3D - LinearRegression3D::Fit(const std::vector<double>& xs, const std::vector<Eigen::Vector3d>& ys) + 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) { @@ -43,7 +59,7 @@ namespace simox::math LinearRegression3D::predict(double x) const { Eigen::Vector2d input; - input << 1.0, x; + input << 1.0, x + inputOffset; return coefficients * input; } diff --git a/SimoxUtility/math/regression/linear3d.h b/SimoxUtility/math/regression/linear3d.h index 763fd3d312c79d674851fff34b77be2406a07646..365c33f23de577d0f11b1cc24d9118470ae2d069 100644 --- a/SimoxUtility/math/regression/linear3d.h +++ b/SimoxUtility/math/regression/linear3d.h @@ -35,15 +35,21 @@ namespace simox::math */ 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); + 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.