o
    ifB                  
   @   s<  d dl mZ d dlmZ d dlZd dlZd dlm  m	Z
 d dlmZ dd Zejjdejfdd	Zd
d ZdejfddZdejdejfddZdejdejfddZdejdejfddZdejdejfddZedfdeedf dejdeejejf fddZd ejdejfd!d"Zd#ejdejfd$d%Zd&ejd'ejdejfd(d)Zd&ejd*ejdejfd+d,Z d-ejd.ejd/ejdejfd0d1Z!d-ejd*ejd/ejdeejejf fd2d3Z"d/ejdejfd4d5Z#	 	dJd6ejd7ed8ee$ dejfd9d:Z%d;d< Z&d=d> Z'd?d@ Z(dAdB Z)dCdD Z*dEdF Z+	dKdHdIZ,dS )L    )SimpleNamespace)OptionalN)einsumc                 C   s   t | tjr=| jdd dkr| S | jdd dkr5t| dddddf }d|d	< tj| |gdd
S td| j t | tjrz| jdd dkrN| S | jdd dkrrt| dddddf }d|d	< tj	| |gddS td| j t
d)z
    Accept (..., 3,4) or (..., 4,4) extrinsics, return (...,4,4) homogeneous matrix.
    Supports torch.Tensor or np.ndarray.
    N)   r   )   r   .   r         ?).r   r   dimz Invalid shape for torch.Tensor: axiszInvalid shape for np.ndarray: z+Input must be a torch.Tensor or np.ndarray.)
isinstancetorchTensorshape
zeros_likecat
ValueErrornpndarrayconcatenate	TypeError)extones r   =/data/cameron/da3_repo/src/depth_anything_3/utils/geometry.pyas_homogeneous   s"   r   Ac                 C   sl   | dd dd df }| dd ddd f }| ddd d d f }t jt j|j|j | gdd|gddS )N.r   r
   r   )r   r   mTr   RTPr   r   r   affine_inverse6   s   *r%   c                 C   s@   | j dk r| S tt| j }|d |d |d< |d< | |S )z
    for np < 2
       r   r   )ndimlistrange	transpose)arraxesr   r   r   transpose_last_two_axes>   s
   

r-   c                 C   sp   | dd dd df }| dd ddd f }| ddd d d f }t jt jt|t| | gdd|gddS )N.r   r   r   r   )r   r   r-   r!   r   r   r   affine_inverse_npJ   s   r.   quaternionsreturnc                 C   s   t | d\}}}}d| |  d }t d||| ||    ||| ||   ||| ||   ||| ||   d||| ||    ||| ||   ||| ||   ||| ||   d||| ||    f	d}|| jdd d S )a,  
    Quaternion Order: XYZW or say ijkr, scalar-last

    Convert rotations given as quaternions to rotation matrices.
    Args:
        quaternions: quaternions with real part last,
            as tensor of shape (..., 4).

    Returns:
        Rotation matrices as tensor of shape (..., 3, 3).
    r          @r   N)r   r   )r   unbindsumstackreshaper   )r/   ijkrtwo_sor   r   r   quat_to_matW   s    r<   matrixc              
   C   s  |  ddks|  ddkrtd| j d| jdd }tj| |d dd\	}}}}}}}}	}
ttjd	| | |
 d	| | |
 d	| | |
 d	| | |
 gdd}tjtj|d
 d |	| || || gddtj|	| |d d || || gddtj|| || |d d ||	 gddtj|| || |	| |d d gddgdd}tdj	|j
|jd}|d|d |  }|tj|jdddddkddf |d }|dg df }t|}|S )a  
    Convert rotations given as rotation matrices to quaternions.

    Args:
        matrix: Rotation matrices as tensor of shape (..., 3, 3).

    Returns:
        quaternions with real part last, as tensor of shape (..., 4).
        Quaternion Order: XYZW or say ijkr, scalar-last
    r   r   r   zInvalid rotation matrix shape .N)	   r
   r	   .r   r&   ).r   ).r&   ).r   g?dtypedevicer1   .Nr   )num_classes      ?)r   .)r   r&   r   r   )sizer   r   r   r2   r5   _sqrt_positive_partr4   tensortorB   rC   maxFone_hotargmaxstandardize_quaternion)r=   	batch_dimm00m01m02m10m11m12m20m21m22q_absquat_by_rijkflrquat_candidatesoutr   r   r   mat_to_quatx   s@   ((((&r_   xc                 C   sH   t | }| dk}t  rt | | ||< |S t |t | |}|S )z[
    Returns torch.sqrt(torch.max(0, x))
    but with a zero subgradient where x is 0.
    r   )r   r   is_grad_enabledsqrtwhere)r`   retpositive_maskr   r   r   rH      s   
rH   c                 C   s    t | dddf dk |  | S )a  
    Convert a unit quaternion to a standard form: one in which the real
    part is non negative.

    Args:
        quaternions: Quaternions with real part last,
            as tensor of shape (..., 4).

    Returns:
        Standardized quaternions as tensor of shape (..., 4).
    .r   r   r   )r   rc   )r/   r   r   r   rO      s    rO   cpur   .rC   c                    sj    fdd| D }t jt j|ddidd}dd t|| D }t|}t jt j|ddidd}||fS )	zKGet normalized (range 0 to 1) coordinates and integer indices for an image.c                    s   g | ]	}t j| d qS )rC   )r   arange).0lengthrg   r   r   
<listcomp>   s    z%sample_image_grid.<locals>.<listcomp>indexingijr   r
   c                 S   s   g | ]
\}}|d  | qS )rF   r   )ri   idxrj   r   r   r   rk      s    xy)r   r4   meshgridzipreversed)r   rC   indicesZstacked_indicescoordinatesr   rg   r   sample_image_grid   s   ru   pointsc                 C   $   t j| t | dddf gddS )z'Convert batched points (xyz) to (xyz1)..Nr   r   r
   )r   r   	ones_likerv   r   r   r   homogenize_points      $rz   vectorsc                 C   rw   )z(Convert batched vectors (xyz) to (xyz0)..Nr   r   r
   )r   r   r   )r|   r   r   r   homogenize_vectors   r{   r}   homogeneous_coordinatestransformationc                 C   s   t || |jdS )z7Apply a rigid-body transformation to points or vectors.... i j, ... j -> ... i)r   rJ   rB   )r~   r   r   r   r   transform_rigid   s
   
r   
extrinsicsc                 C   s
   t | |S )zDTransform points from 3D camera coordinates to 3D world coordinates.)r   )r~   r   r   r   r   transform_cam2world  s   
r   rt   z
intrinsicsc                 C   s6   t | } t|  || |jd}||d  S )z8Unproject 2D camera coordinates with the given Z values.r   rD   )rz   r   floatinverserJ   rB   )rt   r   r   Zray_directionsr   r   r   	unproject  s   
r   c                 C   sh   t | t| d |}||jddd }t|}t||dd df }|dd ddf |j}||fS )Nr@   r   Tr   keepdim.)r   r   rx   normr}   r   broadcast_tor   )rt   r   r   Z
directionsZoriginsr   r   r   get_world_rays"  s   	r   c                    s         fdd}|g d}|g d}|g d}|g d}|| jdd }|| jdd }tj||fddS )	Nc                    s2   t j|  j jd} t| d} | | jddd S )NrA   zb i j, j -> b ir   Tr   )r   rI   rB   rC   r   r   )vectorr   Zintrinsics_invr   r   process_vector?  s   zget_fov.<locals>.process_vector)r   rF   r   )r   rF   r   )rF   r   r   )rF   r   r   r   r
   )r   r   rJ   r3   acosr   r4   )r   r   leftrighttopbottomZfov_xZfov_yr   r   r   get_fov<  s   r   pdfglobal_stepopacity_mappingc                 C   sf   |d urt di |}|jt||j d|j|j   }nd}d| }ddd|  |  | d|    S )Nr   g        r&   rF   r   )r   initialminZwarm_upfinal)r   r   r   cfgr`   exponentr   r   r   map_pdf_to_opacityM  s   $ r   c                 C   s   | | dddf  S )zNormalize the point vectors.r   Nr   ry   r   r   r   normalize_homogenous_points_  s   r   c                 C   s
   t | S ) )r   r   )Zixtsr   r   r   inverse_intrinsic_matrixc  s   
r   c                 C   s&   t | } tdt|| }|| }|S )a~  
    Convert pixel space points to camera space points.

    Args:
        pixel_space_points (torch.Tensor): Pixel space points with shape (h, w, 2)
        depth (torch.Tensor): Depth map with shape (b, v, h, w, 1)
        intrinsics (torch.Tensor): Camera intrinsics with shape (b, v, 3, 3)

    Returns:
        torch.Tensor: Camera space points with shape (b, v, h, w, 3).
    zb v i j , h w j -> b v h w i)rz   r   r   r   )pixel_space_pointsdepthr   camera_space_pointsr   r   r   pixel_space_to_camera_spaceg  s   
r   c                 C   &   t | } td|| }|dddf S )aK  
    Convert camera space points to world space points.

    Args:
        camera_space_points (torch.Tensor): Camera space points with shape (b, v, h, w, 3)
        c2w (torch.Tensor): Camera to world extrinsics matrix with shape (b, v, 4, 4)

    Returns:
        torch.Tensor: World space points with shape (b, v, h, w, 3).
    z b v i j , b v h w j -> b v h w i.Nr   )rz   r   r   )r   c2wworld_space_pointsr   r   r   camera_space_to_world_space~  s   r   c                 C   r   )aV  
    Convert camera space points to pixel space points.

    Args:
        camera_space_points (torch.Tensor): Camera space points with shape (b, v1, v2, h, w, 3)
        c2w (torch.Tensor): Camera to world extrinsics matrix with shape (b, v2, 3, 3)

    Returns:
        torch.Tensor: World space points with shape (b, v1, v2, h, w, 2).
    z$b u i j , b v u h w j -> b v u h w i.Nr&   )r   r   r   )r   r   r   r   r   r   camera_space_to_pixel_space  s
   r   c                 C   s*   t | } td| | }|dddf S )aP  
    Convert world space points to pixel space points.

    Args:
        world_space_points (torch.Tensor): World space points with shape (b, v1, h, w, 3)
        c2w (torch.Tensor): Camera to world extrinsics matrix with shape (b, v2, 4, 4)

    Returns:
        torch.Tensor: Camera space points with shape (b, v1, v2, h, w, 3).
    z"b u i j , b v h w j -> b v u h w i.Nr   )rz   r   r   r   )r   r   r   r   r   r   world_space_to_camera_space  s
   
r   Fc              	   C   sH  |du rt jd| j| jd}|d | jd | jd dd}|sF| jd | jd }}t jt j|| j| jdt j|| j| jdd	d
\}}	nH|durN|dusPJ d| }
d| }d| }| }d|
 }| }d}t jt j|| || |t j	| jdt j|| || |t j	| jddd
\}	}t j
||	fdd}t|| |}t||}|S )z
    Turn the depth map into a 3D point cloud in world space

    Args:
        depth: (b, v, h, w, 1)
        intrinsics: (b, v, 3, 3)
        c2w: (b, v, 4, 4)

    Returns:
        torch.Tensor: World space points with shape (b, v, h, w, 3).
    Nr   )rC   rB   )NNr   r   r   ro   )rl   r	   rA   rm   r   r
   )r   eyerC   rB   repeatr   rp   rh   linspacefloat32r4   r   r   )r   r   r   Zixt_normalizedZnum_patches_xZnum_patches_yhwZx_gridZy_griddxdyZmax_yZmin_yZmax_xZmin_xZ
grid_shiftr   Zcamera_pointsZworld_pointsr   r   r   unproject_depth  sR    

r   )r   N)NFNN)-typesr   typingr   numpyr   r   Ztorch.nn.functionalnn
functionalrL   Zeinopsr   r   jitscriptr   r%   r-   r   r.   r<   r_   rH   rO   rC   tupleintru   rz   r}   r   r   r   r   r   dictr   r   r   r   r   r   r   r   r   r   r   r   <module>   s   !E






