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

[Anonymous]

Я хочу проверить, находится ли объект (определенный четырьмя углами в 3D-пространстве) внутри поля зрения позы камеры.
Я видел это решение и попытался его реализовать. , но я что-то упустил, подскажите, пожалуйста, как это исправить?

представлены 4 точки: 2 внутри, 2 снаружи усеченной камеры.

Код: Выделить всё

import numpy as np
from typing import Tuple

class CameraFrustum:
def __init__(
self, d_dist: float = 0.3, fov: Tuple[float, float] = (50, 40)
):
self.d_dist = d_dist
self.fov = fov
self.frustum_vectors = None
self.n_sight = None
self.u_hvec = None
self.v_vvec = None

def compute_frustum_vectors(self, cam_pose: np.ndarray):
fov_horizontal, fov_vertical = np.radians(self.fov[0] / 2), np.radians(
self.fov[1] / 2
)
self.cam_position = cam_pose[:3, 3]
cam_orientation = cam_pose[:3, :3]
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)
self.frustum_vectors = np.dot(base_vectors, cam_orientation.T)
self.n_sight = np.mean(self.frustum_vectors, axis=0)
self.u_hvec = np.cross(
np.mean(self.frustum_vectors[:2], axis=0), self.n_sight
)
self.v_vvec = np.cross(
np.mean(self.frustum_vectors[1:3], axis=0), self.n_sight
)

def project_point(
self, p_point: np.ndarray, cam_orientation: np.ndarray
) -> bool:
if self.frustum_vectors is None:
self.compute_frustum_vectors(cam_orientation)
#
p_point_vec = p_point - self.cam_position
p_point_vec /= np.linalg.norm(p_point_vec, axis=-1, keepdims=True)
#
d_prime = np.dot(p_point_vec, self.n_sight)

if abs(d_prime) < 1e-6:
print("point is not in front of the camera")
return False
elif d_prime < self.d_dist:
print("point is too close to camera")
return False
#
p_prime_vec = self.d_dist *(
p_point_vec / d_prime
) - self.d_dist * self.n_sight
u_prime = np.dot(p_prime_vec, self.u_hvec)
v_prime = np.dot(p_prime_vec, self.v_vvec)
#

width = 2 * self.d_dist * np.tan(np.radians(self.fov[0]) / 2)
height = 2 * self.d_dist * np.tan(np.radians(self.fov[1]) / 2)
u_min, u_max = -width / 2, width / 2
v_min, v_max = -height / 2, height / 2

if not (u_min < u_prime < u_max):
return False

if not (v_min < v_prime < v_max):
return False

return True

cam_frustum = CameraFrustum()

pts = np.array(
[
[1.54320189, -0.35068437, -0.48266792],
[1.52144436, 0.44898697, -0.48990338],
[0.32197813, 0.41622155, -0.50429738],
[0.34373566, -0.38344979, -0.49706192],
]
)

cam_pose = np.array(
[
[-0.02719692, 0.9447125, -0.3271947, 1.25978471],
[0.99958918, 0.02274412, 0.0, 0.03276859],
[-0.00904433, -0.32711006, -0.94495695, 0.4514743],
[0.0, 0.0, 0.0, 1.0],
]
)

for pt in pts:
res = cam_frustum.project_point(pt, cam_pose)
print(res)

Можете ли вы рассказать мне, как могу ли я это исправить? спасибо.
Я пытался реализовать это следующим образом

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