diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp index 5162ec4e33ace082e1ecd59282002bb8e13826b9..483a5065a3a91f3ddf18604df69ffbaac60076b9 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> @@ -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(), - [×tampsSec, &positions](const wm::EntitySnapshot& snapshot) + [&timeOrigin, ×tampsSec, &positions](const wm::EntitySnapshot& snapshot) { snapshot.forEachInstance( - [&snapshot, &positions, ×tampsSec](const wm::EntityInstance& instance) + [&timeOrigin, &snapshot, &positions, ×tampsSec](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) 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; diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp index 5b7defda0013aff3c1d239d9d251e1d91f79ea9b..9d662129b14991ce570ee7e7201ce864ac6e00b0 100644 --- a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp +++ b/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});