OpenPoseEstimationUtil.h
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @package <PACKAGE_NAME>::<CATEGORY>::Util
17  * @author Stefan Reither ( stef dot reither at web dot de )
18  * @date 2018
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
25 
26 using namespace armarx;
27 
28 namespace visionx
29 {
30  // Mapping from labels used in topic from SecondHands to corresponding ids and labals in the MPI-Model
31  const std::map<std::string, std::pair<unsigned int, std::string>> MPII_TO_MPI
32  {
33  {"head", {0, "Head"}},
34  {"neck", {1, "Neck"}},
35  {"right_shoulder", {2, "RShoulder"}},
36  {"right_elbow", {3, "RElbow"}},
37  {"right_hand", {4, "RWrist"}},
38  {"left_shoulder", {5, "LShoulder"}},
39  {"left_elbow", {6, "LElbow"}},
40  {"left_hand", {7, "LWrist"}},
41  {"right_waist", {8, "RHip"}},
42  {"right_knee", {9, "RKnee"}},
43  {"right_foot", {10, "RAnkle"}},
44  {"left_waist", {11, "LHip"}},
45  {"left_knee", {12, "LKnee"}},
46  {"left_foot", {13, "LAnkle"}},
47  };
48 
49 
50  struct Polygon2D
51  {
52  struct Point
53  {
54  float x;
55  float y;
56  bool operator==(const Point& p) const
57  {
58  return this->x - p.x < std::numeric_limits<float>::epsilon() && this->y - p.y < std::numeric_limits<float>::epsilon();
59  }
60  };
61 
62  using PointList = std::vector<Point>;
63  // ordered list of polygon points
64  // assert(points[0] == points[n]
65  std::vector<Point> points;
66 
67  Polygon2D() {}
69  {
70  ARMARX_CHECK_EXPRESSION(points[0] == points[points.size() - 1]);
71  this->points = points;
72  }
73 
74  // isLeft(): tests if a point is Left|On|Right of an infinite line.
75  // Input: three points p0, p1, and p2
76  // Return: >0 for p2 left of the line through p0 and p1
77  // =0 for p2 on the line
78  // <0 for p2 right of the line
79  int isLeft(Point p0, Point p1, Point p2) const
80  {
81  return static_cast<int>((p1.x - p0.x) * (p2.y - p0.y)
82  - (p2.x - p0.x) * (p1.y - p0.y));
83  }
84 
85  // Algorithm from http://geomalgorithms.com/a03-_inclusion.html#wn_PnPoly() (Date: 05/16/2018)
86  bool isInside(FramedPositionPtr point) const
87  {
88  Point p;
89  p.x = point->x;
90  p.y = point->y;
91  int wn = 0; // the winding number counter
92  // loop through all edges of the polygon
93  for (unsigned int i = 0; i < points.size() - 1; i++) // edge from V[i] to V[i+1]
94  {
95  if (points[i].y <= p.y) // start y <= P.y
96  {
97  if (points[i + 1].y > p.y) // an upward crossing
98  if (isLeft(points[i], points[i + 1], p) > 0) // P left of edge
99  {
100  ++wn; // have a valid up intersect
101  }
102  }
103  else // start y > P.y (no test needed)
104  {
105  if (points[i + 1].y <= p.y) // a downward crossing
106  if (isLeft(points[i], points[i + 1], p) < 0) // P right of edge
107  {
108  --wn; // have a valid down intersect
109  }
110  }
111  }
112  return wn == 1;
113  }
114  };
115 }
visionx
ArmarX headers.
Definition: OpenPoseStressTest.h:38
visionx::Polygon2D::Point::operator==
bool operator==(const Point &p) const
Definition: OpenPoseEstimationUtil.h:56
visionx::Polygon2D::Point
Definition: OpenPoseEstimationUtil.h:52
visionx::Polygon2D::PointList
std::vector< Point > PointList
Definition: OpenPoseEstimationUtil.h:62
visionx::Polygon2D::Polygon2D
Polygon2D(PointList points)
Definition: OpenPoseEstimationUtil.h:68
visionx::Polygon2D::isLeft
int isLeft(Point p0, Point p1, Point p2) const
Definition: OpenPoseEstimationUtil.h:79
visionx::Polygon2D::isInside
bool isInside(FramedPositionPtr point) const
Definition: OpenPoseEstimationUtil.h:86
IceInternal::Handle< FramedPosition >
visionx::Polygon2D::Polygon2D
Polygon2D()
Definition: OpenPoseEstimationUtil.h:67
FramedPose.h
visionx::Polygon2D::Point::y
float y
Definition: OpenPoseEstimationUtil.h:55
ExpressionException.h
visionx::Polygon2D::points
std::vector< Point > points
Definition: OpenPoseEstimationUtil.h:65
ARMARX_CHECK_EXPRESSION
#define ARMARX_CHECK_EXPRESSION(expression)
This macro evaluates the expression and if it turns out to be false it will throw an ExpressionExcept...
Definition: ExpressionException.h:73
armarx::MPII_TO_MPI
const std::map< std::string, std::pair< unsigned int, std::string > > MPII_TO_MPI
Definition: Util.h:30
visionx::Polygon2D::Point::x
float x
Definition: OpenPoseEstimationUtil.h:54
visionx::Polygon2D
Definition: OpenPoseEstimationUtil.h:50
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28