Crop point cloud around person

3dscan/LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2024 Astatin3
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

3dscan/main.py

@@ -0,0 +1,71 @@
+from threading import  Thread
+import cv2
+import numpy as np
+import ktb
+import open3d as o3d
+import math
+
+import skeleton
+
+# vis = o3d.visualization.Visualizer()
+# vis.create_window()
+
+# reconstruction = o3d.geometry.PointCloud()
+# reconstruction.points = o3d.utility.Vector3dVector(np.random.rand(2, 3))
+
+# vis.add_geometry(reconstruction)
+
+running = True
+
+def run_loop():
+    k = ktb.Kinect()
+
+    import socket
+    clientsocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+    clientsocket.connect(('localhost', 65000))
+    print("Connected!")
+
+    while running:
+        points, colors = k.get_ptcld(colorized=True, scale=10)
+        
+        mask = skeleton.calc_mask(k.get_frame(ktb.DEPTH), cv2.cvtColor(k.get_frame(ktb.COLOR), cv2.COLOR_BGR2RGB))
+        cv2.imshow('Person Mask', mask * 255)
+        cv2.waitKey(1)
+        # mask = mask.flatten().reshape((-1, 1))
+        mask = mask.flatten().astype(bool)
+        
+        points = points.reshape((-1, 3))
+        points = points[mask]
+        if points.shape[0] == 0:
+            continue
+        skip_count = math.ceil(points.shape[0]/2000)
+        points = points[0::skip_count]
+        points = np.trunc(points).astype(int)
+        
+        colors = colors.reshape((-1, 3))
+        colors = colors[mask]
+        colors = colors[0::skip_count]
+        colors *= 256
+        colors = np.trunc(colors).astype(int)
+        
+        # reconstruction.points = o3d.utility.Vector3dVector(points)
+        # reconstruction.colors = o3d.utility.Vector3dVector(colors)
+        # vis.update_geometry(reconstruction)
+        
+        for i in range(len(points)):
+            point = points[i]
+            color = colors[i]
+            clientsocket.send(f'{i},{point[0]},{point[1]},{point[2]},{color[0]},{color[1]},{color[2]}\n'.encode())
+ 
+            # print(f'{i},{(point[0])},{(point[1])},{(point[2])},{color[0]},{color[1]},{color[2]}')
+        print("Update!")
+        
+t = Thread(target=run_loop)
+t.start()
+
+# while running:
+#     running = vis.poll_events()
+#     vis.update_renderer()
+
+t.join()
+skeleton.pose.close()

3dscan/skeleton.py

@@ -0,0 +1,86 @@
+import cv2
+import numpy as np
+import ktb
+import mediapipe as mp
+
+from scipy.spatial import ConvexHull
+
+running = True
+
+# Initialize MediaPipe Pose
+mp_pose = mp.solutions.pose
+mp_drawing = mp.solutions.drawing_utils
+pose = mp_pose.Pose(static_image_mode=False, min_detection_confidence=0.5, min_tracking_confidence=0.5)
+
+
+def get_color():
+    return k.get_frame(ktb.COLOR)
+
+def get_depth_map():
+    return k.get_frame(ktb.DEPTH)
+
+def normalize_depth_map(depth_map):
+    min_val, max_val, _, _ = cv2.minMaxLoc(depth_map)
+    normalized = cv2.convertScaleAbs(depth_map, alpha=255.0/(max_val-min_val), beta=-min_val * 255.0/(max_val-min_val))
+    return normalized
+
+def calc_mask(depth_map, rgb_image):
+
+
+    # Process the image and get pose landmarks
+    results = pose.process(rgb_image)
+
+    if results.pose_landmarks:
+        # Create person mask
+        print("Found person!")
+        return create_person_mask(depth_map, results.pose_landmarks, rgb_image.shape)
+    else:
+        return np.zeros(rgb_image.shape[:2], dtype=np.uint8)
+
+
+def create_person_mask(depth_map, pose_landmarks, image_shape, distance_threshold=25):
+    # Create an empty mask
+    mask = np.zeros(image_shape[:2], dtype=np.uint8)
+    
+    # Draw pose landmarks and connections
+    for connection in mp_pose.POSE_CONNECTIONS:
+        start_point = pose_landmarks.landmark[connection[0]]
+        end_point = pose_landmarks.landmark[connection[1]]
+        
+        x1, y1 = int(start_point.x * image_shape[1]), int(start_point.y * image_shape[0])
+        x2, y2 = int(end_point.x * image_shape[1]), int(end_point.y * image_shape[0])
+        
+        cv2.line(mask, (x1, y1), (x2, y2), 1, thickness=distance_threshold*2)
+    
+    # Dilate the mask to include nearby pixels
+    kernel = np.ones((distance_threshold, distance_threshold), np.uint8)
+    mask = cv2.dilate(mask, kernel, iterations=1)
+    
+    # Get depth values for the pose landmarks
+    landmark_depths = []
+    for landmark in pose_landmarks.landmark:
+        x, y = int(landmark.x * image_shape[1]), int(landmark.y * image_shape[0])
+        if 0 <= x < image_shape[1] and 0 <= y < image_shape[0]:
+            landmark_depths.append(depth_map[y, x])
+    
+    # Calculate depth range
+    min_depth = np.percentile(landmark_depths, 0)  # 5th percentile to avoid outliers
+    max_depth = np.percentile(landmark_depths, 100)  # 95th percentile to avoid outliers
+    
+    # Refine the mask using depth information
+    depth_mask = (depth_map >= min_depth) & (depth_map <= max_depth)
+    
+    # Combine the initial mask with the depth mask
+    final_mask = mask & depth_mask
+    
+    return final_mask
+
+
+# while running:
+#     # vis.update_geometry(reconstruction)
+#     # print("E")
+#     if cv2.waitKey(1) & 0xFF == ord('q'):
+#         running = False
+#     running = vis.poll_events()
+#     vis.update_renderer()
+