[Python] How to check if a cuboid is inside camera frustum

I want to check if an object (defined by four corners in 3D space) is inside the Field of View for multiple camera poses.

but the approach that I am using (difference in angles to the frustum center vector) didn't work as expected, i.e. angles difference is positive even if the point is outside the FOV:

# example frustum angles in degrees:
[49.17128492 49.73612077 49.73612077 49.17128492]
# example rays angles in degrees
[48.98552611 49.16959892 49.33200833 49.14829723]
# differences in angles
[0.1857588 0.56652185 0.40411244 0.02298769]


Can you please tell me how can I fix this? thanks.

import numpy as np
from typing import Tuple

class CameraFrustum:
def __init__(
self,
fov: Tuple[float, float] = (65, 40)
):
self.fov: Tuple[float, float] = (np.radians(fov[0] / 2), np.radians(fov[1] / 2))

def _compute_frustum_vectors(self, orientations: np.ndarray) -> np.ndarray:
fov_horizontal, fov_vertical = self.fov
base_vectors = np.array([
[ np.tan(fov_horizontal), np.tan(fov_vertical), 1],
[-np.tan(fov_horizontal), np.tan(fov_vertical), 1],
[-np.tan(fov_horizontal), -np.tan(fov_vertical), 1],
[ np.tan(fov_horizontal), -np.tan(fov_vertical), 1]
])
base_vectors /= np.linalg.norm(base_vectors, axis=1, keepdims=True)
transposed_orientations = np.transpose(orientations, axes=(0, 2, 1))
frustum_vectors = np.einsum('ij,njk->nik', base_vectors, transposed_orientations)
return frustum_vectors

def filter_feasible_poses(
self,
object_pose: np.ndarray,
object_dimensions: Tuple[float, float],
camera_poses: np.ndarray
) -> np.ndarray:
length, width = object_dimensions
bbox_corners = np.array([
[ length / 2, width / 2, 0.0, 1],
[-length / 2, width / 2, 0.0, 1],
[-length / 2, -width / 2, 0.0, 1],
[ length / 2, -width / 2, 0.0, 1]
])
bbox_corners_world = np.dot(bbox_corners, object_pose.T)
bbox_corners_world = bbox_corners_world[:, :3]
orientations = camera_poses[:, :3, :3]
camera_positions = camera_poses[:, :3, 3]
# compute frustum vectors for a set of camera orientations
frustum_vectors = self._compute_frustum_vectors(orientations)
# compute rays from cameras to object corners
rays = bbox_corners_world[None, :, :] - camera_positions[:, None, :]
rays /= np.linalg.norm(rays, axis=-1, keepdims=True)
central_vectors = np.mean(frustum_vectors, axis=1)
# Compute angles between frustum vectors and their central vector
angles_frustum = np.arccos(np.clip(np.einsum('kij,kj->ki', frustum_vectors, central_vectors), -1.0, 1.0))
# Compute angles between rays and the frustum central vector Nx4
angles_rays = np.arccos(np.clip(np.einsum('kij,kj->ki', rays, central_vectors), -1.0, 1.0))
# difference between angles
angle_diffs = angles_frustum - angles_rays
# check if all corners are inside the frustum for each camera
inside_frustum_mask = np.all(angle_diffs >= 0, axis=1)
inside_frustum_mask = np.where( ~inside_frustum_mask)[0]
return inside_frustum_mask

Continue reading...