HumanTracker.cpp
Go to the documentation of this file.
1 #include "HumanTracker.h"
2 
3 #include <boost/geometry.hpp>
4 #include <boost/geometry/geometries/point_xy.hpp>
5 #include <boost/geometry/geometries/polygon.hpp>
6 
8 
11 
12 #include <range/v3/range/conversion.hpp>
13 #include <range/v3/view/transform.hpp>
14 
16 {
17 
18  void
20  {
21  prepareTrackedHumans(measurements.detectionTime);
22 
23  // calculate the poses according to the new received measurements
24  std::vector<DetectedHuman> newPoses =
25  measurements.humanPoses |
27  [measurements, this](const armem::human::HumanPose& humanPose) -> DetectedHuman {
28  return convertHumanPoseToDetectedHuman(measurements.detectionTime, humanPose);
29  }) |
30  ranges::to_vector;
31 
32  // associate the new poses from the measurement with the tracked humans
33  associateHumans(newPoses);
34 
35  // add all not associated poses as new tracked humans
36  for (const auto& detectedHuman : newPoses)
37  {
38  if (not detectedHuman.associated)
39  {
40  //add new tracked human to list of tracked humans
41  trackedHumans.push_back(TrackedHuman{
42  .humanFilter = HumanFilter{detectedHuman.pose, detectedHuman.detectionTime, parameters.useKalmanFilter},
43  .trackingId = detectedHuman.trackingId,
44  .associatedCluster = nullptr,
45  .associated = true,
46  .confidence = parameters.confidenceCamera});
47  }
48  }
49  }
50 
51  std::vector<Cluster>
53  {
54 
55  prepareTrackedHumans(measurements.detectionTime);
56 
57  // only look at the clusters that have a size reasonable enough to contain at most two
58  // footprints
59  std::vector<AdvancedCluster> potentialFootprints;
60  std::vector<Cluster> unusedClusters;
61  for (const auto& cluster : measurements.clusters)
62  {
63  if (getClusterSize(cluster) <= 2 * parameters.maxFootprintSize)
64  {
65  potentialFootprints.push_back(AdvancedCluster{
66  .center = getClusterCenter(cluster), .cluster = cluster, .associated = false});
67  }
68  else
69  {
70  unusedClusters.push_back(cluster);
71  }
72  }
73 
74  // associate humans and clusters by their distances
75  const auto sortedDistances = getSortedDistances(trackedHumans, potentialFootprints);
76 
77  for (auto& posDistance : sortedDistances)
78  {
79  if (posDistance.distance > parameters.maxAssociationDistance)
80  {
81  break;
82  }
83  if (posDistance.oldHuman->associated || posDistance.newCluster->associated)
84  {
85  continue;
86  }
87  // no footprint was found before, so associate first footprint
88  if (!posDistance.oldHuman->associatedCluster)
89  {
90  posDistance.oldHuman->associatedCluster = posDistance.newCluster;
91  posDistance.newCluster->associated = true;
92  }
93  // found both footprints, so associate both footprints and update human with new pose
94  else
95  {
96  //calculate center between both footprints
97  auto& footprint1 = posDistance.oldHuman->associatedCluster->cluster;
98  auto& footprint2 = posDistance.newCluster->cluster;
99 
100  //create pose with orientation from old human
102  pose.translation() =
103  (getClusterCenter(footprint1) + getClusterCenter(footprint2)) / 2;
104  pose.linear() = posDistance.oldHuman->humanFilter.get().pose.linear();
105 
106 
107  posDistance.oldHuman->humanFilter.update(pose, measurements.detectionTime);
108 
109  posDistance.oldHuman->associatedCluster = nullptr;
110  posDistance.oldHuman->associated = true;
111  posDistance.newCluster->associated = true;
112  posDistance.oldHuman->confidence += parameters.confidenceLaser;
113  }
114  }
115 
116  //associate humans where only one footprint was found
117  for (auto& human : trackedHumans)
118  {
119  if (human.associated)
120  {
121  continue;
122  }
123 
124  if (human.associatedCluster)
125  {
126  auto& footprint = human.associatedCluster;
127  //create pose with orientation from old human
129  pose.translation() = footprint->center;
130  pose.linear() = human.humanFilter.get().pose.linear();
131 
132  human.humanFilter.update(pose, measurements.detectionTime);
133 
134  human.associatedCluster = nullptr;
135  human.associated = true;
136  human.confidence += parameters.confidenceLaser;
137  }
138  }
139 
140  // add unused clusters to return list
141  for (auto& cluster : potentialFootprints)
142  {
143  if (!cluster.associated)
144  {
145  unusedClusters.push_back(cluster.cluster);
146  }
147  }
148  return unusedClusters;
149  }
150 
151  std::vector<human::Human>
153  {
154  return trackedHumans |
156  { return h.humanFilter.get(); }) |
157  ranges::to_vector;
158  }
159 
160  void
162  {
163  trackedHumans.clear();
164  }
165 
167  HumanTracker::convertHumanPoseToDetectedHuman(const DateTime& time,
168  const armem::human::HumanPose& humanPose)
169  {
170  const std::map<std::string, armem::human::PoseKeypoint>& keypoints = humanPose.keypoints;
171  ARMARX_CHECK_NOT_EMPTY(keypoints);
172 
173  // calculate the arithmetic mean of all keypoint positions
174  Eigen::Vector3f centerPos = Eigen::Vector3f::Zero();
175  int size = 0;
176  for (const auto& [_, v] : keypoints)
177  {
178  if (v.positionGlobal.has_value())
179  {
180  centerPos += v.positionGlobal.value().toEigen();
181  size++;
182  }
183  }
184  centerPos /= size;
185 
186  // calculate the yaw of the specified keypoint representing the orientation if it exists
187  double yaw = 0;
188  auto head = humanPose.keypoints.find(parameters.rotationKeypoint);
189  if (head != humanPose.keypoints.end())
190  {
191  const armem::human::PoseKeypoint& refKeypoint = head->second;
192  ARMARX_CHECK(refKeypoint.orientationGlobal.has_value());
193  Eigen::Quaternionf qhead = refKeypoint.orientationGlobal->toEigenQuaternion();
194  Eigen::Vector3f vec(1, 0, 0);
195  Eigen::Vector3f rot3 = qhead._transformVector(vec);
196  Eigen::Vector2f rot2(rot3.x(), rot3.y());
197  if (rot2.norm() != 0)
198  {
199  // calculate angle of rot2
200  yaw = atan2(rot2.y(), rot2.x());
201  }
202  }
203 
204  // create the new pose with the calculated position and yaw
206  pose.translation() = conv::to2D(centerPos);
207  pose.linear() = Eigen::Rotation2Df(yaw).toRotationMatrix();
208 
209  return {pose, humanPose.humanTrackingId, time, false};
210  }
211 
212  std::vector<HumanTracker::PosHumanDistance>
213  HumanTracker::getSortedDistances(std::vector<HumanTracker::TrackedHuman>& oldHumans,
214  std::vector<HumanTracker::DetectedHuman>& newHumans)
215  {
216  std::vector<PosHumanDistance> posDistances;
217 
218  for (auto& oldHuman : oldHumans)
219  {
220  if (oldHuman.associated)
221  {
222  continue;
223  }
224  for (auto& newHuman : newHumans)
225  {
226  if (newHuman.associated)
227  {
228  continue;
229  }
230  // calculate distance between every possible combination of tracked and detected
231  // humans where none of them was associated
232  posDistances.push_back(
233  {&oldHuman,
234  &newHuman,
235  (newHuman.pose.translation() - oldHuman.humanFilter.get().pose.translation())
236  .norm()});
237  }
238  }
239 
240  // sort the distances ascending by their numeric value
241  std::sort(posDistances.begin(),
242  posDistances.end(),
243  [](const PosHumanDistance& a, const PosHumanDistance& b) -> bool
244  { return a.distance < b.distance; });
245 
246  return posDistances;
247  }
248 
249  std::vector<HumanTracker::PosLaserDistance>
250  HumanTracker::getSortedDistances(std::vector<TrackedHuman>& oldHumans,
251  std::vector<AdvancedCluster>& newClusters)
252  {
253  std::vector<PosLaserDistance> posDistances;
254 
255  for (auto& oldHuman : oldHumans)
256  {
257  for (auto& newCluster : newClusters)
258  {
259  // calculate distance between every possible combination of tracked human and cluster
260  posDistances.push_back(
261  {&oldHuman,
262  &newCluster,
263  (newCluster.center - oldHuman.humanFilter.get().pose.translation()).norm()});
264  }
265  }
266 
267  // sort the distances ascending by their numeric value
268  std::sort(posDistances.begin(),
269  posDistances.end(),
270  [](const PosLaserDistance& a, const PosLaserDistance& b) -> bool
271  { return a.distance < b.distance; });
272 
273  return posDistances;
274  }
275 
276  void
277  HumanTracker::associateHumans(std::vector<DetectedHuman>& detectedHumans)
278  {
279  // first, associate humans by their tracking id
280  for (auto& oldHuman : trackedHumans)
281  {
282  if (oldHuman.associated || not oldHuman.trackingId)
283  {
284  continue;
285  }
286  for (auto& newHuman : detectedHumans)
287  {
288  if (newHuman.associated || not newHuman.trackingId)
289  {
290  continue;
291  }
292  // check for every possible combination of old and new humans (that were not already
293  // associated and have a tracking id) if their tracking id is the same
294  if (oldHuman.trackingId.value() == newHuman.trackingId.value())
295  {
296  associate(&oldHuman, &newHuman);
297  }
298  }
299  }
300 
301  // second, associate leftover humans by their distances
302  const auto sortedDistances = getSortedDistances(trackedHumans, detectedHumans);
303 
304  for (auto& posDistance : sortedDistances)
305  {
306  if (posDistance.distance > parameters.maxAssociationDistance)
307  {
308  break;
309  }
310  if (posDistance.oldHuman->associated || posDistance.newHuman->associated)
311  {
312  continue;
313  }
314  // associate the pair with the currently smallest distance between non-associated humans
315  associate(posDistance.oldHuman, posDistance.newHuman);
316  }
317  }
318 
319  void
320  HumanTracker::associate(TrackedHuman* trackedHuman, DetectedHuman* detectedHuman)
321  {
322  ARMARX_CHECK(not trackedHuman->associated);
323  ARMARX_CHECK(not detectedHuman->associated);
324 
325 
326  trackedHuman->associated = true;
327  detectedHuman->associated = true;
328 
329  trackedHuman->humanFilter.update(detectedHuman->pose, detectedHuman->detectionTime);
330  trackedHuman->trackingId = detectedHuman->trackingId;
331 
332  trackedHuman->confidence = parameters.confidenceCamera;
333  }
334 
335  float
336  HumanTracker::getClusterSize(Cluster cluster)
337  {
338  return 2 * cluster.circle.radius;
339  }
340 
341  Eigen::Vector2f
342  HumanTracker::getClusterCenter(Cluster cluster)
343  {
344  typedef boost::geometry::model::d2::point_xy<double> boost_point;
345  typedef boost::geometry::model::polygon<boost_point> boost_polygon;
346 
347  ARMARX_CHECK(!cluster.convexHull.empty());
348  std::vector<boost_point> points;
349  for (auto& point : cluster.convexHull)
350  {
351  points.push_back(boost_point(point.x(), point.y()));
352  }
353  //make polygon enclosed
354  points.push_back(
355  boost_point(cluster.convexHull.front().x(), cluster.convexHull.front().y()));
356  boost_polygon poly;
357  boost::geometry::assign_points(poly, points);
358 
359  boost_point p;
360  boost::geometry::centroid(poly, p);
361 
362  return Eigen::Vector2f{p.x(), p.y()};
363  }
364 
365  void
366  HumanTracker::prepareTrackedHumans(DateTime time)
367  {
368  // iterate over all existing tracked humans
369  for (auto it = trackedHumans.begin(); it != trackedHumans.end();)
370  {
371  auto& human = *it;
372  // when the tracked human recieved no new measurement for too long or
373  // wasn't detected by the camera for too long, remove it
374  if (human.humanFilter.detectionAge(time) >= parameters.maxTrackingAge ||
375  human.confidence <= 0)
376  {
377  it = trackedHumans.erase(it);
378  }
379  // otherwise the tracked human is prepared for association at the current point in time
380  else
381  {
382  human.associatedCluster = nullptr;
383  human.associated = false;
384  human.humanFilter.propagation(time);
385  it++;
386  }
387  }
388  }
389 
390 } // namespace armarx::navigation::human
armarx::navigation::human::HumanTracker::getTrackedHumans
std::vector< human::Human > getTrackedHumans() const
HumanTracker::getTrackedHumans Returns all humans that are currently tracked.
Definition: HumanTracker.cpp:152
armarx::navigation::human::Human
Definition: types.h:33
armarx::navigation::human::HumanTracker::DetectedHuman
Definition: HumanTracker.h:64
armarx::navigation::human::HumanTracker::Parameters::maxAssociationDistance
float maxAssociationDistance
Definition: HumanTracker.h:111
armarx::armem::human::PoseKeypoint::orientationGlobal
std::optional< armarx::FramedOrientation > orientationGlobal
Definition: types.h:26
armarx::navigation::human::HumanTracker::Parameters::confidenceCamera
int confidenceCamera
Definition: HumanTracker.h:118
armarx::navigation::human::HumanTracker::LaserMeasurement::detectionTime
DateTime detectionTime
Definition: HumanTracker.h:60
armarx::navigation::human::HumanTracker::TrackedHuman::humanFilter
HumanFilter humanFilter
Definition: HumanTracker.h:81
types.h
armarx::navigation::human
This file is part of ArmarX.
Definition: aron_conversions.cpp:9
ARMARX_CHECK_NOT_EMPTY
#define ARMARX_CHECK_NOT_EMPTY(c)
Definition: ExpressionException.h:224
HumanTracker.h
armarx::navigation::human::HumanTracker::reset
void reset()
HumanTracker::reset Resets this instance to the same state as if it just would have been created.
Definition: HumanTracker.cpp:161
armarx::navigation::human::HumanTracker::CameraMeasurement::detectionTime
DateTime detectionTime
Definition: HumanTracker.h:54
ARMARX_CHECK
#define ARMARX_CHECK(expression)
Shortcut for ARMARX_CHECK_EXPRESSION.
Definition: ExpressionException.h:82
armarx::navigation::human::HumanTracker::AdvancedCluster::center
Eigen::Vector2f center
Definition: HumanTracker.h:74
armarx::navigation::core::Pose2D
Eigen::Isometry2f Pose2D
Definition: basic_types.h:34
armarx::navigation::human::HumanTracker::TrackedHuman
Definition: HumanTracker.h:79
armarx::navigation::human::HumanTracker::Parameters::maxFootprintSize
float maxFootprintSize
Definition: HumanTracker.h:113
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
armarx::ctrlutil::a
double a(double t, double a0, double j)
Definition: CtrlUtil.h:45
armarx::armem::human::PoseKeypoint
Definition: types.h:16
armarx::navigation::human::HumanTracker::CameraMeasurement
Definition: HumanTracker.h:52
Eigen::Quaternionf
Quaternion< float, 0 > Quaternionf
Definition: EigenForwardDeclarations.h:61
armarx::armem::human::HumanPose
Definition: types.h:29
armarx::armem::human::HumanPose::humanTrackingId
std::optional< std::string > humanTrackingId
Definition: types.h:35
armarx::navigation::human::HumanFilter::get
const Human & get() const
HumanFilter::get returns the human whose pose is filtered containing the most recent state.
Definition: HumanFilter.cpp:95
armarx::navigation::human::HumanTracker::Parameters::confidenceLaser
int confidenceLaser
Definition: HumanTracker.h:119
armarx::navigation::conv::to2D
std::vector< Eigen::Vector2f > to2D(const std::vector< Eigen::Vector3f > &v)
Definition: eigen.cpp:26
ExpressionException.h
armarx::core::time::DateTime
Represents a point in time.
Definition: DateTime.h:24
armarx::navigation::human::HumanFilter
The HumanFilter class can be used to track and filter the state of a single human.
Definition: HumanFilter.h:38
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:39
armarx::navigation::human::HumanTracker::LaserMeasurement::clusters
std::vector< Cluster > clusters
Definition: HumanTracker.h:61
armarx::transform
auto transform(const Container< InputT, Alloc > &in, OutputT(*func)(InputT const &)) -> Container< OutputT, typename std::allocator_traits< Alloc >::template rebind_alloc< OutputT > >
Convenience function (with less typing) to transform a container of type InputT into the same contain...
Definition: algorithm.h:315
armarx::navigation::human::HumanTracker::Parameters::useKalmanFilter
bool useKalmanFilter
Definition: HumanTracker.h:122
armarx::navigation::human::HumanTracker::Parameters::rotationKeypoint
const std::string rotationKeypoint
Definition: HumanTracker.h:105
armarx::navigation::human::HumanTracker::CameraMeasurement::humanPoses
std::vector< armem::human::HumanPose > humanPoses
Definition: HumanTracker.h:55
armarx::navigation::human::HumanTracker::AdvancedCluster
Definition: HumanTracker.h:72
armarx::navigation::human::HumanTracker::Parameters::maxTrackingAge
Duration maxTrackingAge
Definition: HumanTracker.h:108
armarx::armem::human::HumanPose::keypoints
KeyPointMap keypoints
Definition: types.h:34
eigen.h
armarx::navigation::human::HumanTracker::LaserMeasurement
Definition: HumanTracker.h:58
armarx::navigation::human::Cluster
memory::LaserScannerFeature Cluster
Definition: HumanTracker.h:39
armarx::navigation::human::HumanTracker::update
void update(const CameraMeasurement &measurements)
HumanTracker::update Updates the tracked humans with the human measurements from a camera.
Definition: HumanTracker.cpp:19