a
    Wze.                     @   s  d Z ddlmZ ddlZddlZddlZddlZej	ej
eZeds\ej	eZqDeejvrreje ddlmZ G dd deZdd ZG d	d
 d
eZd&ddZd'ddZd(ddZd)ddZdd Zdd ZedkredZd\ZZ eee fZ!e"de#e!Z!e$d%ee e!de! d d  eede!d  e&d!e e'dd"@ d#krBqjqBej(e) ej*d$Z+e$d%%e+ dS )*z
'''
///////////////////////////////////////
3D LiDAR Object Detection - ADAS
Pranav Durai
//////////////////////////////////////
'''
# Description: The utils of the kitti dataset
    )print_functionNsfac                   @   s8   e Zd ZdZdd Zdd Zdd Zdd	 Zd
d ZdS )Object3dz 3d object label c                 C   s&  | d}dd |dd  D |dd < |d | _| | j| _|d | _t|d | _|d | _|d | _|d	 | _	|d
 | _
|d | _t| j| j	| j
| jg| _|d | _|d | _|d | _|d |d |d f| _tj| j| _|d | _| dkr|d nd| _d | _|  | _d S )N c                 S   s   g | ]}t |qS  )float).0xr   r   T/home/opencvuniv/work/pranav/ADAS_2_LIDAR/SFA3D/sfa/data_process/kitti_data_utils.py
<listcomp>        z%Object3d.__init__.<locals>.<listcomp>   r                        	   
                     g      )splittypecls_type_to_idZcls_id
truncationint	occlusionalphaxminyminxmaxymaxnparraybox2dhwltlinalgnormZ
dis_to_camry__len__score	level_strget_obj_levellevel)selfZlabel_file_linedatar   r   r
   __init__   s*    











zObject3d.__init__c                 C   s   |t j vrdS t j| S )N)cnfZCLASS_NAME_TO_IDkeys)r7   Zcls_typer   r   r
   r   :   s    zObject3d.cls_type_to_idc                 C   s   t | jd t | jd  d }|dkrF| jdkrF| jdkrFd| _dS |dkrl| jdkrl| jdkrld	| _d
S |dkr| jdkr| jd
krd| _dS d| _dS d S )Nr   r   (   g333333?r   ZEasy   g333333?ZModerater   g      ?ZHardZUnKnownr   )r   r*   r    r"   r4   )r7   heightr   r   r
   r5   @   s     zObject3d.get_obj_levelc                 C   s|   t d| j| j| j| jf  t d| j| j| j| jf  t d| j	| j
| jf  t d| jd | jd | jd | jf  d S )Nz2Type, truncation, occlusion, alpha: %s, %d, %d, %fz%2d bbox (x0,y0,x1,y1): %f, %f, %f, %fz3d bbox h,w,l: %f, %f, %fz&3d bbox location, ry: (%f, %f, %f), %fr   r   r   )printr   r    r"   r#   r$   r%   r&   r'   r+   r,   r-   r.   r1   )r7   r   r   r
   print_objectP   s    zObject3d.print_objectc                 C   sl   d| j | jt| j| j| jd | jd | jd | jd | j| j| j| j	d | j	d | j	d | j
| jf }|S )NzK%s %.2f %d %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2fr   r   r   r   )r   r    r!   r"   r#   r*   r+   r,   r-   r.   r1   r3   )r7   Z	kitti_strr   r   r
   to_kitti_formatZ   s    $4zObject3d.to_kitti_formatN)	__name__
__module____qualname____doc__r9   r   r5   rA   rB   r   r   r   r
   r      s   
r   c                 C   s$   dd t | D }dd |D }|S )Nc                 S   s   g | ]}|  qS r   )rstripr   liner   r   r
   r   c   r   zread_label.<locals>.<listcomp>c                 S   s   g | ]}t |qS r   )r   rH   r   r   r
   r   d   r   )open)Zlabel_filenamelinesobjectsr   r   r
   
read_labelb   s    rM   c                   @   s(   e Zd ZdZdd Zdd Zdd ZdS )	Calibrationa   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.
    c                 C   s   |  |}|d | _t| jddg| _|d | _t| jddg| _|d | _t| jddg| _|d | _t| jddg| _| jd | _| jd | _| jd	 | _	| jd
 | _
| jd | j	  | _| jd | j
  | _d S )NP2r   r   P3Tr_velo2camR_rect)r   r   )r   r   )r   r   )r   r   )r   r   )r   r   )read_calib_filerO   r(   reshaperP   V2CR0Zc_uZc_vZf_uZf_vZb_xZb_y)r7   Zcalib_filepathZcalibsr   r   r
   r9      s    




zCalibration.__init__c           	      C   s  t |}| }W d    n1 s&0    Y  |d  ddd  }tj|tjd}|d  ddd  }tj|tjd}|d  ddd  }tj|tjd}|d  ddd  }tj|tjd}|dd|dd|dd|dddS )	Nr   r   r   dtyper   r   r   )rO   rP   rR   rQ   )rJ   	readlinesstripr   r(   r)   float32rT   )	r7   filepathfrK   objrO   rP   rV   ZTr_velo_to_camr   r   r
   rS      s    
&



zCalibration.read_calib_filec                 C   s(   t |t j|jd dft jdf}|S )zN
        :param pts: (N, 3 or 2)
        :return pts_hom: (N, 4 or 3)
        r   r   rW   )r(   hstackonesshaper[   )r7   Zpts_3dZpts_homr   r   r
   cart2hom   s    $zCalibration.cart2homN)rC   rD   rE   rF   r9   rS   rb   r   r   r   r
   rN   h   s    rN   ffffff?c                 C   s   | \}}d}|| }|| d|  d|  }t |d d| |  }|| d }d}	d||  }
d| | | }t |
d d|	 |  }|
| d }d| }d| ||  }|d | | }t |d d| |  }|| d }t|||S )Nr   r   r   )r(   sqrtmin)Zdet_sizeZmin_overlapr?   widtha1b1c1Zsq1r1a2b2c2Zsq2r2a3b3c3Zsq3r3r   r   r
   compute_radius   s"    rt   r   c                 C   s~   dd | D \}}t j| |d | |d f \}}t || ||   d| |  }d||t |jj|  k < |S )Nc                 S   s   g | ]}|d  d qS )      ?g       @r   )r   ssr   r   r
   r      r   zgaussian2D.<locals>.<listcomp>r   r   r   )r(   ogridexpfinforX   epsmax)ra   sigmamnyr	   r+   r   r   r
   
gaussian2D   s
    &$r   c                 C   s  d| d }t ||f|d d}t|d t|d  }}| jdd \}}	t||t|	| |d  }
}t||t|| |d  }}| || || ||
 || f }||| || ||
 || f }t|jdkrt|jdkrtj||| |d | S )Nr   r   r   )r|   r   )out)r   r!   ra   rf   r(   maximum)heatmapcenterradiuskZdiametergaussianr	   r   r?   rg   leftrighttopbottomZmasked_heatmapZmasked_gaussianr   r   r
   gen_hm_radius   s    $$ r   c                 C   s|  |d }|d }|d }|d }|d }|d }t | d d df |k| d d df |k@ | d d df |k@ | d d df |k@ | d d d	f |k@ | d d d	f |k@ }	| |	 } | d d d	f | | d d d	f< |d urt|d d df |k|d d df |k @ }
|d d d	f |k|d d d	f |k @ }|d d d
f |k|d d d
f |k @ }|
|@ |@ }|| }| |fS | S d S )NminXmaxXminYmaxYminZmaxZr   r   r   r   )r(   where)Zlidarboundarylabelsr   r   r   r   r   r   maskZlabel_xZlabel_yZlabel_zZ
mask_labelr   r   r
   get_filtered_lidar   s2    * 
(((r   c              
   C   s  | j dksJ tj| dd}ttj| d d dd df | d d d ddf  ddd}ttj| d d dddgf | d d dddgf  d dddttj| d d dddgf | d d dddgf  d ddd ttj| d d dddgf | d d d	ddgf  d ddd ttj| d d d
ddgf | d d dddgf  d ddd d }ttj| d d dddgf | d d dddgf  d dddttj| d d dddgf | d d dddgf  d ddd ttj| d d dddgf | d d dddgf  d ddd ttj| d d d	ddgf | d d d
ddgf  d ddd d }t| d d ddf | d d ddf  | d d ddf | d d ddf  t| d d ddf | d d ddf  | d d ddf | d d ddf   t| d d ddf | d d ddf  | d d ddf | d d ddf   t| d d ddf | d d ddf  | d d ddf | d d ddf   d d tjf d }tj|||||gdd	ddS )Nr   r   )axisr   r   T)r   keepdimsr   r   r   r   r:   )
ndimr(   meanabsre   sumarctan2newaxisconcatenaterT   )box3d_cornerxyzr+   r,   r-   yawr   r   r
   box3d_corners_to_center  sR    <@@@@@@@@&"&"&"&"	r   c                 C   s  | \}}}}}}}t | d | d |d |d | d | d |d |d g|d | d | d |d |d | d | d |d gdddd||||gg}t t |t | dgt |t |dgg dg}	t |	|}
|
t t |||gdj }| }|t j	S )Nr   r           )r   r   ru   )r   r   )
r(   r)   cossindottileT	transposeastyper[   )Zbox3d_centerr+   r,   r-   r	   r   zr   ZBoxZrotMatZvelo_boxZcornerPosInVelor   r   r   r
   box3d_center_to_conners%  s    >:r   __main__)`   i@  )r=   2   z#h: {}, w: {}, radius: {}, sigma: {}r   g      @)   r   )r   r   r         )ra   zmax_pos: {})rc   )r   )r   )N),rF   
__future__r   ossysnumpyr(   cv2pathdirnamerealpath__file__src_direndswithappendconfig.kitti_configkitti_configr;   objectr   rM   rN   rt   r   r   r   r   r   rC   zerosr   r+   r,   r   r{   r!   r@   formatimshowwaitKeyunravel_indexargmaxra   max_posr   r   r   r
   <module>   s@   


GR

	



 