From 65284dbe483f3d49765b38f89938606d59906ba0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Mon, 15 Aug 2022 20:31:55 +0200
Subject: [PATCH] Add estimateAt to Human
---
source/armarx/navigation/human/types.cpp | 14 ++++++++++++++
source/armarx/navigation/human/types.h | 2 ++
2 files changed, 16 insertions(+)
create mode 100644 source/armarx/navigation/human/types.cpp
diff --git a/source/armarx/navigation/human/types.cpp b/source/armarx/navigation/human/types.cpp
new file mode 100644
index 00000000..5b6d3d88
--- /dev/null
+++ b/source/armarx/navigation/human/types.cpp
@@ -0,0 +1,14 @@
+#include "types.h"
+
+namespace armarx::navigation::human
+{
+ core::Pose2D
+ Human::estimateAt(DateTime& time) const
+ {
+ double dt = (time - detectionTime).toSecondsDouble();
+ core::Pose2D estimation{pose};
+ estimation.translation() += linearVelocity * dt;
+ return estimation;
+ }
+
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/types.h b/source/armarx/navigation/human/types.h
index ae490622..867849a1 100644
--- a/source/armarx/navigation/human/types.h
+++ b/source/armarx/navigation/human/types.h
@@ -32,6 +32,8 @@ namespace armarx::navigation::human
core::Pose2D pose;
Eigen::Vector2f linearVelocity;
DateTime detectionTime;
+
+ core::Pose2D estimateAt(DateTime& time) const;
};
struct HumanGroup
--
GitLab