用python 可视化 KITTI 中 BEV检测的结果

小灰灰 2022-03-25 02:42 649阅读 0赞

先发个可视化效果：

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3lleWFuZzkxMQ_size_16_color_FFFFFF_t_70][]

两个py 文件构成

可以在github 上下载：[https://github.com/yeyang1021/KITTI\_VIZ\_BEV/][https_github.com_yeyang1021_KITTI_VIZ_BEV]

一个是kitti\_util.py

""" Helper methods for loading and parsing KITTI data.
    Author:
    Date: September 2017
    """
    from __future__ import print_function
    
    import numpy as np
    import cv2
    import os
    
    class Object3d(object):
        ''' 3d object label '''
        def __init__(self, label_file_line):
            data = label_file_line.split(' ')
            data[1:] = [float(x) for x in data[1:]]
    
            # extract label, truncation, occlusion
            self.type = data[0] # 'Car', 'Pedestrian', ...
            self.truncation = data[1] # truncated pixel ratio [0..1]
            self.occlusion = int(data[2]) # 0=visible, 1=partly occluded, 2=fully occluded, 3=unknown
            self.alpha = data[3] # object observation angle [-pi..pi]
    
            # extract 2d bounding box in 0-based coordinates
            self.xmin = data[4] # left
            self.ymin = data[5] # top
            self.xmax = data[6] # right
            self.ymax = data[7] # bottom
            self.box2d = np.array([self.xmin,self.ymin,self.xmax,self.ymax])
            
            # extract 3d bounding box information
            self.h = data[8] # box height
            self.w = data[9] # box width
            self.l = data[10] # box length (in meters)
            self.t = (data[11],data[12],data[13]) # location (x,y,z) in camera coord.
            self.ry = data[14] # yaw angle (around Y-axis in camera coordinates) [-pi..pi]
    
        def print_object(self):
            print('Type, truncation, occlusion, alpha: %s, %d, %d, %f' % \
                (self.type, self.truncation, self.occlusion, self.alpha))
            print('2d bbox (x0,y0,x1,y1): %f, %f, %f, %f' % \
                (self.xmin, self.ymin, self.xmax, self.ymax))
            print('3d bbox h,w,l: %f, %f, %f' % \
                (self.h, self.w, self.l))
            print('3d bbox location, ry: (%f, %f, %f), %f' % \
                (self.t[0],self.t[1],self.t[2],self.ry))
    
    
    class Calibration(object):
        ''' Calibration matrices and utils
            3d XYZ in <label>.txt are in rect camera coord.
            2d box xy are in image2 coord
            Points in <lidar>.bin are in Velodyne coord.
            y_image2 = P^2_rect * x_rect
            y_image2 = P^2_rect * R0_rect * Tr_velo_to_cam * x_velo
            x_ref = Tr_velo_to_cam * x_velo
            x_rect = R0_rect * x_ref
            P^2_rect = [f^2_u,  0,      c^2_u,  -f^2_u b^2_x;
                        0,      f^2_v,  c^2_v,  -f^2_v b^2_y;
                        0,      0,      1,      0]
                     = K * [1|t]
            image2 coord:
             ----> x-axis (u)
            |
            |
            v y-axis (v)
            velodyne coord:
            front x, left y, up z
            rect/ref camera coord:
            right x, down y, front z
            Ref (KITTI paper): http://www.cvlibs.net/publications/Geiger2013IJRR.pdf
            TODO(rqi): do matrix multiplication only once for each projection.
        '''
        def __init__(self, calib_filepath, from_video=False):
            if from_video:
                calibs = self.read_calib_from_video(calib_filepath)
            else:
                calibs = self.read_calib_file(calib_filepath)
            # Projection matrix from rect camera coord to image2 coord
            self.P = calibs['P2'] 
            self.P = np.reshape(self.P, [3,4])
            # Rigid transform from Velodyne coord to reference camera coord
            self.V2C = calibs['Tr_velo_to_cam']
            self.V2C = np.reshape(self.V2C, [3,4])
            self.C2V = inverse_rigid_trans(self.V2C)
            # Rotation from reference camera coord to rect camera coord
            self.R0 = calibs['R0_rect']
            self.R0 = np.reshape(self.R0,[3,3])
    
            # Camera intrinsics and extrinsics
            self.c_u = self.P[0,2]
            self.c_v = self.P[1,2]
            self.f_u = self.P[0,0]
            self.f_v = self.P[1,1]
            self.b_x = self.P[0,3]/(-self.f_u) # relative 
            self.b_y = self.P[1,3]/(-self.f_v)
    
        def read_calib_file(self, filepath):
            ''' Read in a calibration file and parse into a dictionary.
            Ref: https://github.com/utiasSTARS/pykitti/blob/master/pykitti/utils.py
            '''
            data = {}
            with open(filepath, 'r') as f:
                for line in f.readlines():
                    line = line.rstrip()
                    if len(line)==0: continue
                    key, value = line.split(':', 1)
                    # The only non-float values in these files are dates, which
                    # we don't care about anyway
                    try:
                        data[key] = np.array([float(x) for x in value.split()])
                    except ValueError:
                        pass
    
            return data
        
        def read_calib_from_video(self, calib_root_dir):
            ''' Read calibration for camera 2 from video calib files.
                there are calib_cam_to_cam and calib_velo_to_cam under the calib_root_dir
            '''
            data = {}
            cam2cam = self.read_calib_file(os.path.join(calib_root_dir, 'calib_cam_to_cam.txt'))
            velo2cam = self.read_calib_file(os.path.join(calib_root_dir, 'calib_velo_to_cam.txt'))
            Tr_velo_to_cam = np.zeros((3,4))
            Tr_velo_to_cam[0:3,0:3] = np.reshape(velo2cam['R'], [3,3])
            Tr_velo_to_cam[:,3] = velo2cam['T']
            data['Tr_velo_to_cam'] = np.reshape(Tr_velo_to_cam, [12])
            data['R0_rect'] = cam2cam['R_rect_00']
            data['P2'] = cam2cam['P_rect_02']
            return data
    
        def cart2hom(self, pts_3d):
            ''' Input: nx3 points in Cartesian
                Oupput: nx4 points in Homogeneous by pending 1
            '''
            n = pts_3d.shape[0]
            pts_3d_hom = np.hstack((pts_3d, np.ones((n,1))))
            return pts_3d_hom
     
        # =========================== 
        # ------- 3d to 3d ---------- 
        # =========================== 
        def project_velo_to_ref(self, pts_3d_velo):
            pts_3d_velo = self.cart2hom(pts_3d_velo) # nx4
            return np.dot(pts_3d_velo, np.transpose(self.V2C))
    
        def project_ref_to_velo(self, pts_3d_ref):
            pts_3d_ref = self.cart2hom(pts_3d_ref) # nx4
            return np.dot(pts_3d_ref, np.transpose(self.C2V))
    
        def project_rect_to_ref(self, pts_3d_rect):
            ''' Input and Output are nx3 points '''
            return np.transpose(np.dot(np.linalg.inv(self.R0), np.transpose(pts_3d_rect)))
        
        def project_ref_to_rect(self, pts_3d_ref):
            ''' Input and Output are nx3 points '''
            return np.transpose(np.dot(self.R0, np.transpose(pts_3d_ref)))
     
        def project_rect_to_velo(self, pts_3d_rect):
            ''' Input: nx3 points in rect camera coord.
                Output: nx3 points in velodyne coord.
            ''' 
            pts_3d_ref = self.project_rect_to_ref(pts_3d_rect)
            return self.project_ref_to_velo(pts_3d_ref)
    
        def project_velo_to_rect(self, pts_3d_velo):
            pts_3d_ref = self.project_velo_to_ref(pts_3d_velo)
            return self.project_ref_to_rect(pts_3d_ref)
    
        # =========================== 
        # ------- 3d to 2d ---------- 
        # =========================== 
        def project_rect_to_image(self, pts_3d_rect):
            ''' Input: nx3 points in rect camera coord.
                Output: nx2 points in image2 coord.
            '''
            pts_3d_rect = self.cart2hom(pts_3d_rect)
            pts_2d = np.dot(pts_3d_rect, np.transpose(self.P)) # nx3
            pts_2d[:,0] /= pts_2d[:,2]
            pts_2d[:,1] /= pts_2d[:,2]
            return pts_2d[:,0:2]
        
        def project_velo_to_image(self, pts_3d_velo):
            ''' Input: nx3 points in velodyne coord.
                Output: nx2 points in image2 coord.
            '''
            pts_3d_rect = self.project_velo_to_rect(pts_3d_velo)
            return self.project_rect_to_image(pts_3d_rect)
    
        # =========================== 
        # ------- 2d to 3d ---------- 
        # =========================== 
        def project_image_to_rect(self, uv_depth):
            ''' Input: nx3 first two channels are uv, 3rd channel
                       is depth in rect camera coord.
                Output: nx3 points in rect camera coord.
            '''
            n = uv_depth.shape[0]
            x = ((uv_depth[:,0]-self.c_u)*uv_depth[:,2])/self.f_u + self.b_x
            y = ((uv_depth[:,1]-self.c_v)*uv_depth[:,2])/self.f_v + self.b_y
            pts_3d_rect = np.zeros((n,3))
            pts_3d_rect[:,0] = x
            pts_3d_rect[:,1] = y
            pts_3d_rect[:,2] = uv_depth[:,2]
            return pts_3d_rect
    
        def project_image_to_velo(self, uv_depth):
            pts_3d_rect = self.project_image_to_rect(uv_depth)
            return self.project_rect_to_velo(pts_3d_rect)
    
     
    def rotx(t):
        ''' 3D Rotation about the x-axis. '''
        c = np.cos(t)
        s = np.sin(t)
        return np.array([[1,  0,  0],
                         [0,  c, -s],
                         [0,  s,  c]])
    
    
    def roty(t):
        ''' Rotation about the y-axis. '''
        c = np.cos(t)
        s = np.sin(t)
        return np.array([[c,  0,  s],
                         [0,  1,  0],
                         [-s, 0,  c]])
    
    
    def rotz(t):
        ''' Rotation about the z-axis. '''
        c = np.cos(t)
        s = np.sin(t)
        return np.array([[c, -s,  0],
                         [s,  c,  0],
                         [0,  0,  1]])
    
    
    def transform_from_rot_trans(R, t):
        ''' Transforation matrix from rotation matrix and translation vector. '''
        R = R.reshape(3, 3)
        t = t.reshape(3, 1)
        return np.vstack((np.hstack([R, t]), [0, 0, 0, 1]))
    
    
    def inverse_rigid_trans(Tr):
        ''' Inverse a rigid body transform matrix (3x4 as [R|t])
            [R'|-R't; 0|1]
        '''
        inv_Tr = np.zeros_like(Tr) # 3x4
        inv_Tr[0:3,0:3] = np.transpose(Tr[0:3,0:3])
        inv_Tr[0:3,3] = np.dot(-np.transpose(Tr[0:3,0:3]), Tr[0:3,3])
        return inv_Tr
    
    def read_label(label_filename):
        lines = [line.rstrip() for line in open(label_filename)]
        objects = [Object3d(line) for line in lines]
        return objects
    
    def load_image(img_filename):
        return cv2.imread(img_filename)
    
    def load_velo_scan(velo_filename):
        scan = np.fromfile(velo_filename, dtype=np.float32)
        scan = scan.reshape((-1, 4))
        return scan
    
    def project_to_image(pts_3d, P):
        ''' Project 3d points to image plane.
        Usage: pts_2d = projectToImage(pts_3d, P)
          input: pts_3d: nx3 matrix
                 P:      3x4 projection matrix
          output: pts_2d: nx2 matrix
          P(3x4) dot pts_3d_extended(4xn) = projected_pts_2d(3xn)
          => normalize projected_pts_2d(2xn)
          <=> pts_3d_extended(nx4) dot P'(4x3) = projected_pts_2d(nx3)
              => normalize projected_pts_2d(nx2)
        '''
        n = pts_3d.shape[0]
        pts_3d_extend = np.hstack((pts_3d, np.ones((n,1))))
        print(('pts_3d_extend shape: ', pts_3d_extend.shape))
        pts_2d = np.dot(pts_3d_extend, np.transpose(P)) # nx3
        pts_2d[:,0] /= pts_2d[:,2]
        pts_2d[:,1] /= pts_2d[:,2]
        return pts_2d[:,0:2]
    
    
    def compute_box_3d(obj, P):
        ''' Takes an object and a projection matrix (P) and projects the 3d
            bounding box into the image plane.
            Returns:
                corners_2d: (8,2) array in left image coord.
                corners_3d: (8,3) array in in rect camera coord.
        '''
        # compute rotational matrix around yaw axis
        R = roty(obj.ry)    
    
        # 3d bounding box dimensions
        l = obj.l;
        w = obj.w;
        h = obj.h;
        
        # 3d bounding box corners
        x_corners = [l/2,l/2,-l/2,-l/2,l/2,l/2,-l/2,-l/2];
        y_corners = [0,0,0,0,-h,-h,-h,-h];
        z_corners = [w/2,-w/2,-w/2,w/2,w/2,-w/2,-w/2,w/2];
        
        # rotate and translate 3d bounding box
        corners_3d = np.dot(R, np.vstack([x_corners,y_corners,z_corners]))
        #print corners_3d.shape
        corners_3d[0,:] = corners_3d[0,:] + obj.t[0];
        corners_3d[1,:] = corners_3d[1,:] + obj.t[1];
        corners_3d[2,:] = corners_3d[2,:] + obj.t[2];
        #print 'cornsers_3d: ', corners_3d 
        # only draw 3d bounding box for objs in front of the camera
        if np.any(corners_3d[2,:]<0.1):
            corners_2d = None
            return corners_2d, np.transpose(corners_3d)
        
        # project the 3d bounding box into the image plane
        corners_2d = project_to_image(np.transpose(corners_3d), P);
        #print 'corners_2d: ', corners_2d
        return corners_2d, np.transpose(corners_3d)
    
    
    def compute_orientation_3d(obj, P):
        ''' Takes an object and a projection matrix (P) and projects the 3d
            object orientation vector into the image plane.
            Returns:
                orientation_2d: (2,2) array in left image coord.
                orientation_3d: (2,3) array in in rect camera coord.
        '''
        
        # compute rotational matrix around yaw axis
        R = roty(obj.ry)
       
        # orientation in object coordinate system
        orientation_3d = np.array([[0.0, obj.l],[0,0],[0,0]])
        
        # rotate and translate in camera coordinate system, project in image
        orientation_3d = np.dot(R, orientation_3d)
        orientation_3d[0,:] = orientation_3d[0,:] + obj.t[0]
        orientation_3d[1,:] = orientation_3d[1,:] + obj.t[1]
        orientation_3d[2,:] = orientation_3d[2,:] + obj.t[2]
        
        # vector behind image plane?
        if np.any(orientation_3d[2,:]<0.1):
          orientation_2d = None
          return orientation_2d, np.transpose(orientation_3d)
        
        # project orientation into the image plane
        orientation_2d = project_to_image(np.transpose(orientation_3d), P);
        return orientation_2d, np.transpose(orientation_3d)
    
    def draw_projected_box3d(image, qs, color=(255,255,255), thickness=2):
        ''' Draw 3d bounding box in image
            qs: (8,3) array of vertices for the 3d box in following order:
                1 -------- 0
               /|         /|
              2 -------- 3 .
              | |        | |
              . 5 -------- 4
              |/         |/
              6 -------- 7
        '''
        qs = qs.astype(np.int32)
        for k in range(0,4):
           # Ref: http://docs.enthought.com/mayavi/mayavi/auto/mlab_helper_functions.html
           i,j=k,(k+1)%4
           # use LINE_AA for opencv3
           cv2.line(image, (qs[i,0],qs[i,1]), (qs[j,0],qs[j,1]), color, thickness, cv2.CV_AA)
    
           i,j=k+4,(k+1)%4 + 4
           cv2.line(image, (qs[i,0],qs[i,1]), (qs[j,0],qs[j,1]), color, thickness, cv2.CV_AA)
    
           i,j=k,k+4
           cv2.line(image, (qs[i,0],qs[i,1]), (qs[j,0],qs[j,1]), color, thickness, cv2.CV_AA)
        return image

另一个是

plot.py

import numpy as np
    import os
    import matplotlib.pyplot as plt
    from mpl_toolkits.mplot3d import Axes3D
    import matplotlib.patches as patches
    import yaml
    import pandas as pd
    from kitti_util import *
    from pylab import *
    from matplotlib.lines import Line2D
    # ==============================================================================
    #                                                         POINT_CLOUD_2_BIRDSEYE
    # ==============================================================================
    def point_cloud_2_top(points,
                          res=0.1,
                          zres=0.3,
                          side_range=(-20., 20-0.05),  # left-most to right-most
                          fwd_range=(0., 40.-0.05),  # back-most to forward-most
                          height_range=(-2., 0.),  # bottom-most to upper-most
                          ):
        """ Creates an birds eye view representation of the point cloud data for MV3D.
        Args:
            points:     (numpy array)
                        N rows of points data
                        Each point should be specified by at least 3 elements x,y,z
            res:        (float)
                        Desired resolution in metres to use. Each output pixel will
                        represent an square region res x res in size.
            zres:        (float)
                        Desired resolution on Z-axis in metres to use.
            side_range: (tuple of two floats)
                        (-left, right) in metres
                        left and right limits of rectangle to look at.
            fwd_range:  (tuple of two floats)
                        (-behind, front) in metres
                        back and front limits of rectangle to look at.
            height_range: (tuple of two floats)
                        (min, max) heights (in metres) relative to the origin.
                        All height values will be clipped to this min and max value,
                        such that anything below min will be truncated to min, and
                        the same for values above max.
        Returns:
            numpy array encoding height features , density and intensity.
        """
        # EXTRACT THE POINTS FOR EACH AXIS
        x_points = points[:, 0]
        y_points = points[:, 1]
        z_points = points[:, 2]
        reflectance = points[:,3]
    
        # INITIALIZE EMPTY ARRAY - of the dimensions we want
        x_max = int((side_range[1] - side_range[0]) / res)
        y_max = int((fwd_range[1] - fwd_range[0]) / res)
        z_max = int((height_range[1] - height_range[0]) / zres)
        top = np.zeros([y_max+1, x_max+1, z_max+1], dtype=np.float32)
    
        # FILTER - To return only indices of points within desired cube
        # Three filters for: Front-to-back, side-to-side, and height ranges
        # Note left side is positive y axis in LIDAR coordinates
        f_filt = np.logical_and(
            (x_points > fwd_range[0]), (x_points < fwd_range[1]))
        s_filt = np.logical_and(
            (y_points > -side_range[1]), (y_points < -side_range[0]))
        filt = np.logical_and(f_filt, s_filt)
    
        for i, height in enumerate(np.arange(height_range[0], height_range[1], zres)):
    
            z_filt = np.logical_and((z_points >= height),
                                    (z_points < height + zres))
            zfilter = np.logical_and(filt, z_filt)
            indices = np.argwhere(zfilter).flatten()
    
            # KEEPERS
            xi_points = x_points[indices]
            yi_points = y_points[indices]
            zi_points = z_points[indices]
            ref_i = reflectance[indices]
    
            # CONVERT TO PIXEL POSITION VALUES - Based on resolution
            x_img = (-yi_points / res).astype(np.int32)  # x axis is -y in LIDAR
            y_img = (-xi_points / res).astype(np.int32)  # y axis is -x in LIDAR
    
            # SHIFT PIXELS TO HAVE MINIMUM BE (0,0)
            # floor & ceil used to prevent anything being rounded to below 0 after
            # shift
            x_img -= int(np.floor(side_range[0] / res))
            y_img += int(np.floor(fwd_range[1] / res))
    
            # CLIP HEIGHT VALUES - to between min and max heights
            pixel_values = zi_points - height_range[0]
            # pixel_values = zi_points
    
            # FILL PIXEL VALUES IN IMAGE ARRAY
            top[y_img, x_img, i] = pixel_values
    
            # max_intensity = np.max(prs[idx])
            top[y_img, x_img, z_max] = ref_i
            
        top = (top / np.max(top) * 255).astype(np.uint8)
        return top
    
    def transform_to_img(xmin, xmax, ymin, ymax,
                          res=0.1,
                          side_range=(-20., 20-0.05),  # left-most to right-most
                          fwd_range=(0., 40.-0.05),  # back-most to forward-most
                          ):
    
        xmin_img = -ymax/res - side_range[0]/res
        xmax_img = -ymin/res - side_range[0]/res
        ymin_img = -xmax/res + fwd_range[1]/res
        ymax_img = -xmin/res + fwd_range[1]/res
        
        return xmin_img, xmax_img, ymin_img, ymax_img
        
        
    def draw_point_cloud(ax, points, axes=[0, 1, 2], point_size=0.1, xlim3d=None, ylim3d=None, zlim3d=None):
        """
        Convenient method for drawing various point cloud projections as a part of frame statistics.
        """
        axes_limits = [
            [-20, 80], # X axis range
            [-40, 40], # Y axis range
            [-3, 3]   # Z axis range
        ]
        axes_str = ['X', 'Y', 'Z']
        ax.grid(False)
        
        ax.scatter(*np.transpose(points[:, axes]), s=point_size, c=points[:, 3], cmap='gray')
        ax.set_xlabel('{} axis'.format(axes_str[axes[0]]))
        ax.set_ylabel('{} axis'.format(axes_str[axes[1]]))
        if len(axes) > 2:
            ax.set_xlim3d(*axes_limits[axes[0]])
            ax.set_ylim3d(*axes_limits[axes[1]])
            ax.set_zlim3d(*axes_limits[axes[2]])
            ax.xaxis.set_pane_color((1.0, 1.0, 1.0, 0.0))
            ax.yaxis.set_pane_color((1.0, 1.0, 1.0, 0.0))
            ax.zaxis.set_pane_color((1.0, 1.0, 1.0, 0.0))
            ax.set_zlabel('{} axis'.format(axes_str[axes[2]]))
        else:
            ax.set_xlim(*axes_limits[axes[0]])
            ax.set_ylim(*axes_limits[axes[1]])
        # User specified limits
        if xlim3d!=None:
            ax.set_xlim3d(xlim3d)
        if ylim3d!=None:
            ax.set_ylim3d(ylim3d)
        if zlim3d!=None:
            ax.set_zlim3d(zlim3d)
            
    def compute_3d_box_cam2(h, w, l, x, y, z, yaw):
        """
        Return : 3xn in cam2 coordinate
        """
        R = np.array([[np.cos(yaw), 0, np.sin(yaw)], [0, 1, 0], [-np.sin(yaw), 0, np.cos(yaw)]])
        x_corners = [l/2,l/2,-l/2,-l/2,l/2,l/2,-l/2,-l/2]
        y_corners = [0,0,0,0,-h,-h,-h,-h]
        z_corners = [w/2,-w/2,-w/2,w/2,w/2,-w/2,-w/2,w/2]
        corners_3d_cam2 = np.dot(R, np.vstack([x_corners,y_corners,z_corners]))
        corners_3d_cam2 += np.vstack([x, y, z])
        return corners_3d_cam2
    
    def draw_box(ax, vertices, axes=[0, 1, 2], color='black'):
        """
        Draws a bounding 3D box in a pyplot axis.
        
        Parameters
        ----------
        pyplot_axis : Pyplot axis to draw in.
        vertices    : Array 8 box vertices containing x, y, z coordinates.
        axes        : Axes to use. Defaults to `[0, 1, 2]`, e.g. x, y and z axes.
        color       : Drawing color. Defaults to `black`.
        """
        vertices = vertices[axes, :]
        connections = [
            [0, 1], [1, 2], [2, 3], [3, 0],  # Lower plane parallel to Z=0 plane
            [4, 5], [5, 6], [6, 7], [7, 4],  # Upper plane parallel to Z=0 plane
            [0, 4], [1, 5], [2, 6], [3, 7]  # Connections between upper and lower planes
        ]
        for connection in connections:
            ax.plot(*vertices[:, connection], c=color, lw=0.5)
    
    
    def read_detection(path):
        df = pd.read_csv(path, header=None, sep=' ')
        df.columns = ['type', 'truncated', 'occluded', 'alpha', 'bbox_left', 'bbox_top',
                    'bbox_right', 'bbox_bottom', 'height', 'width', 'length', 'pos_x', 'pos_y', 'pos_z', 'rot_y']
    #     df.loc[df.type.isin(['Truck', 'Van', 'Tram']), 'type'] = 'Car'
    #     df = df[df.type.isin(['Car', 'Pedestrian', 'Cyclist'])]
        df = df[df['type']=='Car']
        df.reset_index(drop=True, inplace=True)
        return df
    
    img_id = 120
    
    calib = Calibration('/home1/dataset/Kitti/training/calib/%06d.txt'%img_id)
    
    path = '/home1/dataset/Kitti/training/velodyne/%06d.bin'%img_id
    points = np.fromfile(path, dtype=np.float32).reshape(-1, 4)
    
    df = read_detection('/home1/dataset/Kitti/training/label_2/%06d.txt'%img_id)
    df.head()
    
    print(len(df))
    
    fig, ax = plt.subplots(figsize=(10, 10))
    
    top = point_cloud_2_top(points, zres=1.0, side_range=(-40., 40-0.05), fwd_range=(0., 80.-0.05))
    top = np.array(top, dtype = np.float32)
    print (top)
    ax.imshow(top, aspect='equal')
    print(top.shape)
    for o in range(len(df)):
        corners_3d_cam2 = compute_3d_box_cam2(*df.loc[o, ['height', 'width', 'length', 'pos_x', 'pos_y', 'pos_z', 'rot_y']])
        corners_3d_velo = calib.project_rect_to_velo(corners_3d_cam2.T)
        print (corners_3d_velo)
        x1,x2,x3,x4 = corners_3d_velo[0:4,0]
        y1,y2,y3,y4 = corners_3d_velo[0:4,1]
        '''
        xmax = np.max(corners_3d_velo[:, 0])
        xmin = np.min(corners_3d_velo[:, 0])
        ymax = np.max(corners_3d_velo[:, 1])
        ymin = np.min(corners_3d_velo[:, 1])
        '''
        x1, x2, y1, y2 = transform_to_img(x1, x2, y1, y2, side_range=(-40., 40-0.05), fwd_range=(0., 80.-0.05))
        x3, x4, y3, y4 = transform_to_img(x3, x4, y3, y4, side_range=(-40., 40-0.05), fwd_range=(0., 80.-0.05))
        ps=[]
        polygon = np.zeros([5,2], dtype = np.float32)
        polygon[0,0] = x1 
        polygon[1,0] = x2      
        polygon[2,0] = x3 
        polygon[3,0] = x4 
        polygon[4,0] = x1
        
        polygon[0,1] = y1 
        polygon[1,1] = y2      
        polygon[2,1] = y3 
        polygon[3,1] = y4 
        polygon[4,1] = y1    
    
        
        line1 = [(x1,y1), (x2,y2)]
        line2 = [(x2,y2), (x3,y3)]
        line3 = [(x3,y3), (x4,y4)]
        line4 = [(x4,y4), (x1,y1)]
        (line1_xs, line1_ys) = zip(*line1)
        (line2_xs, line2_ys) = zip(*line2)
        (line3_xs, line3_ys) = zip(*line3)
        (line4_xs, line4_ys) = zip(*line4)
        ax.add_line(Line2D(line1_xs, line1_ys, linewidth=1, color='red'))
        ax.add_line(Line2D(line2_xs, line2_ys, linewidth=1, color='red'))
        ax.add_line(Line2D(line3_xs, line3_ys, linewidth=1, color='red'))
        ax.add_line(Line2D(line4_xs, line4_ys, linewidth=1, color='red'))
    
    print(ax)
    print(fig)
    plt.axis('off')
    plt.tight_layout()
    plt.draw()
    plt.show()

只需要修改对应的路径即可！

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3lleWFuZzkxMQ_size_16_color_FFFFFF_t_70]: /images/20220325/332495c631d543e09274ce9c97b90503.png
[https_github.com_yeyang1021_KITTI_VIZ_BEV]: https://github.com/yeyang1021/KITTI_VIZ_BEV/