o
    ߥi?                     @   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)pointsknn_idxpoints_flipped
new_points r   o/home/ubuntu/.local/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)pcnsampleradiusdistkidxtmp_idx
grouped_pc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biasT)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mlpr0   r@   r1   r2   r4   r3   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   FN)r   r
   r/   r   r   r0   r   furthest_point_samplegather_operationr>   	transpose	enumerater7   r2   r8   r4   r3   r   meanmaxr@   )rA   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.   r]   __classcell__r   r   rG   r   r'   /       	+r'   c                       r&   )	PointNetSetAbstractionFTc              	      r(   r)   )r-   rg   r.   rV   r   r   r0   r1   r2   r3   r4   r5   r6   r7   r8   r9   r:   r;   r<   r   r=   r>   r?   r@   )rA   rV   r   r   rB   rC   r0   r@   r1   r2   r4   r3   rD   rE   rF   rG   r   r   r.      rI   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   FrJ   N)r   r   r   r0   rV   r   rK   rL   r>   rM   rN   r7   r2   r8   r4   r3   r   rO   rP   r@   )rA   rQ   r   rR   rS   rT   rU   rW   rX   r   rY   rZ   r[   r\   r   r   r   r]      s8   



zPointNetSetAbstraction.forwardr^   r_   r   r   rG   r   rg      rf   rg   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)r1      )    r   Fr*   )r-   rh   r.   r5   r6   r7   r8   	apply_mlpr   r9   Conv1dBatchNorm1dr   r?   r>   r:   r<   )rA   rB   rC   
learn_maskr   rE   rF   rG   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rt||gd}n|}| jrt| 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|=      ?rJ   T)keepdimr   ri   )r   r   r   r   r   three_nnr   r   r   r	   viewr>   rN   r7   softmaxcatrl   r8   rc   rd   )rA   pos1pos2feature1feature2hiddenpos1_tpos2_trS   rT   rU   distsidxweightinterpolated_featr    new_featrY   rZ   r[   feat_newr\   r   r   r   r]      sF   




z"PointNetFeaturePropogation.forward)Fr   r^   r`   ra   rb   r.   r]   re   r   r   rG   r   rh      s    rh   c                       s$   e Zd Z fddZdd Z  ZS )Sinkhornc                    s   t    d S r^   )r-   r.   )rA   rG   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:0yE>)
r   onesr   r   r   rangebmmrM   powmul)rA   correpsilongammamax_iterpoweraprob1prob2rY   KTabKbTr   r   r   r]   7  s>   zSinkhorn.forwardr   r   r   rG   r   r   2  s    r   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   rk   r   T)inplace@   rJ   r*   )r-   r.   r   r   use_curvaturer5   
Sequentialrm   ReLU	pos_embedqk_netcurvate_netmlp_conv)rA   r   r   rB   rC   r   rG   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rt| 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   rJ   rp   T)r   rq   g?g        )r   r   r   r   r   r   r   r   r   r   r   r   ru   r   r	   rt   mathsqrtr   r   r   r%   r   r   floatclone)rA   rv   rw   rx   ry   r   r   pos1_rawreturn_scorer{   r|   r    r~   r"   pos1_embed_normpos2_embed_normfeat1_w_posfeat2_w_posfeat2_groupedscorecostcurvate1_rawcurvate1curvate_costpos2_groupedindex	score_tmpr   r   r   r]   r  s~   








zPointWiseOptimLayer.forward)T)NNFr   r   r   rG   r   r   Z  s    r   )r   r   torch.nnr5   torch.nn.functional
functionalrc    r   r   RADIUSr   r%   Moduler'   rg   rh   r   r   r   r   r   r   <module>   s   \[L(