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

Add comments to HumanTracker.cpp and to header files

parent bf0b081f
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.h
+ 5
− 0
View file @ a4705cb2
......@@ -30,6 +30,11 @@
namespace armarx::navigation::human
{
/**
* @brief The HumanFilter class can be used to track and filter the state of a single human. It
* hides implementation detail on how the filtering is done. New information about the human can
* be fed by using the update method. The human itself can be obtained using the get method.
*/
class HumanFilter
{
......
source/armarx/navigation/human/HumanTracker.cpp
+ 121
− 60
View file @ a4705cb2
......@@ -9,54 +9,28 @@
namespace armarx::navigation::human
{
HumanTracker::DetectedHuman
convertHumanPoseToPosition(const DateTime& time, const armem::human::HumanPose& humanPose)
{
const std::map<std::string, armem::human::PoseKeypoint>& keypoints = humanPose.keypoints;
ARMARX_CHECK_NOT_EMPTY(keypoints);
Eigen::Vector3f centerPos;
int size = 0;
for (const auto& [_, v] : keypoints)
{
if (v.positionGlobal.has_value())
{
centerPos += v.positionGlobal.value().toEigen();
size++;
}
}
centerPos /= size;
core::Pose2D pose = core::Pose2D::Identity();
pose.translation() = conv::to2D(centerPos);
double yaw = 0;
if (humanPose.keypoints.find("HEAD") != humanPose.keypoints.end())
{
yaw = humanPose //from all human pose keypoints
.keypoints
.find("HEAD") //find the keypoint representing the head
->second
.orientationGlobal //get its global orientation
->toEigen()
.eulerAngles(2, 1, 0)[0]; //and extract the yaw (rotation around z axis)
}
pose.linear() = Eigen::Rotation2Df(yaw).toRotationMatrix();
return {pose, humanPose.humanTrackingId, time, false};
}
/**
* @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)
{
// iterate over all existing tracked humans
for (auto it = trackedHumans.begin(); it != trackedHumans.end();)
{
auto& human = *it;
// when the tracked human recieved no new measurement for too long, remove it
if ((measurements.detectionTime - human.humanFilter.get().detectionTime) >=
parameters.maxTrackingAge)
{
it = trackedHumans.erase(it);
}
// otherwise the tracked human is prepared for association at the current point in time
else
{
human.associated = false;
......@@ -65,19 +39,22 @@ namespace armarx::navigation::human
}
}
// calculate the poses according to the new received measurements
std::vector<DetectedHuman> newPoses =
measurements.humanPoses |
ranges::views::transform(
[measurements](const armem::human::HumanPose& humanPose) -> DetectedHuman
{ return convertHumanPoseToPosition(measurements.detectionTime, humanPose); }) |
[measurements, this](const armem::human::HumanPose& humanPose) -> DetectedHuman {
return convertHumanPoseToDetectedHuman(measurements.detectionTime, humanPose);
}) |
ranges::to_vector;
// associate the new poses from the measurement with the tracked humans
associateHumans(newPoses);
// add all not associated humans as new tracked humans
// add all not associated poses as new tracked humans
for (const auto& detectedHuman : newPoses)
{
if (!detectedHuman.associated)
if (not detectedHuman.associated)
{
//add new tracked human to list of tracked humans
trackedHumans.push_back(TrackedHuman{
......@@ -88,6 +65,81 @@ 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
{
return trackedHumans |
ranges::views::transform([](const TrackedHuman& h) -> human::Human
{ return h.humanFilter.get(); }) |
ranges::to_vector;
}
/**
* @brief HumanTracker::reset Resets this instance to the same state as if it just would have
* been created.
*/
void
HumanTracker::reset()
{
trackedHumans.clear();
}
/**
* @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)
{
const std::map<std::string, armem::human::PoseKeypoint>& keypoints = humanPose.keypoints;
ARMARX_CHECK_NOT_EMPTY(keypoints);
// calculate the arithmetic mean of all keypoint positions
Eigen::Vector3f centerPos;
int size = 0;
for (const auto& [_, v] : keypoints)
{
if (v.positionGlobal.has_value())
{
centerPos += v.positionGlobal.value().toEigen();
size++;
}
}
centerPos /= size;
// calculate the yaw of the head keypoint if it exists
double yaw = 0;
if (humanPose.keypoints.count("HEAD") > 0)
{
yaw = humanPose //from all human pose keypoints
.keypoints
.at("HEAD") //find the keypoint representing the head
.orientationGlobal //get its global orientation
->toEigen()
.eulerAngles(2, 1, 0)[0]; //and extract the yaw (rotation around z axis)
}
// create the new pose with the calculated position and yaw
core::Pose2D pose = core::Pose2D::Identity();
pose.translation() = conv::to2D(centerPos);
pose.linear() = Eigen::Rotation2Df(yaw).toRotationMatrix();
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;
......@@ -95,6 +147,14 @@ 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)
......@@ -113,6 +173,8 @@ namespace armarx::navigation::human
{
continue;
}
// calculate distance between every possible combination of tracked and detected
// humans where none of them was associated
posDistances.push_back(
{&oldHuman,
&newHuman,
......@@ -121,6 +183,7 @@ namespace armarx::navigation::human
}
}
// sort the distances ascending by their numeric value
std::sort(posDistances.begin(),
posDistances.end(),
[](const PosDistance& a, const PosDistance& b) -> bool
......@@ -129,10 +192,16 @@ 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)
{
// associate humans by their tracking id
// first, associate humans by their tracking id
for (auto& oldHuman : trackedHumans)
{
if (oldHuman.associated || !oldHuman.trackingId)
......@@ -145,6 +214,8 @@ namespace armarx::navigation::human
{
continue;
}
// check for every possible combination of old and new humans (that were not already
// associated and have a tracking id) if their tracking id is the same
if (oldHuman.trackingId.value() == newHuman.trackingId.value())
{
associate(&oldHuman, &newHuman);
......@@ -152,8 +223,7 @@ namespace armarx::navigation::human
}
}
// associate leftover humans by their distances
// second, associate leftover humans by their distances
const auto sortedDistances = getSortedDistances(trackedHumans, detectedHumans);
for (auto& posDistance : sortedDistances)
......@@ -166,10 +236,17 @@ namespace armarx::navigation::human
{
continue;
}
// associate the pair with the currently smallest distance between non-associated humans
associate(posDistance.oldHuman, posDistance.newHuman);
}
}
/**
* @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)
{
......@@ -183,20 +260,4 @@ namespace armarx::navigation::human
trackedHuman->trackingId = detectedHuman->trackingId;
}
std::vector<human::Human>
HumanTracker::getTrackedHumans() const
{
return trackedHumans |
ranges::views::transform([](const TrackedHuman& h) -> human::Human
{ return h.humanFilter.get(); }) |
ranges::to_vector;
}
void
HumanTracker::reset()
{
trackedHumans.clear();
}
} // namespace armarx::navigation::human
source/armarx/navigation/human/HumanTracker.h
+ 15
− 3
View file @ a4705cb2
......@@ -39,6 +39,12 @@ namespace armarx::navigation::human
using Vector = Eigen::Matrix<T, 2, 1>;
using SystemModelT = kalman_filter::SystemModelSO2xR2<T>;
/**
* @brief The HumanTracker class can be used to track and filter multiple humans. It hides
* implementation detail on how new detected humans are associated to the old, already tracked
* humans. New detected humans can be fed by using the update method. The tracked humans can
* be obtained using the getTrackedHumans method.
*/
class HumanTracker
{
public:
......@@ -67,11 +73,14 @@ namespace armarx::navigation::human
struct Parameters
{
// the duration after which tracked humans will be erased if no new measurement for this human is found
// the duration after which tracked humans will be erased if no new measurement for
// this human is found
Duration maxTrackingAge = Duration::MilliSeconds(500);
// the maximum distance in millimeters of two human measurements to be associated with each other
// the maximum distance in millimeters of two human measurements where they are still
// associated with each other
float maxAssociationDistance = 600;
// alpha value from interval [0,1] to determine how much the new (and respectively the old) velocity should be weighted
// alpha value from interval [0,1] to determine how much the current (and respectively
// the old) velocity should be weighted when calculating the new velocity
float velocityAlpha = 0.7;
};
......@@ -84,6 +93,9 @@ namespace armarx::navigation::human
private:
void associateHumans(std::vector<DetectedHuman>& detectedHumans);
void associate(TrackedHuman* tracked, DetectedHuman* detected);
HumanTracker::DetectedHuman
convertHumanPoseToDetectedHuman(const DateTime& time,
const armem::human::HumanPose& humanPose);
private:
std::vector<TrackedHuman> trackedHumans;
......
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