diff --git a/source/armarx/navigation/human/HumanFilter.cpp b/source/armarx/navigation/human/HumanFilter.cpp
index e45d942ad4a123c50fff874c394f30a92dad2bad..570203b6ff947a47c8e0256b4a92a54f88de1363 100644
--- a/source/armarx/navigation/human/HumanFilter.cpp
+++ b/source/armarx/navigation/human/HumanFilter.cpp
@@ -3,12 +3,6 @@
namespace armarx::navigation::human
{
- /**
- * @brief HumanFilter::HumanFilter creates a new filter that filters 2D poses (position and
- * rotation in a two dimensional plane)
- * @param pose0 the first known pose of the human
- * @param detectionTime the point of detection
- */
HumanFilter::HumanFilter(const core::Pose2D& pose0, const DateTime& detectionTime)
{
//initialize new human for detected human
@@ -42,12 +36,6 @@ namespace armarx::navigation::human
ukf = std::make_unique<UnscentedKalmanFilter<SystemModelT>>(Q, R, alpha, state0, P0);
}
- /**
- * @brief HumanFilter::propagation propagate the system model to the given point in time. This
- * means that the human pose is updated according to the filters prediction for the given point
- * in time
- * @param detectionTime the point in time for which the pose should be predicted
- */
void
HumanFilter::propagation(const armarx::core::time::DateTime& detectionTime)
{
@@ -60,12 +48,6 @@ namespace armarx::navigation::human
human.pose = fromUkfState(ukf->getCurrentState());
}
- /**
- * @brief HumanFilter::update update the filter a new detected pose of the tracked human and
- * the detection time
- * @param pose the new detected pose of the human
- * @param detectionTime the detection time
- */
void
HumanFilter::update(const core::Pose2D& pose, const DateTime& detectionTime)
{
@@ -91,11 +73,6 @@ namespace armarx::navigation::human
human.linearVelocity = newVelocity;
}
- /**
- * @brief HumanFilter::get returns the human whose pose is filtered containing the most recent
- * state
- * @return the human
- */
const Human&
HumanFilter::get() const
{
diff --git a/source/armarx/navigation/human/HumanFilter.h b/source/armarx/navigation/human/HumanFilter.h
index 749750d422a7b1d44f2c0c0207314a7065dd42bd..998b491bfe0b64090a2c3581d23f3370cf81e41e 100644
--- a/source/armarx/navigation/human/HumanFilter.h
+++ b/source/armarx/navigation/human/HumanFilter.h
@@ -53,11 +53,33 @@ namespace armarx::navigation::human
float velocityAlpha = 0.7;
};
+ /**
+ * @brief HumanFilter::HumanFilter creates a new filter that filters 2D poses (position and
+ * rotation in a two dimensional plane)
+ * @param pose0 the first known pose of the human
+ * @param detectionTime the point of detection
+ */
HumanFilter(const core::Pose2D& pose0, const DateTime& detectionTime);
+ /**
+ * @brief HumanFilter::propagation propagate the system model to the given point in time. This
+ * means that the human pose is updated according to the filters prediction for the given point
+ * in time
+ * @param detectionTime the point in time for which the pose should be predicted
+ */
void propagation(const DateTime& detectionTime);
+ /**
+ * @brief HumanFilter::update update the filter a new detected pose of the tracked human and
+ * the detection time
+ * @param pose the new detected pose of the human
+ * @param detectionTime the detection time
+ */
void update(const core::Pose2D& pose, const DateTime& detectionTime);
-
+ /**
+ * @brief HumanFilter::get returns the human whose pose is filtered containing the most recent
+ * state
+ * @return the human
+ */
const Human& get() const;
private:
diff --git a/source/armarx/navigation/human/HumanTracker.cpp b/source/armarx/navigation/human/HumanTracker.cpp
index dd6ecfc45ee131aff39242ef78668c47b5337a56..c1baf749bb3deec9efcd95478e4fad574cb6948c 100644
--- a/source/armarx/navigation/human/HumanTracker.cpp
+++ b/source/armarx/navigation/human/HumanTracker.cpp
@@ -9,14 +9,6 @@
namespace armarx::navigation::human
{
- /**
- * @brief HumanTracker::update Updates the tracked humans with the measurements. 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
HumanTracker::update(const Measurements& measurements)
{
@@ -65,10 +57,6 @@ namespace armarx::navigation::human
}
}
- /**
- * @brief HumanTracker::getTrackedHumans Returns all humans that are currently tracked.
- * @return the tracked humans
- */
std::vector<human::Human>
HumanTracker::getTrackedHumans() const
{
@@ -78,10 +66,6 @@ namespace armarx::navigation::human
ranges::to_vector;
}
- /**
- * @brief HumanTracker::reset Resets this instance to the same state as if it just would have
- * been created.
- */
void
HumanTracker::reset()
{
@@ -89,13 +73,6 @@ namespace armarx::navigation::human
}
- /**
- * @brief convertHumanPoseToDetectedHuman Calculates all information necessary for a
- * DetectedHuman from the given HumanPose and returns a new detected human.
- * @param time The point in time where the detection was made.
- * @param humanPose The HumanPose that should be converted to a DetectedHuman.
- * @return A new DetectedHuman according to the HumanPose
- */
HumanTracker::DetectedHuman
convertHumanPoseToDetectedHuman(const DateTime& time, const armem::human::HumanPose& humanPose)
{
@@ -135,11 +112,6 @@ namespace armarx::navigation::human
return {pose, humanPose.humanTrackingId, time, false};
}
-
- /**
- * @brief The PosDistance struct contains a distance between an old, tracked human and a new,
- * detected human aswell as references to them.
- */
struct PosDistance
{
HumanTracker::TrackedHuman* oldHuman;
@@ -147,14 +119,6 @@ namespace armarx::navigation::human
float distance;
};
- /**
- * @brief getSortedDistances Returns all distances sorted by their numeric value between
- * possible combinations (T, D) where T is an old, tracked human and D is a new, detected human
- * and T as well as D were not already associated
- * @param oldHumans the old, tracked humans
- * @param newHumans the new, detected humans
- * @return the sorted distances, where the smallest distance is the first entry in the vector
- */
std::vector<PosDistance>
getSortedDistances(std::vector<HumanTracker::TrackedHuman>& oldHumans,
std::vector<HumanTracker::DetectedHuman>& newHumans)
@@ -192,25 +156,19 @@ namespace armarx::navigation::human
return posDistances;
}
- /**
- * @brief HumanTracker::associateHumans Associates those tracked and detected humans that
- * belong together.
- * @param detectedHumans The detected humans against which the saved list of tracked humans is
- * matched.
- */
void
HumanTracker::associateHumans(std::vector<DetectedHuman>& detectedHumans)
{
// first, associate humans by their tracking id
for (auto& oldHuman : trackedHumans)
{
- if (oldHuman.associated || !oldHuman.trackingId)
+ if (oldHuman.associated || not oldHuman.trackingId)
{
continue;
}
for (auto& newHuman : detectedHumans)
{
- if (newHuman.associated || !newHuman.trackingId)
+ if (newHuman.associated || not newHuman.trackingId)
{
continue;
}
@@ -241,17 +199,11 @@ namespace armarx::navigation::human
}
}
- /**
- * @brief HumanTracker::associate Associates the given tracked and detected human. Therefore it
- * updates all necessary variables of the TrackedHuman
- * @param trackedHuman the tracked human
- * @param detectedHuman the detected human
- */
void
HumanTracker::associate(TrackedHuman* trackedHuman, DetectedHuman* detectedHuman)
{
- ARMARX_CHECK(!trackedHuman->associated);
- ARMARX_CHECK(!detectedHuman->associated);
+ ARMARX_CHECK(not trackedHuman->associated);
+ ARMARX_CHECK(not detectedHuman->associated);
trackedHuman->associated = true;
detectedHuman->associated = true;
diff --git a/source/armarx/navigation/human/HumanTracker.h b/source/armarx/navigation/human/HumanTracker.h
index 311817b45b862e38d716441f60181cae25f13700..580e68402c63dabd609fadd2600feb172853aeb8 100644
--- a/source/armarx/navigation/human/HumanTracker.h
+++ b/source/armarx/navigation/human/HumanTracker.h
@@ -83,16 +83,48 @@ namespace armarx::navigation::human
// the old) velocity should be weighted when calculating the new velocity
float velocityAlpha = 0.7;
};
-
+ /**
+ * @brief HumanTracker::update Updates the tracked humans with the measurements. 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);
-
+ /**
+ * @brief HumanTracker::getTrackedHumans Returns all humans that are currently tracked.
+ * @return the tracked humans
+ */
std::vector<human::Human> getTrackedHumans() const;
-
+ /**
+ * @brief HumanTracker::reset Resets this instance to the same state as if it just would have
+ * been created.
+ */
void reset();
private:
+ /**
+ * @brief HumanTracker::associateHumans Associates those tracked and detected humans that
+ * belong together.
+ * @param detectedHumans The detected humans against which the saved list of tracked humans is
+ * matched.
+ */
void associateHumans(std::vector<DetectedHuman>& detectedHumans);
+ /**
+ * @brief HumanTracker::associate Associates the given tracked and detected human. Therefore it
+ * updates all necessary variables of the TrackedHuman
+ * @param trackedHuman the tracked human
+ * @param detectedHuman the detected human
+ */
void associate(TrackedHuman* tracked, DetectedHuman* detected);
+ /**
+ * @brief convertHumanPoseToDetectedHuman Calculates all information necessary for a
+ * DetectedHuman from the given HumanPose and returns a new detected human.
+ * @param time The point in time where the detection was made.
+ * @param humanPose The HumanPose that should be converted to a DetectedHuman.
+ * @return A new DetectedHuman according to the HumanPose
+ */
HumanTracker::DetectedHuman
convertHumanPoseToDetectedHuman(const DateTime& time,
const armem::human::HumanPose& humanPose);