
     h,                     :   d dl mZ d dlZ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mZmZmZ d dlmZ d dlmZ d dlmZmZmZ  ej.                         dd       Z ej.                         d	        Zd
 Z	 ddZd Zd Zd ZddZed        Z ddZ!d Z"d Z#d Z$y)    )cacheN)tqdm)geotrfinvget_med_dist_between_poses)estimate_focal_knowing_depth)to_numpy)edge_stri_j_ijcompute_edge_scoresc           	      D   | j                   }t        |       \  }}}|| j                  k(  sJ d       t        |       \  }}}	|| j                  k(  sJ | j	                         }
i }t        t        | j                  | j                               D ]6  \  }\  }}t        ||      }t        j                  d|      }| j                  |   t        || j                  |   j                         dz
        kD  }t        | j                  |   t!        |	|   j#                               |
|   |||      \  }}t%        t        j&                  ||f      |||g         \  }}}| j)                  | j*                  ||||       t!        | j                  |   j#                               }||j-                  |d      d	   kD  s/|||f||<   9 t/        | j                        D ]@  }||   sJ ||   \  }}}| j0                  |   d d d d d
f   }| j3                  |||z         B y )Nznot all poses are known)disable   deviceg?)ppmskr   	niter_PnPscaler   r      )r   get_known_posesn_imgsget_known_focalsget_principal_points	enumerater   edgesverboser
   torcheyeconf_iminfast_pnppred_jfloatmeanalign_multiple_posesstack	_set_posepw_posesgetrangepred_i_set_depthmap)selfr   min_conf_thrr   nkpknown_poses_mskknown_posesnkf_	im_focalsim_ppbest_depthmapseiji_jP1r   P2sRTscorenr   depths                             L/home/cameronsmith/repos/controll3r/dust3r/dust3r/cloud_opt/init_im_poses.pyinit_from_known_posesrG      s   [[F )8(=%C+$++888 ).CI$++%%'ENtDJJDLL8HIJ .	6Aqq!n YYq(kk#\4;;s3C3G3G3IC3O!PPS)511B1B1D+E!!H#f	S2 'u{{B8'<k1a&>QR1a 	t}}aAQ7 dkk#&++-.>%%a.q11 %sAN1%.* 4;; -q!!&q)3C Aq)1eem,	-    c                 6   | j                   }t        | j                  | j                  | j                  | j
                  | j                  | j                  | j                  | j                  |f	| j                  | j                  d|\  }}}}t        | |||      S )zq Init all camera poses (image-wise and pairwise poses) given
        an initial set of pairwise estimations.
    )has_im_posesr   )r   minimum_spanning_treeimshapesr   r.   r%   r"   conf_jim_confr1   rJ   r   init_from_pts3d)r0   kwr   pts3dr6   r7   im_posess          rF   init_minimum_spanning_treerS   B   s    
 [[F$9$--:>++t{{TXT_T_aealalnrnznz  }A  }N  }N:@%@OSO`O`jnjvjv%@ =?%@!E1i
 4	8<<rH   c                    t        |       \  }}}|dk(  rt        d      |dkD  r`t        ||   ||         \  }}}	t        |||	|j                        }
|
|z  }|d d d dd dfxx   |z  cc<   |D ]  }t        |
|      |d d   t        | j                        D ]c  \  }\  }}t        ||      }t        | j                  |   ||   | j                  |         \  }}}	| j                  | j                  |||	|       e | j                         }|d d d ddfxx   |z  cc<   |D ]  }||z  }	 | j                  rt!        | j"                        D ]l  }||   }t        t%        |      ||         d   }| j'                  ||       | j                  | j(                  ||       ||   X| j+                  |||          n | j,                  rt/        dt1         |                     y y )	N   zMWould be simpler to just align everything afterwards on the single known poser      confr   ).r   z init loss =)r   NotImplementedErrorr(   
sRT_to_4x4r   r   r   r   r
   rigid_points_registrationr.   r"   r*   r+   get_pw_norm_scale_factorrJ   r-   r   r   r/   rR   
_set_focalr   printr&   )r0   rQ   r7   rR   r2   r3   r4   r@   rA   rB   trf	img_pts3dr:   r;   r<   r=   s_factor	cam2worldrE   s                      rF   rO   rO   P   s   (7(=%C+
ax!"qrr	q&x'@+oB^_1aAq););< >BQBq  	2I!#y1IaL	2 tzz* 8	6Aqq!n+DKK,<eAhT[[Y\M]^1at}}aAQ7	8 ,,.HQAX(" 	X	 t{{# 	1A I3y>584V<Eq%(NN4==!Y7|'9Q<0	1 ||neDFm, rH   c                    t        |       }t        t        t        t        |      ||             }t
        j                  j                  |      j                         }d gt        |       z  }t        t        |j                   |j                  |j                              }d g|z  }d g|z  }|j                         \  }}}|rt        d| d| d|       t!        ||      }||   j#                         ||<   ||   j#                         ||<   ||h}|	r+t%        j&                  d|      ||<   t)        ||         ||<   ||fg}|r|j                         \  }}}||   t)        ||         ||<   ||v r|rt        d| d| d|       ||vsJ t!        ||      }t+        ||   ||   ||         \  }}}t-        ||||      }t/        |||         ||<   |j1                  |       |j3                  ||f       |	r||   t-        d|||      ||<   n||v r|rt        d| d| d	|       ||vsJ t!        ||      }t+        ||   ||   ||         \  }}}t-        ||||      }t/        |||         ||<   |j1                  |       |j3                  ||f       |	r,||   't-        d|||      ||<   n|j5                  d
|||f       |r|	r
t7        |j9                               }t;        j<                  t7        |j?                                     t;        j@                  |         }|jC                         D ]&  \  }}||   t)        |t!        ||               ||<   ( tE        |      D ]R  }||   +||   |kD  }tG        ||   ||   |||
      } | r| \  ||<   ||<   ||   9t%        j&                  d|      ||<   T t%        jH                  |      }nd x}}||||fS )Nz init edge (z*,z	*) score=r   r   ,rW   rU   z) score=r   )r   r   r   )%lendict_to_sparse_graphr   mapr   spcsgraphrK   tocoosortedzipdatarowcolpopr^   r
   cloner    r!   estimate_focalr[   rZ   r   addappendinsertlistvaluesnparraykeysargsorttolistr-   r$   r)   )!rL   r   r.   r%   r"   rM   rN   r1   r   rJ   r   r   r   sparse_graphmsprQ   todorR   r7   rC   r;   r<   r=   done	msp_edgesr@   rA   rB   r_   pair_scoresedges_from_best_to_worser   ress!                                    rF   rK   rK   {   s&   ]F()<S=OQWY_)`aaL
**
*
*<
8
>
>
@C FS]"E#sxxi#''23DvHI ((*KE1aQCr!Jx01
1a.Cc{  "E!Hc{  "E!Hq6Dii&1%fSk2	! QI
hhjq!Q<)&+6IaL9QCq:uh78D= 1a.C/sU1XFSVKXGAq!Q1f-Cc6#;/E!HHHQKaV$ 3(Aq&9$YQCr!Iuh78D= 1a.C/sU1XFSVKXGAq!Q1f-Cc6#;/E!HHHQKaV$ 3(Aq&9 KKE1a=)K N <..01#%88D1B1B1D,E#FrzzR]G^#_ ,335 	FDAq|#-fXa^.DE	!	F v 	:A{"aj</uQx13vYbc03-IaL(1+{"#ii&9	: ;;x(##9)Y00rH   c                     t        d | D              dz   }t        j                  ||f      }| j                         D ]
  \  }}|||<    |S )Nc              3   2   K   | ]  }t        |        y wN)max).0r:   s     rF   	<genexpr>z'dict_to_sparse_graph.<locals>.<genexpr>   s     %AQ%s   rU   )r   rh   	dok_arrayitems)dicr   r   edgevalues        rF   rf   rf      sQ    %%%)F
,,'
(Cyy{ eD	JrH   c                     t        j                  | j                  dd      |j                  dd      |j                         d      \  }}}|||fS )NrV   T)weightscompute_scaling)romar[   reshaperavel)pts1pts2rX   rA   rB   r@   s         rF   r[   r[      sJ    ,,RT\\"a0$**,X\^GAq!a7NrH   c                 ~    t        j                  d|      }|| z  |d dd df<   |j                         |d ddf<   |S )Nr   r   rV   )r    r!   r   )r   rA   rB   r   r_   s        rF   rZ   rZ      sG    
))Af
%Ce)CBQBKCAJJrH   c                    |@| j                   \  }}}|dk(  sJ t        j                  |dz  |dz  f| j                        }t	        | j                  d      |j                  d      d      j                         }t        |      S )NrV   r   r   r   	weiszfeld)
focal_mode)shaper    tensorr   r   	unsqueezer   r&   )pts3d_ir   HWTHREEfocals         rF   rr   rr      su    	zmm1ez\\1Q3!*W^^<():):1)=r||A[fgmmoE<rH   c                     t         j                  d |d | f   j                  j                  t         j                        S r   )rx   mgridrB   astypefloat32)r   r   s     rF   
pixel_gridr      s2    88BQBF$$RZZ00rH   c           
         |j                         dk  ry t        t        | |f      \  } }| j                  \  }}}|dk(  sJ t	        ||      }	|*t        ||      }
t        j                  |
dz  |
dz  d      }n|g}||dz  |dz  f}nt        |      }d}|D ]{  }t        j                  |d|d   fd||d   fdg      }t        j                  | |   |	|   |d |d	t        j                  
      \  }}}}|s_t        |      }|sm||d   kD  sv||||f}} |d   sy |\  }}}}t        j                  |      d   }t        t        j                  ||f      \  }}|t!        t#        d|||            fS )Nr   rV   r      r   r   rU   )r   r   rU      )iterationsCountreprojectionErrorflags)sumrg   r	   r   r   r   rx   	geomspacer   cv2solvePnPRansacSOLVEPNP_SQPNPre   	Rodriguesr    
from_numpyr   rZ   )rQ   r   r   r   r   r   r   r   r   pixelsStentative_focalsbestKsuccessrA   rB   inliersrC   r6   
best_focals                        rF   r$   r$      s   
wwy1}Xs|,JE3++KAq%A:1F}1I<<!QqS"5!7	zc1Q3Zb\D! 
&JJ2a5)Aube+<iHI!$!3!3E#JsQPTDMabjmj|j|"~AwGutAw!Q%D
& 7Aq!ZaAu!Q(DAqs:aAv6777rH   c                     | j                   r[t        j                  | j                  D cg c]  }|j                    c}      }| j                         }|j                         ||fS yc c}w N)r   NN)rJ   r    r   rR   requires_gradget_im_posesr   )r0   pr3   r4   s       rF   r   r   "  s\    ,,t}}'U!Q__(='UV'')""$o{BB	 (Vs   A)c                     | j                   r3| j                         }| j                         }|j                         ||fS yr   )rJ   get_known_focal_mask
get_focalsr   )r0   known_focal_mskknown_focalss      rF   r   r   +  s>    335(""$o|CCrH   c                     t        |       }| j                  |j                  cxk(  r	|ddfk(  sJ  J d }t        j                   ||        ||      d      \  }}}|||fS )Nr   c           	          t        |       dz  }t        j                  | d d d ddf   | d d d ddf   || d d d ddf   z  z   f      S )Nd   rV   r   )r   r    cat)posesepss     rF   center_and_zz*align_multiple_poses.<locals>.center_and_z8  sY    (/#5yy%2A2q/5BQB?Sq"1"ax=P+PQRRrH   T)r   )re   r   r   r[   )	src_posestarget_posesNr   rA   rB   r@   s          rF   r(   r(   4  sj    IA??l00=Q1I====S ,,\)-DlS_F`rvwGAq!a7NrH   )
   rV   )Tr   Tr   )Nr   )%	functoolsr   numpyrx   scipy.sparsesparserh   r    r   r   r   dust3r.utils.geometryr   r   r   dust3r.post_processr   
dust3r.vizr	   dust3r.cloud_opt.commonsr
   r   r   no_gradrG   rS   rO   rK   rf   r[   rZ   rr   r   r$   r   r   r(    rH   rF   <module>r      s        
   I I <  J J '- '-T 
= 
=(-X LPV1r 1 1(8VrH   