
     h	                     B    d dl Zd dlZd dlmZ ddej
                  fdZy)    N)xy_gridmediang        c                 4   | j                   \  }}}}|dk(  sJ t        ||| j                        j                  ddd      |j                  ddd      z
  }	| j	                  dd      } |dk(  rt        j                         5  |	j                  d      \  }
}| j                  d      \  }}}|
|z  |z  }||z  |z  }t        j                  |j                  |d      |j                  |d      fd      }t        j                  |d      j                  }ddd       n|d	k(  r| d
ddf   | d
ddf   z  j                  dd      }||	z  j                  d      }|j                         j                  d      }|j                  d      |j                  d      z  }t        d      D ]v  }|	|j                  ddd      |z  z
  j!                  d      }|j#                  d      j%                         }||z  j                  d      ||z  j                  d      z  }x nt'        d|      t)        ||      dt+        j,                  t+        j.                  d      dz        z  z  }j#                  ||z  ||z        }|S # 1 sw Y   _xY w)z Reprojection method, for when the absolute depth is known:
        1) estimate the camera focal using a robust estimator
        2) reproject points onto true rays, minimizing a certain error
       )device      r   )dimN	weiszfeld.r   )posinfneginf
   g:0yE>)minzbad focal_mode=<   )r   max)shaper   r   viewflattentorchno_gradunbindcat	nanmedianvalues
nan_to_numsumsquaremeanrangenormclip
reciprocal
ValueErrorr   nptandeg2rad)pts3dpp
focal_mode	min_focal	max_focalBHWTHREEpixelsuvxyzfx_votesfy_votesf_votesfocal	xy_over_z	dot_xy_px	dot_xy_xyiterdisw
focal_bases                             A/home/cameronsmith/repos/controll3r/dust3r/dust3r/post_process.pyestimate_focal_knowing_depthrC      s   
 [[NAq!UA: Q%,,/44QA>QPQARRFMM!QEX]]_ 		<==R=(DAqllrl*GAq!A{HA{H iiq"!5x}}Q7K LRTUGOOG4;;E		< 		< 
{	" 37^eC1Ho5AASTAU	',,,4	$$&**r*2	1%	1(== "I 	NDEJJr1a09<<BBrBJCT"--/A]((Q(/1y=2F2F12F2MME	N +
}-..Qa"&&B!);"<<=JJJ9Z/Yz5IJJELG		< 		<s   BJJ)numpyr%   r   dust3r.utils.geometryr   infrC        rB   <module>rI      s&      ) 8@2Y[Y_Y_ 0rH   