
     h5                     0   d dl Zd dlZd dlmZ d dlZd dlmZm	Z	m
Z
 d dlmZ d dlmZmZ 	 d dlZd ZddZdd	Zd
 Zd Zd Z G d d      Z	 ddZ	 	 ddZd Z ej:                  g dg dg dg dg      Zg dZd Z d Z!y# e$ r  ed       Y dw xY w)    N)Rotation)geotrfget_med_dist_between_poses'depthmap_to_absolute_camera_coordinates)to_numpy)rgb
img_to_arrzA/!\ module trimesh is not installed, cannot visualize results /!\c           	          t        | t        j                  t        j                  f      r| g} t        j
                  t        |       D cg c]  }|j                  dd       c}      S c c}w N   )
isinstancenpndarraytorchTensorconcatenater   reshape)vecsps     8/home/cameronsmith/repos/controll3r/dust3r/dust3r/viz.pycat_3dr      sL    $U\\23v>>Xd^D199R+DEEDs   
A*c                     t        j                         }t        j                  t        |       t        |            }|j	                  |       |j                  d|i       y )Ncolors
point_sizeline_settings)trimeshScene
PointCloudr   add_geometryshow)pts3dr   r   scenepcts        r   show_raw_pointcloudr'      sH    MMOE


VE]6&>
BC	s	JJlJ7J8    c           	      X   | j                   \  }}}|dk(  sJ | j                   |j                   k(  sJ |j                  dd      }t        j                  t	        |            j                  ||      }|d dd df   j                         }|d ddd f   j                         }	|dd d df   j                         }
|dd dd f   j                         }t        j                  t        j                  ||	|
f   t        j                  |
|	|f   t        j                  |	|
|f   t        j                  ||
|	f   fd      }t        j                  | d dd df   j                  dd      | d dd df   j                  dd      | dd dd f   j                  dd      | dd dd f   j                  dd      fd      }|B|j                   ||fk(  sJ |j                         }||   j                  d      }||   }||   }t	        |      t	        |      k(  sJ t        |||      S )Nr   r      r   axisverticesface_colorsfaces)
shaper   r   arangelenravelr   c_alldict)imgr$   validHWTHREEr.   idxidx1idx2idx3idx4r0   r/   
valid_idxsvalid_facess                   r   pts3d_to_trimeshrD   &   s:   ))KAq%A:99##}}R#H ))CM
"
*
*1a
0CssCRCx= DssBCx= DrsCRCx= DrsBCx= DNN
dD$
dD$
dD$
dD$	
 E ..CRC"Hb!$CRC"Hb!$BCHb!$BCHb!$	"
 K {{q!f$$[[]
 '+++4k"!+.u:[))){%HHr(   c           	         t        | D cg c]  }|d   |d   |d   f c} \  }}}t        j                  dg|D cg c]  }t        |       c}z         }t	        t        |            D ]  }||   d d xxx ||   z  ccc  t        j
                  |      }t        j
                  |      }t        j
                  |      }t        |||      S c c}w c c}w )Nr.   r0   r/   r   r-   )zipr   cumsumr3   ranger   r7   )meshesmr.   r0   r   v
n_verticesis           r   
cat_meshesrN   N   s    ![a#bVWQz]AgJ-@P$Q#bcHeVA391A99:J3u: %az!}$% ~~h'H^^F#FNN5!EvUCC $c9s
   CC
c                 v   dd l m} |j                          t        t	        | d               D ]  }| d   |   }|d   |   }t        | d   |         }t        |d   |         }	|d   |   j                         }
|d   |   j                         }|
j                         |j                         z  }t        d| d| d| d	|d
       |j                          |j                  d      j                  |       |j                  d      j                  |	       |j                  d      j                  |
dd       |j                  d      j                  |dd       |d   |   }|d   |   }|j                  dddddd       t        d      dk(  sgt        t        ||      t        ||	      d        y )Nr   instancer=   r8   confz>> Showing pair # -z score=g         r*      )vminvmax   r$   pts3d_in_other_viewzshow pointcloud? (y/n) y   )r   )matplotlib.pyplotpyplotionrH   r3   r   squeezemeanprintclfsubplotimshowsubplots_adjustinputr'   cat)view1view2pred1pred2plerM   jimg1img2conf1conf2scorepts1pts2s                  r   show_duster_pairsry   Z   s   "FFH3uZ()* P%LO%LO5<?#5<?#fa ((*fa ((*

UZZ\)!!AaS!HeQZ89



3t$


3t$


3u126


3u126W~a *+A.
1aAq!,*+s2D$T4QO%Pr(   c                     dt        |       z  S )Ng?)r   )im_posess    r   auto_cam_sizer|   s   s    +H555r(   c                   z    e Zd Zd Zddej
                  dfdZd	dZddej
                  dfdZd
dZddZ	ddZ
y)SceneVizc                 6    t        j                         | _        y N)r   r    r%   )selfs    r   __init__zSceneViz.__init__x   s    ]]_
r(   Nc                     t        |      }|B|j                  \  }}}	t        ||      }
t        j                  |
d|dz  gd|
|dz  gg dg      }t        |||      }| j                  |||
||k        S |      S )Nr      r   r   r*   )	cam2worldmask)r	   r1   maxr   float32depthmap_to_pts3dadd_pointcloud)r   imagedepth
intrinsicsr   zfarr   r:   r;   r<   focalr$   s               r   add_rgbdzSceneViz.add_rgbd{   s    5! ++KAq%1IEeQ!_q%1oy$QRJ "%yI""5%dluTz"]]X\"]]r(   c                 h   t        |      }t        |      }t        |t              s&|j                  dd      g}||j	                         g}t        |t
        t        f      s|j                  dd      g}|t        d       gt        |      z  }t        j                  t        ||      D cg c]
  \  }}||    c}}      }t        j                  |      }t        |t        t        j                  t        j                  f      rt        |      }t        j                  t        ||      D cg c]
  \  }}||    c}}      }	|	j                   |j                   k(  sJ t#                      t%        |	j                  dd            |j&                  _        nHt        |      dk(  sJ t        j*                  t%        |      |j                         |j&                  _        |rt        j,                  |j.                  d      }
t        j0                  j3                  |j.                  |
z
  d      }t        j4                  |d      }||k  }t        j                  |j.                  |   |j&                  j(                  |         }| j6                  j9                  |       | S c c}}w c c}}w )Nr   r   r   r+   gGz?)color)r   r   listr   r4   tupleslicer3   r   r   rF   r   r!   r   r   r   r1   bbuint8visualvertex_colorsbroadcast_tomedianr.   linalgnormquantiler%   r"   )r   r$   r   r   denoiser   rJ   ptsr&   colcentroiddist_to_centroiddist_thrr9   s                 r   r   zSceneViz.add_pointcloud   s    ~%&]]2a()E

~%%.]]2a()E<$K=3u:-Dnnc%o>sqad>?  %edBJJ=>UOE..#eD/!B31Q!A$!BCC99		)/24/',S[[A->'?CJJ$u:?"')uU|SYY'OCJJ$yyA6H!yy~~s||h/FR~P{{#3T:H%0E$$S\\%%8

@X@XY^@_`C

$- ?
 "Cs   .J(
2J.
c                     |B|j                   \  }}}	t        ||      }
t        j                  |
d|dz  gd|
|dz  gg dg      }t	        |||      \  }}|||k  z  }|||z  }| j                  |||      S )Nr   r   r   r   )r1   r   r   r   r   r   )r   r   r   r   r   r   r   r:   r;   r<   r   r$   mask2s                r   r   zSceneViz.add_rgbd   s    ++KAq%1IEeQ!_q%1oy$QRJ ?ujR[\u%* TME""5%e"<<r(   c           
         t        ||||f      \  }}}}t        |      }t        |t        j                        r1|j
                  dk(  r"|}|d   |d   z  dz  }|d|d   z  d|d   z  f}t        | j                  ||||||d        | S )	N)r   r   r   r   )r*   r*   g      ?r   )r   r   )r*   r   )imsizescreen_widthmarker)r   r	   r   r   r   r1   add_scene_camr%   )r   pose_c2wr   r   r   r   cam_sizer   s           r   
add_camerazSceneViz.add_camera   s    (0(E5%1P(Q%%5!eRZZ(U[[E-AJ_z#63>E~JsO+Qz#->?djj(E5%]enrsr(   c           
          d }t        |      D ]9  \  }}	 | j                  |	 |||      f |||       |||       |||      d| ; | S )Nc                     | d S | |   S r    )arrr=   s     r   <lambda>z&SceneViz.add_cameras.<locals>.<lambda>   s    ckd s3x r(   )r   r   r   )	enumerater   )
r   posesfocalsimagesimsizesr   kwgetrM   r   s
             r   add_cameraszSceneViz.add_cameras   se    ?$U+ 	|KAxDOOHc&m{3va=PSTZ[\P]fijqrsft{xz{	|r(   c                 @    | j                   j                  d|i       y )Nr   r   )r%   r#   )r   r   s     r   r#   zSceneViz.show   s    

j'ABr(   )r   r   r   NF)Nr   NNQ?)NNNNr   )__name__
__module____qualname__r   r   infr   r   r   r   r#   r   r(   r   r~   r~   w   sH    % 15266X\ ^"H 15266X\ ="
Cr(   r~   c           
         t        |      t        |      cxk(  r-t        |       cxk  rt        |      cxk  rt        |      k(  sJ  J t        |      }t        |       } t        |      }t        |      }t        j                         }t	        j
                  t        ||      D 	
cg c]
  \  }	}
|	|
    c}
}	      }t	        j
                  t        | |      D 	
cg c]
  \  }	}
|	|
    c}
}	      }t        j                  |j                  dd      |j                  dd            }|j                  |       t        |      D ]_  \  }}t        |t              r||   }n|xs t        |t        t              z     }t        ||||t        |       k  r| |   nd||   |       a |j                  d|i       yc c}
}	w c c}
}	w )a   Visualization of a pointcloud with cameras
        imgs = (N, H, W, 3) or N-size list of [(H,W,3), ...]
        pts3d = (N, H, W, 3) or N-size list of [(H,W,3), ...]
        focals = (N,) or N-size list of [focal, ...]
        cams2world = (N,4,4) or N-size list of [(4,4), ...]
    r   r   r   N)r   r   r   )r3   r   r   r    r   r   rF   r!   r   r"   r   r   r   
CAM_COLORSr   r#   )imgsr$   r   r   
cams2worldr   r   	cam_colorr%   r   rJ   r   r   r&   rM   r   camera_edge_colors                    r   show_raw_pointcloud_with_camsr      s    u:TQc$iQ3z?Qc&kQQQQUOED>DfF*%JMMOE ..3ud+;<41a!A$<
=C
..3tT?;41a!A$;
<C


S[[Q/B8J
KC	s !, \8i& )! ) LZC
O8K-LeX'8!"SYd1gD&)RZ	\\ 
JJlJ7J8 =;s   (F>
G
c           	      	   |bt        j                  |      }|j                  \  }}	}
|
dk(  sJ |j                  t         j                  k7  r/t        j                  d|z        }n||\  }	}n||dz  x}}	ndx}}	t        |t         j                        r|d   }|st        ||	      dz  }t        |dz  ||z  |z        }|dz  }t        j                  d      }t        j                  d	t        j                  d
            j                         |d dd df<   | |d<   t        j                  d      }|	|z  |d<   |t        z  |z  |z  }t        j                   j#                  ||d      }|t%        ||j&                  g d         }t        j(                  g dg dg dg dg      }t        j*                  ||      }t        j,                  ddgddgddgddgg      }t        j.                  j1                  |t2        j4                  j7                  |            |_        | j9                  |       t        j                  d      }t        j                  d	t        j                  d            j                         |d dd df<   t         j:                  |j&                  d|j&                  z  t%        ||j&                        f   }t%        ||      }g }|j<                  D ]  }d|v r|\  }}}|t?        |j&                        z   \  }}}|dt?        |j&                        z  z   \  }}}|jA                  |||f       |jA                  |||f       |jA                  |||f       |jA                  |||f       |jA                  |||f       |jA                  |||f        ||D cg c]  \  }}}|||f c}}}z  }t        j*                  ||      }||j.                  jB                  d d d df<   | j9                  |       |dk(  rqt        j                   jE                  d|dz        }|xj&                  |d ddf   z  c_        ||j.                  jB                  d d d df<   | j9                  |       y y c c}}}w )Nr      g?r*   r   
   g;f?   z-   )r   r   r   )sections)r   r^   r*   r   )r   r*   r   )r   r   r   )r   r*   r   )r   r   r   )r.   r0   )r   r   gffffff?o)radius)#r   asarrayr1   dtyper   r   r   minr   eyer   
from_eulerdeg2rad	as_matrixOPENGLr   creationconer   r.   arrayTrimeshr   r   TextureVisualsPILImage	fromarrayr"   r_r0   r3   appendr/   	icosphere) r%   r   
edge_colorr   r   r   r   r   r:   r;   r<   heightwidthrot45aspect_ratio	transformcamr.   r0   r8   	uv_coordsrot2faceabca2b2c2a3b3c3s                                    r   r   r      s(   

5!kk1ez;;"(("HHSY'E		1		A	A%$aAa3 ,r/5<#7!#;=F8#EFF1IE''RZZ^<FFHE"1"bqb&M'E$K66!9L1L6!L058I




v

:C )S\\,%?@)Y	9EFooxu=JJAAAA?@	^^229CIIDWDWX]D^2_
3 66!9D&&sBJJqM:DDFD!RaRLuuS\\4#4fT3<<6PPQHi*HE		 !91aC--
BAc#,,///
B 	aBZ aQZ b!QZ aBZ aQZ b!QZ !" 
u--GAq!q!Qi--E
//85
9C$.CJJ1bqb5!	s}!!++Al1n+E8BQBqD>)*4!!!BQB$'6"	  .s   2R
c                 p    t        j                  | j                  dd      |j                  dd      f      S r   )r   r   r   )r   r   s     r   rj   rj   B  s,    >>199R+QYYr1-=>??r(   )r*   r   r   r   )r   r   r   r   )r   r   r   r   )r   r   r   r*   ))r   r   r   )r   r   r   )r   r   r   )r   r   r   )r      r   )r   r   r   )   r   r   )r   r   r   )r   r   r   r   )r   r   r   c                 @   t        | t        j                        st        j                  |       } t        j                  | j
                  t        j                        r| dz  } d| j                         k  r| j                         dk  sJ t        j                  |       S )Nr   r      )
r   r   r   r   
issubdtyper   floatingr   r   r   r   s    r   r   r   P  sj    fbjj)&!	}}V\\2;;/#

!3388Fr(   c                    dd l }ddlm} t        |       } t	        j
                  | j                  t        j                        r)t	        j                  d| j                  dd      z        } |j                  | |j                        }t	        j                  g d      }t	        j                  g d      }|j                  |||      j                  t              }||d d d d df   dk  |d d d d d	f   d
kD  z  z  }||d d d d df   dk  |d d d d d	f   dkD  z  z  }||d d d d df   dk  |d d d d d	f   dkD  z  z  }t	        j                   dt        j                        }|j#                  ||      }|j%                  |j                  t        j                        d      \  }	}
}}	|dd |j&                  f   }|j)                         d d d   }d}g }|t+        |      k  rS|||      ||d      d	z  kD  r?|j-                  d||   z          |dz  }|t+        |      k  r|||      ||d      d	z  kD  r?t	        j.                  |
|      j1                  |
j2                        }t5        j6                  |      S )Nr   )ndimager   r*   )r   r   )r   r   d   )rX   r   r   r   r      rX      2      )r^   r^   )	structure   )connectivityr   )cv2scipyr  r   r   r  r   r  r   clipcvtColorCOLOR_BGR2HSVr   inRangeviewboolonesbinary_openingconnectedComponentsWithStatsCC_STAT_AREAargsortr3   r   in1dr   r1   r   
from_numpy)r   r  r  hsv
lower_blue
upper_bluer   kernelr   _labelsstatscc_sizesorderrM   	selectionmask3s                    r   segment_skyr(  Y  s[    UOE	}}U[["++.UZZA1Z556
,,uc//
0C +&J.)J;;sJ
388>D 	SAq\B3q!Qw<##566DSAq\B3q!Qw<##566DSAq\B3q!Qw<##566D WWVRXX&F""46":E ::5::bhh;O^_:`AvuaQR)))*Htt$E	AI
c%j.XeAh/(582Dq2HHU1X&	Q c%j.XeAh/(582Dq2HH GGFI&..v||<E E""r(   r   r   )r   g?N)NNNr   N)"	PIL.Imager   numpyr   scipy.spatial.transformr   r   dust3r.utils.geometryr   r   r   dust3r.utils.devicer   dust3r.utils.imager   r	   r   ImportErrorrd   r   r'   rD   rN   ry   r|   r~   r   r   rj   r   r   r   r   r(  r   r(   r   <module>r0     s      ,  m m ( .QF9%IP	DP26ZC ZC| JN9D OS,0I#X@ 
<  ! 
"]
$#O
  Q	
OPQs   B BB