o
    iN                     @   s   d dl Z d dlmZ ddlmZ 					d!d	d
Zdd Zdd Z					d"ddZdd Z						d#ddZ
						d$ddZdd Zd%ddZd&dd ZdS )'    N)repeat   )unproject_depth-C6?皙?d      c	                 C   s  | j }	| j\}
}}tt|dddddf |kt| dddddf |k}|  } | }| dddddf |  | dddddf |   < | dddddf |  | dddddf |   < |dddddf |  |dddddf |   < |dddddf |  |dddddf |   < | ddddddf } |ddddddf }|dusJ dd|| < g }d}td| jd |D ]:}t| |||  ||||  ||||  |||||dd	}||	}tj	
|dk }||  ||< || qtj|dd}g }g }g }t|jd D ]@}t|| \}}||d d  }t|d d |d d f}t|d d |d d f}|| || || q&t|}t|}t|}|||fS )	a<  
    Args:
        rays_origin (torch.Tensor): (B, N, 3)
        rays_target (torch.Tensor): (B, N, 3)
        z_threshold (float): Threshold for z value to be considered valid.

    Returns:
        R (torch.tensor): (3, 3)
        focal_length (torch.tensor): (2,)
        principal_point (torch.tensor): (2,)
    N   r   r   zweights must be providedi@  )n_itern_samplenum_sample_for_ransacreproj_thresholdrand_sample_iters_idxmax_inlier_numdim)deviceshapetorchlogical_andabsclonerange*ransac_find_homography_weighted_fast_batchtolinalgdetappendcatql_decompositionstack)Zrays_originZrays_targetZz_thresholdr   weightsr   r
   r   r   r   BN_Zz_maskZA_listZmax_chunk_sizeiAZA_need_inv_maskZR_listf_listZpp_listRLfpp r,   >/data/cameron/da3_repo/src/depth_anything_3/utils/ray_utils.py)compute_optimal_rotation_intrinsics_batch   sb   <8888







r.   c                 C   s(  t jg dg dg dg| jd }t | |}t j|\}}t ||}t t |||}t |}|d d df  t |d 9  < |d d df  t |d 9  < |d d df  t |d 9  < |d  t |d 9  < |d  t |d 9  < |d  t |d 9  < ||fS )N)r   r   r   )r   r   r   )r   r   r   r   r   r   r	   )	r   tensorr   floatmatmulr   qrdiagsign)r&   PZA_tildeZQ_tildeZR_tildeQr)   dr,   r,   r-   r   a   s   &
"""r   c                 C   sj  | j |j ksJ | j d }|dk rtd|j |fksJ | d}| ddddf }| ddddf }|ddddf }|ddddf }t|}	tj| | | | | |	|	|	|| | || | || g	dd}
tj|	|	|	| | | | | || | || | || g	dd}tj|
|gdd}tj|\}}}|d 	d	d	}||d
  }|S )z
    src_pts: (N,2) source points (torch.Tensor, float32/float64)
    dst_pts: (N,2) target points (torch.Tensor, float32/float64)
    confident_weight: (N,) weights (torch.Tensor)
    Returns: (3,3) homography matrix H (torch.Tensor)
    r      z5At least 4 points are required to compute homography.r   Nr	   r      )r:   r:   )
r   
ValueErrorsqrt	unsqueezer   
zeros_liker   r   svdreshape)src_ptsdst_ptsconfident_weightr#   wxyuvzerosA1A2r&   r$   VhHr,   r,   r-   ,find_homography_least_squares_weighted_torchp   s$   

BBrO         @   c              	   C   s  |dur	t | | jd }|dksJ t j|dd}	t|t|| }
|	d|
 }d}d}t|D ]}|t |
d|  }zt| | || || }W n t	y`   t j
d| j| jd}Y nw t j| t j|d| j| jdgdd	}||j j}|dddd
f |ddd
df  }|| d
 jdd	 }||k }||   }|  }|dk rq2||kr|}|}q2t| | || || }|S )z
    RANSAC version of weighted Homography estimation.
    Sample 4 points from the top 50% weighted points each time.
    reproj_threshold: points with reprojection error less than this value are inliers
    Returns: best_H
    Nr   r9   T
descendingr;   dtyper   r   r   r	   )r   manual_seedr   argsortmaxintr   randpermrO   	ExceptioneyerU   r   r   onesTsumr=   item)rB   rC   rD   r
   sample_ratior   r   random_seedr#   
sorted_idxr   candidate_idxbest_inlier_maskZ
best_scorer$   idxrN   src_homoprojerrorinlier_masktotal_scoreZn_inlierH_inlierr,   r,   r-   ransac_find_homography_weighted   sJ   

(rm   c                 C   sp  | j \}}}| d}| ddddddf }| ddddddf }|ddddddf }	|ddddddf }
t|}tj| | | | | |||||	 | ||	 | |	| g	dd}tj|||| | | | | ||
 | ||
 | |
| g	dd}tj||gdd}tj|\}}}|dddf |dd}||ddddddf  }|S )z
    Batch version of weighted least squares Homography
    src_pts_batch: (B, K, 2)
    dst_pts_batch: (B, K, 2)
    confident_weight_batch: (B, K)
    Returns: (B, 3, 3)
    r	   Nr   r   r   r:   r;   )	r   r=   r>   r   r?   r   r   r@   rA   )src_pts_batchdst_pts_batchconfident_weight_batchr"   Kr$   rE   rF   rG   rH   rI   rJ   rK   rL   r&   rM   rN   r,   r,   r-   2find_homography_least_squares_weighted_torch_batch   s   

BBrr   c	           !         s  |dur	t | | jd }	| j |	dksJ t j|dd}
|
d }|du r:t j fddt|D dd}|| }| | }|| }|| }t|||}t j| t j	|	d	| j
| jd
gd	d}|d||	d}|d||	d}|d||	}t ||d	d}|ddddddf |ddddddf  }|| d jdd }||k }|| jd	d}t |}|| }| | }|| }|| }d}t j|dd}
|
t t|
d|  }
||
 }||
 }||
 }t|||} | S )zW
    Batch version of RANSAC weighted Homography estimation.
    Returns: H_inlier
    Nr   r9   TrR   c                    "   g | ]}t j d d qS r/   Nr   rZ   .0r$   r   r   r   r,   r-   
<listcomp>     " z8ransac_find_homography_weighted_fast.<locals>.<listcomp>r   r   rT   r;   r	   '  )r   rV   r   r   rW   r    r   rr   r   r]   rU   r>   expandbmm	transposer_   r=   argmaxrZ   lenrO   )!rB   rC   rD   r   r
   r   r   rb   r   r#   rc   rd   rand_idxrn   ro   rp   H_batchrg   src_homo_expanddst_pts_expandconfident_weight_expandrh   proj_xyri   rj   rk   best_idxre   inlier_src_ptsinlier_dst_ptsinlier_confident_weightr   rl   r,   rx   r-   $ransac_find_homography_weighted_fast   sZ   

4
r   r{   c
           -   	      s*  |dur	t | | j\}
}}|dksJ | j t j|ddd}|dddf }|	du r@t j fddt|D dd	}	|dd|	f }t j|
 d
|
dd	|
|}| ||f }|||f }|||f }|jdd \}}t
|dd|dd|dd}|d||f}t j| t j|
|d| j| jdgdd	}|d	|
||d}|d	|
||d}|d	|
||}|ddd}|d|d}t ||dd}|ddddddf |ddddddf  }||
||d}|| d jdd	 }||k } | | jdd	}!t j|!dd	}"| t j|
 d
|"f }#g }$t|
D ]`}%|#|% }&| |% |& }'||% |& }(||% |& })t j|)dd}t||krsttt|d |}*|d|* }t jt| d
d| }+||+ }|'| }'|(| }(|)| })t|'|(|)},|$|, q+t j|$dd	},|,S )z
    Batch version of RANSAC weighted Homography estimation (supports batch).
    Input:
        src_pts: (B, N, 2)
        dst_pts: (B, N, 2)
        confident_weight: (B, N)
    Returns:
        H_inlier: (B, 3, 3)
    Nr9   Tr   )rS   r   c                    rs   rt   ru   rv   rx   r,   r-   ry   ]  rz   z>ransac_find_homography_weighted_fast_batch.<locals>.<listcomp>r   r   r/   r	   rT   r;   r:   rR   gffffff?)r   rV   r   r   rW   r    r   arangeviewr|   rr   flatten	unflattenr   r]   rU   r>   rA   r}   r~   r_   r=   r   r   rX   rY   rZ   rO   r   )-rB   rC   rD   r   r
   r   r   r   rb   r   r"   r#   r$   rc   rd   r   Zb_idxrn   ro   rp   ZcBZcNr   rg   r   r   r   ZH_batch_flatZsrc_homo_expand_flatrh   r   ri   rj   rk   r   re   ZH_inlier_listbmaskr   r   r   Zkeep_lenpermrl   r,   rx   r-   r   9  sv   
"4r   c                    sN   d}d}dt t| | tj fddt|D dd}||fS )Nr   g333333?r   c                    rs   rt   ru   rv   rx   r,   r-   ry     rz   z)get_params_for_ransac.<locals>.<listcomp>r   r   )rX   rY   r   r    r   )r#   r   r
   ra   r   r,   rx   r-   get_params_for_ransac  s   r   Fc              
   C   s  |du rt | dddddddddf }| j\}}}}}t jd| j| jd}	d|	d< d|	d< |	dd||dd}	t j||||d	| j| jd}
t	|
|	dd
||d}| 
dd	
d	d} |
dd	
d	d}|
dd	
d	d}| jd }| j}t||\}}}}|r|   } |  }|  }t|| ddddddf ||||||d\}}}t j| ddddddf |d d	dt j|dd
d }|||dd}|||d}|||d}|||d}||d| |d fS )z
    Args:
        camray: (B, S, num_patches_y, num_patches_x, 6)
        confidence: (B, S, num_patches_y, num_patches_x)
    Returns:
        R: (B, S, 3, 3)
        T: (B, S, 3)
        focal_lengths: (B, S, 2)
        principal_points: (B, S, 2)
    Nr   r;   rT   g      ?)r   r	   )r   r	   r:   r   T)c2wixt_normalizednum_patches_xnum_patches_yr	   )r   r!   r   r
   r   r   r   )r   keepdim)r   	ones_liker   r\   rU   r   r>   r|   r]   r   r   r   r   detachr.   r_   rA   )camray
confidencer   trainingr"   Sr   r   r$   ZI_KZcam_plane_depthZI_cam_plane_unprojr#   r   r
   r   r   r   r(   Zfocal_lengthsZprincipal_pointsr^   r,   r,   r-   camray_to_caminfo  s`   (	
0r   c           
      C   sx   t | |d|d\}}}}tjtj||dgddttjg d|j|jdd|j	d |j	d d	gd
d}	|	||fS )Nr:   )r   r   r   )r   r   r   r   rT   zc -> b s 1 cr   r   )r   sr   )
r   squeezer   r   r>   r   r0   rU   r   r   )
r   confZpatch_size_yZpatch_size_xr   Zpred_RZpred_Tpred_focal_lengthspred_principal_pointspred_extrinsicr,   r,   r-   get_extrinsic_from_camray  s   	
r   )r   r   NNr   r   N)r   r   rP   rQ   N)r   rP   r   NN)r   rP   r   r{   NN)Nr   F)F)r   einopsr   geometryr   r.   r   rO   rm   rr   r   r   r   r   r   r,   r,   r,   r-   <module>   sD   
M'
= 
S
n
G