
    0	h_E                        d dl  d dlZd dlmZ d dlZd dlZd dlmZ d dl	mc m
Z d dlZd dlZddlmZ ddlmZmZ d;dej&                  dej&                  d	eeej,                  f   d
ededej&                  fdZd;dej&                  dej&                  d	eeej,                  f   d
ededej&                  fdZd;dej&                  dej&                  d	eeej,                  f   d
ededej&                  fdZd<dedededej8                  dej:                  dej&                  fdZdej&                  dededej&                  fdZdej&                  fdZ dej&                  fdZ!d=dej&                  dej&                  defd Z"d!ej&                  fd"Z#d#ej&                  fd$Z$d%ej&                  fd&Z%d>d#ej&                  d'ej&                  dej&                  d(e&eef   fd)Z'	 d?d*eej&                  e(ej&                     df   d'ejR                  d+e&eef   d,ede&eej&                  e(ej&                     df   ejR                  e&ejT                  d-f   f   f
d.Z+d@d/ej&                  d'ej&                  d0e,d1   d2edef
d3Z-dAd/ej\                  d'ejR                  ded4efd5Z/dAd/ej\                  d'ejR                  ded4efd6Z/dBdej&                  d'ejR                  d7e,d8   d9edej&                  f
d:Z0y)C    )*N)
namedtuple   )timeit)solve_optimal_focal_shiftsolve_optimal_shiftxwdimkeepdimepsreturnc                     || j                  ||      S |j                  | j                        }| |z  j                  ||      |j                  ||      j                  |      z  S )Nr   r   )meantodtypeaddr	   r
   r   r   r   s        E/home/cameronsmith/repos/controll3r/MoGe/moge/utils/geometry_torch.pyweighted_meanr      sa    yvv#wv//DDMA||W|53PW8X8\8\]`8aaa    c                 L   |>| j                  |      j                         j                  ||      j                         S |j                  | j                        }t        | j                  |      j                         ||||      j                  |      j                         S )Nr   r   r   r   )r   
reciprocalr   r   r   r   r   s        r   harmonic_meanr      s    yuuSz$$&++W+EPPRRDDMQUU3Z224aS'WZ[__`cdooqqr   c                 ,   |=| j                  |      j                         j                  |      j                         S |j	                  | j
                        }t        | j                  |      j                         ||||      j                         S )Nr   r   )r   logr   expr   r   r   r   s        r   geometric_meanr!       sp    yuuSz~~$$$-1133DDMQUU3Z^^-qc7PSTXXZZr   widthheightaspect_ratior   devicec                 h   || |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   r%   xyindexingr   )torchlinspacemeshgridstack)
r"   r#   r$   r   r%   span_xspan_yuvuvs
             r   normalized_view_plane_uvr7   (   s    v~Q!22s::F!la''C//Fw%!),u4f	6JU6RTYafouvAw&1*-6&1*8MPV8VX^fktz{A>>!Q.DAq	aV	$BIr   inputkernel_sizesigmac                    t        j                  t        j                  | dz  dz   |dz  dz   | j                  | j                        dz   d|dz  z  z        }||j                         z  }|d d d f   |d d d f   z  j                  dd||      }t        j                  | |dz  |dz  |dz  |dz  fd      } t        j                  | || j                  d         } | S )Nr'   r   r)   	replicatemode)groups)r.   r    aranger   r%   sumreshapeFpadconv2dshape)r8   r9   r:   kernels       r   gaussian_blur_2drH   7   s   YY{la&7!&;[A=MPQ=QY^YdYdmrmymyz~  A  EF  IN  RS  IS  ES  T  UFfjjl"FQWotQw/88A{KXFEE%+*K1,<kQ>NP[_`P`ahstEHHUF5;;q>:ELr   focalc                 8    dt        j                  d| z        z  S )Nr'   r(   r.   atan)rI   s    r   focal_to_fovrM   @   s    uzz#+&&&r   fovc                 8    dt        j                  | dz        z  S )Nr(   r'   )r.   tan)rN   s    r   fov_to_focalrQ   D   s    37###r   v1v2c                     t        j                  t        j                  | |d      j                  d      |z   | |z  j	                  d            S )Nr-   r   )r.   atan2crossnormrA   )rR   rS   r   s      r   angle_diff_vec3rX   H   sC    ;;u{{2rr277B7?#ER}}Y[}G\]]r   
intrinsicsc                     | d   }| d   }dt        j                  d|z        z  dt        j                  d|z        z  fS )z
    Returns field of view in radians from normalized intrinsics matrix.
    ### Parameters:
    - intrinsics: torch.Tensor of shape (..., 3, 3)

    ### Returns:
    - fov_x: torch.Tensor of shape (...)
    - fov_y: torch.Tensor of shape (...)
    ).r   r   ).r   r   r'   r(   rK   )rY   focal_xfocal_ys      r   intrinsics_to_fovr]   K   sG     #G#Guzz#-((!ejjw.G*GGGr   pointsc           	         | j                   dd \  }}|dz  |dz  z   dz  }t        ||| j                  | j                        }|| ddd f   z  j	                  dd      }t        j                  | dd df   |j                  | dd df          gd      j	                  dd	      }|j                  d	d      |z  }t        j                  |d
t        j                  d      j                  |      z  z         |j                  d	d      |d   z  z  j                  d      }|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)rF   r7   r   r%   flattenr.   r1   	expand_as	transposeinverseeyer   squeezeunbindrL   )r^   r#   r"   diagonalr6   bAMsolutionrI   shiftdepthfov_xfov_ys                 r   point_map_to_depth_legacyrt   Z   s   LLB'MFE!eqj(S0H	!%v||FMM	ZB 
fS!"Wo	&&r2.AVC!G_r||F37O'D&DE2NVVWY[]^A	BaAa$1);";;<BPR@SVWXaVb@bcllmopH??2&LE56NU?33EJJux'%/014EJJv(501A5E%%%r   r6   c                    t        | j                  d   | j                  d   | j                  | j                        }| |z  j	                         | j                         j	                         j                  d      z  }|S )Nrb   r`   )r"   r#   r%   r   -q=)r7   rF   r%   r   rA   squarer   )r6   	normed_uvrI   s      r   view_plane_uv_to_focalry   m   sd    (rxx|BHHRLY[YbYbjljrjrsI)^  "RYY[__%6%:%:5%AAELr   maskdownsample_sizec                    | j                   }| j                   d   | j                   d   }}|dz  |dz  z   dz  } | j                  dg|dd  } |dn |j                  dg|dd  }||j                  d      nd}t        ||| j                  | j                        }t        j                  | j                  dd	d
d      |d      j                  ddd	d
      }	t        j                  |j                  d      j                  dd	d
d      |d      j                  d      j                  d
dd      }
|dnUt        j                  |j                  t        j                        j                  d
      |d      j                  d
      dkD  }|
j                         j                         }|	j                         j                         j                         }||j                         j                         nd}|dn|j                         j                         }g g }}t!        | j                   d         D ]  }|||   n
||   ||      }||n|||      }|j                   d   dk  r#|j#                  d
       |j#                  d       V|*t%        ||      \  }}|j#                  t'        |             nt)        ||||         }|j#                  t'        |              t        j*                  || j                  | j                        j                  |dd       }|Bt        j*                  || j                  | j                        j                  |dd       }||fS |j                  |dd       }||fS )a  
    Recover the depth map and FoV from a point map with unknown z shift and focal.

    Note that it assumes:
    - the optical center is at the center of the map
    - the map is undistorted
    - the map is isometric in the x and y directions

    ### Parameters:
    - `points: torch.Tensor` of shape (..., H, W, 3)
    - `downsample_size: Tuple[int, int]` in (height, width), the size of the downsampled map. Downsampling produces approximate solution and is efficient for large maps.

    ### Returns:
    - `focal`: torch.Tensor of shape (...) the estimated focal length, relative to the half diagonal of the map
    - `shift`: torch.Tensor of shape (...) Z-axis shift to translate the point map to camera space
    r`   rb   r'   r(   r-   Nr)   r      r   nearestr=   r%   r   )rF   rB   r7   r   r%   rC   interpolatepermute	unsqueezeri   r   r.   float32cpunumpydetachrangeappendr   floatr   tensor)r^   rz   rI   r{   rF   r#   r"   rk   r6   	points_lruv_lrmask_lruv_lr_nppoints_lr_npfocal_np
mask_lr_npoptim_shiftoptim_focalipoints_lr_i_np
uv_lr_i_npoptim_shift_ioptim_focal_is                          r   recover_focal_shiftr   s   sV   " LLELL$fll2&6EF!eqj(S0HV^^B,rs,F<4\T\\"%DuR|%DD!&!2EMM"E	!%v||FMM	ZBfnnQ1a8/PYZbbcdfgijlmnIMM",,q/11!Q1=U^_gghijrrstvwyz{Elddggemm6L6V6VWX6Y[jqz({  )D  )D  EF  )G  JK  )KGyy{  "H##%))+113L&+&7uyy{  "TH7;;=+>+>+@J!2K6<<?# 1,0Lal1ojYZm>\!%X8JqM3J
A"q!q!=+DZQ_+`(M=u]34/
NHUVKXM5/01 ,,{6==U]]^cdgeg^hiK}ll;v}}FLLYaabghkikblm ## mmE#2J/##r   inputssizereturn_index.c           
      p
  ( |j                   dd \  }}|\  }}|j                  }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        j                  |      }||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                   ||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                  |||      }|j%                  |      }t        j&                  |t        j(                  ||d   z
  d      t        j*                        }t        j,                  |dd      }t        j.                  ||d      j1                  d      } t        j2                  |d      }!| |z  | |z  }#}"t5        |j                   dd       D $%cg c]N  \  }$}%t        j                  |%|      j                  dg|$z  |%gz   dg|j7                         |$z
  dz
  z  z         P }&}$}%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 (target_width, target_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, .
    rb   Nr   r'   )r"   r#   r   r%   r)   r   F.)r   r   r   rb   r-   ).r   ).r   rc   r-   Tr   )indexr   )r%   c              3   (   K   | ]	  }|     y w)N ).0r	   r   s     r   	<genexpr>z,mask_aware_nearest_resize.<locals>.<genexpr>   s     1Q%1s   zInvalid input type: )"rF   r%   maxmathceilutils3dr.   image_pixel_centerr   r@   longrB   fullboolsliding_window_2dimage_uvr   roundre   whererW   infargmingatherri   any	enumerater   
isinstanceTensorSequencetuple
ValueErrortype))r   rz   r   r   r#   r"   target_widthtarget_heightr%   
filter_h_f
filter_w_f
filter_h_i
filter_w_ifilter_size	padding_h	padding_wr6   indices	padded_uvpadded_maskpadded_indiceswindowed_uvwindowed_maskwindowed_indices	target_uvtarget_lefttoptarget_windowtarget_window_uvtarget_window_masktarget_window_indicesdistr~   nearest_idxtarget_mask	nearest_i	nearest_jr   nbatch_indicesoutputsr   s)                                           @r   mask_aware_nearest_resizer      s   & JJrsOMFE"&L-[[F F]$:;SELDX=Y
J!YYz2DIIj4I
Jz)K%?Q.
a!0CyI 
	)	)fEMMbh	)	iBll6E>FKSSTZ\abG

FQ]2EA	M4I1MqX]XeXentuIKMIi	F**Ii%6G,GGH**^tzz#2^Y0F^PQT]P]H]^`emrmwmw  AG  HKRVKYy6119Y=N3NNOZZ!i-!7Y9N OQRZ_ZdZdmstNPWN9Y//9u;L1LLM--11)j*=UWX^d1eKMM33K*jAY[\bj3kM}}66~
T^G_abhn6o &&\-W\WdWdms&tw|  xD  xD  FK  MS  ET  \a  \i  \i  rx  xy  yIzA~zA~.NV[VcVclr!ssNKK/446yR[F\didndnw}9~~M"=#8-:OQRTUWX#XYaaboq}  @A  CN  O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1;;<NO ;;)5::6FS\I]6]ce+fhmhqhqrDll4R6G,,4GLTTUWXK)).B7K&%/u1DyIt}  C  I  I  JM  KM  N  uO  PlplmopU\\!F3;;QC!GqcMQCSWS[S[S]`aSadeSeLf<fg  PM  P2m2Y2	2E~	FELL	)-	FH	%1&11/V~>??U**##! Ps   'AT2rq   pooler)minr   rtolc                    | j                   ^ }}}| j                  dd||      } |j                  dd||      }|dk(  rLt        j                  t	        j
                  || t        j                         |d|dz        }|| d|z   z  kD  }	n`|dk(  rMt        j                  t	        j
                  || t        j                         |d|dz         }|| d|z
  z  k  }	nt        d|        |	j                  g ||| }	|	S )Nr-   r   r   r'   stridepaddingr   zUnsupported pooler: )rF   rB   rC   
max_pool2dr.   r   r   r   )
rq   rz   r   r   r9   batch_shaper#   r"   pooled_depthoutput_masks
             r   theshold_depth_changer      s   "'++[&%MM"a/E<<Avu-D~||EKKeeiiZ$H+^_itxyiyz"Ua$h%77	%ekk$uyy&I%I;_`juyzjz{{#eq4x&88/x899%+%%B{BFBEBKr   tolc                    | j                   | j                  }}t        j                  |d| z  d      }t	        j
                  ||dz  |dz  |dz  |dz  fd      }t	        j
                  ||dz  |dz  |dz  |dz  fd      }t        j                  j                  |||fdd      j                  d      }	t        j                  j                  |||fdd      j                  d      }
t        j                  | dz  |dz  |||	      }t        j                  g t        j                  ||d
      t        j                  ||f||	      d      j                  |dz  d      }|
d   |z  }t        j                  d||	      }|	dd d d f   |z  t        j                  |j                   |z  d|z  z         z  |j                   z  j#                  d      ddd d f   }t        j                  |
t        j$                  ||	      t        j&                  ||	      z  dz
  d      }|||kD  j)                  d      z  }t+        |	|
d      }|||kD  z  }|| z  }||fS )Nr   r   r'   valueFr   r   rb   r   r*   r+   r-   r}   rc   .gh㈵>rv   )r%   r   r.   r   rC   rD   r   r   rd   r/   r1   r0   onesrB   rh   rg   mT	clamp_minmaximumminimumr   r   )rq   rz   r9   r   r%   r   dispdisp_padmask_paddisp_windowmask_windowr	   rm   Iaffine_disp_windowdiff	edge_mask	disp_meanfg_edge_maskbg_edge_masks                       r   depth_occlusion_edger     s   LL%++EF;;tQY*DuuTK1,kQ.>q@PR]abRbcklmHuuTK1,kQ.>q@PR]abRbckpqH--11([+<VXY_g1hppqstK--11([+<VXY_g1hppqstK|q(+*:KPV^cdA~ennQD9~5::{T_F`iow|;}~  EG  	H  	P  	P  Q\  `a  Qa  cd  	eAI"A		!F%0A%c4l3a7%--qSWZ[S[H[:\\_`_c_ccnnotuvy{|~v  A;;{EMM2Dk$RUZUbUbcu  xC  VD  %D  GH  %H  JK  LDs
''B'//Ik;B?Iy 01L},L%%r   c                    | j                   | j                  }}t        j                  |d| z  d      }t	        j
                  ||dz  |dz  |dz  |dz  fd      }t	        j
                  ||dz  |dz  |dz  |dz  fd      }t        j                  j                  |||fdd      }	t        j                  j                  |||fdd      }
t        |	|
d      }|||z  d|z   kD  z  }|||z  d|z   kD  z  }|t	        j                  |j                         |dz   d|dz  dz         j                         z  }|t	        j                  |j                         |dz   d|dz  dz         j                         z  }||fS )	Nr   r   r'   r   Fr   r   r   )r%   r   r.   r   rC   rD   r   r   r   r   r   r   )rq   rz   r9   r   r%   r   r   r   r   r   r   r   r   r   s                 r   r   r   "  s   LL%++EF;;tQY*DuuTK1,kQ.>q@PR]abRbcklmHuuTK1,kQ.>q@PR]abRbckpqH--11([+<VXY_g1hK--11([+<VXY_g1hKk;HEI4)+a#g56L9t+a#g56L!,,|/A/A/C[ST_]^hswxhx{|h|"}  #C  #C  #E  EL!,,|/A/A/C[ST_]^hswxhx{|h|"}  #C  #C  #E  EL%%r   filter)r   r   r   median
iterationsc           
         t        j                  g dg dg dg| j                  t         j                        }t	        |      D ]  }t
        j                   j                  t        j                  | ddd      dd	d
      }|t
        j                   j                  t        j                  |ddd      dd	d
      z  }|dk(  rVt        j                  || t        j                  ||t         j                        j                  d
      j                        } n|dk(  rVt        j                  || t        j                  ||t         j                         j                  d
      j                        } n|dk(  rKt        j                  || t        j                  ||t         j                        j                  d
            } nh|dk(  rct        j                  || t        j                  ||t         j                        j!                  d      j#                  d      j                        } |j%                  d
      } | |fS )N)FTF)TTTr   )r   r   r   r   constantr   )r>   r   r}   r   r   )window_sizer   r   Fr   r   r   r   r   rb   r-   )r.   r   r%   r   r   r   r   rC   rD   r   r   r   valuesr   nannanmeanrd   	nanmedianr   )r8   rz   r   r   rG   _input_windowr   s           r   dilate_with_maskr	  5  s   \\/1CEYZchcocow|  xB  xB  CF: -}}66quuULWaij7kyz  DE  KS6  Tw}}>>quuT<^hpu?v  EF  OP  V^>   _  _E>KKeU[[lTYT]T]-^-b-bgo-b-p-w-wxEe^KKeU[[lUZU^U^T^-_-c-chp-c-q-x-xyEvKKeU[[lTYT]T]-^-f-fks-f-tuEhKKeU[[lTYT]T]-^-f-fgi-j-t-ty{-t-|  .D  .D  EE8,- $;r   )NNFgHz>)NNN)rv   )NN)@   r
  )F)g?r}   )r}   g?)r   r   )1typingr   collectionsr   r   npr.   torch.nnnntorch.nn.functional
functionalrC   torch.typesr   toolsr   geometry_numpyr   r   r   UnionintSizer   r   r   r   r!   r   r%   r7   rH   rM   rQ   rX   r]   rt   ry   Tupler   r   
BoolTensor
LongTensorr   Literalr   FloatTensorr   r	  r   r   r   <module>r     s&     "         JbU\\ bell bc5::o@V bhl b  |A b  MR  MY  MY brU\\ rell rc5::o@V rhl r  |A r  MR  MY  MY r[ell [u|| [sEJJAW [im [  }B [  NS  NZ  NZ [C  E Y^YdYd uz  vB  vB   NS  NZ  NZ ELL s 5 U\\ ' '$ell $^ ^%,, ^U ^H%,, H&ell &&u|| 7$ 7$ELL 7$PUP\P\ 7$v{|  BE  }E  wF 7$| 	H$%,, 6<=H$


H$ S/H$ 	H$
 5x5t;<e>N>NPUV[VfVfhkVkPllmH$V U\\ 7S_K` hm   CF  & 1 1 &9I9I &X[ &fk &4& 1 1 &9I9I &X[ &fk &&ELL 0@0@ 'RpJq   IL   UZ  Ua  Ua r   