feat: draft fvt and ilv

.gitignore

@@ -0,0 +1,2 @@
+__pycache__
+data

fvi.py

@@ -0,0 +1,141 @@
+import numpy as np
+
+
+def fast_voxel_intersect(start, end, origin, step) -> tuple[list, list]:
+    """Compute the voxels intersected by a line segment.
+
+    Args:
+        start (array-like): start point of line segment
+        end (array-like): end point of line segment
+        origin (array-like): origin of voxel grid
+        step (array-like): step size of voxel grid
+
+    Returns:
+        list: list of intersection points
+        list: list of intersected voxels
+    """
+
+    # Convert to numpy arrays
+    start = np.asarray(start)
+    end = np.asarray(end)
+    origin = np.asarray(origin)
+    step = np.asarray(step)
+
+    # Translate line segment to voxel grid
+    start = start - origin
+    end = end - origin
+
+    # Initialize list of intersected voxels
+    intersections = []
+    voxels = []
+
+    # Compute direction of line segment
+    direction = end - start
+    global_distance = np.linalg.norm(direction, axis=0)
+    if global_distance == 0:
+        return intersections
+    direction = direction / global_distance
+
+    # Compute the sign of the direction
+    direction_signs = np.sign(direction)
+    is_positive = direction_signs > 0
+    is_negative = direction_signs < 0
+
+    # Initialize current position to start
+    position = start.copy()
+
+    # Main loop
+    while True:
+        # Compute the distance to the next boundaries
+        next_boundaries = np.divide(position + step * direction_signs, step)
+        distances = (is_positive * np.floor(next_boundaries) +
+                     is_negative * np.ceil(next_boundaries)) * step - position
+
+        # Determine the nearest boundary to be reached
+        boundary_distances = np.abs(distances / direction)
+        clothest_boundary = np.argmin(boundary_distances)
+        clothest_boundary_distance = boundary_distances[clothest_boundary]
+
+        # Check if we are done
+        distance_to_end = abs((end[0] - position[0]) / direction[0])
+        if clothest_boundary_distance > distance_to_end:
+            break
+
+        # Update position
+        position = position + clothest_boundary_distance * direction
+
+        # Correct position to be on boundary
+        position[clothest_boundary] = round(
+            position[clothest_boundary] / step[clothest_boundary]) * step[clothest_boundary]
+
+        # Get corresponding voxel
+        boundary_reached_negativly = np.zeros(start.shape, dtype=bool)
+        boundary_reached_negativly[clothest_boundary] = is_negative[clothest_boundary]
+        voxel = np.floor(position) - boundary_reached_negativly * step
+
+        # Add voxel to list
+        intersections.append(position + origin)
+        voxels.append(voxel + origin)
+
+    return intersections, voxels
+
+
+if __name__ == '__main__':
+    import matplotlib.pyplot as plt
+
+    def update_figure():
+        positions, voxels = fast_voxel_intersect(start, end, origin, step)
+
+        plt.clf()
+
+        # Plot hitted voxels
+        for voxel in voxels:
+            plt.fill([voxel[0], voxel[0] + step[0], voxel[0] + step[0], voxel[0]],
+                     [voxel[1], voxel[1], voxel[1] + step[1], voxel[1] + step[1]],
+                     color='#e25', alpha=0.5)
+
+        # Plot line segment
+        plt.plot([start[0], end[0]], [start[1], end[1]], 'k-')
+        plt.plot(start[0], start[1], 'go')
+        plt.plot(end[0], end[1], 'ro')
+
+        # Plot intersection points
+        for pos in positions:
+            plt.plot(pos[0], pos[1], 'bo')
+
+        # Plot voxel grid
+        plt.axis('equal')
+        plt.xlim((-10, 10))
+        plt.ylim((-10, 10))
+        xmin, xmax = plt.xlim()
+        ymin, ymax = plt.ylim()
+        plt.xticks(np.arange(xmin + origin[0],
+                             xmax + origin[0] + step[0], step[0]))
+        plt.yticks(np.arange(ymin + origin[1],
+                             ymax + origin[1] + step[1], step[1]))
+        plt.grid()
+        plt.draw()
+
+    def onclick(event):
+        global start, end
+        # if event.button == 1:
+        #     start = np.array([event.xdata, event.ydata])
+        # elif event.button == 3:
+        #     end = np.array([event.xdata, event.ydata])
+        start = np.random.rand(2) * 10 - 5
+        end = np.random.rand(2) * 10 - 5
+        update_figure()
+
+    # Define voxel grid
+    origin = np.array([.1, -.3])
+    step = np.array([1.0, 1.0])
+
+    # Define segment
+    start = np.random.rand(2) * 10 - 5
+    end = np.random.rand(2) * 10 - 5
+
+    # Plot
+    fig = plt.figure()
+    fig.canvas.mpl_connect('button_press_event', onclick)
+    update_figure()
+    plt.show()

intersec_line_voxel.py

@@ -0,0 +1,102 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from itertools import product
+
+
+def check_line_voxel(
+    px, py, pz,
+    dx, dy, dz,
+    vx, vy, vz,
+    c
+):
+    """Check if a line intersects a voxel."""
+
+    # Compute the intersection bounds
+    kx1 = (px - dx) / vx
+    ky1 = (py - dy) / vy
+    kz1 = (pz - dz) / vz
+    kx2 = (px - dx + c) / vx
+    ky2 = (py - dy + c) / vy
+    kz2 = (pz - dz + c) / vz
+
+    # Order the bounds
+    kxmin = np.min(np.concatenate([
+        kx1[:, np.newaxis],
+        kx2[:, np.newaxis]
+    ], axis=1), axis=1)
+    kymin = np.min(np.concatenate([
+        ky1[:, np.newaxis],
+        ky2[:, np.newaxis]
+    ], axis=1), axis=1)
+    kzmin = np.min(np.concatenate([
+        kz1[:, np.newaxis],
+        kz2[:, np.newaxis]
+    ], axis=1), axis=1)
+    kxmax = np.max(np.concatenate([
+        kx1[:, np.newaxis],
+        kx2[:, np.newaxis]
+    ], axis=1), axis=1)
+    kymax = np.max(np.concatenate([
+        ky1[:, np.newaxis],
+        ky2[:, np.newaxis]
+    ], axis=1), axis=1)
+    kzmax = np.max(np.concatenate([
+        kz1[:, np.newaxis],
+        kz2[:, np.newaxis]
+    ], axis=1), axis=1)
+
+    # Check if the bounds overlap
+    kmax = np.min(np.concatenate([
+        kxmax[:, np.newaxis],
+        kymax[:, np.newaxis],
+        kzmax[:, np.newaxis]
+    ], axis=1), axis=1)
+    kmin = np.max(np.concatenate([
+        kxmin[:, np.newaxis],
+        kymin[:, np.newaxis],
+        kzmin[:, np.newaxis]
+    ], axis=1), axis=1)
+    return kmin <= kmax
+
+c = 1.0
+points = np.array([[x, y, z] for x, y, z in product(
+    np.arange(-5.0, 4.0, c),
+    np.arange(-5.0, 4.0, c),
+    np.arange(-5.0, 4.0, c))
+])
+while True:
+
+    fig = plt.figure()
+    ax = plt.axes(projection='3d')
+
+    d = np.random.rand(3) * 1 - 0.5
+    v = np.random.rand(3) * 1 - 0.5
+
+    px, py, pz = points[:, 0], points[:, 1], points[:, 2]
+    dx, dy, dz = d
+    vx, vy, vz = v
+
+    bool_vect = check_line_voxel(px, py, pz, dx, dy, dz, vx, vy, vz, c)
+
+    # plot cube
+    for i, (px, py, pz) in enumerate(points):
+        if not bool_vect[i]:
+            continue
+
+        ax.plot([px, px+c], [py, py], [pz, pz], 'b')
+        ax.plot([px, px+c], [py, py], [pz+c, pz+c], 'b')
+        ax.plot([px, px+c], [py+c, py+c], [pz, pz], 'b')
+        ax.plot([px, px+c], [py+c, py+c], [pz+c, pz+c], 'b')
+        ax.plot([px, px], [py, py+c], [pz, pz], 'b')
+        ax.plot([px, px], [py, py+c], [pz+c, pz+c], 'b')
+        ax.plot([px+c, px+c], [py, py+c], [pz, pz], 'b')
+        ax.plot([px+c, px+c], [py, py+c], [pz+c, pz+c], 'b')
+        ax.plot([px, px], [py, py], [pz, pz+c], 'b')
+        ax.plot([px, px], [py+c, py+c], [pz, pz+c], 'b')
+        ax.plot([px+c, px+c], [py, py], [pz, pz+c], 'b')
+        ax.plot([px+c, px+c], [py+c, py+c], [pz, pz+c], 'b')
+
+    # plot line
+    ax.plot([dx, dx+vx], [dy, dy+vy], [dz, dz+vz], 'g')
+
+    plt.show()

main.py

@@ -1,30 +1,57 @@
 import cv2
 import numpy as np
+from itertools import product
+from rich.progress import track
 
 from matrices_reader import *
 
-VOXEL_SIZE = 1e-3
+VOXEL_SIZE = 2e-2
 X_MIN, X_MAX = -2.0, 2.0
 Y_MIN, Y_MAX = -2.0, 2.0
 Z_MIN, Z_MAX = -2.0, 2.0
 
-# grid = [[[
-#     1 for z in np.arange(Z_MIN, Z_MAX, VOXEL_SIZE)
-#     ] for y in np.arange(Y_MIN, Y_MAX, VOXEL_SIZE)
-#     ] for x in np.arange(X_MIN, X_MAX, VOXEL_SIZE)
-# ]
-
 projection_matrices = matrices_reader('data/torus/matrices.txt')
 nb_frame = len(projection_matrices)
-point = np.array([1.0, 0.0, 0.0, 1.0])
+
+points = np.array([[x, y, z, 1.0] for x, y, z in product(
+    np.arange(X_MIN, X_MAX, VOXEL_SIZE),
+    np.arange(Y_MIN, Y_MAX, VOXEL_SIZE),
+    np.arange(Z_MIN, Z_MAX, VOXEL_SIZE))])
+
+background = np.array([18, 18, 18])
 
 for k in range(nb_frame):
+    frame = cv2.imread(f'data/torus/torus{k+1:04}.png')
+    proj_mat = projection_matrices[k]
+    
+    cam_points = proj_mat @ points.T
+    cam_points /= cam_points[2,:]
+    cam_points = np.round(cam_points, 0).astype(np.int32)
+
+    visible = np.logical_and.reduce((0 <= cam_points[0,:], cam_points[0,:] < frame.shape[1], 0 <= cam_points[1,:], cam_points[1,:] < frame.shape[0]))
+    cam_points = cam_points[:,visible]
+    points = points[visible,:]
+
+    solid = np.invert(((frame[cam_points[1,:],cam_points[0,:]] == background)).all(1))
+    cam_points = cam_points[:,solid]
+    points = points[solid,:]
+
+for k in range(nb_frame):
+    frame = cv2.imread(f'data/torus/torus{k+1:04}.png')
+    # frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
+    # frame = 255 * (frame == 18).astype(np.uint8)
+    # frame = cv2.filter2D(frame, -1, np.ones((5, 5)) / 25)
+    # frame = 255 * (frame > 255/2).astype(np.uint8)
 
     proj_mat = projection_matrices[k]
-    cam_point = proj_mat @ point
-    cam_point /= cam_point[2]
     
-    frame = cv2.imread(f'data/torus/torus{k+1:04}.png')
+    cam_points = proj_mat @ points.T
-    cv2.circle(frame, (int(cam_point[0]), int(cam_point[1])), 2, (0, 0, 255))
+    cam_points /= cam_points[2,:]
+    cam_points = np.round(cam_points, 0).astype(np.int32)
+
+    for cam_point in cam_points.T:
+        cv2.circle(frame, (cam_point[0], cam_point[1]), 2, (255, 0, 0))
+            
+
     cv2.imshow('Frame', frame)
     cv2.waitKey(0)