From ee30cbb7c5cc3f596f5052d2a2477ebd7fb28089 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Thu, 5 May 2022 09:26:11 +0200
Subject: [PATCH] Move linear prediction code to simox
---
.../armem_objects/server/instance/Segment.cpp | 49 +++++--------------
.../armem_objects/server/instance/Segment.h | 6 ---
2 files changed, 11 insertions(+), 44 deletions(-)
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index 5162ec4e3..8d645a742 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/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)
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
index db97c715a..81dab749e 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
+++ b/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;
--
GitLab