Skip to content
Snippets Groups Projects
Commit ae6a9eb5 authored by Corvin-N's avatar Corvin-N
Browse files

Move method descriptions from cpp to h files

parent a4705cb2
No related branches found
No related tags found
3 merge requests!68Add human tracking,!53Draft: Implement basic version of kalman filter for human tracking,!28Draft: Dev -> Main
Hide whitespace changes
Inline Side-by-side
source/armarx/navigation/human/HumanFilter.cpp
+ 0
− 23
View file @ ae6a9eb5
......@@ -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
{
......
source/armarx/navigation/human/HumanFilter.h
+ 23
− 1
View file @ ae6a9eb5
......@@ -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:
......
source/armarx/navigation/human/HumanTracker.cpp
+ 4
− 52
View file @ ae6a9eb5
......@@ -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;
......
source/armarx/navigation/human/HumanTracker.h
+ 35
− 3
View file @ ae6a9eb5
......@@ -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);
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment