Merge branch 'linear-prediction-from-simox' into 'linear-prediction'

Move linear prediction code to simox

See merge request ArmarX/RobotAPI!243

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>
@@ -583,8 +584,6 @@ namespace armarx::armem::server::obj::instance
 
         const ObjectID objID = armarx::fromIce<ObjectID>(request.objectID);
 
-        const double predictionTime = static_cast<double>(
-            armarx::fromIce<Time>(request.timestamp).toMicroSecondsSinceEpoch());
         const Duration lookbackDuration = Duration::SecondsDouble(p.linearPredictionLookbackSeconds);
 
         const wm::Entity* entity = findObjectEntity(objID);
@@ -596,16 +595,20 @@ namespace armarx::armem::server::obj::instance
             return result;
         }
 
+        const Time timeOrigin = Time::Now();
+
         std::vector<double> timestampsSec;
         std::vector<Eigen::Vector3d> positions;
 
         entity->forEachSnapshotInTimeRange(
             Time::Now() - lookbackDuration, Time::Invalid(),
-            [&timestampsSec, &positions](const wm::EntitySnapshot& snapshot)
+            [&timeOrigin, &timestampsSec, &positions](const wm::EntitySnapshot& snapshot)
             {
                 snapshot.forEachInstance(
-                    [&snapshot, &positions, &timestampsSec](const wm::EntityInstance& instance)
+                    [&timeOrigin, &snapshot, &positions, &timestampsSec](const wm::EntityInstance& instance)
                     {
+                        // ToDo: How to handle attached objects?
+
                         arondto::ObjectInstance dto;
                         dto.fromAron(instance.data());
 
@@ -613,7 +616,7 @@ namespace armarx::armem::server::obj::instance
                         fromAron(dto, pose);
 
                         timestampsSec.push_back(
-                            snapshot.time().toDurationSinceEpoch().toSecondsDouble());
+                            (snapshot.time() - timeOrigin).toSecondsDouble());
                         positions.push_back(
                             simox::math::position(pose.objectPoseGlobal).cast<double>());
                     });
@@ -629,9 +632,18 @@ 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);
+
+            const Time predictionTime = armarx::fromIce<Time>(request.timestamp);
+            prediction = model.predict((predictionTime - timeOrigin).toSecondsDouble());
+
+            // 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 +661,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;
 
 

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

@@ -248,7 +248,8 @@ namespace armarx::armem::server::obj::instance
             request.objectID = armarx::toIce<armarx::data::ObjectID>(id);
             request.settings.predictionEngineID = "Linear Position Regression";
             request.timestamp = armarx::toIce<armarx::core::time::dto::DateTime>(
-                Time::Now() - Duration::MilliSeconds(100));
+                // ToDo: Make parametrizable.
+                Time::Now() + Duration::MilliSeconds(1000));
             auto predictionResult = predictor(request);
             if (predictionResult.success)
             {
@@ -300,6 +301,8 @@ namespace armarx::armem::server::obj::instance
 
         useArticulatedModels.setValue(visu.useArticulatedModels);
 
+        showLinearPredictions.setValue(visu.showLinearPredictions);
+
         GridLayout grid;
         int row = 0;
         grid.add(Label("Enabled"), {row, 0}).add(enabled, {row, 1});