diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp index 5162ec4e33ace082e1ecd59282002bb8e13826b9..8d645a7427dd343bdd6d2525a50e7f55770f5e49 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 db97c715ad24694916b6c9be7235202657fa0bba..81dab749e5afec3471cd2e3085b7c8122f7a10ea 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;