Update

Author: chutsu

scripts/benchmark.py

@@ -0,0 +1,46 @@
+import time
+import numpy as np
+
+
+def method1(points, R, t):
+  return np.dot(points, R.T) + t
+
+
+def method2(hpoints, tf):
+  transformed_points = np.dot(hpoints, tf.T)
+  return transformed_points[:, :3]
+
+
+def benchmark(N, repetitions=100):
+  # Generate random Nx3 points
+  points = np.random.rand(N, 3)
+  ones = np.ones((points.shape[0], 1))
+  hpoints = np.hstack((points, ones))
+
+  # Create random rotation matrix and translation vector
+  R = np.random.rand(3, 3)
+  t = np.random.rand(3)
+
+  # Create a 4x4 homogeneous transformation matrix
+  tf = np.eye(4)
+  tf[:3, :3] = R
+  tf[:3, 3] = t
+
+  # Benchmark Method 1
+  start_time = time.time()
+  for _ in range(repetitions):
+    method1(points, R, t)
+  time_method1 = (time.time() - start_time) / repetitions
+
+  # Benchmark Method 2
+  start_time = time.time()
+  for _ in range(repetitions):
+    method2(hpoints, tf)
+  time_method2 = (time.time() - start_time) / repetitions
+
+  print(f"Mean time for Method 1 (SO3 + R3): {time_method1:.2e} [s]")
+  print(f"Mean time for Method 2 (SE3):      {time_method2:.2e} [s]")
+
+
+# Run the benchmark
+benchmark(N=100000000)

scripts/fast_projector.py

@@ -0,0 +1,44 @@
+#!/usr/bin/env python3
+import multiprocessing as mp
+
+import numpy as np
+
+
+def project_points(hpts_world, K):
+  uvd = hpts_world[:, :3] @ K.T
+  # uvd /= uvd[:, 2]
+  return uvd
+
+
+def process_chunk(start_idx, chunk_size, hpts_world, K):
+  num_points = hpts_world.shape[0]
+  end_idx = min(start_idx + chunk_size, num_points)
+  return project_points(hpts_world[start_idx:end_idx, :], K)
+
+
+def project_points_multiprocessing(hpts_world, K, num_procs=3):
+  num_points = hpts_world.shape[0]
+  chunk_size = num_points // num_procs
+
+  with mp.Pool(processes=num_procs) as pool:
+    results = [
+        pool.apply_async(
+            process_chunk,
+            args=(i * chunk_size, chunk_size, hpts_world, K),
+        ) for i in range(num_procs)
+    ]
+    output = np.vstack([result.get() for result in results])
+
+  return output
+
+
+if __name__ == "__main__":
+  # Setup
+  N = 100000
+  pts_world = np.random.rand(N, 3)
+  hpts_world = np.hstack((pts_world, np.ones((N, 1))))
+  K = np.array([[1000, 0, 640], [0, 1000, 360], [0, 0, 1]])
+
+  # Project points in a multiprocessing way
+  uvd = project_points_multiprocessing(hpts_world, K, num_procs=8)
+  print(uvd)