(3d) import some stuff from image_recognition

people_recognition_2d/src/people_recognition_2d/people_recognizer_2d.py

@@ -134,7 +134,7 @@ def _get_body_parts_pose_estimation(group_id, recognitions):
         Get a list of all bodyparts associated with a particular group ID
 
         :param: group_id: The group ID of the bodyparts to be fetched
-        :param: recognitions: All bodyparts recieved from pose estimation
+        :param: recognitions: All bodyparts received from pose estimation
         :return: List of body_parts
         """
         return [r for r in recognitions if r.group_id == group_id]

people_recognition_3d/package.xml

@@ -23,6 +23,8 @@
   <exec_depend>cv_bridge</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>image_geometry</exec_depend>
+  <exec_depend>image_recognition_pose_estimation</exec_depend>
+  <exec_depend>image_recognition_util</exec_depend>
   <exec_depend>message_filters</exec_depend>
   <exec_depend>people_recognition_2d</exec_depend>
   <exec_depend>people_recognition_msgs</exec_depend>

people_recognition_3d/src/people_recognition_3d/people_recognizer_3d.py

@@ -1,21 +1,22 @@
 #!/usr/bin/env python
-from __future__ import print_function, division
 
-import PyKDL as kdl
 from collections import namedtuple
-import numpy as np
+from typing import Mapping
 
 import image_geometry
+import numpy as np
+import PyKDL as kdl
 import rospy
 from cv_bridge import CvBridge
-from geometry_msgs.msg import Point, Vector3, Pose, Quaternion
-from sensor_msgs.msg import Image, CameraInfo
+from geometry_msgs.msg import Point, Pose, Quaternion, Vector3
+from image_recognition_util.image_writer import color_map
+from image_recognition_pose_estimation.body_parts import BODY_PART_LINKS
+from people_recognition_msgs.msg import Person3D
+from people_recognition_msgs.srv import RecognizePeople2D
+from sensor_msgs.msg import CameraInfo, Image
 from std_msgs.msg import ColorRGBA
 from visualization_msgs.msg import Marker, MarkerArray
 
-from people_recognition_msgs.srv import RecognizePeople2D
-from people_recognition_msgs.msg import Person3D
-
 
 def _get_and_wait_for_service(srv_name, srv_class):
     """
@@ -96,40 +97,12 @@ class Skeleton(object):
     {L, R}{Shoulder, Elbow, Wrist, Hip, Knee, Ankle, Eye, Ear}
     """
 
-    def __init__(self, body_parts):
+    def __init__(self, body_parts: Mapping[str, str]):
         """
         Constructor
 
         :param body_parts: {name: Joint}
         """
-        self.links = [
-            # Head left half
-            ('LEar', 'LEye'),
-            ('LEye', 'Nose'),
-            # Head right half
-            ('REar', 'REye'),
-            ('REye', 'Nose'),
-            # Head center
-            ('Nose', 'Neck'),
-
-            # Upper body left half
-            ('Neck', 'LShoulder'),
-            ('LShoulder', 'LElbow'),
-            ('LElbow', 'LWrist'),
-            # Upper body right half
-            ('Neck', 'RShoulder'),
-            ('RShoulder', 'RElbow'),
-            ('RElbow', 'RWrist'),
-
-            # Lower body left half
-            ('Neck', 'LHip'),
-            ('LHip', 'LKnee'),
-            ('LKnee', 'LAnkle'),
-            # Lower body right half
-            ('Neck', 'RHip'),
-            ('RKnee', 'RAnkle'),
-            ('RHip', 'RKnee'),
-        ]
 
         self.body_parts = body_parts
 
@@ -142,7 +115,7 @@ def filter_body_parts(self, threshold):
         :return: Skeleton object containing body parts within the threshold
         """
         return_list = set()
-        for (a, b) in self.links:
+        for (a, b) in BODY_PART_LINKS:
             if a in self.body_parts and b in self.body_parts:
                 p1 = self.body_parts[a].point
                 p2 = self.body_parts[b].point
@@ -178,7 +151,7 @@ def get_links(self):
         """
         :returns [Point], with point pairs for all the links
         """
-        for (a, b) in self.links:
+        for (a, b) in BODY_PART_LINKS:
             if a in self.body_parts and b in self.body_parts:
                 yield self.body_parts[a].point
                 yield self.body_parts[b].point
@@ -189,27 +162,6 @@ def __repr__(self):
         return '%s(%r)' % (self.__class__.__name__, self.body_parts)
 
 
-def color_map(N=256, normalized=False):
-    def bitget(byteval, idx):
-        return ((byteval & (1 << idx)) != 0)
-
-    dtype = 'float32' if normalized else 'uint8'
-    cmap = np.zeros((N, 3), dtype=dtype)
-    for i in range(N):
-        r = g = b = 0
-        c = i + 1  # skip the first color (black)
-        for j in range(8):
-            r |= bitget(c, 0) << 7 - j
-            g |= bitget(c, 1) << 7 - j
-            b |= bitget(c, 2) << 7 - j
-            c >>= 3
-
-        cmap[i] = np.array([r, g, b])
-
-    cmap = cmap / 255 if normalized else cmap
-    return cmap
-
-
 class PeopleRecognizer3D(object):
     def __init__(self, recognize_people_srv_name, probability_threshold,
                  link_threshold, heuristic, arm_norm_threshold,