
    =+h#              
       4   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.g dZ/d*dZ0 G d dejb                  jd                  jf                        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(  rdZNd ZOg dZ/e/d!   ZPd"ePj                  d#      d$   z   ZRej                  j                  eR      s ej                  eR        ej                         ZWg ZX eYeN      D ]=  ZZ ej                  eLeZeNeReOeWePf%      Z\e\j                          eXj                  e\       ?  e8eO&      5 Z_ eYeO      D ]#  Z`eWj                          e_j                  d'       % 	 ddd       eXD ]  Z\e\j                           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/kpt3r/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/*/*/*.jpg)zfranka_panda/panda.urdfzkuka_iiwa/model.urdfzxarm/xarm6_robot.urdfc                    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+        |j,                        dk(  r|r| d   t.        j                         z  t1        j2                  t        j                  |      j                  ddd      d    j                         dz  | d   j,                  dd        d| d   j                         z
  z  z   | d<   | j5                         D ci c]  \  }}||d    c}}S c c}}w )Ng?)p   皙?)
blur_limitr"   g333333?F   g?)r_shift_limitg_shift_limitb_shift_limitr"   g?img   r         )r   r   fg)AComposeRandomBrightnessContrast
MotionBlur
MedianBlurBlurHueSaturationValueRGBShiftChannelShuffler   
from_numpytouint8permutenumpyfloatpltimreadrandomr   bg_pathslenshaper/   Finterpolateitems)data_use_rand_bguse_color_aug	transformrand_bgkvs          r   format_render_samplerO   4   s   		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#kkm,sqAadF,,,s   =Ic                   $    e Zd ZdZddZd Zd Zy)PyBulletFolderzODataset 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/*.pti)r   scene_pathsval)selfpathrT   s      r   __init__zPyBulletFolder.__init__C   sF    d7l+!1!1$%!8 "..u5Tr   c                      y)Ni  )rU   s    r   __len__zPyBulletFolder.__len__I   s    ir   c                    t        j                  | j                  d d        }t        j                  |      }|j                  d      d   }t        j                  d|z        j                  dddd      d    |d<   t        j                  t        D cg c]  }|j                  d      d    c}j                  |      g      |d	<   t        |d
d
      S c c}w )N/r.   z-/data/cameron/robot_calibration_testing/%s.ptr   r#   r,   r-   	link_imgsrob_idF)rI   rJ   )rA   r   rS   r   loadsplitr<   tensorurdfsindexrO   )rU   idx
scene_pathrH   
embodimentr   s         r   __getitem__zPyBulletFolder.__getitem__J   s    ==!1!1!!45
jj$##C(,
 ::&UV`&`aiijklmnopqrswxk&F1qwws|A&F&L&LZ&X%YZh $Ee%PP 'Gs   CN).F)__name__
__module____qualname____doc__rW   rZ   rg   rY   r   r   rQ   rQ   @   s    Y6 (Qr   rQ   )tqdm   c                 x    t        j                  | t        j                  | j                  d   df      gd      S )Nr   r,   )axis)npconcatenateonesrD   r   s    r   r   r   ^   s+    277AGGAJ?#;<1E r   <   )r$   g      @)r,   r   r   r   )r   r   r   )r   r   ru   r   )r   r   r   r,      c                 
   |t        d       t        j                          t        j                  t        j                         t        j
                  ddd       t        j                  t        j                                t        t        |          t        j                  t        |         }|t        j                  d      \  }}}t        j                  t        j                  |||            j!                  ddd      }t        j"                  j%                  t&              t        j                  |      j!                  ddd      z  }| t        j(                  |       n3t+        t-        |             D ]  }t        j.                  ||| |           t        j0                  t2        t4        |j7                  d      j9                         t:        t        j<                  	      \  }	}	}
}}t        j                   |
t4        t2        df      d d d d d d
f   j?                  t        j@                        dz  }t        jB                  tD        tF        z  tD        tD        tF        z
  |z  z
  z  tH        tJ        tL        tN              }t        jP                  |      \  }}|D ci c]  }|||   
 }}t        jR                  tU        |      D cg c]M  }t        j                  |dk\  rt        jV                  ||d      d   nt        jX                  |      d         O c}      }t        jZ                  d|t]        |            d d d d
f   }|dk7  j!                  d      }|d d d f   |d d d f   z
  }i }t_        j`                  |      jc                  ddd      je                  dt4        t2        f      d    |d<   t_        j`                  |      jf                  je                  dt4        t2        f      d    |d<   t_        j`                  |
dd d
f         jc                  ddd      d    dz  dz  dz
  |d<   t_        j`                  |      d    |d<   t_        j`                  |      je                  dt4        t2        f      d   |d<   t_        j`                  |      d    |d<   t_        j`                  |      d    |d<   t_        j`                  t        j                  t        jh                  |tk        t+        t        jl                  |                        D cg c]  }|d   	 c}            d    |d<   |S c c}w c c}w c c}w )Nzloading new urdfr   皙#F)fixedrv   rE   )order)widthheight
viewMatrixprojectionMatrixflagsr#   g     o@T)computeForwardKinematicsz	ij,nj->niru   r-   r,   
joint_sdfs
points_cam.r+   r*   cam2world_cv)NNr/   link_positions_camlink_positionsjoint_states)7printr"   
disconnectconnectDIRECT
setGravitysetAdditionalSearchPathpybullet_datagetDataPathrb   loadURDFgeometrysample_camera_positionsrq   arraycomputeViewMatrixreshapelinalginvTcrandomly_move_jointsrangerC   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stacksortedgetLinkStategetBasePositionAndOrientationeinsumhomr   r9   r<   	unflattenTgetJointStateslistgetNumJoints)r   view_matrix_r^   r   urdfcamera_positionlook_at_positionrolljoint_i_rgbdepthseg
target_rgbr   
curr_linkslinks_with_visualrM   r    r   r   r/   r   datasr   s                            r   format_pybullet_renderr   e   s    !	qyy*1<<1d+C	!!-";";"=>eFmE&M* 2:2R2RY^2_/)4xx 3 3OEUW[ \]eefghiqteuyy}}R(288L+A+I+I!QVY+I+ZZ %%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#+#C#CF#K J ):;A*Q-;J;88  ek  l}  e~  @  `aRXXefhieiannVQY]&^_`&aop  pO  pO  PV  pW  XY  pZ  [  @  AN;c.>QRSTVXWXVXSXYRB#AdF+JtAv,>>J
E((4<<QqDNNrSYZ_R`abfgE,((466@@VENSTXYE,!!#c"1"f+.66q1=dCCGI!KE%L**<8>E.  $..q&@KE$K % 0 01C DT JE
!,,^<TBE
 ++BHHADTDTU[]abghihvhvw}h~b  ^A  EB  6Cqad  6C  -D  E  FJ  KE.L! < @ 6Cs   ,UAU$Uc                    t        j                  t         j                         t        j                  ddd       t        j                  t        j                                t        j                  |      }t        | ||      D ]=  }t        d d |      }t        j                  || d|dd       |j                  d       ? y )Nr   rx   r\   05dz.ptr,   )r"   r   r   r   r   r   r   r   r   r   r   saveput)		worker_idnum_workersdatadir
num_scenesprogress_queuer   r^   r    datas	            r   workerr      s    IIahhQ40m779:ZZF9j+6 %dD&9

4G9AaWC011r   __main__   i  r-   z-/data/cameron/robot_calibration_testing/kpts/r\   r.   )targetargs)totalr,   )r$   )ru   )FF)NNr   NN)dmatplotlib.pyplotpyplotr?   cv2os
statisticsmultiprocessingmptorch.nn.functionalnn
functionalrE   r   rA   imageior=   rq   r   collectionsr   pdbr   	itertoolsr   r   
imageio.v3v3iiotorchvisionr   sysgzipjsontorchvision.transformsPILr	   r   r1   augmentationeinopsr   r   ch_secr   albumentationsr0   rB   rb   rO   utilsr   DatasetrQ   pybulletr"   r   rm   r{   r|   fovaspectr   r   get_camera_intrinsicsr   r   r   r   camera_matrixr   r   r   computeProjectionMatrixFOVr   r   r   r   ri   	n_workersn_scenesr   r`   r   rV   existsmakedirsQueuer   procsr   widProcessp2startappendpbarr   getupdatejoinrY   r   r   <module>r     s     
 	          #   "    " 
  	    +  "z!!$' $	:U 	J	KP	-QU[[%%-- Q4    fE		c > > >uVC P BBRXX 02BOTU]]^_`ab+q++CvtSI.`     z
 IH
TE	qD@CQSATTG77>>'"KBKK$8 RXXZN EY CRZZc9gxQ_`d-efgigogogqrwr~r~  @B  sCC 
H	 x 	A KKN	  
	7 * s   =/JJ