diff --git a/source/RobotAPI/libraries/ArmarXObjects/predictions.cpp b/source/RobotAPI/libraries/ArmarXObjects/predictions.cpp index f3550f8746ec4a2b8470d11f907acbb0d7845f9e..427868519c15f4f3c31342f980d3ee245f528a88 100644 --- a/source/RobotAPI/libraries/ArmarXObjects/predictions.cpp +++ b/source/RobotAPI/libraries/ArmarXObjects/predictions.cpp @@ -31,22 +31,13 @@ namespace armarx::objpose { objpose::ObjectPosePredictionResult - predictObjectPose(const std::map<DateTime, ObjectPose>& poses, + predictObjectPoseLinear(const std::map<DateTime, ObjectPose>& poses, const DateTime& time, - const ObjectPose& latestPose, - const armem::client::PredictionSettings& settings) + const ObjectPose& latestPose) { objpose::ObjectPosePredictionResult result; result.success = false; - if (!settings.predictionEngineID.empty() && - settings.predictionEngineID != "Linear Position Regression") - { - result.errorMessage = "Prediction engine '" + settings.predictionEngineID + - "' not available for object pose prediction."; - return result; - } - const DateTime timeOrigin = DateTime::Now(); std::vector<double> timestampsSec; diff --git a/source/RobotAPI/libraries/ArmarXObjects/predictions.h b/source/RobotAPI/libraries/ArmarXObjects/predictions.h index 90075859090689255ab28d5269b4e208719ee3a9..d9b647f9dd81a1e04ff435a0e20c94368cdb89f4 100644 --- a/source/RobotAPI/libraries/ArmarXObjects/predictions.h +++ b/source/RobotAPI/libraries/ArmarXObjects/predictions.h @@ -30,7 +30,10 @@ namespace armarx::objpose { /** - * @brief Predict the pose of an object given a history of poses. + * @brief Predict the pose of an object given a history of poses + * based on a linear regression. + * + * If `poses` is empty, `latestPose` is returned. * * @param poses the history of poses to base the prediction on * @param time the timestamp to make the prediction for @@ -39,8 +42,9 @@ namespace armarx::objpose * @return the result of the prediction */ objpose::ObjectPosePredictionResult - predictObjectPose(const std::map<DateTime, ObjectPose>& poses, - const DateTime& time, - const ObjectPose& latestPose, - const armem::client::PredictionSettings& settings); + predictObjectPoseLinear( + const std::map<DateTime, ObjectPose>& poses, + const DateTime& time, + const ObjectPose& latestPose); + } // namespace armarx::objpose diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp index 25bf9760377f55cbcff0dff975a595cbe05c6ef1..7c7dac41c1a74bca1606721b86de565f821a7608 100644 --- a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp +++ b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp @@ -489,13 +489,28 @@ namespace armarx::armem::server::obj::instance for (size_t i = 0; i < requests.size(); ++i) { - if (results.at(i).success) + const objpose::ObjectPosePredictionRequest& request = requests.at(i); + objpose::ObjectPosePredictionResult& result = results.at(i); + + if (result.success) { - results.at(i) = objpose::predictObjectPose( - poses.at(i), - armarx::fromIce<DateTime>(requests.at(i).timestamp), - latestPoses.at(i), - armem::client::PredictionSettings::fromIce(requests.at(i).settings)); + armem::client::PredictionSettings settings = + armem::client::PredictionSettings::fromIce(request.settings); + + if (settings.predictionEngineID.empty() + or settings.predictionEngineID == "Linear Position Regression") + { + result = objpose::predictObjectPoseLinear( + poses.at(i), + armarx::fromIce<DateTime>(request.timestamp), + latestPoses.at(i)); + } + else + { + result.errorMessage = "Prediction engine '" + settings.predictionEngineID + + "' not available for object pose prediction."; + result.success = false; + } } } return results; diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp index 7d9c94ded121bfa2c22c48b78a94bffbbea85e0d..47206a42fa74036015b875f104c0503172d9ac2e 100644 --- a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp +++ b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp @@ -280,13 +280,10 @@ namespace armarx::armem::server::obj::instance } if (showLinearPredictions) { - armem::client::PredictionSettings settings; - settings.predictionEngineID = "Linear Position Regression"; - auto predictionResult = objpose::predictObjectPose( + auto predictionResult = objpose::predictObjectPoseLinear( poseHistory, Time::Now() + Duration::SecondsDouble(linearPredictionTimeOffsetSeconds), - objectPose, - settings); + objectPose); if (predictionResult.success) { auto predictedPose =