
     h;                     8   d dl mZ d dlZd dlZd dlmZ d dlZd dl mZ d dlZd dl	m
Z
mZ d dlm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mZmZmZmZmZmZmZmZ d dl m!c m"Z#  G d	 d
ejH                        Z%ddZ&d Z' ejP                         	 dd       Z)y)    )deepcopyN)invgeotrf)to_numpy)rgb)SceneVizsegment_skyauto_cam_size)adjust_learning_rate_by_lr)	edge_str	ALL_DISTSNoGradParamDictget_imshapessigned_expm1signed_log1pcosine_schedulelinear_scheduleget_conf_trfc            	           e Zd ZdZd Zddddddej                  d	d
f	 fd	Zed        Z	ed        Z
ed        Zed        Zd+ fd	Z fdZd Z ej                          d        Zd Zd Zd,dZd Zd Zd Zd Zd Zd-dZd-dZd Zd Zd  Zd.d!Zd" Z d-d#Z!d-d$Z"d% Z#d-d&Z$ejJ                  jL                  jO                  d'      d/d(       Z( ej                          d)        Z)d0d*Z* xZ+S )1BasePCOptimizerz Optimize a global scene, given a list of pairwise observations.
    Graph node: images
    Graph edges: observations = (pred1, pred2)
    c                    t        |      dk(  rZt        |      dk(  rLt        |d         }dj                         }| j                  j	                  |D ci c]  }|||   
 c}       y  | j
                  |i | y c c}w )N   r   a  edges is_symmetrized dist n_imgs pred_i pred_j imshapes 
                        min_conf_thr conf_thr conf_i conf_j im_conf
                        base_scale norm_pw_scale POSE_DIM pw_poses 
                        pw_adaptors pw_adaptors has_im_poses rand_pose imgs verbose)lenr   split__dict__update_init_from_views)selfargskwargsotherattrsks         G/home/cameronsmith/repos/controll3r/dust3r/dust3r/cloud_opt/base_opt.py__init__zBasePCOptimizer.__init__!   s    t9>c&kQ.T!W%EW X]W\W^  MM  u!=!!U1X+!=>!D!!4262 ">s   A<l1log   g      ?F   NTc           	         t         |           t        |d   t              s|d   j	                         |d<   t        |d   t              s|d   j	                         |d<   t        |d   |d         D cg c]  \  }}t        |      t        |      f c}}| _        t        | j                        | j                  D ch c]	  \  }}||f c}}k(  | _	        t        |   | _        || _        | j                         | _        |d   }|d   }t        t!        | j"                        D ci c]  \  }}|||    c}}      | _        t        t!        | j"                        D ci c]  \  }}|||    c}}      | _        t)        | j                  ||      | _        |d   }|d   }|| _        t/        |      | _        t        t!        | j"                        D ci c]  \  }}|||    c}}      | _        t        t!        | j"                        D ci c]  \  }}|||    c}}      | _        | j7                  ||      | _        t;        t=        | j8                              D ]  }d| j8                  |   _         || _         d| _!        |
| _"        d| _#        tI        jJ                   || jL                  d| jF                  z   f            | _'        tI        jJ                  tQ        jR                  | jL                  d	f            | _*        | jT                  jW                  |	       d| _,        || _-        d | _.        d
|v rd
|v r| j*                  D cg c]  }tQ        jR                  d|z          }}t;        t=        | j                              D ](  }|d   |   }|d
   |   ||<   |d   |   }|d
   |   ||<   * t_        |      | _.        y y y c c}}w c c}}w c c}}w c c}}w c c}}w c c}}w c c}w )Nidxpts3dpts3d_in_other_viewconfFT   r      img)r(   )0superr%   
isinstancelisttolistzipintedgessetis_symmetrizedr   distverbose_check_edgesn_imgsr   	enumerate	str_edgespred_ipred_jr   imshapesmin_conf_thrr   conf_trfconf_iconf_j_compute_img_confim_confranger   requires_grad
base_scalenorm_pw_scalepw_breakPOSE_DIMnn	Parametern_edgespw_posestorchzerospw_adaptorsrequires_grad_has_im_poses	rand_poseimgsr   )r   view1view2pred1pred2r;   r.   rD   rL   allow_pw_adaptorsrN   rY   iterationsCountr<   ij	pred1_pts	pred2_ptsnij
pred1_conf
pred2_confhwrZ   vr+   	__class__s                             r$   r   z BasePCOptimizer._init_from_views,   sy    	%,- <..0E%L%,- <..0E%L36uU|U5\3RS41as1vs1v&S
!$**oTZZ1PTQ1a&1PPdO	'') 'N	/0	%Yt~~E^&_EArr9Q<'7&_`%Yt~~E^&_EArr9Q<'7&_`$TZZIF 6]
6]
($T*%iPTP^P^F_&`UQr:a='8&`a%iPTP^P^F_&`UQr:a='8&`a--j*Es4<<() 	2A,1DLLO)	2 %! Yao/N%OP<<T\\14E(FG''(9:!" 	E>eun37==AREKKR(ADA3tzz?+ ,El1o!%LOS	El1o!%LOS		,
 D	DI ->O T1P '`&_ 'a&`& Bs*   7 OO-O 
'O&
O,
O2
O8c                 ,    t        | j                        S N)r   r8   r   s    r$   rR   zBasePCOptimizer.n_edgesk   s    4::    c                 `    | j                   D cg c]  \  }}t        ||       c}}S c c}}w rm   )r8   r   )r   ra   rb   s      r$   r@   zBasePCOptimizer.str_edgeso   s$    +/::641aA666s   *c                 P    | j                   D cg c]	  \  }}||f c}}S c c}}w rm   )rC   )r   hws      r$   imsizeszBasePCOptimizer.imsizess   s"    #'==141aA111s   "c                 Z    t        t        | j                                     j                  S rm   )nextiter
parametersdevicern   s    r$   ry   zBasePCOptimizer.devicew   s    D*+,333ro   c                     t         |          }|j                         D ci c]  \  }}|j                  d      |k7  s|| c}}S c c}}w )N)_zpred_i.zpred_j.zconf_i.zconf_j.)r2   
state_dictitems
startswith)r   	trainable
all_paramsr#   rj   rk   s        r$   r|   zBasePCOptimizer.state_dict{   sZ    W')
!+!1!1!3  EAq||Du7v  {D  8D1  E  	E  Es   A	 A	c                 H    t         |   | j                  d      |z        S )NF)r   )r2   load_state_dictr|   )r   datark   s     r$   r   zBasePCOptimizer.load_state_dict   s#    w&t'G$'NOOro   c                     t        | j                  D ch c]  }|D ]  }|  c}}      }|t        t        t	        |                  k(  sJ d       t	        |      S c c}}w )Nz!bad pair indices: missing values )sortedr8   r4   rJ   r   )r   edgera   indicess       r$   r=   zBasePCOptimizer._check_edges   sV    

ADAq!A!AB$uS\233X5XX7| Bs   A
c           	      d   t        j                  | j                  D cg c]#  }t        j                  || j
                        % c}      }t        | j                        D ]F  \  }\  }}t        j                  ||   ||         ||<   t        j                  ||   ||         ||<   H |S c c}w )N)ry   )	rP   ParameterListrC   rT   rU   ry   r?   r8   maximum)r   rg   rh   ri   rI   era   rb   s           r$   rH   z!BasePCOptimizer._compute_img_conf   s    ""RVR_R_#`BEKK4;;$G#`a"4::. 	BIAv1wqz:a=AGAJwqz:a=AGAJ	B 	 $as   (B-c                     | j                   }t        j                  |d d ddf   |fd      }| j                  r||j	                  dd      z
  }|| j
                  z  j                         S )Nr   r   )dimT)r   keepdim)rV   rT   catrM   meanrN   exp)r   adapts     r$   get_adaptorszBasePCOptimizer.get_adaptors   sf      		5AaC=%0b9EJJ1dJ;;E%**,,ro   c                     |d d d df   }t        |d d ddf         }t        j                  ||      j                         j	                         }|S )N   r/   )r   romaRigidUnitQuat	normalizeto_homogeneous)r   posesQTRTs        r$   
_get_poseszBasePCOptimizer._get_poses   sR    !RaR%Lq!A#v'1%//1@@B	ro   c                    ||   }|j                   s|s|S |j                  dk(  r|J |d ddf   }|d dd df   }|"t        j                  |      |j                  dd |t        ||xs dz        |j                  dd |>|j                  d   dv sJ t        j                  t        |            |j                  d<   |S )	N)r   r   r(   r   r   r   r/   r   )      )	rK   shaper   rotmat_to_unitquatr   r   npr'   float)r   r   r+   Rr   scaleforceposes           r$   	_set_posezBasePCOptimizer._set_pose   s    Sz""eK77f9"1"a%A"1"bqb&	A=!44Q7DIIaN=)!uz*:;DIIaN;;r?g--FF5<0DIIbMro   c                     | j                   rOt        j                  | j                        | j                  d d df   j                         z
  j                         S y)Nr   r   )rM   r   r'   rL   rS   r   r   rn   s    r$   get_pw_norm_scale_factorz(BasePCOptimizer.get_pw_norm_scale_factor   sG     FF4??+dmmArE.B.G.G.IINNPPro   c                 n    | j                   d d df   j                         }|| j                         z  }|S )Nr   )rS   r   r   )r   r   s     r$   get_pw_scalezBasePCOptimizer.get_pw_scale   s5    ae$((*5577ro   c                     | j                  | j                        }|j                         }|d d d dfxx   | j                         j	                  ddd      z  cc<   |S )Nr(   r   r   )r   rS   cloner   view)r   r   	scaled_RTs      r$   get_pw_poseszBasePCOptimizer.get_pw_poses   sU    __T]]+HHJ	!RaR%D--/44RA>>ro   c                 Z    | j                   D cg c]  }|| j                  kD   c}S c c}w rm   )rI   rD   )r   r.   s     r$   	get_maskszBasePCOptimizer.get_masks   s%    7;||Dt)))DDDs   (c                     t               rm   NotImplementedErrorrn   s    r$   depth_to_pts3dzBasePCOptimizer.depth_to_pts3d       !##ro   c           
          | j                         }|sDt        || j                        D cg c]!  \  }\  }}|d ||z   j                  ||d      # }}}}|S c c}}}w )Nr(   )r   r6   rC   r   )r   rawresdmrr   rs   s         r$   	get_pts3dzBasePCOptimizer.get_pts3d   s^    !!#<?T]]<STTjb&1a2dqs8==Aq)TCT
 Us   &Ac                     t               rm   r   )r   r+   focalr   s       r$   
_set_focalzBasePCOptimizer._set_focal   r   ro   c                     t               rm   r   rn   s    r$   
get_focalszBasePCOptimizer.get_focals   r   ro   c                     t               rm   r   rn   s    r$   get_known_focal_maskz$BasePCOptimizer.get_known_focal_mask   r   ro   c                     t               rm   r   rn   s    r$   get_principal_pointsz$BasePCOptimizer.get_principal_points   r   ro   c                 ~    || j                   n
t        |      }| j                  D cg c]
  } ||       c}S c c}w rm   )rE   r   rI   )r   modetrfcs       r$   get_confzBasePCOptimizer.get_conf   s2    #|dmmd1C $-1A---s   :c                     t               rm   r   rn   s    r$   get_im_poseszBasePCOptimizer.get_im_poses   r   ro   c                     t               rm   r   )r   r+   depthr   s       r$   _set_depthmapzBasePCOptimizer._set_depthmap   r   ro   c                     t               rm   r   )r   r   s     r$   get_depthmapszBasePCOptimizer.get_depthmaps   r   ro   c                 *   t        | j                               }| j                         }| j                         }| j	                         }t        | j                  ||||fi |}t        |      D ]!  \  }}|| j                  |   j                  d d  # | S rm   )	r   r   get_intrinsicsr   r   clean_pointcloudrI   r?   r   )	r   kwcamsK	depthmaps	all_pts3dnew_im_confsra   new_confs	            r$   r   z BasePCOptimizer.clean_pointcloud   s    4$$&'!&&(	NN$	'ay)ZWYZ$\2 	/KAx&.DLLO  #	/ro   c                    | j                         }| j                         }| j                         }| j                  j	                         D ci c]  \  }}|| j                  |       }}}| j                  j	                         D ci c]  \  }}|| j                  |       }}}d}	|r,t        j                  | j                  | j                  f       }
t        | j                        D ]  \  }\  }}t        ||      }t        ||   ||   | j                  |   z        }t        ||   ||   | j                  |   z        }| j!                  ||   |||         j#                         }| j!                  ||   |||         j#                         }|	|z   |z   }	|s||z   
||f<    |	| j$                  z  }	|r|	
fS |	S c c}}w c c}}w )Nr   )weight)r   r   r   rF   r}   rE   rG   rT   onesr>   r?   r8   r   r   rA   rB   r;   r   rR   )r   ret_detailsrS   pw_adapt
proj_pts3di_jr   weight_iweight_jlossdetailsr   ra   rb   aligned_pred_ialigned_pred_jliljs                     r$   forwardzBasePCOptimizer.forward   s   $$&$$&^^%
8<8I8I8KLfc1Cq))LL8<8I8I8KLfc1Cq))LLzz4;;"<==G"4::. 
	(IAv11a.C#HQK!t{{3?O1OPN#HQK!t{{3?O1OPN:a=.#OTTVB:a=.#OTTVB"9r>D "R1
	( 	= - MLs   G
G)enabledc                     |nX|dk(  s|dk(  rt        j                  | |       n6|dk(  r#t        j                  | | j                  |       nt	        d|      t        | fi |S )Nmspmst)	niter_PnPknown_poses)rD   r   zbad value for init=)init_funinit_minimum_spanning_treeinit_from_known_posesrD   
ValueErrorglobal_alignment_loop)r   initr   r   s       r$   compute_global_alignmentz(BasePCOptimizer.compute_global_alignment  sp    <U]dem//	J]"**4d>O>O5>@ 3dW566$T0R00ro   c                     t        |       }t        | j                        D ],  }t        | j                  |         }d|j
                  |   |<   . |S )Nr   )r   rJ   r>   r	   rZ   rI   )r   r   ra   skys       r$   mask_skyzBasePCOptimizer.mask_sky!  sN    tnt{{# 	$Adiil+C"#CKKN3	$ 
ro   c                 R   t               }| j                  t        j                  j	                  dd| j
                  df      }t        t        t        |j                                     }t        | j
                        D ]:  }|j                  | j                         |   ||   | j                         |          < nf|j                  | j                         | j                  | j                                t        j                  j	                  d| j
                  df      }t        | j                               }|t!        |      }|j#                  || j%                         || j                  | j&                  |       |r| j)                         }	|j#                  |	d|       |rbt+        | j,                        D 
cg c].  \  }
\  }}t/        |	|
   | j0                  t3        ||               0 }}}
}|j                  |d        |j4                  d	i | |S c c}}}
w )
Nr      r(   )size)colorsimagesrt   cam_size)   r   r  )colorr  )   r   r   )r   rZ   r   randomrandintr>   r4   maptupler5   rJ   add_pointcloudr   r   r   r   r
   add_camerasr   rt   r   r?   r8   r   rA   r   show)r   show_pw_camsshow_pw_pts3dr  r   vizr  re   im_posesrS   r   ra   rb   ptss                 r$   r  zBasePCOptimizer.show)  s   j99YY&&q#T[[!4D&EF#eV]]_56F4;;' X""4>>#3A#6q	4>>CSTUCVWX t~~/DNN<LMYY&&s$++q1A&BF D--/0$X.H$//"3F#yy$,, 	 	S((*HOOHMHOMYbcgcmcmYnooIAvPQSTvhqk4;;x1~+FGoo""362
	 ps   3H")T)NNF)Frm   )N
   )FFN),__name__
__module____qualname____doc__r%   rT   randnr   propertyrR   r@   rt   ry   r|   r   r=   no_gradrH   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   cudaampautocastr   r   r  __classcell__)rk   s   @r$   r   r      s\   
	3 ##&'$'+0"$#(;;)-!%="~   7 7 2 2 4 4EP
 U]]_ -*
E$$$$$.$$$
: ZZ^^U+1 ,1 U]]_ ro   r   c           
         | j                         D cg c]  }|j                  s| }}|s| S | j                  }|rDt        d       t        | j	                         D 	cg c]  \  }}	|	j                  s| c}	}       |}
t
        j                  j                  ||d      }t        d      }|rt        j                  |      5 }|j                  |j                  k  rat        | |j                  ||
|||      \  }}|j                  d|dd|d       |j                          |j                  |j                  k  rad d d        |S t        |      D ]  }t        | |||
|||      \  }} |S c c}w c c}	}w # 1 sw Y   |S xY w)	Nz#Global alignement - optimizing for:)?r#  )lrbetasinf)totalzlr=gz loss=)rx   rK   r<   printnamed_parametersrT   optimAdamr   tqdmre   r'  global_alignment_iterset_postfix_strr   rJ   )netr$  niterschedulelr_minpparamsr<   namevaluelr_base	optimizerr   barre   r{   s                   r$   r   r   F  sk   )=AQ__a=F=
kkG34s';';'=UeATATtUVG  Bj AI<DYYU# 	s%%#))#0ceeUGVU^`hib##sr1gVD8$<=

 %%#))#	 K u 	aA+CE7FIW_`GD!	aK- > V	 Ks#   E3E3 E8
5E8
A:E>>Fc                    ||z  }|dk(  rt        |||      }n!|dk(  rt        |||      }nt        d|      t        ||       |j	                           |        }	|	j                          |j                          t        |	      |fS )Ncosinelinearzbad lr schedule=)r   r   r   r   	zero_gradbackwardstepr   )
r0  cur_iterr1  r8  r3  r9  r2  tr$  r   s
             r$   r.  r.  `  s    5A8Q0	X	Q0,8+.//y"-5DMMONN;?ro   c                 4   t        |       t        |      cxk(  r-t        |      cxk(  rt        |      cxk(  rt        |      k(  sJ  J d|cxk  rdk  sJ  J | D cg c]  }|j                          }	}t        ||       D 
cg c]#  \  }
} |
j                  g |j                  d % }}
}t        ||       D cg c]  \  }} |j                  |j                     }}}t        |      D ]  \  }}t        t        |            D ]  }||k(  r	t        ||   |      }|dddddf   }t        ||   |dd      j                         j                         j                  d      \  }}| |   j                  \  }}|dkD  d|k  z  ||k  z  d|k  z  ||k  z  }||   ||   f}||   d|z
  ||   |   z  k  |	|   |   |	|   |   k  z  }|j                         }|||<   |	|   |   j                  |      |	|   |<     |	S c c}w c c}}
w c c}}w )	z Method: 
    1) express all 3d points in each camera coordinate frame
    2) if they're in front of a depthmap --> then lower their confidence
    r   r   r(   Nr0   )normncolr   )max)r   r   r6   r   r   r?   rJ   r   roundlongunbindclip_)im_confsr   r   r   r   tolbad_confdbgr   r   r4  dra   r,   rb   proj
proj_depthurj   HWmsk_imsk_j
bad_points	bad_msk_is                            r$   r   r   q  sE    x=CISQS3y>SS^SSSS<a<<&
'1779
'C
' 039h/GH!###HIH-0H-EFca!FIFi( F5s9~& 	FAAvx $q'5)Da!eJ1t!!4::<AACJJ2NCAa A;$$DAq!^Q/1q59Q!VDANEeHah&E %U+qu	!U8K.KKPSTUPVW\P]`cde`fgl`mPmnJI)Ie #Ay 1 7 7H 7 ECF9%	FF* J7 ( IFs   H	(H #H)g{Gz?i,  r<  gư>)gMbP?r   r	  )*copyr   numpyr   rT   torch.nnrP   r   r-  dust3r.utils.geometryr   r   dust3r.utils.devicer   dust3r.utils.imager   
dust3r.vizr   r	   r
   dust3r.optim_factoryr   dust3r.cloud_opt.commonsr   r   r   r   r   r   r   r   r   dust3r.cloud_opt.init_im_poses	cloud_optinit_im_posesr   Moduler   r   r.  r  r   r	  ro   r$   <module>rf     s           - ( " ; ; ;V V V 1 1hryy hV	4" 13# #ro   