draw_utils.py 文件源码

def draw_camera(pose, zmin=0.0, zmax=0.1, fov=np.deg2rad(60)): 

    frustum = Frustum(pose, zmin=zmin, zmax=zmax, fov=fov)
    nul, nll, nlr, nur, ful, fll, flr, fur = frustum.vertices
    # nll, nlr, nur, nul, fll, flr, fur, ful = frustum.vertices

    faces = []

    # Triangles: Front Face
    faces.extend([ful, fur, flr])
    faces.extend([flr, ful, fll])

    # Triangles: Back Face
    faces.extend([nul, nur, nlr])
    faces.extend([nlr, nul, nll])

    # Triangles: Four walls (2-triangles per face)
    left, top, right, bottom = [fll, nll, ful, ful, nll, nul], \
                               [ful, nul, fur, fur, nul, nur], \
                               [fur, nur, flr, flr, nur, nlr], \
                               [flr, nlr, fll, fll, nlr, nll]
    faces.extend([left, top, right, bottom]) # left, top, right, bottom wall
    faces = np.vstack(faces)

    # Lines: zmin-zmax
    pts = []
    pts.extend([ful, fur, flr, fll, ful])
    pts.extend([ful, fll, nll, nul, ful])
    pts.extend([ful, nul, nur, fur, ful])
    pts.extend([fur, nur, nlr, flr, fur])
    pts.extend([flr, nlr, nll, fll, flr])
    pts = np.vstack(pts)

    return (faces, np.hstack([pts[:-1], pts[1:]]).reshape((-1,3)))