From d60279161499b3e48b158975d6a06b781fd2dec1 Mon Sep 17 00:00:00 2001
From: Corvin-N <corvin@navarro.de>
Date: Mon, 24 Oct 2022 17:40:10 +0200
Subject: [PATCH] Add new method for lasersensor measurement tracking in
header.
---
.../dynamic_scene_provider/Component.cpp | 2 +-
.../armarx/navigation/human/HumanTracker.cpp | 2 +-
source/armarx/navigation/human/HumanTracker.h | 25 ++++++++++++++++---
3 files changed, 24 insertions(+), 5 deletions(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
index 29c23bbf..185d5568 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
@@ -351,7 +351,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
ARMARX_INFO << "Running human tracker";
- humanTracker.update(human::HumanTracker::Measurements{
+ humanTracker.update(human::HumanTracker::CameraMeasurement{
.detectionTime = timestamp, .humanPoses = humanPoseResult.humanPoses});
diff --git a/source/armarx/navigation/human/HumanTracker.cpp b/source/armarx/navigation/human/HumanTracker.cpp
index 2332377e..c879af3a 100644
--- a/source/armarx/navigation/human/HumanTracker.cpp
+++ b/source/armarx/navigation/human/HumanTracker.cpp
@@ -10,7 +10,7 @@
namespace armarx::navigation::human
{
void
- HumanTracker::update(const Measurements& measurements)
+ HumanTracker::update(const CameraMeasurement& measurements)
{
// iterate over all existing tracked humans
for (auto it = trackedHumans.begin(); it != trackedHumans.end();)
diff --git a/source/armarx/navigation/human/HumanTracker.h b/source/armarx/navigation/human/HumanTracker.h
index 1a83fb65..d5c92f1c 100644
--- a/source/armarx/navigation/human/HumanTracker.h
+++ b/source/armarx/navigation/human/HumanTracker.h
@@ -24,6 +24,7 @@
#include <ArmarXCore/core/time.h>
+#include <RobotAPI/libraries/armem_vision/types.h>
#include <RobotAPI/libraries/ukfm/UnscentedKalmanFilter.h>
#include <VisionX/libraries/armem_human/types.h>
@@ -50,12 +51,18 @@ namespace armarx::navigation::human
public:
HumanTracker() = default;
- struct Measurements
+ struct CameraMeasurement
{
DateTime detectionTime;
std::vector<armem::human::HumanPose> humanPoses;
};
+ struct LaserMeasurement
+ {
+ DateTime detectionTime;
+ std::vector<armem::vision::LaserScannerFeature> clusters;
+ };
+
struct DetectedHuman
{
core::Pose2D pose;
@@ -93,14 +100,26 @@ namespace armarx::navigation::human
float velocityAlpha = 0.7;
};
/**
- * @brief HumanTracker::update Updates the tracked humans with the measurements. When a
+ * @brief HumanTracker::update Updates the tracked humans with the human measurements from a camera. When a
* measurement is close enough to an existing tracked human, they are associated, otherwise a
* new tracked human is created. Tracked humans that were not associated with a new measurement
* for a specified amount of time are removed. New associated measurements for a tracked human
* are always filtered to provide a less noisy state.
* @param measurements the new measurements of the environment
*/
- void update(const Measurements& measurements);
+ void update(const CameraMeasurement& measurements);
+ /**
+ * @brief HumanTracker::update Updates the tracked humans with the measurements from a lasersensor.
+ * When a measurement is close enough to an existing tracked human, they are associated,
+ * otherwise the measurement is not used for human tracking but returned in the list of unused
+ * measurements. Tracked humans that were not associated with a new measurement for a
+ * specified amount of time are removed. New associated measurements for a tracked human
+ * are always filtered to provide a less noisy state.
+ * @param measurements
+ * @return
+ */
+ std::vector<armem::vision::LaserScannerFeature>
+ update(const LaserMeasurement& measurements);
/**
* @brief HumanTracker::getTrackedHumans Returns all humans that are currently tracked.
* @return the tracked humans
--
GitLab