util.cpp
Go to the documentation of this file.
1 #include "util.h"
2 
3 #include <limits>
4 
8 #include <SimoxUtility/algorithm/string.h>
9 
10 namespace armarx::armem::human
11 {
12 
13  std::optional<Eigen::Vector3f>
14  computeMeanPosition(const HumanPose &humanPose,
15  KeyPointCoordinateSystem coordSystem)
16  {
17  Eigen::Vector3f sumPosition = Eigen::Vector3f::Zero();
18  int sumWeights = 0;
19  for (const auto& [id, keypoint] : humanPose.keypoints)
20  {
21  switch (coordSystem)
22  {
23  case Camera:
24  {
25  sumPosition += keypoint.positionCamera.toEigen();
26  sumWeights += 1;
27  break;
28  }
29  case Global:
30  [[fallthrough]];
32  {
33  if (keypoint.positionGlobal)
34  {
35  sumPosition += keypoint.positionGlobal.value().toEigen();
36  sumWeights += 1;
37  }
38  break;
39  }
40  case Robot:
41  {
42  if (keypoint.positionRobot)
43  {
44  sumPosition += keypoint.positionRobot.value().toEigen();
45  sumWeights += 1;
46  }
47  break;
48  }
49  default:
50  break;
51  }
52  }
53  if (sumWeights == 0) return std::nullopt;
54  if (coordSystem == KeyPointCoordinateSystem::GlobalFloorProjection) sumPosition.z() = 0.0;
55  return sumPosition / sumWeights;
56  }
57 
58 
59  std::optional<HumanPose>
60  getNearestHuman(const std::vector<HumanPose> &humanPoses,
61  const NearestHumanParameters &parameters)
62  {
63  std::optional<HumanPose> nearestHumanPose = std::nullopt;
64  double minDistance = std::numeric_limits<double>::infinity();
65  for (const HumanPose &humanPose : humanPoses)
66  {
67  if (parameters.modelIDs.empty() or parameters.modelIDs.count(humanPose.poseModelId))
68  {
69  const std::optional<Eigen::Vector3f> meanPosition
70  = computeMeanPosition(humanPose, parameters.coordSystem);
71  if (not meanPosition) continue;
72  const double distance = (meanPosition.value() - parameters.positionMM).norm();
73  if (distance < minDistance)
74  {
75  minDistance = distance;
76  nearestHumanPose = humanPose;
77  }
78  }
79  }
80  if (parameters.distanceThresholdMM and minDistance > parameters.distanceThresholdMM)
81  {
82  return std::nullopt;
83  }
84  return nearestHumanPose;
85  }
86 
88  const std::string &prefix)
89  {
90  const std::string prefix_ = prefix + "nearestHuman.";
91  def->optional(positionMM.x(), prefix_ + "position.x", "Anchor position (x-coordinate) in mm.");
92  def->optional(positionMM.y(), prefix_ + "position.y", "Anchor position (y-coordinate) in mm.");
93  def->optional(positionMM.z(), prefix_ + "position.z", "Anchor position (z-coordinate) in mm.");
94  def->optional(coordSystem, prefix_ + "coordSystem", "Coordinate system of the anchor position.")
95  .map("Camera", KeyPointCoordinateSystem::Camera)
96  .map("Global", KeyPointCoordinateSystem::Global)
97  .map("GlobalFloorProjection", KeyPointCoordinateSystem::GlobalFloorProjection)
98  .map("Robot", KeyPointCoordinateSystem::Robot)
99  .setCaseInsensitive(true);
100  def->optional(distanceThresholdMM, prefix_ + "distanceThreshold", "Optional minimum distance threshold in mm.");
101  def->optional(modelIDs, prefix_ + "allowedModelIDs", "Allowed pose model ids, seperated by commas.");
102  }
103 
105  {
109  {
111  }
112  const std::vector<std::string> ids = simox::alg::split(properties.modelIDs, ",");
113  modelIDs = std::set<std::string>(ids.begin(), ids.end());
114 
115  ARMARX_INFO << "Setting nearest human position to " << positionMM.transpose() << " in frame " << coordSystem;
116  }
117 
118 } // namespace armarx::armem::human
util.h
armarx::armem::human::Camera
@ Camera
Definition: util.h:14
armarx::armem::human::KeyPointCoordinateSystem
KeyPointCoordinateSystem
Definition: util.h:12
armarx::armem::human::NearestHumanParameters::Properties::modelIDs
std::string modelIDs
Definition: util.h:28
armarx::armem::human::NearestHumanParameters
Definition: util.h:21
armarx::armem::human::NearestHumanParameters::Properties::coordSystem
KeyPointCoordinateSystem coordSystem
Definition: util.h:26
armarx::armem::human::NearestHumanParameters::Properties::addTo
void addTo(armarx::PropertyDefinitionsPtr &def, const std::string &prefix=std::string())
Definition: util.cpp:87
armarx::armem::human::NearestHumanParameters::updateFromProperties
void updateFromProperties()
Definition: util.cpp:104
armarx::armem::human::NearestHumanParameters::positionMM
Eigen::Vector3f positionMM
Definition: util.h:37
armarx::armem::human::NearestHumanParameters::Properties::distanceThresholdMM
double distanceThresholdMM
Definition: util.h:27
armarx::armem::human::Robot
@ Robot
Definition: util.h:14
armarx::armem::human::Global
@ Global
Definition: util.h:14
armarx::armem::human::NearestHumanParameters::Properties::positionMM
Eigen::Vector3f positionMM
Definition: util.h:25
armarx::armem::human::HumanPose
Definition: types.h:30
armarx::armem::human::NearestHumanParameters::modelIDs
std::set< std::string > modelIDs
Definition: util.h:40
armarx::armem::human::getNearestHuman
std::optional< HumanPose > getNearestHuman(const std::vector< HumanPose > &humanPoses, const NearestHumanParameters &parameters)
Definition: util.cpp:60
PropertyDefinition.hpp
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
PropertyDefinitionContainer.h
armarx::armem::human::GlobalFloorProjection
@ GlobalFloorProjection
Definition: util.h:14
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::armem::human::NearestHumanParameters::coordSystem
KeyPointCoordinateSystem coordSystem
Definition: util.h:38
armarx::armem::human::NearestHumanParameters::properties
struct armarx::armem::human::NearestHumanParameters::Properties properties
armarx::armem::human::computeMeanPosition
std::optional< Eigen::Vector3f > computeMeanPosition(const HumanPose &humanPose, KeyPointCoordinateSystem coordSystem)
Definition: util.cpp:14
distance
double distance(const Point &a, const Point &b)
Definition: point.hpp:88
armarx::armem::human::HumanPose::keypoints
KeyPointMap keypoints
Definition: types.h:35
armarx::armem::human
Definition: aron_conversions.cpp:16
PropertyDefinition.h
armarx::armem::human::NearestHumanParameters::distanceThresholdMM
std::optional< double > distanceThresholdMM
Definition: util.h:39
armarx::split
std::vector< std::string > split(const std::string &source, const std::string &splitBy, bool trimElements=false, bool removeEmptyElements=false)
Definition: StringHelpers.cpp:36
norm
double norm(const Point &a)
Definition: point.hpp:94