o
    ÷úÙi¥  ã                   @   s   d Z ddlZddlZddlZddlZddlZddlm  m	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mZ ddlmZmZmZmZ ddlZej dej ej  e!¡dd	¡¡ ddl"Z#dd
l"m$Z$m%Z%m&Z& ddl'm(Z(m)Z) dZ*ej+g d¢ej,dZ-ej+g d¢ej,dZ.dZ/dZ0dZ1dZ2dZ3dZ4dZ5dZ6dZ7dZ8dZ9dd„ Z:dd„ Z;dd „ Z<d!d"„ Z=dNd%d&„Z>dOd)d*„Z?dPd+d,„Z@dQd.d/„ZAd0d1„ ZBd2d3„ ZCdPd4d5„ZD	6	-	8dRd9d:„ZEdSd<d=„ZFd>d?„ ZGdTdCdD„ZHdUdFdG„ZIdVdIdJ„ZJdKdL„ ZKeLdMkrþeKƒ  dS dS )Wa§  
Generate a 3D method visualization for PARA:
  1. Start at the policy camera view (what the robot sees)
  2. Smooth pull-back to an observer camera, showing the camera frustum
  3. Animate a ray through the GT target pixel with height bins colored by heatmap
  4. Fire more rays, then reveal the full heatmap volume
  5. Highlight the GT 3D point

Usage:
    export PYTHONPATH=/data/cameron/LIBERO:/data/cameron/para_normalized_losses/libero:$PYTHONPATH
    export DINO_REPO_DIR=/data/cameron/keygrip/dinov3
    export DINO_WEIGHTS_PATH=/data/cameron/keygrip/dinov3/weights/dinov3_vits16plus_pretrain_lvd1689m-4057cbaa.pth
    python ood_libero/generate_method_visualization.py
é    N)ÚPath©ÚRotation)ÚOffScreenRenderEnv)Ú	benchmarkÚget_libero_path)Úget_camera_transform_matrixÚget_camera_extrinsic_matrixÚget_camera_intrinsic_matrixÚ#project_points_from_world_to_cameraz..Úlibero)ÚTrajectoryHeatmapPredictorÚN_HEIGHT_BINSÚ	PRED_SIZE)Ú*recover_3d_from_direct_keypoint_and_heightÚ_unproject_2d_to_rayiÀ  )g
×£p=
ß?gÉv¾Ÿ/Ý?g–C‹lçûÙ?©Údtype)gZd;ßOÍ?gyé&1¬Ì?gÍÌÌÌÌÌÌ?é   g333333ë?)r   éZ   )r   éx   )r   éÃ   )r   éá   )r   é  )r   é6  )r   éT  )r   é®  r   c                 C   s"   t  | dd¡} | |  dd|    S )u%   Smooth ease in-out [0, 1] â†’ [0, 1].r   é   é   é   )ÚnpÚclip)Út© r#   ú>/data/cameron/para/ood_libero/generate_method_visualization.pyÚ
smoothstepC   s   r%   c                 C   s¤   t j |¡}t jg d¢|ddg||dgd|dggt jd}g }| dd…df }| dd…dd…f }|D ]}	||	 }
|
|
d  | }
||
 | }| |¡ q3t  |¡S )u“   Compute the 4 corners of the image plane at a given depth in world space.

    Returns:
        corners_world: (4, 3) array â€” TL, TR, BR, BL
    ©r   r   r   r   r   r   Nr   r   )r    ÚlinalgÚinvÚarrayÚfloat64Úappend)Úcamera_poseÚcam_KÚ
image_sizeÚdepthZK_invZ
corners_pxZcorners_worldÚcam_posZR_camÚcÚray_camZpt_worldr#   r#   r$   Úcompute_frustum_cornersI   s$   üû
r3   c                 C   sÒ   t | ||ƒ\}}g }t|ƒD ]1}	||	t|d dƒ ||   }
t|d ƒdk r(q|
|d  |d  }|dkr?| |||  ¡ qt|d ƒdkrYt|d  |d  }|||  }n||d  }t |¡| ¡ |fS )zíCompute 3D positions of height bins along the ray through a pixel.

    Returns:
        bin_points: (n_bins, 3) array of 3D world positions
        ray_start: (3,) camera position
        ray_end: (3,) intersection with table plane
    r   r   çíµ ÷Æ°>r   ç       @)	r   ÚrangeÚmaxÚabsr+   ÚTABLE_Zr    r)   Úcopy)Zpixel_uvr,   r-   Ú
min_heightÚ
max_heightÚn_binsr0   Zray_dirÚ
bin_pointsÚiÚhr"   Út_tableÚray_endr#   r#   r$   Úcompute_ray_pointsb   s   €rC   c                 C   s¨   g }| D ]M}t | dd¡ tj¡|||dd }ttt|d ƒƒƒ}ttt|d ƒƒƒ}d|  kr6|k rLn nd|  krB|k rLn n| ||f¡ q| d¡ q|S )a  Project 3D world points to 2D pixel coordinates in observer view.

    Args:
        points_3d: (N, 3) array
        world_to_camera: (4, 4) transform
        image_size: int

    Returns:
        pixels: list of (u, v) tuples or None for behind-camera points
    r   r   )ÚpointsÚworld_to_camera_transformÚcamera_heightÚcamera_widthr   N)	r   ÚreshapeÚastyper    r*   ÚintÚroundÚfloatr+   )Z	points_3dÚworld_to_camerar.   ÚresultsÚptÚpix_rcÚuÚvr#   r#   r$   Úproject_3d_to_2d~   s    üû0rS   r   ç      ð?c           
      C   s¢   t t ||g¡||ƒ}|d durM|d durO|dk r<|  ¡ }	t |	|d |d ||tj¡ t |	|| d| d| ¡ dS t | |d |d ||tj¡ dS dS dS )zCDraw a line between two 3D points projected into the observer view.r   Nr   rT   )rS   r    r)   r:   Úcv2ÚlineÚLINE_AAÚaddWeighted)
ÚframeZp1_3dZp2_3drM   r.   ÚcolorÚ	thicknessÚalphaÚptsÚoverlayr#   r#   r$   Údraw_line_3dš   s   "úr_   é   éÿÿÿÿc           
      C   sˆ   t t |g¡||ƒ}|d durB|dk r3|  ¡ }	t |	|d |||tj¡ t |	|| d| d| ¡ dS t | |d |||tj¡ dS dS )zRDraw a circle at a 3D point projected into the observer view, with optional alpha.r   Ngffffffî?r   )rS   r    r)   r:   rU   ÚcirclerW   rX   )
rY   Úpt_3drM   r.   rZ   Úradiusr[   r\   r]   r^   r#   r#   r$   Údraw_circle_3d¦   s   úre   c                 C   sf   t  t| d ƒdd¡}t t j|ggt jdtj¡}t|d ƒt|d ƒt|d ƒ}}}|||fS )zYMap value [0, 1] to a color using a warm colormap.
    Returns (B, G, R) for OpenCV.
    éÿ   r   r   ©r   r   r   r&   )r   r   r   )r    r!   rJ   rU   ÚapplyColorMapr)   Úuint8ÚCOLORMAP_PLASMA)Úvaluer\   Z	val_uint8Z	color_imgÚbÚgÚrr#   r#   r$   Úheatmap_color¶   s   (
ro   çffffffæ?c                 C   s8  t dd„ |D ƒƒrdS |jdd… \}}tjddg|dg||gd|ggtjd}tj|tjd}t ||¡}tj||| jd | jd ftjtj	d}	tj
| jd | jd ftjd}
t |
| tj¡d	¡ |
dd…dd…df  tj¡d
 }|  tj¡}|	 tj¡}|d||   || |  }t | | tj¡¡ dS )ad  Warp the policy camera image into the observer view as a floating plane.

    Args:
        frame: observer view image (H, W, 3) uint8, modified in-place
        policy_image: (H, W, 3) uint8 RGB image from policy camera
        frustum_corners_2d: list of 4 (u, v) tuples (TL, TR, BR, BL) in observer view
        alpha: opacity of the floating image
    c                 s   s    | ]}|d u V  qd S ©Nr#   ©Ú.0r1   r#   r#   r$   Ú	<genexpr>Ï   ó   € z(render_floating_image.<locals>.<genexpr>Nr   r   r   r   )ÚflagsZ
borderModerf   ç     ào@)ÚanyÚshaper    r)   Úfloat32rU   ÚgetPerspectiveTransformÚwarpPerspectiveÚINTER_LINEARÚBORDER_CONSTANTÚzerosri   ÚfillConvexPolyrI   Úint32Úcopyto)rY   Zpolicy_imageZfrustum_corners_2dr\   r@   ÚwZsrc_ptsZdst_ptsÚMZwarpedÚmaskZmask_3chZframe_fZwarped_fZblendedr#   r#   r$   Úrender_floating_imageÆ   s    	(ÿ "r†   c                 C   sÒ   t |ƒ}| d|  ||  }t |d |d |d |d g¡}t |d |d |d |d g¡}t |¡}	t |¡}
ddlm} |ddgt |	|
g¡ƒ}||ƒ}| ¡ }t |d |d |d |d g¡}||fS )a  Smoothly interpolate camera pose using smoothstep easing.

    Args:
        pos_start/end: (3,) camera positions
        quat_start/end: (4,) MuJoCo quaternions (w, x, y, z)
        t: interpolation factor [0, 1]

    Returns:
        pos, quat: interpolated camera pose
    r   r   r   r   )ÚSlerp)	r%   r    r)   ÚRÚ	from_quatÚscipy.spatial.transformr‡   ÚconcatenateÚas_quat)Z	pos_startZ
quat_startZpos_endZquat_endr"   ÚsÚposÚq0Úq1Úr0Úr1r‡   ZslerpZr_interpZq_interpÚquatr#   r#   r$   Úinterpolate_cameraê   s   ""

"r”   c                 C   sÖ  ddl m} | | d }| d |d  d |d< | | }tj |¡d }|tj |¡ }t d¡}t d¡}	t |¡t |¡}
}t |
| dg||
dgg d	¢g¡}|| }t 	t g d
¢¡|¡}tj |¡dkr~|tj |¡ }| 
|	| ¡ ¡ }|| }|||  }|| }|tj |¡ }t g d
¢¡}t 	||¡}tj |¡dk r¬t g d¢¡}|tj |¡ }t 	||¡}|tj |¡ }tj||| gdd}| |¡}| ¡ }t |d |d |d |d g¡}||fS )uí   Compute an observer camera position that shows the frustum from the side.

    Places the observer further back, higher, and rotated ~60Â° to the side.
    The look_at is adjusted to be between camera and scene so both are in frame.
    r   r   ç      à?r   çÍÌÌÌÌÌÜ?gš™™™™™@é2   é
   r&   )r   r   rT   r4   )r   r   ç        r   )Úaxisr   )rŠ   r   r    r'   ÚnormÚradiansÚcosÚsinr)   ÚcrossÚfrom_rotvecÚ	as_matrixÚstackÚfrom_matrixrŒ   )Z
policy_posZpolicy_quatZlook_atrˆ   Zlook_at_adjustedZdefault_dirrd   Zdefault_dir_normZphi_radZ	theta_radZcos_phiZsin_phiZrot_zZrotated_dirÚrightZrot_elevZobs_posÚforwardZworld_upÚupZcam_matZcam_rotZ	quat_xyzwZobs_quatr#   r#   r$   Úcompute_observer_camera  sF   


ý
"r§   c           	   
   C   s\   d}|D ]}t | |||||d|ƒ qtdƒD ]}t | || ||d d  ||dd|ƒ qdS )zQDraw the camera frustum (4 lines from origin to image plane corners + rectangle).)éÜ   éð   rf   r   é   r   ©rf   rf   rf   N)r_   r6   )	rY   Ú
cam_originÚfrustum_cornersrM   r.   r\   rZ   Úcornerr?   r#   r#   r$   Údraw_frustumF  s   
ÿÿr¯   ©r   éÈ   rf   Fé   c                    s4  t | |||||d|
ƒ |du rt|ƒ}t|ƒdkr$t ˆ dt|ƒ… ¡nd}tˆ  ¡ dƒ}ttt|t|ƒƒƒƒ}|j‡ fdd„d |D ]S}|tˆ ƒk rPˆ | nd	}|| }t 	|d
¡}t
|ƒ}dd|  }|	r‰||kr‰t| || ||d|d dƒ t| || ||d|d dƒ qDt| || ||||d|d qDdS )a…  Draw a ray from camera through the scene with colored height bins.

    All bins have the same size. Color differentiates probability (plasma colormap).
    Alpha also scales with probability: low-prob bins are nearly transparent (0.1),
    high-prob bins are fully opaque (1.0). This prevents dark blue dots from
    occluding the bright yellow/red high-probability bins behind them.
    r   Nr   ra   ç:Œ0âŽyE>c                    s   | t ˆ ƒk r
ˆ |  S dS )Nr™   )Úlen)r?   ©Ú	bin_probsr#   r$   Ú<lambda>k  s    z$draw_ray_with_bins.<locals>.<lambda>)Úkeyr™   r•   gš™™™™™¹?gÍÌÌÌÌÌì?r«   r`   r   )r   rf   r±   ©r\   )r_   r´   r    Úargmaxr7   Úlistr6   ÚminÚsortÚpowerro   re   )rY   r¬   rB   r>   r¶   rM   r.   Ú	ray_colorÚn_bins_to_showÚhighlight_maxÚ	ray_alphaÚ
bin_radiusZmax_idxÚp_maxÚindicesr?   ÚprobZ	prob_normZprob_visrZ   Z	bin_alphar#   rµ   r$   Údraw_ray_with_binsT  s4   ÿ&
ÿ
ÿÿñrÇ   r™   c           
   	   C   sÌ   t t |g¡||ƒ}|d durd|d \}}ddt |d tj ¡  }td| ƒ}	t | ||f|	ddtj¡ t | ||f|	d d	d
tj¡ t | ||fdddtj¡ t 	| ||fdtj
ddtj¡ dS dS )z5Draw a prominent marker at the GT 3D target position.r   NrT   ç333333Ó?r   é   )r   rf   éd   r   )r   é´   éF   r   é   ra   r«   é   )rS   r    r)   rž   ÚpirJ   rU   rb   rW   Ú
drawMarkerÚMARKER_CROSS)
rY   Zgt_3drM   r.   Úpulse_tr]   rQ   rR   ZpulseZouter_rr#   r#   r$   Údraw_gt_marker€  s   "örÓ   c                 C   s´   t t |g¡||ƒ}|d durX|d \}}t | |d |d f|d |d fdd¡ t | |d |d f|d |d fdd¡ t | ||fd	d
d¡ t | ||fd	dd¡ dS dS )z1Draw a camera icon at the policy camera position.r   Né   é	   )r¨   r¨   r¨   ra   r«   r   r`   )é(   rÖ   rÖ   )rÊ   rÊ   rÊ   r   )rS   r    r)   rU   Ú	rectanglerb   )rY   r0   rM   r.   r]   rQ   rR   r#   r#   r$   Údraw_camera_icon  s   **ùrØ   ÚbottomçÍÌÌÌÌÌä?r«   c              
   C   sè   | j dd… \}}tj}t ||||¡\\}	}
}|dkr(||	 d |d }}n |dkr8||	 d |
d }}n|dkrDd|
d }}n|\}}t | |d ||
 d f||	 d || d fd	d
¡ t | |||f||||tj¡ dS )z!Add text overlay with background.Nr   rÙ   é   Útopé   ztop-leftrÍ   rg   ra   )ry   rU   ÚFONT_HERSHEY_SIMPLEXÚgetTextSizer×   ÚputTextrW   )rY   ÚtextÚpositionÚ
font_scaler[   rZ   r@   rƒ   ÚfontZtwÚthÚbaselineÚxÚyr#   r#   r$   Úadd_text¡  s   .ÿ"ré   rª   c           
      C   sx   | g}g d¢}|d|… D ],\}}t  | d ||  d|d ¡}t  | d ||  d|d ¡}	| t  ||	g¡¡ q|S )z³Get a set of sample pixels for the volume visualization.

    Returns the GT pixel first, then n_extra surrounding pixels spread across
    the image to show volume coverage.
    ))çš™™™™™É¿ç333333Ã¿)ç333333Ã?rê   )rë   çš™™™™™É?)rí   rì   )r™   ç333333Ó¿)rî   r™   Nr   r˜   r   )r    r!   r+   r)   )
Úgt_pixelr.   Ú	pred_sizeÚn_extraÚpixelsÚoffsetsÚdxÚdyÚpxÚpyr#   r#   r$   Úget_sample_pixelsº  s   	rø   é   c                 C   s®   | | }g }d}t |ƒD ]}t |ƒD ]}|d | }|d | }	| t ||	g¡¡ qq|durUtdƒ}
d}t|ƒD ]\}}tj || ¡}||
k rN|}
|}q:| ¡ ||< |S )zçGet a grid of pixels for the full volume visualization.

    Generates a grid_size x grid_size grid of evenly spaced pixels.
    If gt_pixel is provided, snaps the nearest grid point to it so
    the GT ray is always included.
    Fr•   NÚinfr   )	r6   r+   r    r)   rL   Ú	enumerater'   r›   r:   )r.   Ú	grid_sizerï   Ústriderò   Zgt_replacedÚiyÚixrQ   rR   Úmin_distÚmin_idxr?   rö   Údr#   r#   r$   Úget_volume_pixelsÔ  s(   ý€r  c            ƒ         s6  t jdd} | jdtdd | jdtdd | jdtd d | jd	td
d | jdtddd | jdtddd | jdtddd |  ¡ }t |jrM|jntj	 
¡ rTdnd¡}td|› ƒ tdƒ tj|j|d}t| dtj¡ƒt_t| dtj¡ƒt_t| dtj¡ƒt_t| dtj¡ƒt_d|v rºt|d tƒr¡|d n|d  ¡ t_t|d tƒr³|d n|d  ¡ t_|d  d! }|jd
 t }td"|› d#tjd$›d%tjd$›d&ƒ t|d'}|j|d  d(d) | |¡ ¡ }td*ƒ t  ¡ d+ ƒ }| !d
¡}t"j# $t%d,ƒ|j&|j'¡}	t(|	t)t)d-gd.‰ˆ *d
¡ ˆ +¡  ˆj,j-‰t"j# $t%d/ƒ| .d
¡¡}
t/ 0|
d0¡c}d1|j1› }t2 3||› d2 ¡}t2 3||› d3 ¡}||j4  5¡ ‰|d d …d4f }|j4}t6d5t7|ƒƒD ]}||d5  d
k r||| d
kr||} nqftd6|› ƒ ˆ 8ˆ¡}ˆ 9¡  W d   ƒ n	1 s™w   Y  td7ƒ d8D ]!}zˆj: ;|¡}t2 3g d9¢¡ˆj:j<|< W q¤ t=yÅ   Y q¤w g d:¢}t>ƒ }|D ]}z| ?ˆj: ;|¡¡ W qÏ t=yè   Y qÏw t6ˆj:j@ƒD ]}ˆj:jA| |v rd;ˆj:jB| d<< qïˆ 9¡  ˆ 8ˆ¡}ˆ 9¡  ˆj, C¡ }d-‰ˆj: Dˆ¡‰ ˆj:jEˆ   5¡ }ˆj:jFˆ   5¡ }tGˆˆt)t)ƒ}tHˆˆƒ}tIˆˆt)t)ƒ}|d
  t)  < |d5  t)  < | 5¡ }|d
  t)9  < |d5  t)9  < td=|› ƒ t2j3|d> t2jJd?}td@|› ƒ ˆ 8|| ¡}ˆ 9¡  ˆj, C¡ }t2j3|d> t2jJd?} tdA|› dB| › ƒ | dC }!tdD|!d$›dEtjd$›d%tjd$›dFƒ ˆ 8ˆ¡}ˆ 9¡  ˆj, C¡ }t2 Kt2 L|dG ¡ 5¡ ¡}"tdHƒ |" Mt2jN¡dI }#|#tO tP }#t Q|# RdCd
d5¡¡ ¡  Sd
¡ |¡}$tT| Ud5d<¡|t)t)ƒd
 }%tjVt|%d5 ƒt|%d
 ƒgtjNd? |¡}&t W¡  ||$|&ƒ\}'}(}(})W d   ƒ n	1 s1w   Y  tdJ|'j› ƒ |'dK }*tXjY|* UdL¡d
dM U|*j¡}+|+jZd
dMd
  [¡  \¡ },t] ^|,t)t)f¡}-|, _¡ }.|.t` |.t` }/}0t)t` }1t2 3|0dN |1 |/dN |1 g¡}2tdO|2d
 dP›d%|2d5 dP›dQƒ |+d d …|/|0f  [¡  \¡ }3tdRt2 _|3¡› dS|3 Z¡ d$›ƒ tdTƒ ta||t)|jbƒ}4tc|2||tjtjtƒ\}5}6}7tdUt7|5ƒ› dV|6› dW|7› ƒ td|2t)t`dXdY}8g }9|8d5d … D ]K}:t2 3|:d
 |1 dN |:d5 |1 dN g¡};t2 e|;d
t`d5 ¡ Mt¡};|+d d …|;d5 |;d
 f  [¡  \¡ }<tc|:||tjtjtƒ\}=}>}?|9 f|=|>|?|<|:f¡ qëtgt)d|2dZ}@g }A|@D ]R}:t2 3|:d
 |1 dN |:d5 |1 dN g¡};t2 e|;d
t`d5 ¡ Mt¡};|+d d …|;d5 |;d
 f  [¡  \¡ }<tc|:||tjtjtƒ\}=}>}?t7|=ƒd
kr“|A f|=|>|?|<|:f¡ qBth i|d5 |dC |d< |d
 g¡}B|B j¡ }C|Cd d …dCf  }Dtk|DdC ƒd[krÍtl|dC  |DdC  }E||E|D  }Fn
|dN|D  }Ftl|FdC< tm|||Fƒ\}G}Htd\|G› ƒ |-|- n¡  |- Z¡ d]  }It2 o|Id^¡}Jt] p|Jd_  Mt2jq¡t]jr¡}Kt] s|Kt]jt¡}Lt2 e|" Mt2jN¡d^ |L Mt2jN¡d`  d
d_¡ Mt2jq¡}Mt] u|Mt|2d
 ƒt|2d5 ƒfdat]jvdbd<t]jw¡ t] x|Mt|2d
 ƒt|2d5 ƒfdcdadCt]jw¡ tddty› detz› dfƒ t"j{|j|dgdh t"j# $|j|di¡}Nt]j}djŽ }Ot] ~|N|Otzt)t)f¡}Pt] s|"t]j¡}Qt] s|Mt]j¡}R‡ ‡‡‡‡fdkdl„}S|S|G|Hƒ\}T}Ut6tyƒD ]Â}V|Vd5 dm d
kr¶tdn|Vd5 › doty› ƒ |Vt€d5 k rÚ|VtZt€d5 d5 d5ƒ }t|||G|H|ƒ\}W}X|S|W|Xƒ\}Y}Zn—|Vt‚d
 krk|Vt‚d
 krýˆ 8ˆ¡ ˆ 9¡  d
ˆj,_ƒd(ˆj,_„dpˆj,_…ˆj, C¡ }[t2j3|[d> t2jJd?}\| |\ }]t2j† ‡|]¡}^|^dqkr;t2 e|]dr dsdt¡}_t2jˆdut2jNd?}`|_|`d d<…< ds|`d4< ˆ ‰|`¡ |Gˆj:jEˆ < |Hˆj:jFˆ < ˆ 9¡  ˆj, C¡ }at2 Kt2 L|adG ¡ 5¡ ¡}bt] s|bt]j¡}YtGˆˆt)t)ƒ}Zn|T 5¡ }Y|U}Z|VtŠd
 k r{dt‰n|VtŠd5 k r˜dtt‹|VtŠd
  tZtŠd5 tŠd
  d5ƒ ƒ ‰nd;‰ˆdvkrtŒ|4|Zt)ƒ}ctdwdx„ |cD ƒƒ}d|Vt€d5 k rÇ|VtZt€d5 d5 d5ƒ }t‹|ƒdy }endy}e|eˆ9 }e|drø|edrkrøt2j3dzd{„ |cD ƒt2jNd?}ft] Ž|f Mt2j¡¡}g|gd|krøt|Y|R|c|ed} t‘|Y||Zt)ƒ t’|Y||4|Zt)ˆƒ |Vt“d
 krJˆdvkrJtndt|Vt“d
  t“d5 t“d
   ƒ}htZd5tt‹|hƒt7|5ƒ ƒƒ}i|Vt”d
 k}jt•|Y||7|5|3|Zt)d~|i|jdˆ d€d |Vt–d
 kr˜ˆdvkr˜|Vt–d
  t–d5 t–d
   }ktZd5tt‹|kƒt7|9ƒ ƒƒ}lt6tn|lt7|9ƒƒƒD ]}m|9|m \}=}>}?}n}:t•|Y|>|?|=|n|Zt)d‚d(dNˆ d4dƒ qz|Vt—d
 kr£ˆdvkr£t‹tndt|Vt—d
  tZt—d5 t—d
  d5ƒ d„ ƒƒ}od…}ptZd†dx„ |AD ƒd]d‡}qg }rg }s|AD ]|\}=}>}?}n}:t7|=ƒd
krßqÏt6tnt7|=ƒt7|nƒƒƒD ]_}t|n|t }utŒt2 3|=|t g¡|Zt)ƒ}v|vd
 d u rqê|u|q }w|w|pkrAtt2 et2 ˜|w¡d_ d
d_¡ƒ}xt] pt2j3|xggt2jqd?t]jr¡}yt™dˆdx„ |ydK D ƒƒ}z|s f|vd
 |zf¡ qê|r f|vd
 ¡ qêqÏ|Y 5¡ }{d‰}||rD ]}}t] x|{|}dŠ||dLt]jw¡ qTd‹|o ˆ }~t] š|{|~|Ydt|~ d
|Y¡ |sD ]+\}}}zt™‡fdŒdx„|zD ƒƒ}t] x|Y|}du|dLt]jw¡ ˆdNkr¡t] x|Y|}dudd5t]jw¡ qw|Vt”d
 krº|Vt”d
  tz }€t›|Y| |Zt)|€ƒ |Vtœd5 k rÉt|YdŽdd n’t“d
 |V  krÙt–d
 k rän n	t|Yd‘dd’d“ nwt–d
 |V  krôt—d
 k rÿn n	t|Yd”ddNd“ n\t—d
 |V  k	rt”d
 k 	rn nt|Yd•dd nBt”d
 |V  k	r)tŠd
 k 	r3n nt|Yd–dd n(tŠd
 |V  k	rCt‚d
 k 	rMn nt|Yd–dd n|Vt‚d
 k	r[t|Yd—dd |P ž|Y¡ qž|P Ÿ¡  td˜|N› ƒ |N  d™dš¡}t" ¡d›|N› dœ|› d¡}‚|‚d
k	r‘t"  ||N¡ tdž|N› ƒ ˆ ¢¡  tdŸƒ d S ) NzPARA method 3D visualization)Údescriptionz--output_dirz6/data/cameron/para/.agents/reports/project_site/media/)ÚtypeÚdefaultz--checkpointzQ/data/cameron/para_normalized_losses/libero/checkpoints/para_v2_exp4_n64/best.pthz--devicez
--demo_idxr   z--frame_idxé   z4Demo frame to visualize (pick one with clear target))r  r  Úhelpz--image_plane_depthr–   z6Depth of the floating image plane from camera (meters)z--volume_gridrù   z?Grid size for full volume visualization (16 = 16x16 = 256 rays)ÚcudaÚcpuzDevice: zLoading PARA model...)Úmap_locationr;   r<   Úmin_gripperÚmax_gripperÚmin_rotÚmax_rotÚmodel_state_dictzvolume_head.weightz  N_WINDOW=z
, height=[z.4fz, ú])Ún_windowF)Ústrictz"Initializing LIBERO environment...Úlibero_spatialÚ
bddl_filesÚ	agentview)Úbddl_file_nameÚcamera_heightsÚcamera_widthsÚcamera_namesÚdatasetsrn   z
data/demo_z/statesz/actionsrÍ   r   z  Grasp frame: zCleaning scene...)Úwooden_cabinet_1_mainÚflat_stove_1_main)r   r   g      À)Úakita_black_bowl_2_mainÚcookies_1_mainÚ#glazed_rim_porcelain_ramekin_1_mainr™   r   z  Policy camera pos: Úrobot0_eef_posr   z  EEF position: z  Grasp EEF (frame z): r   z  Grasp height: z	 (range [z])Úagentview_imagezRunning PARA inference...rw   z  Volume logits shape: )r   r   ra   )Údimr•   z  Predicted pixel: (z.1fú)z  Max height bin: z, prob: zComputing 3D geometry...z  Ray: z height bins, start=z, end=rª   )rñ   )rü   rï   r4   z  Observer camera pos: r³   gš™™™™™Ù?rf   g333333ã?)r   rf   r   é   rÔ   zGenerating z frames at z fps...T)Úexist_okzpara_method_3d.mp4Úmp4vc                    sp   | ˆj jˆ < |ˆj jˆ < ˆ ˆ¡ ˆ ¡  ˆj ¡ }t t 	|d ¡ 
¡ ¡}t |tj¡}tˆˆttƒ}||fS )zõRender MuJoCo scene from given camera pose, return (bgr_frame, w2c_matrix).

        Must call env.set_init_state + sim.forward + _get_observations to force
        a fresh MuJoCo render (plain _get_observations returns cached results).
        r"  )Úmodelr0   Úcam_quatÚset_init_stater¥   ÚenvÚ_get_observationsr    ÚflipudÚasarrayr:   rU   ÚcvtColorÚCOLOR_RGB2BGRr   Ú
IMAGE_SIZE)Zcam_pZcam_qrn   ÚrgbÚbgrÚw2c)Úcam_idÚcam_namer+  ÚsimÚstater#   r$   Úrender_at_cameraü  s   

zmain.<locals>.render_at_camerar   z  Frame ú/i † g{®Gázt?gš™™™™™©?g      ð¿rT   r²   g{®Gáz„?c                 s   s    | ]}|d uV  qd S rq   r#   rr   r#   r#   r$   rt   N  ru   zmain.<locals>.<genexpr>rp   c                 S   s   g | ]}|‘qS r#   r#   rr   r#   r#   r$   Ú
<listcomp>Y  s    zmain.<locals>.<listcomp>iô  r¹   )r   éæ   rf   gš™™™™™é?é   )r¿   rÀ   rÁ   rÂ   rÃ   )r   rË   r¨   )r¿   rÁ   rÂ   rÃ   r5   gü©ñÒMbP?c                 s   s,    | ]}t |d  ƒd kr|d  ¡ V  qdS )r   r   N)r´   r7   )rs   rn   r#   r#   r$   rt   €  s   €* )r  c                 s   s    | ]}t |ƒV  qd S rq   ©rJ   ©rs   rç   r#   r#   r$   rt   ’  ru   )éŒ   r—   éP   r`   rÈ   c                 3   s    | ]	}t |ˆ  ƒV  qd S rq   r>  r?  )Úoverlay_fader#   r$   rt   ¡  s   € r«   zCamera frustum over the scenerÙ   )râ   z?Ray through predicted pixel: height bins colored by probabilityg¸…ëQ¸Þ?)râ   rã   z+Per-pixel height predictions along each rayzFull heatmap volumezArgmax -> 3D grasp targetzMove robot to predicted targetzSaved raw video: z.mp4z	_h264.mp4zffmpeg -y -i "z?" -c:v libx264 -preset ultrafast -crf 23 -movflags +faststart "z" 2>/dev/nullzRe-encoded to H.264: zDone!)£ÚargparseÚArgumentParserÚadd_argumentÚstrrJ   rL   Ú
parse_argsÚtorchÚdevicer	  Úis_availableÚprintÚloadÚ
checkpointÚgetÚmodel_moduleÚ
MIN_HEIGHTÚ
MAX_HEIGHTÚMIN_GRIPPERÚMAX_GRIPPERÚ
isinstancer»   ÚtolistÚMIN_ROTÚMAX_ROTry   r   r   Úload_state_dictÚtoÚevalÚbm_libÚget_benchmark_dictÚget_taskÚosÚpathÚjoinr   Úproblem_folderÚ	bddl_filer   r1  ÚseedÚresetr+  r7  Úget_task_demonstrationÚh5pyÚFileZdemo_idxr    r)   Ú	frame_idxr:   r6   r´   r*  r¥   r(  Úbody_name2idÚbody_posÚ	ExceptionÚsetÚaddÚngeomÚgeom_bodyidÚ	geom_rgbar,  Úcamera_name2idr0   r)  r   r	   r
   r*   r-  r.  rI   rz   ÚIMAGENET_MEANÚIMAGENET_STDÚ
from_numpyÚ	transposeÚ	unsqueezer   rH   ÚtensorÚno_gradÚFÚsoftmaxr7   r
  ÚnumpyrU   Úresizerº   r   r3   Úimage_plane_depthrC   rø   r!   r+   r  rˆ   r‰   r¡   r8   r9   r§   r¼   r¾   rh   ri   rj   r/  ÚCOLOR_BGR2RGBrÐ   rÑ   rW   rb   ÚTOTAL_FRAMESÚFPSÚmakedirsÚ
output_dirÚVideoWriter_fourccÚVideoWriterr0  ÚPHASE_TRANSITIONr”   ÚPHASE_SERVOÚtimestepÚdoneÚhorizonr'   r›   r   ÚstepÚ
PHASE_FADEr%   rS   ÚallÚcontourArear   r†   rØ   r¯   ÚPHASE_FIRST_RAYÚPHASE_HIGHLIGHTrÇ   ÚPHASE_MORE_RAYSÚPHASE_FULL_VOLUMEÚsqrtÚtuplerX   rÓ   ÚPHASE_ESTABLISHré   ÚwriteÚreleaseÚreplaceÚsystemÚclose)ƒÚparserÚargsrI  ÚckptÚvol_wr  r(  r   Útaskrb  Ú	demo_pathÚfZdemo_keyÚstatesÚactionsZgripper_actionsZgrasp_framer"   ÚobsÚnameÚbidZdistractor_namesZdistractor_bidsÚgeom_idÚpolicy_cam_posÚpolicy_cam_quatrM   r,   Ú
cam_K_normr-   Úeef_posZ	obs_graspZ	grasp_eefZgrasp_heightÚ
policy_rgbÚimgÚ
img_tensorrP   Ústart_kpÚvolume_logitsÚ_ÚfeatsÚvol_t0Ú	vol_probsÚheat_2dZheat_2d_upsampledÚflat_idxÚpred_pyÚpred_pxÚscaleZ
pred_pixelZheight_probsr­   Zgt_ray_binsZ	ray_startrB   Zsample_pixelsZsample_raysrö   Zpx_predZh_probsÚbinsÚrsÚreZvolume_pixelsZvolume_raysÚdefault_rotÚdefault_matÚforward_dirrA   Úscene_centerÚobserver_posÚobserver_quatÚ	heat_normÚheat_boostedÚ
heat_colorÚheat_color_rgbZpolicy_view_heatmapÚoutput_pathÚfourccÚwriterZpolicy_rgb_bgrZpolicy_heatmap_bgrr9  Zobserver_bgrZobserver_w2crh  Úcur_posZcur_quatrY   Zcur_w2cÚcur_obsÚcur_eefÚdeltaÚdistÚdelta_clippedÚactionZ	obs_servoZ	frame_rgbÚfc_2dZhas_fcZ	img_alphar]   ÚareaZray_tZn_bins_showÚ	highlightZmore_tZn_rays_showr?   ÚhpZvol_fadeZPEAK_THRESHZglobal_pmaxZcloud_2dZpeak_2dÚbirÆ   Zpt_2dÚpnZcv_valÚcir1   r^   Z
purple_bgrrO   ÚblendZc_fadedrÒ   Ú	h264_pathÚretr#   )r5  r6  r+  rB  r7  r8  r$   Úmain÷  sÔ  ÿÿ
ÿ
ÿ
ÿÿ$$$

ÿü
ÿ €

òÿÿ€



&

$ÿþ*
ÿ" 

þ&"

ÿ&"

ÿ€ÿþýÿ
ÿ
þ





ÿ

"ý

þÿÿ
ô
€$ÿ$ÿ$$$ÿÿ
rÚ  Ú__main__)r   rT   )r`   ra   rT   )rT   )rp   )r°   NFrp   r²   )r™   )rÙ   rÚ   r   r«   )rª   )rù   N)MÚ__doc__rC  r^  Úsysr{  r    rH  Útorch.nn.functionalÚnnÚ
functionalry  rU   Úpathlibr   rŠ   r   rˆ   Úlibero.libero.envsr   Úlibero.liberor   r[  r   Úrobosuite.utils.camera_utilsr   r	   r
   r   rf  r_  Úinsertr`  ÚdirnameÚ__file__r(  rO  r   r   r   Úutilsr   r   r1  r)   rz   rr  rs  r€  r9   r…  r”  rŽ  r  r‘  r  r‹  r†  r  r%   r3   rC   rS   r_   re   ro   r†   r”   r§   r¯   rÇ   rÓ   rØ   ré   rø   r  rÚ  Ú__name__r#   r#   r#   r$   Ú<module>   sv    "



$
>
ý
,


#   \
ÿ