o
    ߥiX                     @   s`   d dl Z d dlZd dlZd dlZddlmZ dd Zdd Zdd	 Z	d
d Z
dd Zdd ZdS )    N   )cfgc                 C   s  | j \}}}|d}tddd|d |d | j}| d} z;tj| ddddddddf | jd}tj| ddddddddf | jd}tt	| ddddddddf | ddddddddf ||||d| ddddddddf  |ddddddddf  d| ddddddddf  |ddddddddf  gdt	| ddddddddf | ddddddddf ||||d| ddddddddf  |ddddddddf  d| ddddddddf  |ddddddddf  gdt	| ddddddddf | ddddddddf ||||d| ddddddddf  |ddddddddf  d| ddddddddf  |ddddddddf  gdt	| ddddddddf | ddddddddf ||||d| ddddddddf  |ddddddddf  d| ddddddddf  |ddddddddf  gdt	|||| ddddddddf | ddddddddf |d| ddddddddf  |ddddddddf  d| ddddddddf  |ddddddddf  gdt	|||| ddddddddf | ddddddddf |d| ddddddddf  |ddddddddf  d| ddddddddf  |ddddddddf  gdt	|||| ddddddddf | ddddddddf |d| ddddddddf  |ddddddddf  d| ddddddddf  |ddddddddf  gdt	|||| ddddddddf | ddddddddf |d| ddddddddf  |ddddddddf  d| ddddddddf  |ddddddddf  gdgd
dddddd}	t	|dddddddf |dddddddf |dddddddf |dddddddf |dddddddf |dddddddf |dddddddf |dddddddf gd
dddd}
t|	}t||
d}t|tj|ddddddf |jdgd
|j d dd}|ddd}|
|j }|}W n tyA   tj| ddddddf | jd}tj| ddddddf | jd}tjd| jd}t|d D ]}t|d D ]}tt	| dddd||f | dddd||f ||||d| dddd||f  |dddd||f  d| dddd||f  |dddd||f  gdt	| dddd|d |f | dddd|d |f ||||d| dddd|d |f  |dddd|d |f  d| dddd|d |f  |dddd|d |f  gdt	| dddd||d f | dddd||d f ||||d| dddd||d f  |dddd||d f  d| dddd||d f  |dddd||d f  gdt	| dddd|d |d f | dddd|d |d f ||||d| dddd|d |d f  |dddd|d |d f  d| dddd|d |d f  |dddd|d |d f  gdt	|||| dddd||f | dddd||f |d| dddd||f  |dddd||f  d| dddd||f  |dddd||f  gdt	|||| dddd|d |f | dddd|d |f |d| dddd|d |f  |dddd|d |f  d| dddd|d |f  |dddd|d |f  gdt	|||| dddd||d f | dddd||d f |d| dddd||d f  |dddd||d f  d| dddd||d f  |dddd||d f  gdt	|||| dddd|d |d f | dddd|d |d f |d| dddd|d |d f  |dddd|d |d f  d| dddd|d |d f  |dddd|d |d f  gdgd}	t	|ddd||f |ddd|d |f |ddd||d f |ddd|d |d f |ddd||f |ddd|d |f |ddd||d f |ddd|d |d f gd}
z2t|	}t||
d}t|t|ddddddf |jgd
|j d dd}W n
 ty+   Y nw ||dddddd||f< qqY nw |
dd|d |d }|S )	z
    @param: grids the location of origin grid vertices [2, H, W]
    @param: new_grids_loc the location of desired grid vertices [2, H, W]

    @return: homo_t homograph projection matrix for each grid [3, 3, H-1, W-1]
    r         N)devicer      )shape	unsqueezetorchzerostor   	ones_like
zeros_likecatstackviewpermuteinversebmm	Exceptioneyerange)gridsnew_grids_loc_HW	new_gridsHomoonezeroAB_	A_inverseH_recoveredH_ijhomo_t r*   r/home/ubuntu/.local/lib/python3.10/site-packages/modelscope/models/cv/video_stabilization/utils/ProjectionUtils.pyHomoCalc   s  
"
,,>BB@BB2BB2BB$ BB$ BB BB8BB234	
	
$
$$&226$6$2&",22   $$"78	

 "Xr,   c              	   C   s  t j|ddddf tjjdd }t j|ddddf tjjdd }tjjtjj d }tjjtjj d }|dddf |k  }|d ||ddf< |dddf |k  }|d ||ddf< | 	|j
} |dddf | dd|dddf |dddf f  |dddf | dd|dddf |dddf f   | dd|dddf |dddf f  }|dddf | dd|dddf |dddf f  |dddf | dd|dddf |dddf f   | dd|dddf |dddf f  }|dddf | dd|dddf |dddf f  |dddf | dd|dddf |dddf f   | dd|dddf |dddf f  }	|	}	||	 }
||	 }t |
|gdS )z
    @param: homo [3, 3, G_H-1, G_W-1]
    @param: pts  [N, 2(W, H)] - [:, 0] for width and [:, 1] for height

    @return: projected pts [N, 2(W, H)] - [:, 0] for width and [:, 1] for height
    Nr   r   floor)rounding_moder   )r   divr   MODELPIXELSlongWIDTHHEIGHTnonzeror   r   r   )homoptspts_location_xpts_location_ymaxWidth	maxHeightindexx_dominatory_dominator
noiminatornew_kp_xnew_kp_yr*   r*   r+   HomoProj   sH   
D&&D&&D&&rB   c           2   
   C   s|	  ddl m} t|ddddf |ddddf gd| ddddf  }|   dd}t|dddf |dddf gd	| j
}t|ddddf |ddddf gd   dd}|| }|ddd|}t|tjjd krd| }||dk d ddf }	||dk d ddf }
t|	|
tj\}}|du rtg dg dg d	g}|d
 |dddf  |d |dddf   |d  }tt|d |dddf  |d |dddf   |d  | |d |dddf  |d |dddf   |d  | gdtj	|j
dd}|dk d }t|ddddf }||ddf }||ddf }tt|d||ddf< |
|	 }|| }t|d}t|d}t||}t|tjj ko|}|r8t||tj\}}|du r|}t!t"dtjj#tjj$t"dtjj%tjj$\}}|d | |d |  |d  }|d | |d |  |d  }|d
 | |d |  |d  }t&t|| || gdd}|d | |d |  |d  }|d | |d |  |d  }|d
 | |d |  |d  }t&t|| || gdd} t|dddf dt&|d }!t|dddf dt&|d }"|!d |"d  }#|#tjj'd k }$|$tj|dd }%t|%dt|$dd  }#t&t|#d|  td|# d|  tjj%tjj$ tjj#tjj$ df}&t|&tj	|j
ddd}'t!t"dtjj#tjj$t"dtjj%tjj$\}}|( dd}|( dd}|	|j
}|	|j
}t||gd}(|(dddddf |'dddddf  })|(dddddf |'dddddf  }*t)|(|'}+t*|+|},|,	|j
dd}nt|tj	|j
}t!t"dtjj#tjj$t"dtjj%tjj$\}}|( dd}|( dd}|	|j
}|	|j
}t|	|j
}-t|||-gd}(t+||(,ddj,|(j- }'|(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   }*tt!t"dtjj#tjj$t"dtjj%tjj$d	| j
ddd&dddd}.|..d( }.t|)|*gd,ddd	| j
}/|t|ddddf |ddddf gd }0| ddddf | ddddf  | ddddf< | .d/|.j-d dd} | ddddddf |. tjj# | ddddddf< | ddddddf tjj0 }1|0.d| ddddddf  tjj0 | ddddddf< | |/tjj0 |1fS )9
    @param: motion [4, N]
    @param: kp     [2, N]
    r   )KMeansr   r   N)
n_clustersrandom_state      ?        rI   rI   rH   rI   rI   rI   rH   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   )r   r   )r   r   g&.>r   r   )1sklearn.clusterrD   r   r   cpudetachnumpy	transposer   r   r   fit_predictnpsumr   TRAINTOPKr5   cv2findHomographyRANSACarray
from_numpyastypefloat32r   r   expand_dimsr   meanMotionDistanceMeasurer0   THRESHOLDPOINTmeshgridaranger3   r1   r4   reshapeRADIUSfloatr,   rB   mmr   r	   r
   repeatFLOWC)2motionkprD   new_kpnew_points_numpy
old_pointsold_points_numpymotion_numpypred_Ycluster1_old_pointscluster1_new_pointsr   r   	dominatornew_points_projectedr<   	attributecluster2_old_pointscluster2_new_pointscluster1_motionclsuter2_motioncluster1_meanMotioncluster2_meanMotiondistanceMeasurethreholdHomo_2meshes_xmeshes_yr=   r>   r?   projected_1projected_2
distance_x
distance_ydistancedistance_maskdistance_mask_valueproject_posmeshes_projectedmeshes	x_motions	y_motionshomo_calproject_ptsmeshes_zr   projected_motionredisual_kp_motionorigin_motionr*   r*   r+   multiHomoEstimate   s  D0*




 









,

(("
(8<"r   c              
   C   s  t |ddddf |ddddf gd| ddddf  }|   dd}t |dddf |dddf gd| j}t |ddddf |ddddf gd   dd}|}|}t	
||t	j\}}	|du rtg dg dg dg}|d |dddf  |d	 |dddf   |d
  }
t t|d |dddf  |d |dddf   |d  |
 |d |dddf  |d |dddf   |d  |
 gdtj|jdd}t |tj|j}t t dtjjtjjt dtjjtjj\}}| dd}| dd}||j}||j}t ||j}t |||gd}t ||ddj|j }|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   }t t t dtjjtjjt dtjjtjjd| jddddddd}|d }t ||gdddd| j}|t |ddddf |ddddf gd }| ddddf | ddddf  | ddddf< | d |jd dd} | ddddddf | tjj | ddddddf< | ddddddf tjj! }|d| ddddddf  tjj! | ddddddf< | |tjj! |fS )rC   r   r   Nr   rG   rJ   rK   rL   rM   rN   rO   rP   rQ   rR   rS   rT   r   r   )"r   r   rV   rW   rX   rY   r   r   r   r_   r`   ra   r[   rb   rc   rd   re   r   rj   rk   r   r0   r3   r1   r4   rn   r   ro   r   r	   rl   r
   rp   rq   )rr   rs   rt   ru   rv   rw   rz   r{   r   r   r|   r}   r   r   r   r   r   r   r   r   r   r   r   r*   r*   r+   singleHomoEstimate  s   D0*(*
(8<"r   c                 C   s   t | d | d t j d S )Nr   r      )mathatan2pi)xr*   r*   r+   f_rot  s   r   c           
      C   s   t t | d }t t |d }t || }t| }t|}t || }|dkr2d| }|tjjk}|tjjk}	|pA|	S )z
    MotionDistanceMeasure
    @params motion1 np.array(2) (w, h)
    @params motion2 np.array(2) (w, h)

    @return bool describe whether the two motion are close or not, True for far and False for close
    r   r   ih  )	r[   sqrtr\   absr   r   	ThresholdMANGROT)
motion1motion2mangnitue_motion1mangnitue_motion2diff_mangnituderot_motion1rot_motion2diff_rottemp_value_12temp_value_13r*   r*   r+   rh     s   	rh   )r   r_   rX   r[   r   
DUT.configr   r,   rB   r   r   r   rh   r*   r*   r*   r+   <module>   s    >( +L