Add input offset

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

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.