
     hQ                     j    d dl Zd dlZd dlmZ d dlmZ d dlmZ d dl	m
Z
mZ d dlmZ  G d de      Zy)    N)BasePCOptimizer)geotrf)to_cputo_numpy)depthmap_to_pts3dc                        e Zd ZdZdddd fd
ZddZddZddZdd	Zd
 Z	d Z
ddZd ZddZd Zd Zd ZddZd Zd Zd Z xZS )ModularPointCloudOptimizerz Optimize a global scene, given a list of pairwise observations.
    Unlike PointCloudOptimizer, you can fix parts of the optimization process (partial poses/intrinsics)
    Graph node: images
    Graph edges: observations = (pred1, pred2)
    F   )optimize_pp	fx_and_fyfocal_brakec                    t        	   |i | d _        | _        t	        j
                  d  j                  D               _        t	        j
                   fdt         j                        D               _
         j                  D cg c]1  \  }} j                  t        j                  t        ||            z  3 }}}t	        j
                  fd|D               _        t	        j
                  d t         j                        D               _         j                  j!                  |       y c c}}w )NTc              3   Z   K   | ]#  \  }}t        j                  ||      d z  dz
   % yw)
      N)torchrandn).0HWs      P/home/cameronsmith/repos/controll3r/dust3r/dust3r/cloud_opt/modular_optimizer.py	<genexpr>z6ModularPointCloudOptimizer.__init__.<locals>.<genexpr>   s)     ,^1U[[A->r-A!-C,^s   )+c              3   T   K   | ]  }j                  j                         ! y wN)	rand_posePOSE_DIM)r   _selfs     r   r   z6ModularPointCloudOptimizer.__init__.<locals>.<genexpr>   s     (c1)F(cs   %(c              3   V   K   | ]   }t        j                  r||gn|g       " y wr   )r   FloatTensor)r   fr   s     r   r   z6ModularPointCloudOptimizer.__init__.<locals>.<genexpr>!   s7      *G23 +0*;*;iQF*+V- +. *Gs   &)c              3   F   K   | ]  }t        j                  d         yw))   N)r   zeros)r   r   s     r   r   z6ModularPointCloudOptimizer.__init__.<locals>.<genexpr>#   s     %TAekk$&7%Ts   !)super__init__has_im_posesr   nnParameterListimshapesim_depthmapsrangen_imgsim_posesnplogmax	im_focalsim_pprequires_grad_)
r   r   r   r   argskwargsr   r   default_focals	__class__s
   ` `      r   r&   z#ModularPointCloudOptimizer.__init__   s    $)&) & ,,,^PTP]P],^^(((cPUVZVaVaPb(ccKO==Y41a$**RVVC1I->>YY)) *G7E*G G%%%Tt{{AS%TT


!!+.	 Zs   6D=c           
         t        |t        j                        r|j                  dk(  r|g}t	        | j                  |      |      D ]k  \  }}| j                  rt        d| d|d ddf    d       | j                  | j                  | j                  |t        j                  |      d             m t        d | j                  D              }|d	k  | _        y )
Nr#   z (setting pose # = r   )Tforcec              3   8   K   | ]  }|j                   d u   yw)FN)requires_grad)r   ps     r   r   z9ModularPointCloudOptimizer.preset_pose.<locals>.<genexpr>/   s     N1Q__5Ns      )
isinstancer   Tensorndimzip_get_msk_indicesverboseprint_no_grad	_set_poser.   tensorsumnorm_pw_scale)r   known_posespose_mskidxposen_known_posess         r   preset_posez&ModularPointCloudOptimizer.preset_pose&   s    k5<<0[5E5E5J&-KT228<kJ 	^IC||(SbqbdA>?MM$..U\\$=OW[.\]	^ NNN+q0    c                 l   t        |t        j                        r|j                  dk(  r|g}|D ]  }|j                  dk(  rJ  | j                  |D cg c]#  }|j                         d d j                         % c}|       | j                  |D cg c]  }|d ddf    c}|       y c c}w c c}w )Nr#   )r   r   )	rB   r   rC   rD   shapepreset_focaldiagonalmeanpreset_principal_point)r   known_intrinsicsmskKs       r   preset_intrinsicsz,ModularPointCloudOptimizer.preset_intrinsics2   s    &5:J:O:OST:T 01! 	%A77f$$	%<LMq1::<+002MsS##7G$H!Qrr1uX$H#N N$Hs   (B,B1c                     t        | j                  |      |      D ]F  \  }}| j                  rt        d| d| d       | j	                  | j                  ||d             H y )Nz (setting focal #r:   r;   Tr<   )rE   rF   rG   rH   rI   
_set_focal)r   known_focalsr\   rP   focals        r   rW   z'ModularPointCloudOptimizer.preset_focal:   sc    d33C8,G 	CJC||)#c%:;MM$//#uD/AB	CrT   c                     t        | j                  |      |      D ]F  \  }}| j                  rt        d| d| d       | j	                  | j                  ||d             H y )Nz (setting principal point #r:   r;   Tr<   )rE   rF   rG   rH   rI   _set_principal_point)r   known_ppr\   rP   pps        r   rZ   z1ModularPointCloudOptimizer.preset_principal_point@   sf    4005x@ 	JGC||3C5B4qABMM$33C43HI	JrT   c                 $    |j                  d      S )NF)r4   )r   rK   s     r   rI   z#ModularPointCloudOptimizer._no_gradF   s    $$U++rT   c                    |t        | j                        S t        |t              r|gS t        |t        t
        f      r$| j                  t        j                  |            S |j                  t        t        j                  t        j                  fv r2t        |      | j                  k(  sJ t        j                  |      d   S t        j                  |j                  t        j                         r|S t#        d|      )Nr   zbad msk=)r,   r-   rB   inttuplelistrF   r/   arraydtypeboolr   bool_lenwhere
issubdtypeinteger
ValueError)r   r\   s     r   rF   z+ModularPointCloudOptimizer._get_msk_indicesI   s    ;%%S!5LeT]+((#77YY4RXX66s8t{{**88C=##]]399bjj1JyC6]++rT   c                     | j                   |   }|j                  s|r/| j                  t        j                  |      z  |j
                  d d  |S r   )r2   r?   r   r/   r0   data)r   rP   rb   r=   params        r   r`   z%ModularPointCloudOptimizer._set_focalX   s@    s#% ,,rvve}<EJJqMrT   c                     t        j                  t        | j                        d      }|| j                  z  j                         S )Nr   )dim)r   stackrk   r2   r   exp)r   
log_focalss     r   
get_focalsz%ModularPointCloudOptimizer.get_focals^   s5    [[dnn!51=
T---2244rT   c                     | j                   |   }| j                  |   \  }}|j                  s|r/t        t	        |      |dz  |dz  fz
        dz  |j
                  d d  |S Nr#   r   )r3   r*   r?   r   r   rv   )r   rP   rf   r=   rw   r   r   s          r   rd   z/ModularPointCloudOptimizer._set_principal_pointb   s]    

3}}S!1%"8B<1Q3!*#<=BEJJqMrT   c                     t        j                  t        | j                  | j                        D cg c]'  \  }\  }}|j                  |dz  |dz  f      d|z  z   ) c}}}      S c c}}}w r   )r   rz   rE   r3   r*   new)r   rf   r   r   s       r   get_principal_pointsz/ModularPointCloudOptimizer.get_principal_pointsi   sZ    {{s4::W[WdWdGeffVaBFFAaC1:.r"u4fggfs   ,A)c                 B   t        j                  | j                  ddf| j                        }| j	                         j                  | j                  d      }|d d df   |d d ddf<   |d d df   |d d ddf<   | j                         |d d d ddf<   d|d d ddf<   |S )Nr   )devicer   rA   r#   )r   r$   r-   r   r}   viewr   )r   r]   focalss      r   get_intrinsicsz)ModularPointCloudOptimizer.get_intrinsicsl   s    KKa+DKK@"''R8AqD\!Q'
ArE]!Q'
//1!RaR(!Q'
rT   c                 t    | j                  t        j                  t        | j                                    }|S r   )
_get_posesr   rz   rk   r.   )r   	cam2worlds     r   get_im_posesz'ModularPointCloudOptimizer.get_im_posesu   s(    OOEKKT]]0C$DE	rT   c                     | j                   |   }|j                  s|r-|j                         j                  d      |j                  d d  |S )Nr   )neginf)r+   r?   r0   
nan_to_numrv   )r   rP   depthr=   rw   s        r   _set_depthmapz(ModularPointCloudOptimizer._set_depthmapy   sC    !!#&%!IIK22!2<EJJqMrT   c                 \    | j                   D cg c]  }|j                          c}S c c}w r   )r+   r{   )r   ds     r   get_depthmapsz(ModularPointCloudOptimizer.get_depthmaps   s"    !%!2!23A333s   )c                    	  j                         	 j                         } j                         } j                         }	 fd}t	        |j
                  d         D cg c]%  }t        ||   d     ||      |||dz          d   ' }}t        ||      D cg c]  \  }}t        ||       c}}S c c}w c c}}w )Nc                 n     |    d   j                   dg|    j                  j                  |     S )N).NNrA   )expandrV   r*   )ir   r   s    r   focal_exz;ModularPointCloudOptimizer.depth_to_pts3d.<locals>.focal_ex   s>     Aq	/ : A A! ifQioo iX\XeXefgXh iirT   r   rA   )rf   )	r}   r   r   r   r,   rV   r   rE   r   )
r   rf   r.   r   r   r   
rel_ptmapsrQ   ptmapr   s
   `        @r   depth_to_pts3dz)ModularPointCloudOptimizer.depth_to_pts3d   s    "&&($$&""$ 	j_demesestuev_wxZ['a1QqS	RSTUx
x7:8Z7PQetU#QQ yQs   #*B8B=c                 "    | j                         S r   )r   )r   s    r   	get_pts3dz$ModularPointCloudOptimizer.get_pts3d   s    ""$$rT   r   )F)__name__
__module____qualname____doc__r&   rS   r^   rW   rZ   rI   rF   r`   r}   rd   r   r   r   r   r   r   r   __classcell__)r8   s   @r   r	   r	      sk     +05b /
2OCJ,,5h4R%rT   r	   )numpyr/   r   torch.nnr(   dust3r.cloud_opt.base_optr   dust3r.utils.geometryr   dust3r.utils.devicer   r   r   r	    rT   r   <module>r      s*       5 ( 0 3@%/ @%rT   