python - How to check if a cuboid is inside camera frustum - Stack Overflow

admin管理员组

文章数量:1205156

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

I saw this solution and tried to implement it, but I missed something, can you please tell me how to fix it?

the provided 4 points are 2 inside, 2 outside camera frustum.

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:
            selfpute_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)

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

I tried to implement this as follows

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

I saw this solution and tried to implement it, but I missed something, can you please tell me how to fix it?

the provided 4 points are 2 inside, 2 outside camera frustum.

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)

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

I tried to implement this as follows

• python
• math
• graphics
• 3d
• camera

Share Improve this question Follow edited Jan 24 at 15:26 Christoph Rackwitz 15.4k55 gold badges3939 silver badges5151 bronze badges asked Jan 20 at 17:04 bhomaidan90bhomaidan90 19411 silver badge1212 bronze badges 10

• 1 An inefficient solution is to split the cuboid in triangles. Then you can project the triangles in the camera space using a matrix multiplication. Then you can check if any point of the target cuboid, or more specifically its triangles, are in the one of the camera view (which is now a basic normalized cuboid so with no rotation AFAIK). There is a catch though: if there is no point inside, this does not means the cuboid is not visible. You also need to check if any triangle cross the boundary of the camera cuboid, which can also be divided in triangles (triangle collisions are often simpler) – Jérôme Richard Commented Jan 20 at 17:17
• 1 PS: I assume that the view frustum is not completely included within the cuboid (corner case). I guess this should not happen anyway, but better be safe than sorry ;) . – Jérôme Richard Commented Jan 20 at 17:34
• 1 this looks like standard computer graphics, part of every "rendering pipeline". this would be described in any book on the topic, any lecture notes, lecture videos, tutorials, and I suspect there exist past questions with answers on SO about this. – Christoph Rackwitz Commented Jan 23 at 9:47
• Are you absolutely certain that any of those points are inside the frustrum? My amended version of your code is suggesting that they are all just outside. – lastchance Commented Jan 24 at 12:10
• 1 Does this help you? – CcmU Commented Jan 24 at 14:36

 |  Show 5 more comments

2 Answers 2

Sorted by: Reset to default 4 +50

EDIT: pending a response from the OP. There is a problem with your cam_pose matrix. The [0:3,0:3] components (first three rows and first three columns) should be a rotation matrix. However, it isn't: the first and third columns aren't orthogonal.

Well, no matter how I try to do it, I think all those points lie outside the frustum (really a pyramid). Could you check that those are the particular points that you intended (because the picture in your post suggests that you also tried other camera locations). It would be really good if somebody else looked at this independently. Maybe there's some distinction between the camera position and the focal point that I don't know about. (I'm currently making no distinction.)

I tried two things - Method 1: fix your code; Method 2: reverse the transformation and calculate the angles to compare with FOV/2. I also cobbled together something to plot the points.

Method 1: Try to fix your code.

You calculate a local basis triad, n_sight, u_hvec and v_vvec. These need to be normalised, because you use projections onto them to calculate the relevant coordinate lengths d_prime, u_prime and v_prime. Your method of finding self.h_uvec and self.v_vvec is unnecessarily complicated and hard to envisage (even though correct). I've tried to indicate where these are relative to the local frustrum points below.

You don't need self.dist in find u_prime and v_prime (despite what your link says) because you have already got a cross-wise plane at distance d_prime (or you would have if n_sight was normalised to unit length). Your projection plane is at distance n.p from the camera and the vector p_prime_vec is the components of the original displacement perpendicular to n_sight. You shouldn't normalise p_point_vec (or you will get distance d_prime wrong).

I've left some debugging print statements in the code below: if you know the order of points (I eventually worked them out) they may help you.

import numpy as np
import math
from typing import Tuple
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D


class CameraFrustum:
    def __init__( self, d_dist: float = 0.3, fov: Tuple[float, float] = (50, 40) ):      # original
        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  = self.frustum_vectors[0] - self.frustum_vectors[1]                 ##### much easier
        self.v_vvec  = self.frustum_vectors[0] - self.frustum_vectors[3]
        self.n_sight /= np.linalg.norm( self.n_sight )                                   ##### normalise basis vectors to unit length
        self.u_hvec  /= np.linalg.norm( self.u_hvec  )
        self.v_vvec  /= np.linalg.norm( self.v_vvec  )
        print( 'n_sight = ', self.n_sight )                                              # check unit-vector directions
        print( 'u_hvec = ', self.u_hvec )                                                
        print( 'v_vvec = ', self.v_vvec )

    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
        
        d_prime = np.dot(p_point_vec, self.n_sight)                                      # p.n = plane distance from the camera

        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 the camera")
            return False

        p_prime_vec = p_point_vec  - self.n_sight * d_prime                              #  p - (p.n)n   = displacement from centreline
        u_prime = np.dot(p_prime_vec, self.u_hvec)
        v_prime = np.dot(p_prime_vec, self.v_vvec)

        u_max = d_prime * np.tan(np.radians(self.fov[0]) / 2);   u_min = -u_max
        v_max = d_prime * np.tan(np.radians(self.fov[1]) / 2);   v_min = -v_max
        print( "u_prime, v_prime, u_max, v_max=", u_prime, v_prime, u_max, v_max )       # check rotated coordinates
        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)

Output

n_sight =  [-0.3271947   0.         -0.94495695]
u_hvec =  [-0.02719692  0.99958918 -0.00904433]
v_vvec =  [ 0.9447125   0.02274412 -0.32711006]
u_prime, v_prime, u_max, v_max= -0.3963363671027199 0.5645937705948938 0.3683791237780479 0.2875334205549486
False
u_prime, v_prime, u_max, v_max= 0.40342016151710725 0.5645937726848527 0.37488698183988656 0.29261304252107834
False
u_prime, v_prime, u_max, v_max= 0.3963363671027199 -0.5645937705948936 0.5642361967573459 0.44040705127553315
False
u_prime, v_prime, u_max, v_max= -0.4034201615171073 -0.5645937726848521 0.5577283386955073 0.4353274293094035
False

Method 2 - Invert the translation and rotation

You can simply reverse the transformation and calculate the relevant angles, confirming if abs(angle) < FOV/2 in the relevant direction.

import numpy as np
import math

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],
    ]
)

fov = ( 50, 40 )
origin = cam_pose[0:3,3]
rotate = cam_pose[0:3,0:3]
reverse = np.linalg.inv( rotate )
print( 'Angles' )
for p in pts:
    q = np.dot( p - origin, reverse.T )
    angle1 = math.atan2( q[0], q[2] ) * 180 / np.pi
    angle2 = math.atan2( q[1], q[2] ) * 180 / np.pi
    print( f'{angle1:7.2f}   {angle2:7.2f}  ', abs( angle1 ) < fov[0] / 2 and abs( angle2 ) < fov[1] / 2 )

Output:

Angles
 -26.45     35.32   False
  26.86     35.32   False
  18.24    -25.14   False
 -18.54    -25.14   False

Plotting

If anyone wants to try it you can have a go with what I cobbled together to plot it. (You need to rotate it by hand in 3d). I haven't done much more than apply the same rotation and camera (or, at least, focal point) as you have.

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

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],
    ]
)

fov = ( 50, 40 )
origin = cam_pose[0:3,3]
rotate = cam_pose[0:3,0:3]
fov_horizontal, fov_vertical = np.radians(fov[0]/2), np.radians(fov[1]/2)
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)
frustum_vectors = np.dot(base_vectors, rotate.T)
pp = frustum_vectors + origin

fig = plt.figure(figsize=(10, 8))
ax = fig.add_subplot(111, projection='3d')
for xyz in pts:
    ax.scatter( xyz[0], xyz[1], xyz[2], color='r' )
ax.scatter( origin[0], origin[1], origin[2], color='b' )
for p in pp:
    ax.plot( [origin[0],p[0]], [origin[1],p[1]], [origin[2],p[2]], color='g' )
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
plt.show()

Seen from one direction parallel to one face of the frustum - two points are outside this face.

Looking parallel to the opposite face - the other two points are outside.

Once you are in screen space coordinates it should be pretty straightforward to check if either at least on of the quad's points lie within the screen area, or if any of the four screen points lie within the quad (this can be done with at max 12 dot products in 2D). Last one is needed if only part of one edge of the quad is onscreen. That should be it.

You can do the same thing in world space, using distance to plane (points inside view frustrum) and line-triangle intersection tests (frustrum edges against quad).

本文标签： pythonHow to check if a cuboid is inside camera frustumStack Overflow