Move linear prediction code to simox

source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp

@@ -37,6 +37,7 @@
 #include <SimoxUtility/algorithm/string.h>
 #include <SimoxUtility/json.h>
 #include <SimoxUtility/math/pose/pose.h>
+#include <SimoxUtility/math/regression/linear3d.h>
 
 #include <Eigen/Geometry>
 #include <Eigen/Dense>
@@ -629,9 +630,16 @@ namespace armarx::armem::server::obj::instance
             return result;
         }
 
-        const Eigen::Vector3d prediction = makeLinearPrediction(timestampsSec, positions, predictionTime);
-        // ToDo: Remove.
-        ARMARX_IMPORTANT << "Prediction: " << prediction.transpose();
+        Eigen::Vector3d prediction;
+        {
+            using simox::math::LinearRegression3D;
+            const bool inputOffset = false;
+            const LinearRegression3D model = LinearRegression3D::Fit(timestampsSec, positions, inputOffset);
+            prediction = model.predict(predictionTime);
+
+            // ToDo: Remove.
+            ARMARX_IMPORTANT << "Prediction: " << prediction.transpose() << "\n (model: " << model << ")";
+        }
 
         // Used as a template for the returned pose prediction.
         ObjectPose objectPoseTemplate = getLatestObjectPose(*entity);
@@ -649,41 +657,6 @@ namespace armarx::armem::server::obj::instance
         return result;
     }
 
-    Eigen::Vector3d
-    Segment::makeLinearPrediction(std::vector<double>& timestamps,
-                                  std::vector<Eigen::Vector3d>& positions,
-                                  double predictionTime)
-    {
-        Eigen::Matrix3Xd positionMatrix(3, positions.size());
-        for (long col = 0; col < positionMatrix.cols(); ++col)
-        {
-            positionMatrix.col(col) = positions.at(col);
-        }
-
-        // The matrix of the predictor functions evaluated at the corresponding timestamps.
-        // Since this is a linear regression, the constant function a(t) = 1 and identity
-        // b(t) = t are used.
-        Eigen::MatrixX2d linFuncMatrix(timestamps.size(), 2);
-        linFuncMatrix.col(0) = Eigen::RowVectorXd::Ones(timestamps.size());
-        linFuncMatrix.col(1) = Eigen::Map<Eigen::VectorXd>(timestamps.data(), timestamps.size());
-
-        // `linFuncMatrix` is poorly conditioned for timestamps that are close together,
-        // so the normal equation would lose 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 = positionMatrix.row(dim).transpose();
-            coeffs.row(dim) = qrDecomp.solve(coords);
-        }
-
-        Eigen::Vector2d input;
-        input << 1.0, predictionTime;
-        return coeffs * input;
-    }
 
     objpose::AttachObjectToRobotNodeOutput
     Segment::attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput& input)

source/RobotAPI/libraries/armem_objects/server/instance/Segment.h

@@ -140,12 +140,6 @@ namespace armarx::armem::server::obj::instance
 
         std::optional<wm::EntityInstance> findClassInstance(const ObjectID& objectID) const;
 
-
-        static Eigen::Vector3d makeLinearPrediction(std::vector<double>& timestamps,
-                                                    std::vector<Eigen::Vector3d>& positions,
-                                                    double predictionTime);
-
-
         friend struct DetachVisitor;