Work on depth based point cloud masking

2026-06-09 00:28:05 -06:00 · 2024-08-18 19:08:30 -06:00
parent c1a21f70c3
commit 3c0d3635c9
50 changed files with 2263 additions and 2069 deletions
@@ -0,0 +1,8 @@
+# Default ignored files
+/shelf/
+/workspace.xml
+# Editor-based HTTP Client requests
+/httpRequests/
+# Datasource local storage ignored files
+/dataSources/
+/dataSources.local.xml
@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<module type="PYTHON_MODULE" version="4">
+  <component name="NewModuleRootManager">
+    <content url="file://$MODULE_DIR$" />
+    <orderEntry type="inheritedJdk" />
+    <orderEntry type="sourceFolder" forTests="false" />
+  </component>
+</module>
@@ -0,0 +1,6 @@
+<component name="InspectionProjectProfileManager">
+  <profile version="1.0">
+    <option name="myName" value="Project Default" />
+    <inspection_tool class="PyPep8Inspection" enabled="false" level="WEAK WARNING" enabled_by_default="false" />
+  </profile>
+</component>
@@ -0,0 +1,6 @@
+<component name="InspectionProjectProfileManager">
+  <settings>
+    <option name="USE_PROJECT_PROFILE" value="false" />
+    <version value="1.0" />
+  </settings>
+</component>
@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="Black">
+    <option name="sdkName" value="Python 3.11 (3dscan)" />
+  </component>
+  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.11 (3dscan)" project-jdk-type="Python SDK" />
+</project>
@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectModuleManager">
+    <modules>
+      <module fileurl="file://$PROJECT_DIR$/.idea/3dscan.iml" filepath="$PROJECT_DIR$/.idea/3dscan.iml" />
+    </modules>
+  </component>
+</project>
@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="VcsDirectoryMappings">
+    <mapping directory="$PROJECT_DIR$/.." vcs="Git" />
+  </component>
+</project>
@@ -25,6 +25,7 @@ def run_loop():
    clientsocket.connect(('localhost', 65000))
    print("Connected!")

+
    while running:
        points, colors = k.get_ptcld(colorized=True, scale=10)
        
@@ -51,7 +52,7 @@ def run_loop():
        # 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]
@@ -1,7 +1,10 @@
+import math
+
 import cv2
 import numpy as np
 import ktb
 import mediapipe as mp
+from collections import deque

 from scipy.spatial import ConvexHull

@@ -33,10 +36,117 @@ def calc_mask(depth_map, rgb_image):
    if results.pose_landmarks:
        # Create person mask
        print("Found person!")
+        # cv2.imshow("Test", generate_distance_map(results.pose_landmarks, rgb_image.shape))
        return create_person_mask(depth_map, results.pose_landmarks, rgb_image.shape)
    else:
        return np.zeros(rgb_image.shape[:2], dtype=np.uint8)

+def clamp(x, min_value, max_value):
+    return max(min(max_value, x), min_value)
+
+
+def get_blurred_point_value(image, point, kernel_size=(5, 5), sigma=1.0):
+    # Ensure odd kernel size
+    kernel_size = tuple(k + 1 if k % 2 == 0 else k for k in kernel_size)
+
+    # Calculate the half size of the kernel
+    half_width = kernel_size[0] // 2
+    half_height = kernel_size[1] // 2
+
+    # Extract the region of interest (ROI) around the point
+    x, y = point
+    roi = image[max(0, y - half_height):min(image.shape[0], y + half_height + 1),
+          max(0, x - half_width):min(image.shape[1], x + half_width + 1)]
+
+    # Apply Gaussian blur to the ROI
+    blurred_roi = cv2.GaussianBlur(roi, kernel_size, sigma)
+
+    # Get the value at the center of the blurred ROI
+    center_y, center_x = blurred_roi.shape[0] // 2, blurred_roi.shape[1] // 2
+    blurred_value = blurred_roi[center_y, center_x]
+
+    return blurred_value
+
+def gradient_line(mask, x1, y1, x2, y2, depth_1, depth_2, circle_dist=15, step_size=5):
+    circle_dist = 10
+
+    cv2.circle(mask, (x1, y1), circle_dist * 2, int(depth_1), thickness=-1)
+    # cv2.circle(mask, (x2, y2), circle_dist * 2, int(depth_2), thickness=-1)
+
+    # print(x1, y1, x2, y2)
+
+    distance = math.sqrt((x2 - x1)**2 + (y2 - y1)**2)
+    step_count = int(distance / step_size)
+    print(step_count)
+
+    if step_count <= 0:
+        return mask
+
+    distance_x = (x2 - x1) / step_count
+    distance_y = (y2 - y1) / step_count
+
+    if distance_x == 0 or distance_y == 0:
+        return mask
+
+    color_step_distance = (depth_2 - depth_1) / step_size
+
+    for i in range(int(step_count)):
+        x = x1 + i * distance_x
+        y = y1 + i * distance_y
+        color = depth_1 + color_step_distance * i
+
+        tmp_mask = mask.copy()
+        cv2.circle(tmp_mask, (int(x), int(y)), circle_dist * 2, color, thickness=-1)
+        mask = cv2.addWeighted(mask, 0.5, tmp_mask, 0.5, 1.0)
+        del tmp_mask
+    return mask
+
+SKELETON_DEPTH_TO_KINECT = 4096
+
+def generate_distance_map(depth_map, pose_landmarks, image_shape, circle_dist=15, step_size=10):
+    # Create a black background
+    # mask = np.zeros(image_shape[:2], dtype=np.uint8)
+    mask = depth_map
+
+    connections = []
+
+    # 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(clamp(start_point.x, 0, .999) * image_shape[1]), int(clamp(start_point.y, 0, .999) * image_shape[0])
+        x2, y2 = int(clamp(end_point.x, 0, .999) * image_shape[1]), int(clamp(end_point.y, 0, .999) * image_shape[0])
+
+        # print(start_point.z, end_point.z)
+
+        # depth_1 = start_point.z * SKELETON_DEPTH_TO_KINECT
+        # depth_2 = end_point.z * SKELETON_DEPTH_TO_KINECT
+
+        depth_1 = get_blurred_point_value(depth_map, (x1, y1), kernel_size=(15, 15))
+        depth_2 = get_blurred_point_value(depth_map, (x2, y2), kernel_size=(15, 15))
+
+        connections.append({
+            "x1": x1, "y1": y1, "x2":x2, "y2":y2, "depth_1":depth_1, "depth_2":depth_2
+        })
+
+    n = len(connections)  # Get the length of the array
+    if n > 1:
+        for i in range(1, n):  # Iterate over the array starting from the second element
+            key = connections[i]  # Store the current element as the key to be inserted in the right position
+            j = i - 1
+            while j >= 0 and key["depth_1"] > connections[j]["depth_1"]:  # Move elements greater than key one position ahead
+                connections[j + 1] = connections[j]  # Shift elements to the right
+                j -= 1
+            connections[j + 1] = key  # Insert the key in the correct position
+
+
+    for c in connections:
+        mask = gradient_line(mask, c["x1"], c["y1"], c["x2"], c["y2"], c["depth_1"], c["depth_2"], circle_dist)
+
+
+    return mask
+

 def create_person_mask(depth_map, pose_landmarks, image_shape, distance_threshold=25):
    # Create an empty mask
@@ -55,25 +165,11 @@ def create_person_mask(depth_map, pose_landmarks, image_shape, distance_threshol
    # Dilate the mask to include nearby pixels
    kernel = np.ones((distance_threshold, distance_threshold), np.uint8)
    mask = cv2.dilate(mask, kernel, iterations=1)
+
+    mask_2 = generate_distance_map(depth_map, pose_landmarks, image_shape, distance_threshold)
+    cv2.imshow("mask_2", (mask_2 / 2048)*mask)
    
-    # 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
+    return mask


 # while running:
@@ -1,3 +1,70 @@
-[11:01:29] [Server thread/WARN] (Minecraft) Can't keep up! Is the server overloaded? Running 2317ms or 46 ticks behind
-[11:02:15] [Server thread/INFO] (Minecraft) ASTATIN3 lost connection: Disconnected
-[11:02:15] [Server thread/INFO] (Minecraft) ASTATIN3 left the game
+[18:01:25] [main/INFO] (FabricLoader/GameProvider) Loading Minecraft 1.21 with Fabric Loader 0.15.11
+[18:01:25] [main/INFO] (FabricLoader) Loading 42 mods:
+	- fabric-api 0.100.3+1.21
+	- fabric-api-base 0.4.42+6573ed8cd1
+	- fabric-api-lookup-api-v1 1.6.67+b5597344d1
+	- fabric-biome-api-v1 13.0.28+6fc22b99d1
+	- fabric-block-api-v1 1.0.22+0af3f5a7d1
+	- fabric-block-view-api-v2 1.0.10+6573ed8cd1
+	- fabric-command-api-v1 1.2.48+f71b366fd1
+	- fabric-command-api-v2 2.2.27+6a6dfa19d1
+	- fabric-commands-v0 0.2.65+df3654b3d1
+	- fabric-content-registries-v0 8.0.13+b5597344d1
+	- fabric-convention-tags-v1 2.0.14+7f945d5bd1
+	- fabric-convention-tags-v2 2.3.1+8a3b5617d1
+	- fabric-crash-report-info-v1 0.2.29+0af3f5a7d1
+	- fabric-data-attachment-api-v1 1.1.23+6a6dfa19d1
+	- fabric-data-generation-api-v1 20.2.8+16c4ae25d1
+	- fabric-dimensions-v1 4.0.0+6fc22b99d1
+	- fabric-entity-events-v1 1.6.12+6fc22b99d1
+	- fabric-events-interaction-v0 0.7.10+e633f883d1
+	- fabric-game-rule-api-v1 1.0.52+6573ed8cd1
+	- fabric-gametest-api-v1 2.0.1+6fc22b99d1
+	- fabric-item-api-v1 11.0.0+afdfc921d1
+	- fabric-item-group-api-v1 4.1.1+cb5ced13d1
+	- fabric-lifecycle-events-v1 2.3.11+8f3583aed1
+	- fabric-loot-api-v2 3.0.10+6573ed8cd1
+	- fabric-message-api-v1 6.0.13+6573ed8cd1
+	- fabric-networking-api-v1 4.2.0+ab7edbacd1
+	- fabric-object-builder-api-v1 15.1.11+d1321076d1
+	- fabric-particles-v1 4.0.2+6573ed8cd1
+	- fabric-recipe-api-v1 5.0.9+6573ed8cd1
+	- fabric-registry-sync-v0 5.0.22+ab7edbacd1
+	- fabric-rendering-data-attachment-v1 0.3.48+73761d2ed1
+	- fabric-rendering-fluids-v1 3.1.6+b5597344d1
+	- fabric-resource-conditions-api-v1 4.2.1+d153f344d1
+	- fabric-resource-loader-v0 1.1.4+cb5ced13d1
+	- fabric-screen-handler-api-v1 1.3.79+b5597344d1
+	- fabric-transfer-api-v1 5.1.14+b5597344d1
+	- fabric-transitive-access-wideners-v1 6.0.12+6573ed8cd1
+	- fabricloader 0.15.11
+	- java 22
+	- minecraft 1.21
+	- mixinextras 0.3.5
+	- modid 1.0.0
+[18:01:25] [main/INFO] (FabricLoader/Mixin) SpongePowered MIXIN Subsystem Version=0.8.5 Source=file:/home/astatin3/.gradle/caches/modules-2/files-2.1/net.fabricmc/sponge-mixin/0.13.3+mixin.0.8.5/9527e6b0d2449408958fd1302594dc65ec5ade9c/sponge-mixin-0.13.3+mixin.0.8.5.jar Service=Knot/Fabric Env=SERVER
+[18:01:26] [main/INFO] (FabricLoader/Mixin) Loaded Fabric development mappings for mixin remapper!
+[18:01:26] [main/INFO] (FabricLoader/Mixin) Compatibility level set to JAVA_17
+[18:01:26] [main/INFO] (FabricLoader/Mixin) Compatibility level set to JAVA_21
+[18:01:26] [main/INFO] (FabricLoader/MixinExtras|Service) Initializing MixinExtras via com.llamalad7.mixinextras.service.MixinExtrasServiceImpl(version=0.3.5).
+[18:01:30] [main/INFO] (Minecraft) Environment: Environment[sessionHost=https://sessionserver.mojang.com, servicesHost=https://api.minecraftservices.com, name=PROD]
+[18:01:31] [main/INFO] (Minecraft) Loaded 1290 recipes
+[18:01:31] [main/INFO] (Minecraft) Loaded 1399 advancements
+[18:01:32] [main/INFO] (BiomeModificationImpl) Applied 0 biome modifications to 0 of 64 new biomes in 922.1 μs
+[18:01:32] [Server thread/INFO] (Minecraft) Starting minecraft server version 1.21
+[18:01:32] [Server thread/INFO] (Minecraft) Loading properties
+[18:01:32] [Server thread/INFO] (Minecraft) Default game type: SURVIVAL
+[18:01:32] [Server thread/INFO] (Minecraft) Generating keypair
+[18:01:32] [Server thread/INFO] (Minecraft) Starting Minecraft server on *:25565
+[18:01:32] [Server thread/INFO] (Minecraft) Using epoll channel type
+[18:01:32] [Server thread/INFO] (Minecraft) Preparing level "world"
+[18:01:32] [Point Cloud Thread/INFO] (Minecraft) [STDOUT]: Server is listening on port 65000
+[18:01:33] [Server thread/INFO] (Minecraft) Preparing start region for dimension minecraft:overworld
+[18:01:33] [Worker-Main-8/INFO] (Minecraft) Preparing spawn area: 0%
+[18:01:33] [Server thread/INFO] (Minecraft) Time elapsed: 256 ms
+[18:01:33] [Server thread/INFO] (Minecraft) Done (1.107s)! For help, type "help"
+[18:02:12] [Point Cloud Thread/INFO] (Minecraft) [STDOUT]: New client connected
+[18:02:20] [User Authenticator #1/INFO] (Minecraft) UUID of player ASTATIN3 is 1451d26e-5141-4801-8e94-c35164f87be9
+[18:02:21] [Server thread/INFO] (Minecraft) ASTATIN3[/127.0.0.1:41902] logged in with entity id 36 at (1.0013749210145841, 126.98027428779072, 0.21102810844578268)
+[18:02:21] [Server thread/INFO] (Minecraft) ASTATIN3 joined the game
+[19:05:02] [Server thread/WARN] (Minecraft) Can't keep up! Is the server overloaded? Running 4421ms or 88 ticks behind
@@ -1,5 +1,5 @@
 #Minecraft server properties
-#Sat Aug 17 22:12:35 MDT 2024
+#Sun Aug 18 18:01:30 MDT 2024
 accepts-transfers=false
 allow-flight=false
 allow-nether=true
@@ -1 +1 @@
-[{"name":"ASTATIN3","uuid":"1451d26e-5141-4801-8e94-c35164f87be9","expiresOn":"2024-09-17 22:12:43 -0600"}]
+[{"name":"ASTATIN3","uuid":"1451d26e-5141-4801-8e94-c35164f87be9","expiresOn":"2024-09-18 18:02:21 -0600"}]
@@ -1 +1 @@
-{"stats":{"minecraft:dropped":{"minecraft:grass_block":1},"minecraft:custom":{"minecraft:time_since_rest":171824,"minecraft:leave_game":15,"minecraft:play_time":171824,"minecraft:time_since_death":171824,"minecraft:sneak_time":994,"minecraft:total_world_time":171824,"minecraft:drop":1,"minecraft:fly_one_cm":263255},"minecraft:used":{"minecraft:grass_block":4}},"DataVersion":3953}
+{"stats":{"minecraft:used":{"minecraft:grass_block":4},"minecraft:custom":{"minecraft:time_since_rest":293988,"minecraft:leave_game":17,"minecraft:play_time":293988,"minecraft:time_since_death":293988,"minecraft:sneak_time":1071,"minecraft:total_world_time":293988,"minecraft:drop":1,"minecraft:fly_one_cm":274963},"minecraft:dropped":{"minecraft:grass_block":1}},"DataVersion":3953}