
    RgA                        d dl Z d dlZd dlmc mZ d dlZd dlZd dl	Z	d dl
Zd dlmZ d dlmZ d dlmZ d dlmZ d dlmZ d dlmZ d dlmZ d dlmZmZ d dlZd d	lmZ d
 Zd Z d Z!ddZ"d Z#d Z$ddZ%d Z&ddZ' G d dejP                  j:                  jR                        Z*	 	 	 	 	 ddZ+ G d dejP                  j:                  jR                        Z,y)    N)glob)defaultdict)	set_trace)combinations)choice)
transforms)	rearrangerepeat)commonc           	          g }dD ]:  }|j                  t        t        j                  j	                  | |                   < |S )N)z*.pngz*.jpgz*.JPEGz*.JPG)extendr   ospathjoin)r   imgsexts      A/home/cameronsmith/repos/refactor_flowmap_and_splat/data/KITTI.py	glob_imgsr      s;    D4 3DdC0123K    c                 :    | s| S |D cg c]  }| |   	 c}S c c}w N )list	item_idcsis      r   pickr      s"    &'DG'''s   c                 ^    | ddd df   }| ddddf   }| ddddf   }| ddddf   }||||fS )N.r            r   )
intrinsicsfxfycxcys        r   parse_intrinsicsr&   $   s`    	CBQBJ	B	CAaCK	 B	CAaCK	 B	CAaCK	 Br2r>r   c                     t        j                  | t        j                  | j                  |      d   j	                  |            f|      S )Nr   )torchcat	ones_likeunbind	unsqueeze)xr   s     r   <lambda>r.   ,   s7    eiiEOOAHHQKN4L4LQ4O$P QSTU r   c                     t        | d      S )Nz... c x y -> ... (x y) c)r	   r-   s    r   r.   r.   -   s    9Q9: r   c                     t        | j                        t        |j                        k(  r| S t        t        |j                        t        | j                        z
        D ]  }| j                  d      }  | S )N)lenshaperanger,   )r-   yr   s      r   	expand_asr7   /   s\    
177|s177|#3qww<#agg,./ KKO Hr   c                 d   t        |      \  }}}}| t        ||       z
  t        ||       z  |z  }	|t        ||      z
  t        ||      z  |z  }
|rGt        j                  |	|
|t        j                  |      j                  | j                        fd      S t        j                  |	|
|fd      S )z

    :param self:
    :param x: Shape (batch_size, num_points)
    :param y:
    :param z:
    :param intrinsics:
    :return:
    r2   dim)r&   r7   r(   stackr*   todevice)r-   r6   zr!   homogeneousr"   r#   r$   r%   x_lifty_lifts              r   liftrB   9   s     &j1NBB)B""iA&66:F)B""iA&66:F{{FFAuq/A/D/DQXX/NOUWXX{{FFA.B77r   c                     |j                   ^}}| d   }| d   }|}t        ||||d      }	t        j                  d||	      dd df   }
|
S )N).r   ).r   T)r!   r?   zb...ij,b...kj->b...ki.r    )r4   rB   r(   einsum)xydepth	cam2worldr!   
batch_size_x_camy_camz_campixel_points_camworld_coordss              r   world_from_xy_depthrO   N   sj    __NJvJEvJEEue
 << 7DTURaRL r   c                     t        j                  | j                  d d       j                  | j                        }t        | |||      }|dd ddf   }||dd d d f   z
  }|rt        j                  |d      }|S )Nr2   )r!   rG   .r    r9   )r(   onesr4   r<   r=   rO   F	normalize)rE   rG   r!   rS   rL   pixel_pointscam_posray_dirss           r   get_ray_directionsrW   _   s    JJrxx}%((3E&
EjIL RaR
#Ggc4l33H;;xR0Or   c                   <    e Zd ZdZ	 	 	 	 	 	 	 	 ddZd Zd ZddZy)	SceneInstanceDatasetzQThis creates a dataset class for a single object instance (such as a single car).Nc           	         || _         || _        || _        || _        i | _        |
| _        |	| _        t        j                  j                  |d      }t        j                  j                  |d      }dd l
}| j                  j                  d      j                  d      d   j                  d      d   }| j                  j                  d      j                  d      d   }|j                  dj                  | j                  j                  d      j                  d      d d       ||      | _        t!        t        j"                  t        j                  j                  | j                  | j                  d                  | _        t'        t)        t        j                  j                  | j                  d	d
                  | _        t'        t-        |            | _        t'        t)        t        j                  j                  |d                  | _        t        j                  j3                  t        j                  j5                  | j                              | _        |7t9        | j.                  |      | _        t9        | j0                  |      | _        y |ht;        j<                  dt!        | j.                        |dt>              }t9        | j.                  |      | _        t9        | j0                  |      | _        y y )Nposeimager   /r2   rI   zimage_02/datadispsz*.npyz*.txtF)stopnumendpointdtype) instance_idximg_sidelengthinput_img_sidelengthinstance_dircache
res_factorlow_resr   r   r   pykittistripsplitrawrstrip	kitti_rawr3   listdirnum_imgsortedr   r_   r   color_paths
pose_pathsbasenamedirnameinstance_namer   nplinspaceint)selfrd   rg   specific_observation_idcsrf   re   
num_imagesrh   raftrj   ri   pose_dir	color_dirrk   drivedateidcss                    r   __init__zSceneInstanceDataset.__init__o   sK    ),$8!(
"77<<f5GGLLw7	!!'',2237;AA#FrJ  &&s+11#6r: HHT&&--c288=crBCT5
 JJT..0A0A?S
 Dd.?.?'!RST
!)I"67 bggll8W&E!FGWW--bggood>O>O.PQ$0#D$4$46OPD"4??4MNDO#;;D,,-:UXD  $D$4$4d;D"4??D9DO $r   c                     || _         y)zXFor multi-resolution training: Updates the image sidelength with whichimages are loaded.N)re   )r|   new_img_sidelengths     r   set_img_sidelengthz'SceneInstanceDataset.set_img_sidelength   s
    0r   c                     | j                   S r   )rr   r|   s    r   __len__zSceneInstanceDataset.__len__   s    ||r   c           
      t    t        j                         | j                  j                  |            dz  dz
  }|j	                  d      dz  r|d d dd f   }|j	                  d      dz  r|d d d d dd f   }t        j                  t        j                  t        j                  j                  | j                  d| j                  |                     }| j                  j                  j                  d   | j                  j                  j                   z  |j#                         z  }|j	                  d      |j	                  d      z
  dz  }|j	                  d      |j	                  d      z
  dz  }|| || |f   }t        j                  | j                  j                  j                  j%                               }	t        j                  | j                  j                  j&                        j)                         }
t        j                  | j                  j*                  |   j,                        }||
z  j/                         }t        j0                  d|j	                  d      d|j	                  d      f   j3                  t.              j5                  ddd      }t        j                  t        j6                  |d      j%                               j9                         }|j:                  dd  \  }}|t        j<                  |dz
  |dz
  g      z  }t        j>                  |	d   |z  |	d   |z  |	d   f      }	tA        g |jC                  dd      d    jE                  d      |jC                  dd      d    |	d    d	d
i}|dd df   jG                  dd
      jI                         jK                  d||f      j/                         }| jL                  t        jN                  | jP                  g      jI                         j9                         |||	j/                         d}|S )Nr   r   r_   )r   r   r^   r2   r   )axisr?   T.r    )r:   keepdim)rx   rd   c2wrgbr!   ))r   ToTensorrp   get_cam2sizer(   
from_numpyry   loadr   r   r   rg   r_   calibK_cam2b_rgbabscopy
T_cam2_imuinverseoxtsT_w_imufloatmgridastype	transposefliplongr4   tensorr;   rB   flattenr+   normsqueeze	unflattenrx   Tensorrd   )r|   idxcontextinput_contextr   dispzvalydiffxdiffKcam2imu	imu2worldrG   uvhwcamcrdrF   samples                      r   __getitem__z SceneInstanceDataset.__getitem__   s    $j!!#DNN$;$;C$@AAEI88A;q=S12Y88A;q=S1QR[d6G6GRVR\R\]`Ra)b!cd%%,,T2T^^5I5I5O5OORVRZRZR\\"		"-!3"		"-!3UF5L5&,./T^^1188==?@""4>>#7#7#B#BCKKM$$T^^%8%8%=%E%EF	(//1	XXa#((1+oq388A;67>>uEOOPQSTVWXbggbr2779:??Ayy~1%,,!QqSz** KK11Q4!8QqT23p

1a(.55b9p4<<1;Md;SpUVW[U\pjnp #bqb&>((R(>GGITTUVYZ\]X^_ffh!//!LL$*;*;)<=EEGLLN'')
 r   )NNNNNN@      r   )FT)__name__
__module____qualname____doc__r   r   r   r   r   r   r   rY   rY   l   s4    [ #'!5:n1'r   rY   c                    t        t        t        j                  j	                  | d                  }t        |      dk7  s
J d|         |d k7  r|d | }t        |      D cg c]  \  }}t        ||||||       }	}}|	S c c}}w )Nz*/r   zNo objects in the directory )rd   rg   r}   re   rh   r~   )rs   r   r   r   r   r3   	enumeraterY   )
rootmax_num_instancesr}   rh   sidelenmax_observations_per_instanceinstance_dirsr   dirall_instancess
             r   get_instance_datasetsr      s     4T4 89:M}"I&B4&$IID %&8'89 "-0
 C 	&?"4	

M 
 
s   #Bc                   b    e Zd ZdZ	 	 	 	 	 	 	 	 	 	 	 	 	 	 	 	 	 	 d
dZd Zd Zd Zd Zd Z	dd	Z
y)KittiDatasetzODataset for a class of objects, where each datapoint is a SceneInstanceDataset.Nc                 `   || _         d }t        j                  d   }t        t	        d t        j
                  |                  }g }|D ]  }t        t	        d t        j
                  t        j                  j                  ||                        }||D cg c]@  }t        j                  j                  t        j                  j                  |||            B c}z  } t        |      | _
        t        |      t        g       k(  r|d   }|dz   | _        || _        || _        || _        || _        || _        i | _        |
| _        || _        || _        || _        t/        d| dt1        | j                         d       t1        | j                        dk7  sJ d	       || _        |dk(  r/| j                  D cg c]	  }d
|v s| c}| _
        t/        d       t5        | j                        D cg c]  \  }}t7        |||	|||||       c}}| _        | j8                  D cg c]  }t1        |      dkD  s| c}| _        || j8                  d | | _        t        t;        t1        | j8                                    d d d   }t5        | j8                        D cg c]  \  }}||v r|
s||vs|
r| c}}| _        t/        d|
t1        | j8                               | j8                  D cg c]  }t1        |       c}| _        t1        | j8                        | _        g | _         t5        | j8                        D ]D  \  }}t;        t1        |      ||dz   z  z
        D ]  } | j@                  jC                  || f       ! F y c c}w c c}w c c}}w c c}w c c}}w c c}w )N
KITTI_ROOTc                     d| v xr d| vS )N20zipr   r0   s    r   r.   z'KittiDataset.__init__.<locals>.<lambda>  s    TQY95> r   c                 
    d| vS )Ntxtr   r0   s    r   r.   z'KittiDataset.__init__.<locals>.<lambda>  s
    e1n r   r   r   z	Root dir z, z
 instancesz No objects in the data directory2011_09_26_drive_0027_syncznote testing single dir)rd   rg   r}   re   rf   r~   rh   rj   (      zvalidation: )"ri   r   environr   filterrq   r   r   abspathrs   r   typen_skipnum_contextnum_trgtquery_sparsityre   vary_context_numberrh   testtest_context_idcscontext_is_lastcontext_is_firstprintr3   r   r   rY   r   r5   num_per_instance_observationsnum_instancesinstance_img_pairsappend)!r|   r   r   r   r   re   rf   r   r   r}   valr   r   r   rh   videorj   r   ri   root_dirbasedirsdrive_pathsbasedirdirsdir_r-   r   r   	test_idcsr   objrg   js!                                    r   r   zKittiDataset.__init__   s   , # ::l+92::h;OP
  		G,JJrww||Hg>?D SWKOXw EF K		 $K0<b!&Qh& ,,#6 
	!2. 0	(2c$*<*<&=%>jIJ4%%&!+O-OO!2!  --"1MQR1R"D +," &d&8&89
 S !  *C-%98	
 *.););KAs1v{aK(!%!3!34F5F!GDs4#5#56781=	+4T5G5G+H  JCAaQR[^`cijr{i{  EHa  JnST%7%7!89
 CGBTBT-U3c#h-U* !3!34"$'(:(:; 	6NAl3|,VXaZ-@@A 6''..!u56	64"
 L
 J .Vs=   AN?	N	N8N,N  N 'N%:N%=N%8N+c                     i }||S t         j                  j                  | j                  dz  |d      }dD ]  }||   |   ||<    |j	                         D ]  \  }}|dvs||   ||<    |S )Nr   F)r   replace)r   r   )ry   randomr   re   items)r|   dictsparsitynew_dict	rand_idcskeyvs          r   sparsifyzKittiDataset.sparsify_  s    K 		((##q(x ) I % 5 $S	) 45 **, .Qm+$(IHSM. Or   c                 V    || _         | j                  D ]  }|j                  |        y)zYFor multi-resolution training: Updates the image sidelength with which images are loaded.N)re   r   r   )r|   r   instances      r   r   zKittiDataset.set_img_sidelengthq  s.    0** 	<H''(:;	<r   c                 ,    t        | j                        S r   )r3   r   r   s    r   r   zKittiDataset.__len__w  s    4**++r   c                     | j                   rGd}|dk\  r|| j                  |   z  }|dz  }|dk\  r|dz
  t        || j                  |dz
     z         fS t        j                  j                  | j                        dfS )Nr   r   )r   r   r{   ry   r   randintr   )r|   r   obj_idxs      r   get_instance_idxzKittiDataset.get_instance_idxz  s    99G(t99'BB1 ( !C$<<Wq[IIJ 
 99$$T%7%78!;;r   c                 H    | j                   |   j                  |dd      |fS )NTr   r   )r   r   )r|   r  r-   s      r   getframezKittiDataset.getframe  s6    w'334t 4  	
 	
r   c           	      
   g }g }g }d}| j                   |   \  }}|rd}|
|d   |d   }	}t        | j                  |         || j                  | j                  z  z   k  rd}||d   }t        | j                        D ]  }
|| j                  dkD  rt        |       || j                  z  }| j                  |   j                  |dd      }|j                  |       t        j                  dg      |d   d<   | j                  || j                        }|j                  |        d	 } ||      }|d
   j                  d      |d
   j                  d      }}||z  }||d<   t        j                  |dddddgddg      S )Nr   r      Tr  g      ?r2   maskc                     | d   j                         }t        t              }| D ]   }|D ]  }||   j                  ||           " |D ]  }t	        j
                  ||   d      ||<   ! |S #  Y (xY w)Nr   r9   )keysr   r   r   r(   r;   )
batch_listr  resultentryr   s        r   
collate_fnz,KittiDataset.__getitem__.<locals>.collate_fn  s    a=%%'D &F# @?C!3!3E#J!??@ !#(;;vc{#BVC[! M !s   A..A2r   r^   	org_ratiog
@g      @`   i      i  )hires_factorbudgetrj   hi_res)r   r3   r   r   r   r5   r   r   r   r(   r   r   r   r   r   make_sample)r|   r   sceneidxr   trgt
post_inputofr  r   det_idxrI   r   
sub_sampler  r   r   r  s                    r   r   zKittiDataset.__getitem__  s    
,,S1
w'{HQKWG& t!!'*+Qt}}T[[/H-HHAqkAt}}% 
	$A#t}}R'7aA''0<<4t = F f%%*\\3%%8JrN6"vt/B/BCJKK
#
	$	 $ KR e!1!1"!5!A#	#[
 !!$wA^]_`c\dmpqtluvvr   )r   r   FNNNNNNFNFFNTr   r   r   r   )r   r   r   r   r   r   r   r   r  r	  r   r   r   r   r   r      sc    Y !!&*"&'g6R$<,<
J0r   r   )r2   )F)T)NNNNN)-r   multiprocessingmptorch.nn.functionalnn
functionalrR   r(   r   imageionumpyry   r   collectionsr   pdbr   	itertoolsr   r   matplotlib.pyplotpyplotplttorchvisionr   einopsr	   r
   sysdatar   r   r   r&   homch_secr7   rB   rO   rW   utilsDatasetrY   r   r   r   r   r   <module>r5     s    	         #   "   " $ 
 ( V	:8*"
h5;;++33 hZ "
"&8f05;;##++ f0r   