o
    \үi5                     @   s  d dl mZmZ d dl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Z dedeeejf fddZdeeejf deeef deeejf fd	d
ZG dd dZdefddZdede	jjfddZ	dKdejde	jjdeeef deeejf def
ddZe	je	jjddddfddZdLd"d#ZdMd&d'ZdNd)d*Z dNd+d,Z!dejd-ejd.e"fd/d0Z#d1d2 Z$dOd4d5Z%d6d7 Z&dPd;d<Z'	>dQd?d@Z(	=dRdAdBZ)dSdDdEZ*	>dTdGedHefdIdJZ+dS )U    )DictTupleN	json_datareturnc           	      C   s   | d dksJ | d }| d }| d }|d }|d }|d }|d	 |d
 |d |d g}t j||gt jdt j|d|gd||gg dgt jdt |gjd}|S )a0  
    Reads camera intrinsics from OpenCameraImuCalibration to opencv format.
    Example:
    {
        "final_reproj_error": 0.17053819312281043,
        "fps": 60.0,
        "image_height": 1080,
        "image_width": 1920,
        "intrinsic_type": "FISHEYE",
        "intrinsics": {
            "aspect_ratio": 1.0026582765352035,
            "focal_length": 420.56809123853304,
            "principal_pt_x": 959.857586309181,
            "principal_pt_y": 542.8155851051391,
            "radial_distortion_1": -0.011968137016185161,
            "radial_distortion_2": -0.03929790706019372,
            "radial_distortion_3": 0.018577224235396064,
            "radial_distortion_4": -0.005075629959840777,
            "skew": 0.0
        },
        "nr_calib_images": 129,
        "stabelized": false
    }
    Zintrinsic_typeZFISHEYE
intrinsicsimage_heightimage_widthfocal_lengthZprincipal_pt_xZprincipal_pt_yZradial_distortion_1Zradial_distortion_2Zradial_distortion_3Zradial_distortion_4dtyper   r   r      )DIMKD)nparrayint64float64T)	r   Z	intr_datahwfpxpyZkb8opencv_intr_dict r   ?/data/cameron/vidgen/unified_video_action/umi/common/cv_util.pyparse_fisheye_intrinsics   s"   $r   r   target_resolutionc                 C   s   | d \}}| d }|d }|d }|d }|d }|\}	}
|| |
 }|| |
 }||d  | |
 |	d  }|| |
 }t j|d|gd||gg d	gt jd
}t| }t j|	|
gt jd
|d< ||d< |S )z
    Convert fisheye intrinsics parameter to a different resolution,
    assuming that images are not cropped in the vertical dimension,
    and only symmetrically cropped/padded in horizontal dimension.
    r   r   )r   r   )r   r   )r      )r   r    r    r   r   r
   )r   r   r   copydeepcopyr   )r   r   iwihiKZifxZifyipxZipyowohZofxZofyZopxZopyZoKZout_intr_dictr   r   r   %convert_fisheye_intrinsics_resolutionC   s    &
r)   c                   @   s   e Zd Zdd Zdd ZdS )FisheyeRectConverterc           
      C   s   t |}|d d t |d t j d  }t j|d|d d gd||d d gg dgt jd}tj||t d||tj	\}}	|| _
|	| _d S )Nr   r       r   r   r
      )r   r   tanpifloat32cv2fisheyeZinitUndistortRectifyMapeyeZCV_16SC2map1map2)
selfr   r   r   out_sizeZout_fovZout_fZout_Kr3   r4   r   r   r   __init__`   s   
$(
zFisheyeRectConverter.__init__c                 C   s    t j|| j| jt jt jd}|S )N)interpolationZ
borderMode)r0   remapr3   r4   
INTER_AREAZBORDER_CONSTANT)r5   imgZrect_imgr   r   r   forwardo   s   zFisheyeRectConverter.forwardN)__name__
__module____qualname__r7   r<   r   r   r   r   r*   _   s    r*   aruco_config_dictc           	      C   sn   t di | d }t|j}| d }|dd}t }t|D ]}|}||v r+|| }|||< q||d}|S )z
    example:
    aruco_dict:
        predefined: DICT_4X4_50
    marker_size_map: # all unit in meters
        default: 0.15
        12: 0.2
    
aruco_dictmarker_size_mapdefaultN)rA   rB   r   )get_aruco_dictlen	bytesListgetdictrange)	r@   rA   Z	n_markersrB   default_sizeZout_marker_size_map	marker_idsizeresultr   r   r   parse_aruco_config{   s   	


rN   
predefinedc                 C   s   t jtt j| S )N)r0   arucogetPredefinedDictionarygetattr)rO   r   r   r   rD      s   rD   Tr;   rA   rB   fisheye_intr_dictrefine_subpixc              	   C   s   |d }|d }t j }|rt jj|_t jj| ||d\}}	}
t|dkr)t S t }t|	|D ]:\}}t	|d }||vr@q1|| }t j
j||||d}t j|||td\}}}| | | d||< q1|S )Nr   r   )image
dictionary
parametersr   )P)r      )rvectveccorners)r0   rP   ZDetectorParametersZCORNER_REFINE_SUBPIXZcornerRefinementMethodZdetectMarkersrE   rH   zipintr1   ZundistortPointsZestimatePoseSingleMarkersr   zerossqueeze)r;   rA   rB   rS   rT   r   r   paramr\   idsZrejectedImgPointstag_dictZthis_idZthis_cornersZmarker_size_mundistortedrZ   r[   ZmarkerPointsr   r   r   detect_localize_aruco_tags   s2   


re   2   )   rY      c                 C   s:   t j| j|d  | j} t jj||d |d | d}|S )N  )rL   ZsquareLengthZmarkerLengthrV   )r0   rP   
DictionaryrF   Z
markerSizeZCharucoBoard)rA   Ztag_id_offset	grid_sizesquare_length_mmZtag_length_mmboardr   r   r   get_charuco_board   s   rn   ,     c           	      C   sh   t |  }|  d }d}|| |d  | | }| t j}t|| | }| j||d}|S )Nri   gffffff9@r    )ZoutSizeZ
marginSize)	r   r   ZgetChessboardSizeZgetSquareLengthroundastyper   r^   ZgenerateImage)	rm   dpiZ
padding_mmrk   rl   Zmm_per_inchZboard_size_pixelZpadding_pixelZ	board_imgr   r   r   draw_charuco_board   s   rt   ;On?Mb?c                 C   s   || }|| }d }|| v r&| | d }||d   k r |k r&n n|d }d }	|| v rD| | d }||d   k r>|k rDn n|d }	d }
|d urT|	d urT|	| }
|
S |d ur`t |d }
|
S |	d urjt |	d }
|
S )Nr[   r   r    )abs)rc   Zleft_idZright_idZ	nominal_zZz_toleranceZzmaxzminZleft_xr[   Zright_xwidthr   r   r   get_gripper_width   s,   r{     
  c                 C   s.   t | |d  t |d d d d  }|S )Nr   rw         ?r   asarrayr   )coords	img_shapeptsr   r   r   canonical_to_pixel_coords      *r   c                 C   s.   t | t |d d d d  |d  }|S )Nrw   r   r   r   )r   r   r   r   r   r   pixel_coords_to_canonical  r   r   r   colorc                 C   s:   t || jd d }t|tj}tj| ||d | S )Nr    r   )r   shaper   rq   rr   int32r0   fillPoly)r;   r   r   r   r   r   r   draw_canonical_polygon  s   r   c                  C   sl   ddgddgddgddgdd	gd
d	gg} ddg}t | |}| }|d d df  d9  < t||g}|S )Ni    i  i  iN  i.  "  ij  i  i&  r}   r~   r   rw   r   r!   r   stackleft_pts
resolutionleft_coordsZright_coordsr   r   r   r   get_mirror_canonical_polygon     
r   i8  i  c                 C   s   ddgddgg}ddg}t ||}|s|d d df  d9  < t|| d	}t|tj}tt|d d d
f t|d d d
f tt|d d df t|d d df f}|S )Nr   i`    i  r}   r~   r   rw   r   r   )	r   r   r   rq   rr   r   sliceminmax)r   leftr   r   r   slicesr   r   r   get_mirror_crop_slices  s   
,,r   c                  C   sl   ddgddgddgddgdd	gdd
gg} d	d
g}t | |}| }|d d df  d9  < t||g}|S )NiH  i  iL  r   r   i  r   iF  r}   r~   rw   r   r   r   r   r   get_gripper_canonical_polygon-  r   r   Gz?      ?ffffff?c                 C   s   ddg}|\}}d|  }d}|| }|d }	|	|d  }
|	|d  }|	|d  }|	|d  }||9 }||9 }|
|9 }
||9 }||9 }||9 }||g|
|g||g||ggg}t ||d}|S )Nr}   r~         ?       @r   )r   )height	top_widthbottom_widthr   img_himg_wtop_ybottom_yrz   middle_x
top_left_xtop_right_xbottom_left_xbottom_right_xpointsr   r   r   r   get_finger_canonical_polygon>  s0   r   r   r   r   Fc           
      C   s   t  }|r|t  |r|t  |r|t  |D ]'}t|| jd d }t|	tj
}|r7tjntj}	tj| |g||	d q| S )Nr    r   ZlineType)listextendr   r   r   r   r   r   rq   rr   r   r0   LINE_AAZLINE_8r   )
r;   r   mirrorgripperfingeruse_aaZ
all_coordsr   r   flagr   r   r   draw_predefined_maska  s   r   c                 C   s   | j d }| j d }d| }d}|| }	|	d }
|
|d  }|
|d  }|
|d  }|
|d  }||9 }||9 }||9 }||9 }||9 }||9 }tj||g||g||g||gggtjd}tj| ||tjd} | S )Nr   r   r   r   r
   r   )r   r   r   r   r0   r   r   )r;   r   r   r   r   r   r   r   r   rz   r   r   r   r   r   r   r   r   r   get_gripper_with_finger_maskt  s6   

r      c           	      C   s   t j|dd}|||  | }tjg dt||d g ddt dd|t j}| t |d d df d| j	d d t |d d df d| j	d d f }t j
|dd| j}tj| |d t j| d} | S )Nr   )axis)r   r   r    r,      r   r   )N.r   )r   meansiinterp1dr   linspacerr   r   clipr   medianr   r0   r   tolist)	r;   r\   Z	tag_scale	n_samplescenterZscaled_cornersZsample_pointsZsample_colorsZmedian_colorr   r   r   inpaint_tag  s&   ""r   r   
crop_ratio
bgr_to_rgbc                    s   | \\}}t | }t | | | }tj| d }t||| | d }	t|	|	| td  |rAtd d d dtjf fdd}
|
S )Nr    rw   r;   c                    s6   | j dfks
J |  f } tj| d} | S )Nr,   )r8   )r   r0   resize)r;   Zc_sliceZh_slicer$   Zinterp_methodr#   out_resZw_slicer   r   	transform  s   z&get_image_transform.<locals>.transform)rq   r0   r:   r   r   ndarray)Zin_resr   r   r   r'   r(   chcwZw_slice_startZh_slice_startr   r   r   r   get_image_transform  s    r   )T)ro   rp   )ru   rv   )r|   )r   T)r   r   r   )r   TTTF)r   r   r   r   )r   r   )r   F),typingr   r   jsonpathlibmathr!   numpyr   r0   scipy.interpolateinterpolater   rH   strr   r   r^   r)   r*   rN   rP   rj   rD   floatboolre   rQ   ZDICT_4X4_100rn   rt   r{   r   r   tupler   r   r   r   r   r   r   r   r   r   r   r   r   <module>   sr    5

	

&






$


)