
    h0              	       n   d dl m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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jN                  g       Z(d dl)m*Z*m+Z+ d Z,d)dZ-d dl.Z/ ed      Z0d*dZ1 G d dejd                  jf                  jh                        Z5 G d dejd                  jf                  jh                        Z6d dl7Z8d dl9Z9d dl:m:Z: dxZ;Z<d Z-dZ=e;e<z  Z>d\  Z?Z@ e9j                  e;e<e=      \  ZBZCZDZEZF ej                  g dg dg dg dg      j                  dd      ZI e8j                  e=e;e<z  e?e@      ZK ej                  d      ZMd+dZNd dl7Z8d dlOZOd dlZ	d dl:m:Z: d  ZPeQd!k(  rd"ZRd#ZSd$ZTej                  j                  eT      s ej                  eT        e	j                         ZYg ZZ e[eR      D ]<  Z\ e	j                  ePe\eReTeSeYf%      Z^e^j                          eZj                  e^       >  e:eS&      5 Za e[eS      D ]#  ZbeYj                          eaj                  d'       % 	 ddd       eZD ]  Z^e^j                           yy# 1 sw Y   "xY w),    )	make_gridN)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       1/home/cameronsmith/repos/canon_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<   | d   d d d df   j#                         dz  | d<   | d   d d d df   j#                         dz  | d<   | j7                         D ]  \  }}d|vs |j#                         | |<    | d   j9                         | d<   | j7                         D ci c]_  \  }}||j;                  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?targ_img      )r   r   	canon_imgseg?   )   r7   )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boolsizedtype)data_use_rand_bguse_color_augr&   	transformrand_bgkvs           r   format_render_sampler[   3   s$   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(8(8%PZJ[\_J_IcIcdidodoIpqrIsI{I{|}~  AB  JC  JI  JI  JK  :L  MT  :U  )V  )\  )\  )^  )f  )f  gh  ij  kl  )m  nr  )s  tw  )weJ' jjx01G
7==1
+BHHJ6q}}UEUEUV]E^EfEfghijklEmnrEsEyEyE{|E  AF  GQ  AR  AX  AX  Y[  Y\  A]  8^  `a  bg  hl  bm  bs  bs  bu  `u  8v  vjJ'"1"-335c9E*[)!BQB$/557;E+{{} 2!>	uQx2-$$&E&Mfkfqfqfst_b_`abA166":b=ammAGGIi0366qww?VWXYVZZttts   A$M-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pathrb   s      r   __init__zPyBulletFolder.__init__I   sF    d7l+!1!1$%!8 "..u5Tr   c                      yNi  rd   s    r   __len__zPyBulletFolder.__len__O       ir   c                     t        j                  t        j                  | j                  d d              }t        |      S )N)r   loadrJ   r   ra   r[   )rd   idxrT   s      r   __getitem__zPyBulletFolder.__getitem__P   s1    jjt'7'7':;< $E**r   N.F__name__
__module____qualname____doc__rf   rk   rp   ri   r   r   r]   r]   F   s    Y6 (	+r   r]   c                   $    e Zd ZdZddZd Zd Zy)PointCloudFolderr^   c                     t        |dz         | _        |r| j                  dd  | _        y | j                  d d | _        y )Nz/*target.pngr`   )r   ra   rc   s      r   rf   zPointCloudFolder.__init__]   s@    d>12!1!1$%!8 "..u5Tr   c                      yrh   ri   rj   s    r   rk   zPointCloudFolder.__len__b   rl   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(   r1   r   r$   r0   )cam_pointmapr   obj_transfsr#   r/   )rJ   r   ra   nprn   replacer   rB   arrayr
   openrE   rG   )	rd   ro   re   r~   r#   r   r   
img_targetdatass	            r   rp   zPointCloudFolder.__getitem__c   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   Nrq   rs   ri   r   r   ry   ry   Z   s    Y6
 (r   ry   )tqdmr7   c                 x    t        j                  | t        j                  | j                  d   df      gd      S )Nr   r$   )axis)r   concatenateonesrM   r   s    r   r   r   y   s+    277AGGAJ?#;<1E r   <   )r)   g      @)r$   r   r   r   )r   r5   r   r   )r   r   r5   r   )r   r   r   r$      zlink_imgs.ptc                 
   |Wd}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*        t,        z  t*        t*        t,        z
  |
z  z
  z  t.        t0        t2        t4              }t        j6                  |      \  }}t        j8                  |      }i }t;        t<        t?        |            D ]  \  }}tA        |       t        jB                  dt        jD                  jG                  ||         t        jD                  jG                  |      z  tH        z  tK        |d	               d d d df   |d
<   d|d
   |d    <    tM        t<        D cg c]+  }tO        jP                  |d         jS                  ddd      - c}dd      d d }tM        t<        D cg c]E  }tO        jP                  |d	         jU                  dt        t        f      jS                  ddd      G c}dd      }tM        t<        D cg c]E  }tO        jP                  |d
         jU                  dt        t        f      jS                  ddd      G c}dd      }tM        tW        t<              D cg c]@  \  }}tO        jP                  |d         jU                  dt        t        f      d    |dz   z  B c}}dd      d   }tO        jX                  t        t        t<                    D cg c]  }||k(  	 c}      }i }|d    |d<   |d    |d<   tO        jP                  |	      jS                  ddd      d    |d<   tO        jP                  |      d    |d<   |d    |d	<   |d    |d
<   tO        jX                  t?        |      D cg c]  }tO        jP                  ||          c}      d    |d<   tO        jP                  t        j                  t	        jZ                  |t]        t        t	        j^                  |                        D cg c]  }|d   	 c}            d    |d<   |S c c}w c c}w c c}w c c}}w c c}w c c}w c c}w )Ng      ?r   rN   )order)widthheight
viewMatrixprojectionMatrixflagsr(   g     o@z	ij,nj->ni
points_campoints_linkr   r&   rgbr1   r$   )nrowpaddingr#   r3   r/   view_matrix_curr_link_transformsjoint_states)7geometrysample_camera_positionsr   r   r'   computeViewMatrixreshaperandomly_move_jointsrangerL   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zip	link_imgssortedprinteinsumlinalginvTchomr   r   rB   rE   	unflatten	enumeratestackgetJointStateslistgetNumJointsappendpointmap_imgcam2world_cv
curr_linksr#   tensorrP   )r   r   rob_idradiuscamera_positionlook_at_positionrolljoint_i_r   depthr4   
target_rgbr   r   visual_linkspointmap_link_imgsegmaskslink_imglink_idxcomp_img_rgbcomp_img_points_camcomp_img_points_linklink_icomp_segr!   r   rY   r   rZ   s                                 r   format_pybullet_renderr      s    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 6,+?@ 3h"$))KG[\dGe9fikiririviv  xD  jE  :E  HJ  :J  LO  PX  Ye  Pf  Lg  #h  ij  ln  mn  ln  in  #o12$0	3 ]fgQYE,,Xe_=EEa!Lgmnwxyz|{|}L!  HQ  #R  |D5#3#3H\4J#K#U#UVWY_`eXf#g#o#opqrstu#v  #R  XY  bc  d"  JS  $T  ~FE$4$4Xm5L$M$W$WXY[abgZh$i$q$qrstuvw$x  $T  Z[  de  f  ~G  HQ  ~R  Sjyjpqy(($8BB1fU^TUYZ\bcd\de  S  YZ  cd  e  fg  hH[[uS^/DE!(A+EFH
E4.E&M#D)E+#..s3;;AaB4HE***<8>E.+D1E,-d3E-"'++bhiubv.w]^u/?/?@TUV@W/X.w"xy}"~E
 ! ++BHHADTDTU[]abghihvhvw}h~b  ^A  EB  6Cqad  6C  -D  E  FJ  KE.L h #R $T SE /x 6Cs-    0U
A
U+A
UAU
U%*U*1U/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/twoview_justjoints/)r}   args)totalr$   )r)   )r5   )FF)NNr   )ftorchvision.utilsr   matplotlib.pyplotpyplotrH   cv2os
statisticsmultiprocessingmptorch.nn.functionalnn
functionalrN   r   rJ   imageiorF   r   r   collectionsr   pdbr   	itertoolsr   r   
imageio.v3v3iiotorchvisionr	   sysgzipjsontorchvision.transformsPILr
   r   r:   augmentationeinopsr   r   ch_secr   albumentationsr9   rK   r[   utilsr   Datasetr]   ry   pybulletr'   r   r   r   r   fovaspectr   r   get_camera_intrinsicsr   r   r   r   camera_matrixr   r   r   computeProjectionMatrixFOVr   rn   r   r   r   r   rt   	n_workersn_scenesr   re   existsmakedirsQueuer   procsr   widProcessp2startr   pbarr   getupdatejoinri   r   r   <module>r'     s   '  
 	          #   "    " 
  	    +  "z!!$' $	:U 	J	Ku&+U[[%%-- +(u{{''// 6    fE		c > > >uVC P BBRXX 02BOTU]]^_`ab+q++CvtSI
%**^
$	B^     z
 IHNG77>>'"KBKK$8 RXXZN EY RZZy'8^D
 	
R 
H	 x 	A KKN	  
	= 0 s   /J++J4