
     h-              	       >   d dl mZ d dlZd dlZ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 d dlZd dl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	Z$ ejJ                  g       Z&d d
l'm(Z(m)Z) d Z*d'dZ+d dl,Z- ed      Z.d(dZ/ G d dej`                  jb                  jd                        Z3 G d dej`                  jb                  jd                        Z4d dl5Z6d dl7Z7d dl8m8Z8 dxZ9Z:d Z+dZ;e9e:z  Z<d\  Z=Z> e7j~                  e9e:e;      \  Z@ZAZBZCZD ej                  g dg dg dg dg      j                  dd      ZG e6j                  e;e9e:z  e=e>      ZId)dZJd dl5Z6d dlKZKd dlZd dl8m8Z8 d ZLeMdk(  rd ZNd!ZOd"ZPej                  j                  eP      s ej                  eP        ej                         ZUg ZV eWeN      D ]<  ZX ej                  eLeXeNePeOeUf#      ZZeZj                          eVj                  eZ       >  e8eO$      5 Z] eWeO      D ]#  Z^eUj                          e]j                  d%       % 	 ddd       eVD ]  ZZeZj                           yy# 1 sw Y   "xY w)*    N)glob)defaultdict)	set_trace)combinations)choice)
transforms)Imagec                 n    t        j                  |       |z  }| |z   }t        j                  |dd      }|S )Ng        g      ?)torch
randn_likeclamp)imagenoise_factornoisenoisy_images       +/home/cameronsmith/repos/controll3r/data.pyadd_noise_tensorr   #   s8    U#l2E%-K++k34K    )	rearrangerepeatc                     t        | d      S )Nz... c x y -> ... (x y) c)r   xs    r   <lambda>r   -   s    9Q9: r   c                     t        j                  | t        j                  | j                  |      d   j	                  |            f|      S )Nr   )r   cat	ones_likeunbind	unsqueeze)r   is     r   r   r   .   s7    eiiEOOAHHQKN4L4LQ4O$P QSTU r   z:/data/cameron/pytorch_vision_datasets/data_large/*/*/*.jpgc                    | d   j                  d      d   d d d f   x| d<   }t        j                  | | d   fd      | d<   t        j                  t        j
                  d      t        j                  d      t        j                  dd	
      t        j                  dd	
      t        j                  d      t        j                  dddd      t        j                  d      t        j                  d      g      }|rt        j                   || d   dz  j                  t        j                        d   j                  ddd      j!                               d         j#                         j                  ddd      d    dz  | d<   t%        j&                  t)        j*                  t,                    }t/        |j0                        dk(  r|r| d   |j#                         z  t3        j4                  t        j                  |      j                  ddd      d    j#                         dz  | d   j0                  dd        d| d   j#                         z
  z  z   | d<   | j7                         D ci c]_  \  }}||j9                  d      dkD  r@t3        j4                  |j#                         d      d   j                  |j:                        n|d   a c}}S c c}}w )Nsegs   )dimr   fgg?)p   皙?)
blur_limitr&   g333333?F   g?)r_shift_limitg_shift_limitb_shift_limitr&   g?img      )r   r   ?   )   r4   )maxr   r   AComposeRandomBrightnessContrast
MotionBlur
MedianBlurBlurHueSaturationValueRGBShiftChannelShuffle
from_numpytouint8permutenumpyfloatpltimreadrandomr   bg_pathslenshapeFinterpolateitemssizedtype)data_use_rand_bguse_color_augr%   	transformrand_bgkvs           r   format_render_samplerW   2   sr   V}((Q(/21T6::E$K"))bSv/2E&M 		155<Q\\C=PQRQ]Q]ijnqQrstsysy  FG  KN  tO  QR  Qe  Qe  hk  Ql  mn  mw  mw  FH  XZ  jl  ps  mt  uv  u{  u{  ~@  uA  BC  BR  BR  UW  BX  Y  ZI5#3#3IU5\RUEUDYDYZ_ZeZeDfghDiDqDqrstuvwDxD~D~  EA  5B  CJ  5K  $L  $R  $R  $T  $\  $\  ]^  _`  ab  $c  dh  $i  jm  $meEl jjx01G
7==15\"((*,Q]]5;K;KG;T;\;\]^_`ab;cdh;i;o;o;qru;uv{  }B  wC  wI  wI  JL  JM  wN  .O  QR  SX  Y]  S^  Sd  Sd  Sf  Qf  .g  gefkfqfqfst_b_`abA166":b=ammAGGIi0366qww?VWXYVZZttts   <A$K$c                   $    e Zd ZdZddZd Zd Zy)PyBulletFolderODataset for a class of objects, where each datapoint is a SceneInstanceDataset.c                     t        |dz         | _        || _        |r| j                  dd  | _        y | j                  d d | _        y )Nz/*.pt)r   scene_pathsvalselfpathr^   s      r   __init__zPyBulletFolder.__init__C   sF    d7l+!1!1$%!8 "..u5Tr   c                      yNi  r`   s    r   __len__zPyBulletFolder.__len__I       ir   c                     t        j                  t        j                  | j                  d d              }t        |dd      S )NT)rQ   rR   )r   loadrG   r   r]   rW   )r`   idxrP   s      r   __getitem__zPyBulletFolder.__getitem__J   s5    jjt'7'7':;< $EdNNr   N.F__name__
__module____qualname____doc__rb   rg   rl   re   r   r   rY   rY   @   s    Y6 (	Or   rY   c                   $    e Zd ZdZddZd Zd Zy)PointCloudFolderrZ   c                     t        |dz         | _        |r| j                  dd  | _        y | j                  d d | _        y )Nz/*target.pngr\   )r   r]   r_   s      r   rb   zPointCloudFolder.__init__W   s@    d>12!1!1$%!8 "..u5Tr   c                      yrd   re   rf   s    r   rg   zPointCloudFolder.__len__\   rh   r   c                    t        j                  | j                  d d        }t        j                  |j                  dd      j                  dd            }t        j                  |j                  dd      j                  dd            }t        j                  |j                  dd      j                  dd            }t        j                  |j                  dd      j                  dd            }t        j                  t        j                  t        j                  |            dd d	f         j                  d
dd      d    j                         dz  }t        j                  |      j                  d
dd      t        j                  |      j                  d
dd      t        j                  |      t        j                  |      |d}|S )Ntargettransfsz.pngz.npyr"   camera_pointmapobj_pointmap.r'   r0   r   r#   r/   )cam_pointmapr|   obj_transfsr"   r.   )rG   r   r]   nprj   replacer   r?   arrayr	   openrB   rD   )	r`   rk   ra   rz   r"   r|   r}   
img_targetdatass	            r   rl   zPointCloudFolder.__getitem__]   su   ]]4++A./Xi8@@OPWWT\\(62::6&IJWWT\\(3DEMMfU[\]WWT\\(>BJJ6RXYZ%%bhhuzz$/?&A#bqb&&IJRRSTUVWXYZ^_eeghkk
 !& 0 0 > F Fq1 M % 0 0 > F Fq1 M"--g6&&t,# r   Nrm   ro   re   r   r   ru   ru   T   s    Y6
 (r   ru   )tqdmr4   c                 x    t        j                  | t        j                  | j                  d   df      gd      S )Nr   r#   )axis)r   concatenateonesrJ   r   s    r   r   r   s   s+    277AGGAJ?#;<1E r   <   )r(   g      @)r#   r   r   r   )r   r2   r   r   )r   r   r2   r   )r   r   r   r#      c                 >	   |ut         j                  j                  dd      }t        j                  |      \  }}}t        j
                  t        j                  |||            j                  ddd      }| t        j                  |       n3t        t        |             D ]  }t        j                  ||| |           t        j                  t        t        |j!                  d      j#                         t$        t        j&                        \  }}}	}
}t        j                  |	t        t        df      d d d d d df   j)                  t         j*                        d	z  }t        j,                  t.        t0        z  t.        t.        t0        z
  |
z  z
  z  t2        t4        t6        t8              }t        j:                  |      \  }}t        j<                  |      }i }|D ]  }|d
z	  |dz   k(  j                  d      x||<   }t        j>                  dt         j@                  jC                  ||         t         j@                  jC                  |      z  tD        z  tG        ||               d d d df   ||<    i }tI        jJ                  tM        |jO                               D cg c]2  }tI        jP                  ||         j                  t        t              4 c}      d    |d<   tI        jP                  |      jS                  ddd      d    |d<   tI        jP                  |      d    |d<   tI        jP                  |j                  t        t        d            jS                  ddd      d    |d<   tI        jP                  |j                  t        t        d            jS                  ddd      d    |d<   tI        jJ                  tM        |jO                               D cg c]  }tI        jP                  ||          c}      d    |d<   tI        jP                  t        j
                  t        jT                  |tW        t        t        jX                  |                        D cg c]  }|d   	 c}            d    |d<   |S c c}w c c}w c c}w )Nr   r#   r0   r   rK   )order)widthheight
viewMatrixprojectionMatrixflagsr'   g     o@   r2   z	ij,nj->nir"   r.   view_matrix_
points_campoints_linkcurr_link_transformsjoint_states)4r   rG   uniformgeometrysample_camera_positionsr   r&   computeViewMatrixreshaperandomly_move_jointsrangerI   resetJointStategetCameraImager   r   flattentolistproj_matrix_)ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEXastypefloat32depth_to_point_cloudfarnearfxfycxcyget_all_link_transforms
zeros_likeeinsumlinalginvTchomr   stacksortedkeysr?   rB   getJointStateslistgetNumJointsappendpointmap_imgcam2world_cv
curr_linksr"   tensorrM   )r   r   rob_idradiuscamera_positionlook_at_positionrolljoint_i_rgbdepthseg
target_rgbr   r   visual_linkspointmap_link_imgsegmaskslink_idxmaskr   rU   r   rV   s                           r   format_pybullet_renderr   z   ss    !yy00A62:2R2RSY2Z/)4xx 3 3OEUW[ \]eefghiqteu %%f-S./jG1B1B67T`ahTi1jj ,,5T`ThThorThTsTzTzT|  P\  cd  cN  cN  OAq#ucC&%!34Q2A2X>EEbjjQTYYJ..d
scDjTYEY?Y/Z\^`bdfhjkJ)1)I)I&)Q&, j1H  i%(2X8A:$>#G#G#KKD"$))KG[\dGe9fikiririviv  xD  jE  :E  HJ  :J  LO  PZ  [_  P`  La  #b  cd  fh  gh  fh  ch  #i$i
 E++\bckcpcpcr\stWXu//<DDVERtuvz{E&M!!*-55a!<TBE%L**<8>E.((););F5)KLTTUVWXYZ[\`aE,))*;*C*CF5QR*ST\\]^_`abcdhiE-"'++bhiqivivixby.z]^u/?/?@TUV@W/X.z"{  }A  #BE
 ! ++BHHADTDTU[]abghihvhvw}h~b  ^A  EB  6Cqad  6C  -D  E  FJ  KE.L u
 /{ 6Cs   +7R*R1Rc                    t        j                  t         j                         t        j                  ddd       t        j                  t        j                                t        j                  d      }t        | ||      D ]=  }t        d d |      }t        j                  || d|dd       |j                  d       ? y )Nr   g#zfranka_panda/panda.urdf/05dz.ptr#   )r&   connectDIRECT
setGravitysetAdditionalSearchPathpybullet_datagetDataPathloadURDFr   r   r   saveput)	worker_idnum_workersdatadir
num_scenesprogress_queuer   r    datas           r   workerr      s    IIahhQ40m779:ZZ12F9j+6 %dD&9

4G9AaWC011r   __main__   i'  z</data/cameron/robot_calibration_testing/fulldiversecam_redo/)ry   args)totalr#   )r(   )r2   )FF)NNr   )bmatplotlib.pyplotpyplotrE   cv2os
statisticsmultiprocessingmptorch.nn.functionalnn
functionalrK   r   rG   imageiorC   r   r   collectionsr   pdbr   	itertoolsr   r   
imageio.v3v3iiotorchvisionr   sysgzipjsontorchvision.transformsPILr	   r   r7   augmentationeinopsr   r   ch_secr   albumentationsr6   rH   rW   utilsr   DatasetrY   ru   pybulletr&   r   r   r   r   fovaspectr   r   get_camera_intrinsicsr   r   r   r   camera_matrixr   r   r   computeProjectionMatrixFOVr   r   r   r   rp   	n_workersn_scenesr   ra   existsmakedirsQueuer   procsr   widProcessp2startr   pbarr   getupdatejoinre   r   r   <module>r     s     
 	          #   "    " 
  	    +  "z!!$' $	:U 	J	KuOU[[%%-- O(u{{''// 6    fE		c > > >uVC P BBRXX 02BOTU]]^_`ab+q++CvtSI8J     z
 IHOG77>>'"KBKK$8 RXXZN EY RZZy'8^D
 	
R 
H	 x 	A KKN	  
	= 0 s   /JJ