Added: Furthest point sampling [Tested: OK]

Readme.md

@@ -32,4 +32,4 @@ Bound to a long term development. Below is the list of present contents:
 
 - Robotics:
   - Stable inverse jacobian
-  - Trajectory generation
+  - Planar & geodesic trajectory generation

heimdall/pointcloud/sampling.py

@@ -0,0 +1,27 @@
+import torch
+
+
+def furthest_point_sampling(
+    pointcloud: torch.Tensor, n_samples: int = 1000
+) -> torch.Tensor:
+    N, _ = pointcloud.shape
+    n_samples = min(n_samples, N)
+
+    sampled_indices = torch.zeros(n_samples, dtype=torch.long, device=pointcloud.device)
+
+    distances = torch.ones(N, dtype=torch.float32, device=pointcloud.device) * torch.inf
+
+    random_index = torch.randint(0, N, (1,))
+    sampled_indices[0] = random_index
+
+    for i in range(1, n_samples):
+        last_added = sampled_indices[i - 1]
+
+        distances = torch.min(
+            distances, torch.norm(pointcloud - pointcloud[last_added], dim=1)
+        )
+
+        selected = torch.argmax(distances)
+        sampled_indices[i] = selected
+
+    return pointcloud[sampled_indices]

heimdall/robotics/inverse_kinematics.py

@@ -135,9 +135,8 @@ def compute_geodesic_trajectory(
 
         smooth_factor = pose_geodesic_distance(p_target, p_current)
         jacobian_inverse = compute_stable_inverse_jacobian(jacobian)
-        q_next = q_current + smooth_factor * jacobian_inverse @ relative_pose(
-            p_current, p_target
-        )
+        update = smooth_factor * jacobian_inverse @ relative_pose(p_current, p_target)
+        q_next = q_current + update
 
         joint_trajectory.append(q_next)