o
    *Îj?  ã                   @   s²   d dl Z d dlZd dlmZ d dlm  mZ ddlmZ	 dZ
dd„ Zde
fdd	„ZG d
d„ dejƒZG dd„ dejƒZG dd„ dejƒZG dd„ dejƒZG dd„ dejƒZdS )é    Né   )Úpointnet2_utilsg      @c                 C   s2   |   ddd¡ ¡ }t || ¡ ¡  dddd¡}|S )z®
    Input:
        points: input points data, [B, N, C]
        knn_idx: sample index data, [B, N, K]
    Return:
        new_points:, indexed points data, [B, N, K, C]
    r   é   r   é   )ÚpermuteÚ
contiguousÚ
pointutilsÚgrouping_operationÚint)ÚpointsZknn_idxZpoints_flippedÚ
new_points© r   ú|/var/www/html/Deteccion_Ine/venv/lib/python3.10/site-packages/modelscope/models/cv/pointcloud_sceneflow_estimation/common.pyÚindex_points_group   s   ÿÿÿr   é
   c                 C   s®   | j d dks	J ‚|  ddd¡} t ||  ¡ |  ¡ ¡\}}|d urB|d d …d d …df  d¡ dd|¡ |j¡}|||k |||k< t	| |ƒ}t
j||  d¡ ddd }|S )Nr   r   r   r   ©Údimg      "@)Úshaper   r   Úknnr   Ú	unsqueezeÚrepeatÚtoÚdevicer   ÚtorchÚsum)ZpcÚnsampleÚradiusÚdistZkidxÚtmp_idxZ
grouped_pcZpc_curvaturer   r   r   Ú	curvature   s   ÿ ÿÿ
r   c                       ó6   e Zd Zdddejddf‡ fdd„	Zddd„Z‡  ZS )	ÚPointNetSetAbstractionRatioFTc              	      óè   t t| ƒ ¡  || _|| _|| _|| _|| _|	| _|| _	|
| _
t ¡ | _t ¡ | _|r/|d n|}|D ](}| j tj||ddd¡ |rP| j tj|dd¡ n	| j t |¡¡ |}q3|rft | j¡| _n	t ||| j¡| _|| _d S ©Nr   r   F©ZbiasT)Zaffine)Úsuperr!   Ú__init__Úratior   r   Ú	group_allÚuse_xyzÚuse_actÚ	mean_aggrÚactÚnnÚ
ModuleListÚ	mlp_convsÚmlp_bnsÚappendÚConv2dÚInstanceNorm2dÚBatchNorm2dr   ÚGroupAllÚqueryandgroupÚQueryAndGroupÚ
return_fps)Úselfr'   r   r   Ú
in_channelÚmlpr(   r8   r)   r*   r,   r+   Úuse_instance_normÚlast_channelÚout_channel©Ú	__class__r   r   r&   1   ó8   

ÿÿÿ
z$PointNetSetAbstractionRatio.__init__Nc                 C   s  |j \}}}t|| j ƒ}| ¡ }| ddd¡ ¡ }| jdu r4|dkr4|du r-t ||¡}t ||¡}	n|}	|  	||	 
dd¡ ¡ |¡\}
}t| jƒD ]\}}| jr`| j| }|  |||
ƒƒ¡}
qJ||
ƒ}
qJ| jrot |
d¡}
nt |
d¡d }
| jr|	|
|fS |	|
fS )a  
        Input:
            xyz: input points position data, [B, C, N]
            points: input points data, [B, D, N]
        Return:
            new_xyz: sampled points position data, [B, C, S]
            new_points: sample points feature data, [B, D', S]
        r   r   r   FéÿÿÿÿN)r   r
   r'   r   r   r(   r   Úfurthest_point_sampleÚgather_operationr6   Ú	transposeÚ	enumerater/   r*   r0   r,   r+   r   ÚmeanÚmaxr8   )r9   Úxyzr   Úfps_idxÚBÚCÚNÚnpointÚxyz_tÚnew_xyzr   Ú_ÚiÚconvÚbnr   r   r   Úforward\   s:   	ÿÿý


z#PointNetSetAbstractionRatio.forward©N©Ú__name__Ú
__module__Ú__qualname__ÚFÚrelur&   rU   Ú__classcell__r   r   r?   r   r!   /   ó    	ô+r!   c                       r    )	ÚPointNetSetAbstractionFTc              	      r"   r#   )r%   r_   r&   rN   r   r   r(   r)   r*   r+   r,   r-   r.   r/   r0   r1   r2   r3   r4   r   r5   r6   r7   r8   )r9   rN   r   r   r:   r;   r(   r8   r)   r*   r,   r+   r<   r=   r>   r?   r   r   r&      rA   zPointNetSetAbstraction.__init__Nc                 C   sü   |j \}}}| ¡ }| ddd¡ ¡ }| jdu r/| jdkr/|du r(t || j¡}t ||¡}n|}|  || 	dd¡ ¡ |¡\}	}
t
| jƒD ]\}}| jr[| j| }|  |||	ƒƒ¡}	qE||	ƒ}	qE| jrjt |	d¡}	nt |	d¡d }	| jrz||	|fS ||	fS )a  
        Input:
            xyz: input points position data, [B, C, N]
            points: input points data, [B, D, N]
        Return:
            new_xyz: sampled points position data, [B, S, C]
            new_points: sample points feature data, [B, S, D']
        r   r   r   FrB   N)r   r   r   r(   rN   r   rC   rD   r6   rE   rF   r/   r*   r0   r,   r+   r   rG   rH   r8   )r9   rI   r   rJ   rK   rL   rM   rO   rP   r   rQ   rR   rS   rT   r   r   r   rU   ¸   s8   
ÿÿý


zPointNetSetAbstraction.forwardrV   rW   r   r   r?   r   r_   ‹   r^   r_   c                       s(   e Zd Zd‡ fdd„	Zd	dd„Z‡  ZS )
ÚPointNetFeaturePropogationFr   c              	      sÎ   t t| ƒ ¡  t ¡ | _t ¡ | _|d u| _|}|| _| jr9|D ]}| j 	t 
||d¡¡ | j 	t |¡¡ |}q |rctjd ddd| _d}dD ]}| j 	tj||ddd¡ | j 	t |¡¡ |}qHd S d S )	Nr   é	   T)r)   éƒ   )é    r   Fr$   )r%   r`   r&   r-   r.   r/   r0   Ú	apply_mlpr   r1   ÚConv1dZBatchNorm1dr   r7   r6   r2   r4   )r9   r:   r;   Z
learn_maskr   r=   r>   r?   r   r   r&   è   s0   


ÿÿøz#PointNetFeaturePropogation.__init__Nc                 C   sz  |  ddd¡ ¡ }|  ddd¡ ¡ }|j\}}	}
|du r]| jdkr*t ||¡\}}n
t | j||¡\}}d||dk < d| }|tj|dd	d
 }tjt 	||¡| 
|d|
| j¡ dd}n4t d||¡\}}|  |||¡\}}t| jƒD ]\}}||ƒ}qttj|dd}tjt 	||¡| dd}|duržt ||gd¡}n|}| jr»t| jƒD ]\}}| j| }t |||ƒƒ¡}q¨|S )a>  
        Input:
            pos1: input points position data, [B, C, N]
            pos2: sampled input points position data, [B, C, S]
            feature1: input points data, [B, D, N]
            feature2: input points data, [B, D, S]
        Return:
            feat_new: upsampled points data, [B, D', N]
        r   r   r   Nr   g»½×Ùß|Û=ç      ð?rB   T)Úkeepdimr   ra   )r   r   r   r   r   Zthree_nnr   r   r   r	   Úviewr6   rF   r/   ÚsoftmaxÚcatrd   r0   r[   r\   )r9   Úpos1Úpos2Úfeature1Úfeature2ZhiddenÚpos1_tÚpos2_trK   rL   rM   ÚdistsÚidxÚweightZinterpolated_featr   Znew_featrQ   rR   rS   Zfeat_newrT   r   r   r   rU   ÿ   sF   


ÿýÿ
þ
z"PointNetFeaturePropogation.forward)Fr   rV   ©rX   rY   rZ   r&   rU   r]   r   r   r?   r   r`   æ   s    r`   c                       s$   e Zd Z‡ fdd„Zdd„ Z‡  ZS )ÚSinkhornc                    s   t ƒ  ¡  d S rV   )r%   r&   )r9   r?   r   r   r&   4  s   zSinkhorn.__init__c                 C   s  |dkr|S |||  }t j|jd |jd df|j|jd|jd  }t j|jd |jd df|j|jd|jd  }t j|jd |jd df|j|jd|jd  }t|ƒD ]&}	t  | dd¡|¡}
t  ||
d  |¡}t  ||¡}t  ||d  |¡}qXt  	t  	||¡| dd¡¡}|S )Nr   r   )r   Údtyper   g:Œ0âŽyE>)
r   Zonesr   r   rv   ÚrangeZbmmrE   ÚpowÚmul)r9   ZcorrÚepsilonÚgammaZmax_iterÚpowerÚaZprob1Zprob2rQ   ZKTaÚbZKbÚTr   r   r   rU   7  s>   þþÿþþÿþþÿzSinkhorn.forwardrt   r   r   r?   r   ru   2  s    ru   c                       s.   e Zd Zd‡ fdd„	Z			d	dd„Z‡  ZS )
ÚPointWiseOptimLayerTc              	      sú   t ƒ  ¡  || _|| _|| _t t ddd¡tjddt ddd¡¡| _	t t |d |d d¡tjddt |d |d d¡¡| _
| jrmt t ddd¡tjddt ddd¡¡| _tj|d d |d ddd| _d S tj|d |d ddd| _d S )	Nr   rc   r   T)Zinplaceé@   rB   r$   )r%   r&   r   r   Úuse_curvaturer-   Z
Sequentialre   ZReLUÚ	pos_embedÚqk_netÚcurvate_netÚmlp_conv)r9   r   r   r:   r;   r‚   r?   r   r   r&   \  s*   
"ÿ
ýþÿ zPointWiseOptimLayer.__init__NFc	                 C   s$  |  ddd¡ ¡ }	|  ddd¡ ¡ }
|| _|| _t | j|	|
¡\}}| jdurK|dd…dd…df  d¡ dd| j¡ |j	¡}||| jk ||| jk< |  
|¡}|  
|¡}tj||gdd}tj||gdd}|  |¡}|  |¡}t ||¡}tj| d¡| d t |jd ¡ dd}|| d¡| d  jdd}| jrÌt|ƒ  ddd¡}t|ƒ  ddd¡}|  |¡|  |¡ }|d }|  tj||gdd¡}n|  |¡}|rt ||¡}|| jkjddd	 ¡ |jd d
 k}| d¡ d|jd d|jd ¡}| ¡ }d||< |}||||fS ||fS )a>  
        Input:
            pos1: (batch_size, 3, npoint)
            pos2: (batch_size, 3, npoint)
            feature1: (batch_size, channel, npoint)
            feature2: (batch_size, channel, npoint)
        Output:
            pos1: (batch_size, 3, npoint)
            cost: (batch_size, channel, npoint)
        r   r   r   Nr   rB   rf   T)r   rg   gš™™™™™¹?g        )r   r   r   r   r   r   r   r   r   r   rƒ   r   rj   r„   r	   ri   ÚmathÚsqrtr   r   r‚   r   r…   r†   ÚfloatÚclone)r9   rk   rl   rm   rn   r   r   Zpos1_rawZreturn_scorero   rp   r   rr   r   Zpos1_embed_normZpos2_embed_normZfeat1_w_posZfeat2_w_posZfeat2_groupedZscoreZcostZcurvate1_rawZcurvate1Zcurvate_costZpos2_groupedÚindexZ	score_tmpr   r   r   rU   r  s~   
ÿÿþ


ÿ

ÿÿýÿÿ
ÿ
ÿ
ÿÿÿzPointWiseOptimLayer.forward)T)NNFrt   r   r   r?   r   r€   Z  s    ør€   )r‡   r   Ztorch.nnr-   Ztorch.nn.functionalZ
functionalr[   Ú r   r   ZRADIUSr   r   ÚModuler!   r_   r`   ru   r€   r   r   r   r   Ú<module>   s   \[L(