
    0	hJ                        d dl  d dlmZ d dlZd dlZd dlZd dlmZ d dlZd dl	Z	ddl
mZ d9dej                  dej                  d	eeeed
f   f   dededej                  fdZd9dej                  dej                  d	eeeed
f   f   dededej                  fdZdej(                  fdedededej*                  dej                  f
dZdej                  fdZdej                  fdZdej                  deej                  ej                  f   fdZdej                  fdZdej                  dej                  fdZdej                  dej                  defd Zd:dej                  d!ej                  ded"eeef   fd#Z	 d;d$eej                  eej                  d
f   df   d!ej                  d%eeef   d&edeeej                  eej                  d
f   df   ej                  eej                  d
f   f   f
d'Zd(ej                  d!ej                  d)ed*edeeej                  d
f   ej                  f   f
d+Zdej                  dej                  fd,Z d<d-ej                  d!ej                  d.ed/efd0Z!d1edej                  fd2Z"d(ej                  d1edej                  fd3Z#	 d=d4ej                  d5ej                  d6ed7edej                  f
d8Z$y)>    )*)partialN)fftconvolve   )timeitxwaxis.keepdimsepsreturnc                     |t        j                  | |      S |j                  | j                        }| |z  j                  |      t        j                  |j                  |      |d       z  S )Nr
   )npmeanastypedtypeclipr   r	   r
   r   r   s        E/home/cameronsmith/repos/controll3r/MoGe/moge/utils/geometry_numpy.pyweighted_mean_numpyr      s\    ywwqt$$HHQWWA|||&T1BC)NNN    c                     |-ddt        j                  | |d       z  j                  |      z  S |j                  | j                        }dt        d| |z   z  ||||      |z   z  S )Nr   r   )r
   r   r   )r   r   r   r   r   r   r   s        r   harmonic_mean_numpyr      sl    yA3--333>>>HHQWW'QWqth\_`cffggr   widthheightaspect_ratior   c                 d   || |z  }|d|dz  z   dz  z  }dd|dz  z   dz  z  }t        j                  | | dz
  z  | z  || dz
  z  | z  | |      }t        j                  | |dz
  z  |z  ||dz
  z  |z  ||      }t        j                  ||d      \  }}t        j                  ||gd      }|S )	zUV with left-top corner as (-width / diagonal, -height / diagonal) and right-bottom corner as (width / diagonal, height / diagonal)r            ?r   xy)indexingr   )r   linspacemeshgridstack)	r   r   r   r   span_xspan_yuvuvs	            r   normalized_view_plane_uv_numpyr-      s    v~Q!22s::F!la''C//F
VGuqy)E16UQY3G%3OQV^cdA
VGvz*V3Vvz5JV5SU[chiA;;q!d+DAq	1a&r	"BIr   focalc                 8    dt        j                  d| z        z  S )Nr   r    )r   arctan)r.   s    r   focal_to_fov_numpyr1   -   s    ryyu%%%r   fovc                 8    dt        j                  | dz        z  S )Nr    r   )r   tan)r2   s    r   fov_to_focal_numpyr5   1   s    a  r   
intrinsicsc                 B    t        | d         }t        | d         }||fS )N).r   r   ).r   r   )r1   )r6   fov_xfov_ys      r   intrinsics_to_fov_numpyr:   5   s*    z)45Ez)45E%<r   pointsc                    | j                   dd \  }}|dz  |dz  z   dz  }t        ||| j                        }t        j                  | dd df   |      \  }} || ddd f   z  j
                  g | j                   d d d } t        j                  | dd df   | gd      j
                  g | j                   d d dd }|j                  dd      |z  }t        j                  j                  |d	t        j                  d      z  z         |j                  dd      |d
   z  z  j                  d      }	|	\  }
}| d   |d   z   }t        j                  ||z  |
z        dz  }t        j                  ||z  |
z        dz  }||||fS )Nr$   r   r    r!   .r   gư>.N.r   ).NN)shaper-   r   r   broadcast_arraysreshaper'   swapaxeslinalginveyesqueezer0   )r;   r   r   diagonalr,   _bAMsolutionr.   shiftdepthr8   r9   s                  r   point_map_to_depth_legacy_numpyrQ   ;   s   LLB'MFE!eqj(S0H	'vV\\	JBsBQBw4EAr 	'fS!"Wo	&&>Sb(9>2>A9&bqb/B3'b199T6<<;LTbTRSTA	

2rQA		a$"223qzz"b7IAiL7XYbbcefHLE56NU?33EIIeh&./!3EIIfx'%/014E%%%r   r,   xyzc                 8   ddl m} | j                  dd      |dddf   j                  dd      |d   j                  d      }}} dt        j                  d	t        j                  d
t        j                  dt        j                  fd} |t        || ||      ddd      }|d   j                         j                  t        j                        }|||z   dddf   z  }|| z  j                         t        j                  |      j                         z  }	||	fS )zKSolve `min |focal * xy / (z + shift) - uv|` with respect to shift and focalr   least_squaresr$   r   .Nr@   r,   r"   zrO   c                     |||z   d d d f   z  }|| z  j                         t        j                  |      j                         z  }||z  | z
  j                         }|S N)sumr   squareravel)r,   r"   rV   rO   xy_projferrs          r   fnz%solve_optimal_focal_shift.<locals>.fnT   s^    E	1t8,,r\ 299W#5#9#9#;;7{R&&(
r   MbP?lmx0ftolmethodr   )scipy.optimizerU   rC   r   ndarrayr   rH   r   float32rY   rZ   )
r,   rR   rU   r"   rV   r_   rN   optim_shiftr\   optim_focals
             r   solve_optimal_focal_shiftrk   O   s    ,

2q!3sBQBw<#7#7A#>F@S@STV@WABrzz rzz bjj   WRR3TRH3-'')00<KAOQX..GR<$$&7);)?)?)AAK##r   c                    ddl m} | j                  dd      |dddf   j                  dd      |d   j                  d      }}} dt        j                  d	t        j                  d
t        j                  dt        j                  ffd} |t        || ||      ddd      }|d   j                         j                  t        j                        }|S )zASolve `min |focal * xy / (z + shift) - uv|` with respect to shiftr   rT   r$   r   .Nr@   r,   r"   rV   rO   c                 R    |||z   d d d f   z  }|z  | z
  j                         }|S rX   )r[   )r,   r"   rV   rO   r\   r^   r.   s         r   r_   zsolve_optimal_shift.<locals>.fnh   s6    E	1t8,,w#**,
r   r`   ra   rb   r   )	rf   rU   rC   r   rg   r   rH   r   rh   )	r,   rR   r.   rU   r"   rV   r_   rN   ri   s	     `      r   solve_optimal_shiftrn   c   s    ,

2q!3sBQBw<#7#7A#>F@S@STV@WABrzz rzz bjj  
 WRR3TRH3-'')00<Kr   maskdownsample_sizec                    dd l }| j                  d   dk(  sJ d       | j                  d   | j                  d   }}|dz  |dz  z   dz  }t        ||	      }|_ |j                  | ||j                  
      j                  dd      }	 |j                  |||j                  
      j                  dd      }
nt        | |f||      \  \  }	}
}|	j                  dk  ry|t        |
|	      \  }}||fS t        |
|	|      }||fS )Nr   r$      zPoints should (H, W, 3)r=   r>   r   r    )r   r   )interpolation)g      ?g        )
cv2rA   r-   resizeINTER_LINEARrC   mask_aware_nearest_resize_numpysizerk   rn   )r;   ro   r.   rp   rt   r   r   rI   r,   	points_lruv_lrmask_lrrO   s                r   recover_focal_shift_numpyr|   s   s   <<q ;";;LL$fll2&6EF!eqj(S0H	'eF	CB|CJJvcFVFVW__`bdef	

2c>N>NOWWXZ\]^&EvrlTXZi&j#EG~~}0	Bu %< $E9e<%<r   inputsrx   return_indexc           
      x
  ' |j                   dd \  }}|\  }}t        d||z        t        d||z        }	}t        j                  |      t        j                  |	      }}
|
|z  }|
dz  dz   |dz  dz   }}t        j
                  j                  ||t        j                        }t        j                  ||z  t        j                        j                  ||      }t        j                  |d|z  z   |d|z  z   dfdt        j                        }|||||z   |||z   f<   t        j                  g |j                   dd |d|z  z   |d|z  z   dt              }||d	|||z   |||z   f<   t        j                  |d|z  z   |d|z  z   fdt        j                        }|||||z   |||z   f<   t        j
                  j                  ||
|fdd
      }t        j
                  j                  ||
|fdd      }t        j
                  j                  ||
|fdd
      }t        j
                  j                  ||t        j                        t        j                   ||gt        j                        z  }|t        j                   |	dz  |dz  ft        j                        z
  }t        j"                  |      j%                  t        j                        t        j                   ||ft        j                        z   }||d   |d   ddddddf   j                  ||d|      } |d	|d   |d   ddddf   j                  g |j                   dd ||| } ||d   |d   ddddf   j                  g dg|j&                  dz
  z  ||| }t        j(                  ||d   z
        }|d	dddf   |d	dddf   z   }t        j*                  ||t        j,                        }t        j.                  |dd      }t        j0                  ||d      j3                  d      }||z  ||z  }!} t        j4                  |d      }"t7        |j                   dd       D #$cg c]H  \  }#}$t        j                  |$      j                  dg|#z  |$gz   dg|j&                  |#z
  dz
  z  z         J }%}#}$g |%| |!'| d}&n\t9        | t        j:                        r| '   }&n<t9        | t<              rt?        'fd| D              }&ntA        dtC        |              |r|&|"'fS |&|"fS c c}$}#w )a  
    Resize 2D map by nearest interpolation. Return the nearest neighbor index and mask of the resized map.

    ### Parameters
    - `inputs`: a single or a list of input 2D map(s) of shape (..., H, W, ...). 
    - `mask`: input 2D mask of shape (..., H, W)
    - `size`: target size (width, height)

    ### Returns
    - `*resized_maps`: resized map(s) of shape (..., target_height, target_width, ...). 
    - `resized_mask`: mask of the resized map of shape (..., target_height, target_width)
    - `nearest_idx`: if return_index is True, nearest neighbor index of the resized map of shape (..., target_height, target_width) for each dimension.
    r>   Nr   r   r   r   r   r!   r   F.r   r   r   r>   r$   .r   .r   r$   r?   T)r
   r   c              3   (   K   | ]	  }|     y wrX    ).0r   indexs     r   	<genexpr>z2mask_aware_nearest_resize_numpy.<locals>.<genexpr>   s     1Q%1s   zInvalid input type: )"rA   maxmathceilutils3dnumpyimage_pixel_centerr   rh   arangeint32rC   fullboolsliding_window_2dimage_uvarrayroundr   ndimrZ   whereinfargmintake_along_axisrH   any	enumerate
isinstancerg   Sequencetuple
ValueErrortype)(r}   ro   rx   r~   r   r   target_widthtarget_height
filter_h_f
filter_w_f
filter_h_i
filter_w_ifilter_size	padding_h	padding_wr,   indices	padded_uvpadded_maskpadded_indiceswindowed_uvwindowed_maskwindowed_indicestarget_centerstarget_lefttoptarget_windowtarget_window_centerstarget_window_masktarget_window_indicesdistnearest_in_windownearest_idx	nearest_i	nearest_jtarget_maskinbatch_indicesoutputsr   s(                                          @r   rw   rw      s   & JJrsOMFE"&L- F]$:;SELDX=Y
J!YYz2DIIj4I
Jz)K%?Q.
a!0CyI 
	)	)fBJJ	)	WBiibhh7??NG!i-/Y1FJAUWU_U_`IKMIi	F**Ii%6G,GGH''[DJJsO[Va)m-C[UQQZ]EZ[]bjnoKRVKYy6119Y=N3NNOWWfq9}4ea)m6KLaWYW_W_`NPWN9Y//9u;L1LLM--11)j*=UWX_e1fKMM33K*jAY[\ck3lM}}66~
T^G_abio6p ]]++,}\^\f\f+gjljrjrty  |B  tC  KM  KU  KU  kV  VN#bhh
Q
Q/OWYWaWa&bbNHH^,33BHH=)U^I_gigogo@ppM'f(=}V?TVWYZ\](]^ffgt  wC  EF  HS  TgsM&,A=QWCXZ[]^'^_gg  dimisistwuwix  d  {H  d  JV  d  Xc  dh,]6-BMRXDY[\^_-_`hh  nlnkosws|s|  @A  tA  lB  n  ER  n  T`  n  bm  n 99*^I-FFGDQ	?T#q!)_,D88&bff5D		$R$?$$%:<MTVW__`bcK&%/u1DyI&&+"5Kajkokukuvywykza{|Y]YZ\]RYYq\))1#'QC-1#QQRAR:S*ST|M|2m2Y2	2E~	FBJJ	'-	FH	%1&11/V~>??U**##! }s   1AT6imager   r   c           	      	   |j                   dd \  }}| j                   dd ||fk(  rd}nd}|r| d   } t        j                  |d   | d      } t        d||z        t        d||z        }}t	        j
                  |      dz   t	        j
                  |      dz   }
}	|	|
z  }|	dz  dz   |
dz  dz   }}t        j                  j                  ||t        j                  	      }t        j                  ||z  t        j                  
      j                  ||      }t        j                  |d|z  z   |d|z  z   dfdt        j                  
      }|||||z   |||z   f<   t        j                  g |j                   dd |d|z  z   |d|z  z   dt        
      }||d|||z   |||z   f<   t        j                  |d|z  z   |d|z  z   fdt        j                  
      }|||||z   |||z   f<   t        j                  j                  ||	|
fdd      }t        j                  j                  ||	|
fdd      }t        j                  j                  ||	|
fdd      }t        j                  j!                  ||t        j                  	      t        j"                  ||gt        j                  
      z  }|t        j"                  |dz  |dz  ft        j                  
      z
  }|t        j"                  |dz  |dz  ft        j                  
      z   }t        j$                  |      j'                  t        j                        t        j"                  ||ft        j                  
      z   }||d   |d   ddddddf   j                  ||d|      } |d|d   |d   ddddf   j                  g |j                   dd ||| }||d   |d   ddddf   j                  |||      }t        j(                  |dz
  |d         }t        j*                  |dz   |d         }||z
  j-                  dd      }t        j                  ||ddddf   |ddddf   z  d      } | j                  g | j                   dd ||z  d d|ddf   j/                  dd      } t        j0                  |d      dk\  }!t3        | |ddddf   d      }"|r|"d   }"|"|!fS )a0  
    Resize 2D map by nearest interpolation. Return the nearest neighbor index and mask of the resized map.

    ### Parameters
    - `image`: Input 2D image of shape (..., H, W, C)
    - `mask`: Input 2D mask of shape (..., H, W)
    - `target_width`: target width of the resized map
    - `target_height`: target height of the resized map

    ### Returns
    - `nearest_idx`: Nearest neighbor index of the resized map of shape (..., target_height, target_width). 
    - `target_mask`: Mask of the resized map of shape (..., target_height, target_width)
    r>   NTFr?   r   r   r   r   r!   .r   r   r   r   r   r    r=   r$   g      ?)rA   r   r   r   r   r   r   r   r   rh   r   r   rC   r   r   r   r   r   floorr   maximumminimumr   rD   rY   r   )#r   ro   r   r   r   r   omit_channel_dimr   r   r   r   r   r   r   r,   r   r   r   r   r   r   r   target_centerr   target_bottomrightr   r   r   r   target_window_lefttoptarget_window_bottomrighttarget_window_areatarget_window_imager   target_images#                                      r   mask_aware_area_resize_numpyr      s    JJrsOMFE{{23FE?* i HHT)_eQ/E F]$:;SELDX=Y
J!YYz2Q6		*8MPQ8Q
Jz)K%?Q.
a!0CyI 
	)	)fBJJ	)	WBiibhh7??NG!i-/Y1FJAUWU_U_`IKMIi	F**Ii%6G,GGH''[DJJsO[Va)m-C[UQQZ]EZ[]bjnoKRVKYy6119Y=N3NNOWWfq9}4ea)m6KLaWYW_W_`NPWN9Y//9u;L1LLM--11)j*=UWX_e1fKMM33K*jAY[\ck3lM}}66~
T^G_abio6p MM**m[][e[e*fikiqiqsx  {A  sB  JL  JT  JT  jU  UM"RXXzA~zA~.NVXV`V`%aaN&:>:PQ>2RZ\ZdZd)eeHH^,33BHH=)U^I_gigogo@ppM'f(=}V?TVWYZ\](]^ffgt  wC  EF  HS  TgsM&,A=QWCXZ[]^'^_gg  dimisistwuwix  d  {H  d  JV  d  Xc  d,]6-BMRXDY[\^_-_`hhiv  yE  GR  S JJ'<s'BNS\D]^ "

+@3+FHZ[dHe f36KKQQRSUYZ"46HaQR6SVhilnoqrirVs6suvw (%--NSb)9N6E>N2NsTiklOlmvvwy{}~&&+"5=K&':<NsTXZ[|<\cefL#F+$$r   c                     t        j                  t        j                  | d         t        j                  | d         z   t        j                  | d         z         S )z2Faster `np.linalg.norm(x, axis=-1)` for 3D vectorsr   r   r@   )r   sqrtrZ   )r   s    r   norm3dr     sB    77299QvY'"))AfI*>>1V9AUUVVr   rP   kernel_sizetolc                    t        j                  |d| z  d      }t        j                  ||dz  |dz  fd      }t        j                  ||dz  |dz  fd      }t        j                  j                  |||fdd      }t        j                  j                  |||fdd      }t        ||d      }	||d|z   |	z  kD  z  }
||	d|z   |z  kD  z  }|
|fS )Nr   r   r   )constant_valuesFr   r   )r   r   padr   r   r   r   )rP   ro   r   r   dispdisp_padmask_paddisp_windowmask_window	disp_meanfg_edge_maskbg_edge_masks               r   depth_occlusion_edge_numpyr   #  s    88D!e)Q'Dvvd[A-{a/?@RSTHvvd[A-{a/?@RWXH--11([+<VXY`h1iK--11([+<VXY`h1iK#K8LI41s7i"778L9C4'778L%%r   radiusc                     t        j                  |  | dz         }t        j                  ||      \  }}|dz  |dz  z   | dz  k  j                  t         j                        }|t        j
                  |      z  }|S )z
    Generate disk kernel with given radius.
    
    Args:
        radius (int): Radius of the disk (in pixels).
    
    Returns:
        np.ndarray: (2*radius+1, 2*radius+1) normalized convolution kernel.
    r   r   )r   r   r&   r   rh   rY   )r   LXYkernels        r   disk_kernelr   0  sn     			6'6A:&A;;q!DAq!tad{vqy(00<F
bffVnFMr   c                 P   |dk(  r| S t        |      }| j                  dk(  rt        | |d      }|S | j                  dk(  r\g }t        | j                  d         D ]&  }t        | d|f   |d      }|j                  |       ( t        j                  |d      }|S t        d	      )
z
    Apply disk blur to an image using FFT convolution.

    Args:
        image (np.ndarray): Input image, can be grayscale or color.
        radius (int): Blur radius (in pixels).

    Returns:
        np.ndarray: Blurred image.
    r   r   same)moderr   .r$   r   zImage must be 2D or 3D.)	r   r   r   rangerA   appendr   r'   r   )r   r   r   blurredchannelsr   blurred_channels          r   	disk_blurr   D  s     { FzzQeV&9 N 
qu{{1~& 	-A)%Q-fMOOOO,	- ((8"- N 233r   imgr   
focus_dispmax_blur_radiusc                    t        j                  |      }||z  }||z  }g }t        |dz         D ]Y  }|j                  t	        j
                  |t	        j                  t        j                  d|z  dz   d|z  dz   f      d             [ t        j                  t        ||z
        |z  d|      j                  t         j                        }t        |dz         D ]d  }t        j                  t        ||   |z
        |z  d|      j                  t         j                        }||k\  ||k\  z  ||   |kD  z  }	||	   ||	<   f t        j                  |d|      }t	        j                  |d      }t        j                  |      }
i }t        |dz         D ]  }||
vrt        | |      ||<    t        j                  |       }|
D ]  }||k(  }	||   |	   ||	<    |S )a  
    Apply depth of field effect to an image.

    Args:
        img (numpy.ndarray): (H, W, 3) input image.
        depth (numpy.ndarray): (H, W) depth map of the scene.
        focus_depth (float): Focus depth of the lens.
        strength (float): Strength of the depth of field effect.
        max_blur_radius (int): Maximum blur radius (in pixels).
        
    Returns:
        numpy.ndarray: (H, W, 3) output image with depth of field effect applied.
    r   r   )
iterationsr   )   r   )r   r   r   r   rt   dilategetStructuringElementMORPH_ELLIPSEr   absr   r   bluruniquer   
zeros_like)r   r   r   r   max_dispdilated_dispr   
blur_radiidialted_blur_radiiro   unique_radiiprecomputedoutputrs                 r   depth_of_fieldr  _  s   ( vvd|H(?Dh&JL!+, DCJJtS-F-FsGXGX[\]c[cde[eghiogopqgqZr-s  AB  C  	DD TJ.//A1oV]]^`^f^fgJ!+, 4WWSf)=
)J%Ko%]_`bqryyz|  {C  {C  D"f,1Cz1QRVbciVjmqVqr-d3
44 Q8J*f-J 99Z(LK!+, 5%'V4F5 ]]3F ,Q"1~d+t, Mr   )NNFgHz>)NN)@   r	  )F)rr   g?)
   )%typing	functoolsr   r   rt   r   r   scipy.signalr   r   toolsr   rg   UnionintTupler   floatr   r   rh   r   r-   r1   r5   r:   rQ   rk   rn   r|   rw   r   r   r   r   r   r  r   r   r   <module>r     s      
  $   O2:: O"** O5eTWX[T[nI\C] Opt O  DI O  UW  U_  U_ Oh2:: h"** h5eTWX[T[nI\C] hpt h  DI h  UW  U_  U_ h SWjljtjt # s % _a_g_g y{  zD  zD &bjj &!BJJ !

 uRZZ=S7T &BJJ &($"** $2:: $(BJJ RZZ   bjj 

 RW qvwz|w  rA < 	H$"**eBJJO4d:;H$
**H$ S/H$ 	H$
 5U2::s?3T9:BJJbjjZ]oH^^_H$VC%

 C%"** C%TW C%hk C%puv{|~  }G  }G  IL  }L  wM  OQ  OY  OY  wY  qZ C%LWbjj WRZZ W

&bjj 
&

 
&QT 
&_d 
& 

 (RZZ   > 	2	2
**2 2 	2
 ZZ2r   