
     h&                         d dl Zd dlZd dlmZ d dlmZ d dlmZm	Z	 d dl
mZmZ  G d de      Zd ZddZdd	Zdd
Zd Zy)    N)BasePCOptimizer)xy_gridgeotrf)to_cputo_numpyc                        e Zd ZdZddd fd
Zd ZddZddZdd	Zd
 Z	d Z
ddZd Zd ZddZd Zd Zd ZddZddZd ZddZd Z xZS )PointCloudOptimizerz Optimize a global scene, given a list of pairwise observations.
    Graph node: images
    Graph edges: observations = (pred1, pred2)
    F   )optimize_ppfocal_breakc                P	    t           |i | d _        | _        t	        j
                  d  j                  D               _        t	        j
                   fdt         j                        D               _
        t	        j
                   fd j                  D               _        t	        j
                  d t         j                        D               _         j                  j                  |        j                  d    _         j                  D cg c]
  \  }}||z   }}}t        |       _        t#         j                  d j                          _        t#         j                  d       _
        t#         j                  d       _        t#         j                  d       _         j%                  d	t'        j(                   j                  D cg c]  \  }}|d
z  |d
z  f c}}              j%                  dt#         j                  D 	cg c]  \  }}	t+        |	| j,                         c}	} j                                 j%                  dt#         j.                  D 
cg c]   }
 j1                   j2                  |
         " c}
 j                                 j%                  dt#         j.                  D 
cg c]   }
 j1                   j4                  |
         " c}
 j                                 j%                  dt#         j6                   j.                   j                                 j%                  dt#         j8                   j.                   j                                 j%                  dt'        j(                   j:                  D cg c]  \  }}|	 c}}              j%                  dt'        j(                   j:                  D cg c]  \  }}|	 c}}             t=         j:                  D cg c]
  \  }}||    c}}       _        t=         j:                  D cg c]
  \  }}||    c}}       _         y c c}}w c c}}w c c}	}w c c}
w c c}
w c c}}w c c}}w c c}}w c c}}w )NTc              3   Z   K   | ]#  \  }}t        j                  ||      d z  dz
   % yw)
      N)torchrandn).0HWs      H/home/cameronsmith/repos/controll3r/dust3r/dust3r/cloud_opt/optimizer.py	<genexpr>z/PointCloudOptimizer.__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   z/PointCloudOptimizer.__init__.<locals>.<genexpr>   s     (c1)F(cs   %(c           
   3      K   | ]G  \  }}t        j                  j                  t        j                  t        ||            z  g       I y wr   )r   FloatTensorr   nplogmax)r   r   r   r   s      r   r   z/PointCloudOptimizer.__init__.<locals>.<genexpr>   sJ      *M6:a +0*;*;bffSAY//0+2 *Ms   AAc              3   F   K   | ]  }t        j                  d         yw)   N)r   zeros)r   r   s     r   r   z/PointCloudOptimizer.__init__.<locals>.<genexpr>!   s     %TAekk$&7%Ts   !r   )is_paramfill)r'   _ppr%   _griddevice)r(   	_weight_i	_weight_j_stacked_pred_i_stacked_pred_j_ei_ej)!super__init__has_im_posesr   nnParameterListimshapesim_depthmapsrangen_imgsim_poses	im_focalsim_pprequires_grad_imshaper"   max_areaParameterStackregister_bufferr   tensorr   r,   	str_edgesconf_trfconf_iconf_jpred_ipred_jedgessumtotal_area_itotal_area_j)r   r   r   argskwargshwim_areasr   r   i_jij	__class__s   `            r   r4   zPointCloudOptimizer.__init__   st   $)&) & ,,,^PTP]P],^^(((cPUVZVaVaPb(cc)) *M>Bmm*M M%%%Tt{{AS%TT


!!+.}}Q'$(MM2DAqAaC22H +4+<+<tRVR_R_`&t}}tD'F#DJJ>
UELL1WA1Q3!*1W$XYWn;?==I41aWQ$++.IPTP]P]'_ 	` 	[.8<GT]]4;;s+,Gdmm+] 	^[.8<GT]]4;;s+,Gdmm+] 	^ 	.t{{DNNaeanan0op.t{{DNNaeanan0opUELL

1K1!1K$LMUELL

1K1!1K$LM DA! DE DA! DE1 3 2XI HG
 2L1K D Ds6    Q4'Q:'"R  %R%R9R;R'R
R"
c                     t        j                  | j                  |      t        j                  | j                        k(        sJ d       y )Nzincomplete mask!)r    all_get_msk_indicesaranger;   r   msks     r   _check_all_imgs_are_selectedz0PointCloudOptimizer._check_all_imgs_are_selected?   s7    vvd++C0BIIdkk4JJK_M__K    c           
         | j                  |       t        |t        j                        r|j                  dk(  r|g}t        | j                  |      |      D ]i  \  }}| j                  rt        d| d|d ddf    d       | j                  | j                  | j                  |t        j                  |                   k t        d | j                  D              }|dk  | _        | j                  j                  d       d| _        y )	Nr%   z (setting pose # = r   )c              3   8   K   | ]  }|j                   d u   yw)FNrequires_grad)r   ps     r   r   z2PointCloudOptimizer.preset_pose.<locals>.<genexpr>M   s     N1Q__5Ns      F)r^   
isinstancer   TensorndimziprZ   verboseprint_no_grad	_set_poser<   rD   rL   norm_pw_scaler?   )r   known_posespose_mskidxposen_known_posess         r   preset_posezPointCloudOptimizer.preset_poseB   s    ))(3k5<<0[5E5E5J&-KT228<kJ 	RIC||(SbqbdA>?MM$..U\\$=OPQ	R NNN+q0$$U+"r_   c                     | j                  |       t        | j                  |      |      D ]D  \  }}| j                  rt	        d| d| d       | j                  | j                  ||             F | j                  j                  d       y )Nz (setting focal #ra   rb   F)	r^   rk   rZ   rl   rm   rn   
_set_focalr=   r?   )r   known_focalsr]   rs   focals        r   preset_focalz PointCloudOptimizer.preset_focalS   s    ))#.d33C8,G 	7JC||)#c%:;MM$//#u56	7
 	%%e,r_   c                     | j                  |       t        | j                  |      |      D ]D  \  }}| j                  rt	        d| d| d       | j                  | j                  ||             F | j                  j                  d       y )Nz (setting principal point #ra   rb   F)	r^   rk   rZ   rl   rm   rn   _set_principal_pointr>   r?   )r   known_ppr]   rs   pps        r   preset_principal_pointz*PointCloudOptimizer.preset_principal_point]   s    ))#.4005x@ 	>GC||3C5B4qABMM$33C<=	>
 	

!!%(r_   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;   rh   inttuplelistrZ   r    arraydtypeboolr   bool_lenwhere
issubdtypeinteger
ValueErrorr\   s     r   rZ   z$PointCloudOptimizer._get_msk_indicesg   s    ;%%S!5LeT]+((#77YY4RXX66s8t{{**88C=##]]399bjj1JyC6]++r_   c                 *    |j                   sJ d       y )Nz?it must be True at this point, otherwise no modification occursrd   )r   rD   s     r   rn   zPointCloudOptimizer._no_gradv   s    ##f%ff#r_   c                     | j                   |   }|j                  s|r/| j                  t        j                  |      z  |j
                  d d  |S r   )r=   re   r   r    r!   data)r   rs   rz   forceparams        r   rx   zPointCloudOptimizer._set_focaly   s@    s#% ,,rvve}<EJJqMr_   c                     t        j                  t        | j                        d      }|| j                  z  j                         S )Nr   dim)r   stackr   r=   r   exp)r   
log_focalss     r   
get_focalszPointCloudOptimizer.get_focals   s5    [[dnn!51=
T---2244r_   c                 |    t        j                  | j                  D cg c]  }|j                    c}      S c c}w r   )r   rD   r=   re   )r   rf   s     r   get_known_focal_maskz(PointCloudOptimizer.get_known_focal_mask   s*    ||DNNKq!//2KLLKs   9c                     | j                   |   }| j                  |   \  }}|j                  s|r/t        t	        |      |dz  |dz  fz
        dz  |j
                  d d  |S )Nr%   r   )r>   r8   re   r   r   r   )r   rs   r   r   r   r   r   s          r   r}   z(PointCloudOptimizer._set_principal_point   s]    

3}}S!1%"8B<1Q3!*#<=BEJJqMr_   c                 :    | j                   d| j                  z  z   S )Nr   )r)   r>   )r   s    r   get_principal_pointsz(PointCloudOptimizer.get_principal_points   s    xx"tzz/))r_   c                    t        j                  | j                  ddf| j                        }| j	                         j                         }|x|d d ddf<   |d d ddf<   | j                         |d d d ddf<   d|d d ddf<   |S )Nr   r+   r   rg   r%   )r   r&   r;   r,   r   flattenr   )r   Kfocalss      r   get_intrinsicsz"PointCloudOptimizer.get_intrinsics   s    KKa+DKK@"**,"((!Q'
Qq!QwZ//1!RaR(!Q'
r_   c                 <    | j                  | j                        }|S r   )
_get_posesr<   )r   	cam2worlds     r   get_im_posesz PointCloudOptimizer.get_im_poses   s    OODMM2	r_   c                     t        || j                        }| j                  |   }|j                  s|r-|j	                         j                  d      |j                  d d  |S )Nr   )neginf)	_ravel_hwrA   r9   re   r!   
nan_to_numr   )r   rs   depthr   r   s        r   _set_depthmapz!PointCloudOptimizer._set_depthmap   sS    %/!!#&%!IIK22!2<EJJqMr_   c           	          | j                   j                         }|sCt        || j                        D cg c]   \  }\  }}|d ||z   j	                  ||      " }}}}|S c c}}}w r   )r9   r   rk   r8   viewr   rawresdmrQ   rR   s         r   get_depthmapsz!PointCloudOptimizer.get_depthmaps   sb    ##%9<S$--9PQQ:2v12dqs8==A&QCQ
 Rs   %A"c                     | j                         }| j                         }| j                         }| j                  d      }t	        || j
                  ||      }t        ||      S )NTr   )r   )r   r   r   r   _fast_depthmap_to_pts3dr*   r   )r   r   r   r<   r   
rel_ptmapss         r   depth_to_pts3dz"PointCloudOptimizer.depth_to_pts3d   s`    "&&($$&""t", -UDJJ2N
h
++r_   c           
          | j                         }|sDt        || j                        D cg c]!  \  }\  }}|d ||z   j                  ||d      # }}}}|S c c}}}w )Nr   )r   rk   r8   r   r   s         r   	get_pts3dzPointCloudOptimizer.get_pts3d   s^    !!#<?T]]<STTjb&1a2dqs8==Aq)TCT
 Us   &Ac                 
   | j                         }| j                         j                  d      }| j                  d      }t	        ||| j
                  z        }t	        ||| j                  z        }| j                  || j                     || j                        j                         | j                  z  }| j                  || j                     || j                        j                         | j                  z  }||z   S )Nrg   Tr   )weight)get_pw_posesget_adaptors	unsqueezer   r   r/   r0   distr1   r-   rL   rM   r2   r.   rN   )r   pw_posespw_adapt
proj_pts3daligned_pred_ialigned_pred_jliljs           r   forwardzPointCloudOptimizer.forward   s    $$&$$&003^^^-
  (T5I5I*IJ(T5I5I*IJ YYz$((+^DNNYSWWY\`\m\mmYYz$((+^DNNYSWWY\`\m\mmBwr_   r   )F)__name__
__module____qualname____doc__r4   r^   rv   r{   r   rZ   rn   rx   r   r   r}   r   r   r   r   r   r   r   r   __classcell__)rW   s   @r   r	   r	      ss    
 +0R 'FR`#"-),g5M*
,r_   r	   c                 \   |j                  d      }|j                  d      }|j                  t        |       ddfk(  sJ |j                  t        |       ddfk(  sJ |j                  | j                  dz   k(  sJ | j                  d      } t        j                  | ||z
  z  |z  | fd      S )Nrg   r%   r$   r   )r   shaper   r   cat)r   
pixel_gridrz   r   s       r   r   r      s    	aBOOAE;;3u:q!,,,88E
Aq)))u{{T111OOBE99ezB/%7?RHHr_   c                    ||D cg c]  }| |   	 } }|dkD  r| D cg c]  }t        ||       } }| d   j                  t        fd| D              sJ t        j                  t        |             j                         j                         } |sr&t        j                  |       } | j                         | S c c}w c c}w )Nr   c              3   <   K   | ]  }|j                   k(    y wr   rd   )r   rf   re   s     r   r   z!ParameterStack.<locals>.<genexpr>   s     @Aq-/@s   )r   re   rY   r   r   r   floatdetachr6   	Parameterr?   )paramskeysr'   r(   krf   re   s         @r   rB   rB      s    %)*&)**ax.45)At$551I++M@@@@[[f&,,.557F=f%m,M + 6s
   B?Cc           	      &   | j                  | j                  d   | j                  d   z  f| j                  dd  z         } t        |       |k  rCt        j                  | | j                  |t        |       z
  f| j                  dd  z         f      } | S )Nr   rg   r%   )r   r   r   r   r   	new_zeros)rD   r(   s     r   r   r      s    [[&,,q/FLLO;=QR@PPQF
6{TFF$4$4dS[6H5J6<<XYXZK[5[$\]^Mr_   c                     t        | |      dt        j                  t        j                  d      dz        z  z  }||z  ||z  fS )Nr%   <   )r"   r    tandeg2rad)r   r   minfmaxf
focal_bases        r   acceptable_focal_ranger      s@    Qa"&&B!);"<<=J
?DO++r_   c                 0    | j                         } d| |<   | S )Nr   )copy)imgr]   s     r   
apply_maskr      s    
((*CCHJr_   )NNr   )r   )g      ?g      @)numpyr    r   torch.nnr6   dust3r.cloud_opt.base_optr   dust3r.utils.geometryr   r   dust3r.utils.devicer   r   r	   r   rB   r   r   r    r_   r   <module>r      sB       5 1 0y/ yxI",
r_   