
     h                     v    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m	Z	m
Z
mZ d dlmZ d dlmZ  G d de      Zy)    N)BasePCOptimizer)invgeotrf'depthmap_to_absolute_camera_coordinates)edge_str)estimate_focal_knowing_depthc                   d     e Zd ZdZ fdZddZddZddZd Zd Z	d Z
d	 Zd
 Zd Zd Z xZS )
PairViewerz
    This a Dummy Optimizer.
    To use only when the goal is to visualize the results for a pair of images (with is_symmetrized)
    c           
      	   t        |   |i | | j                  r| j                  dk(  sJ d| _        g | _        g | _        g }g }t        | j                        D ]  }t        | j                  t        |d|z
           j                         | j                  t        |d|z
           j                         z        }| j                  rt        d|dd| dd|z
          |j!                  |       | j"                  |   \  }}| j$                  t        |d|z
           }	t'        j(                  |dz  |dz  f      }
t        t+        |	d    |
d	            }| j
                  j!                  |       | j                  j!                  |
       t,        j.                  d |d |f   j0                  j3                  t,        j4                        }| j6                  t        d|z
  |         j9                         }	|	j:                  d d ||fk(  sJ | j=                         |   j9                         }t-        j4                  |d
|
d
   fd
||
d   fdg      }	 t?        j@                  |	|   ||   |d ddt>        jB                        }|\  }}}}|sJ t?        jD                  |      d
   }tG        t,        jH                  t,        jJ                  ||f   dgf         }|j!                  t'        jN                  |j3                  t,        j4                                      |d
   |d   kD  r_t'        jL                  d      |d   g| _(        | j$                  d   d   tS        tG        |d         | j6                  d         d   g| _*        n^|d
   t'        jL                  d      g| _(        tS        tG        |d
         | j6                  d         d   | j$                  d   d   g| _*        tW        jX                  t'        jZ                  | jP                  d
      d      | _(        tW        jX                  t'        j(                  | j
                        d      | _        tW        jX                  t'        jZ                  | j                  d
      d      | _        tW        j\                  | jT                        | _*        | j_                         D ]	  }d|_0         y #  t-        jL                  d      }Y xY w)N   T   z	  - conf=z.3z
 for edge -	weiszfeld)
focal_moder   )r   r   r   d      )iterationsCountreprojectionErrorflags)r   r   r   r      0_1).r   1_0)dimF)requires_grad)1super__init__is_symmetrizedn_edgeshas_im_posesfocalspprangen_imgsfloatconf_ir   meanconf_jverboseprintappendimshapespred_itorchtensorr   npmgridTastypefloat32pred_jnumpyshape	get_maskscv2solvePnPRansacSOLVEPNP_SQPNP	Rodriguesr   r_c_eye
from_numpyim_posesr   depthnn	ParameterstackParameterList
parametersr   )selfargskwargs	rel_posesconfsiconfHWpts3dr!   focalpixelsmskKressuccessRr1   inliersposep	__class__s                         J/home/cameronsmith/repos/controll3r/dust3r/dust3r/cloud_opt/pair_viewer.pyr   zPairViewer.__init__   s+   $)&)""t||q'88  	t{{# 	HAXa1%56;;=HUVXYZ[X[L\@]@b@b@ddeD||
T2Jj1QqSE:;LL==#DAqKKAaC 01EqsAaCj)B6uT{BS^_`EKKu%GGNN2 XXbqb"1"f%''..rzz:FKK1a 01779E;;r?q!f,,.."1%++-C

UAr!u-5"Q%/@)LMA	!((sVC[!T9<PQY\YkYkm),&AwMM!$Q'255q!t|n!<=> U--dkk"**.EFG=	HB 8eAh"YYq\9Q<8DM++e,V4fS1=NPTP[P[\aPb6cdj6klDJ 'q\599Q<8DM Yq\!2DKK4FGOQUQ\Q\]bQcdjQklDJU[[A%FV[\ll5<<#<ER,,u{{477:%P%%djj1
" 	$A#AO	$%!vvays   BSS/c                 2    | j                   rt        d       y )Nz&_set_depthmap is ignored in PairViewer)r(   r)   )rG   idxrA   forces       r\   _set_depthmapzPairViewer._set_depthmapS   s    <<:;    c                 t    | j                   D cg c]  }|j                  | j                         }}|S c c}w N)rA   todevice)rG   rawdrA   s       r\   get_depthmapszPairViewer.get_depthmapsX   s0    ,0JJ7qdkk"77 8s   "5c                 "    || j                   |<   y rc   r    )rG   r^   rQ   r_   s       r\   
_set_focalzPairViewer._set_focal\   s     Cra   c                     | j                   S rc   rj   rG   s    r\   
get_focalszPairViewer.get_focals_   s    {{ra   c                 |    t        j                  | j                  D cg c]  }|j                    c}      S c c}w rc   )r-   r.   r    r   )rG   rZ   s     r\   get_known_focal_maskzPairViewer.get_known_focal_maskb   s*    ||DKKHq!//2HIIHs   9c                     | j                   S rc   )r!   rm   s    r\   get_principal_pointszPairViewer.get_principal_pointse   s    wwra   c                 "   | j                         }| j                         }t        j                  t	        |      ddf| j
                        }t        t	        |            D ]*  }||   x||ddf<   ||ddf<   ||   ||d ddf<   d||ddf<   , |S )N   re   r   r   r   )rn   rr   r-   zeroslenre   r"   )rG   r    ppsrT   rL   s        r\   get_intrinsicszPairViewer.get_intrinsicsh   s    "'')KKVa+DKK@s6{# 	A&,Qi/AaAgJ1a7a&Aa!QhKAaAgJ	 ra   c                     | j                   S rc   )r@   rm   s    r\   get_im_poseszPairViewer.get_im_posesr   s    }}ra   c                    g }t        | j                  | j                         | j                               D ]  \  }}}t	        |j                         j                         |j                         j                         |j                         j                               \  }}|j                  t        j                  |      j                  | j                                |S )Nru   )ziprA   ry   r{   r   cpur5   r*   r-   r?   rd   re   )rG   rP   rg   
intrinsicsim_posepts_s          r\   depth_to_pts3dzPairViewer.depth_to_pts3du   s    &)$**d6I6I6KTM^M^M`&a 	G"Az7<QUUW]]_=G^^=M=S=S=U=D[[]=P=P=RTFC LL))#.111EF		G
 ra   c                     t        d      S )Nnan)r$   rm   s    r\   forwardzPairViewer.forward~   s    U|ra   )F)__name__
__module____qualname____doc__r   r`   rh   rk   rn   rp   rr   ry   r{   r   r   __classcell__)r[   s   @r\   r
   r
      sA    
9$v
!Jra   r
   )r5   r/   r-   torch.nnrB   r8   dust3r.cloud_opt.base_optr   dust3r.utils.geometryr   r   r   dust3r.cloud_opt.commonsr   dust3r.post_processr   r
    ra   r\   <module>r      s0       
 5 V V - <m/ mra   