Как проверить, находится ли кубоид внутри усеченной камеры ⇐ Python

[Anonymous]

Я хочу проверить, находится ли объект (определенный четырьмя углами в трехмерном пространстве) внутри поля зрения для нескольких поз камеры, но подход, который я использую (разница в углах к вектору центра усеченного конуса), не дал результатов. работает как положено, подскажите, пожалуйста, как это исправить? спасибо.
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


Подробнее здесь: https://stackoverflow.com/questions/793 ... ra-frustum