U
    `D                     @   s   d dl mZ d dlZd dlZd dlmZ d dlZd dlmZm	Z	m
Z
mZmZ d dlZd dlmZ d dlmZ d dlmZ d dlmZ d dlZG d	d
 d
eZG dd deZdd Zdd ZdddZdd ZG dd deZdd ZdS )    )TrainerN)tqdm)ransac_pose_estimationrandom_sampleget_angle_deviation
to_o3d_pcdto_array)se3)	dcm2euler)prepare_logger)defaultdictc                   @   s    e Zd ZdZdd Zdd ZdS )IndoorTesterz
    3DMatch tester
    c                 C   s   t | | d S Nr   __init__selfargs r   (/workspace/OverlapPredator/lib/tester.pyr      s    zIndoorTester.__init__c                    s  t d tj j d jj dd tt jd j	 jd j
 } jd  } j  t h tt|D ]R}| }| D ]<\}}t|tkr fdd|D ||< q| j||< q |\}}}	|d d	 }
|d
 d	 d	 }|d |d  }}|d }|
d | |
|d   }}|d | ||d   }}t }|
 |d< |  |d< |  |d< |	  |d< ||d< | |d< | |d< t| j d jj d| d qvW 5 Q R X d S )N%Start to evaluate on test datasets.../T)exist_oktestc                    s   g | ]}|  jqS r   todevice.0itemr   r   r   
<listcomp>$   s     z%IndoorTester.test.<locals>.<listcomp>pointsr   stack_lengthsrottranscorrespondencespcdfeatsZoverlapsZsaliencylen_srcz.pth)printosmakedirssnapshot_dirconfig	benchmarkintlenloaderdataset
batch_size__iter__modelevaltorchno_gradr   rangenextitemstypelistr   r   dictcpudetachsave)r   num_iterc_loader_iteridxinputskvr(   scores_overlapscores_saliencyr'   r)   c_rotc_transZcorrespondencesrc_pcdtgt_pcd	src_feats	tgt_featsdatar   r    r   r      s8     
zIndoorTester.testN__name__
__module____qualname____doc__r   r   r   r   r   r   r      s   r   c                   @   s    e Zd ZdZdd Zdd ZdS )KITTITesterz
    KITTI tester
    c                 C   s   t | | d S r   r   r   r   r   r   r   C   s    zKITTITester.__init__c           +         s  t d g }tt jd j jd j } jd  } j  g g  }}t	
 " tt|D ]}| }| D ]<\}}	t|	tkr fdd|	D ||< q||	 j||< q| |\}
}}|  }|  }|d d d }|d |d  }}||   ||   |
d | |
|d   }}|d	 |d
  }}|d | ||d   }}|d | ||d   }}d}|| }|| }|d|krt|d}||    }tjj||d|d}|| ||  }}|d|krPt|d}||    }tjj||d|d}|| ||  }}d}t||||d|dd}|| qfW 5 Q R X t|}|d d d dd df }|d d d ddf } t|}t|d d d d df }d}!d}"tj  j! d||| |d t"||}#tj#j$| | dd}$|#|!k }%|$|"k }&|%|&@  }'|'|j%d  }(d|(dd})|#|% }#|$|& }$t& }*t't(|#d|*d< t't)|#d|*d< t't(|$d|*d< t't)|$d|*d< t't*|#d|*d< t't*|$d|*d < |)t+|*7 })t |)  j,-|)d  d S )!Nr   r   c                    s   g | ]}|  jqS r   r   r   r    r   r   r!   U   s     z$KITTITester.test.<locals>.<listcomp>r#   r   r$   r%   src_pcd_rawtgt_pcd_rawi  Fsizereplacepg333333?   mutualdistance_thresholdransac_n         z/results)rot_estrot_gt	trans_esttrans_gtaxisz
 Registration recall: z.3f
Zrot_meanZ
rot_medianZ
trans_rmseZtrans_rmedseZrot_stdZ	trans_std).r*   r0   r1   r2   r3   r4   r5   r6   r7   r8   r9   r   r:   r;   r<   r=   r>   r   r   rA   r@   appendnumpyr[   nparangesumflattenrandomchoicer   arraysavezr-   r   linalgnormshaper?   roundmeanmedianstdstrloggerwrite)+r   tsfm_estrC   rD   rg   ri   _rF   rG   rH   r(   rI   rJ   r)   rK   rL   rO   rP   rM   rN   src_overlaptgt_overlapsrc_saliencytgt_saliencyn_points
src_scores
tgt_scoresrE   probsra   ts_estrf   rh   Zrot_thresholdZtrans_thresholdZr_deviationZtranslation_errorsZflag_1Zflag_2Zcorrect	precisionmessageerrorsr   r    r   r   F   s     




zKITTITester.testNrR   r   r   r   r   rW   ?   s   rW   c                 C   s  |d t j|d dddt  }t j| | dd}t j|| dd}| |dddddf  }||dddddf  }|dd||  }t j|d	dd
\}	}
}||	dd }| }|dddddf  d9  < ||	dd }t t |ddddf dk||}t t |dks(t	| |dddddf  |dddddf  }t j
||fdd}t d}||dd< | S )a	  Compute rigid transforms between two point sets

    Args:
        a (torch.Tensor): (B, M, 3) points
        b (torch.Tensor): (B, N, 3) points
        weights (torch.Tensor): (B, M)

    Returns:
        Transform T (B, 3, 4) to get from a to b, i.e. T*a = b
    ).N   T)dimkeepdimr   Nrj   F)some
compute_uvre   r   r^   rc   )r8   rr   _EPS	transposesvdclonewheredetallAssertionErrorcateyero   )abweightsZweights_normalizedZ
centroid_aZ
centroid_bZ
a_centeredZ
b_centeredcovusrH   Zrot_mat_posZv_negZrot_mat_negrot_mattranslation	transformtsfmr   r   r   compute_rigid_transform   s$     &2
r   c              	   C   sj  dd }t  N |}| d }| d dddf }| d dddf }| d	 dddf }t|ddddddf  d
d}t|ddddddf  d
d}|dddddf }	|dddddf }
tj|| d dd}tjt|| dd}t j|	|
 d dd}t jt |	|
 dd}tt	||}|ddddf |ddddf  |ddddf  }t 
t jd|d  dddd tj }|dddddf jdd}t||}|}tt|t	||}t j|||ddd }t j|||ddd }t j|ddt j|dd }||t|t|t|t|t|d}W 5 Q R X |S )z/
    Compute metrics required in the paper
    c                 S   sF   t j| d d d d d d d f |d d d d d d d f  d ddS )Nre   rj   r   )r8   rr   )srcdstr   r   r   square_distance   s    z(compute_metrics.<locals>.square_distancetransform_gt
points_src.Nrc   
points_ref
points_rawxyz)seqre   r   rk   r   r         ?            ?minmax     f@rj   )r_mser_maet_mset_maeZ	err_r_degZerr_tchamfer_dist)r8   r9   r
   ro   rp   r|   absr	   concatenateinverseacosclamppiry   r   r   r   )rQ   pred_transformsr   Zgt_transformsr   r   r   Zr_gt_euler_degZr_pred_euler_degZt_gtZt_predr   r   r   r   concatenated	rot_traceZresidual_rotdegZresidual_transmagsrc_transformedZ	ref_cleanZ	src_cleanZdist_srcZdist_refr   metricsr   r   r   compute_metrics   sD    &&6(
r   Metricsc              	   C   s   |  |d  |  dt|d   |dk	rPddd |D }|  d| |  d	|d
 |d |d |d  |  d|d |d  |  d|d |d  |  d|d  dS )z%Prints out formated metrics to logger:=r   Nz | c                 S   s   g | ]}d  |qS )z{:.5f})format)r   cr   r   r   r!     s     z!print_metrics.<locals>.<listcomp>zLosses by iteration: {}zZDeepCP metrics:{:.4f}(rot-rmse) | {:.4f}(rot-mae) | {:.4g}(trans-rmse) | {:.4g}(trans-mae)Zr_rmser   Zt_rmser   z4Rotation error {:.4f}(deg, mean) | {:.4f}(deg, rmse)Zerr_r_deg_meanZerr_r_deg_rmsez-Translation error {:.4g}(mean) | {:.4g}(rmse)Z
err_t_meanZ
err_t_rmsezChamfer error: {:.7f}(mean-sq)r   )infor1   joinr   )r   summary_metricsZlosses_by_iterationtitleZlosses_all_strr   r   r   print_metrics   s,        r   c                 C   s   i }| D ]}| dr<tt| | ||dd d < q|dr~t| | ||d < tt| | d ||d < qt| | ||< q|S )	zBSummaries computed metrices by taking mean over all data instancesZmseNrmseerr_meanre   Z_rmse)endswithrp   sqrtr|   
startswith)r   Z
summarizedrG   r   r   r   summarize_metrics  s    
&
"r   c                   @   s    e Zd ZdZdd Zdd ZdS )ModelnetTesterz
    Modelnet tester
    c                 C   s   t | | d S r   r   r   r   r   r   r   (  s    zModelnetTester.__init__c           &         s  t d tjtjjdd\}}g }g }g }ttj	d j
j	d j }j	d  }j  t  tt|D ]n}| }	|	 D ]J\}
}t|tkrćfdd|D |	|
< qt|tkrq|j|	|
< q|	d d d d d	d	f |	d d d d d
d
f  |	d d d d ddf  }ttjd|d
  dddd tj }|tt | |	\}}}|! " }|! " }|	d d	 d	 }|d | ||d   }}|	d |	d  }}|d | ||d   }}|d | ||d   }}d}|| }|| }|#d	|krtt$|#d	}||%  & ' }tj(j)||d|d}|| ||  }}|#d	|krt$|#d	}||%  & ' }tj(j)||d|d}|| ||  }}d}t*||||d|dd}|| qW 5 Q R X tj+|d	d}|,d-t.|t/| t0t1|2 d d d d d d d f }j	d  }d	t| } }!dd t|j3d
 D t  tt|D ]}| }	d
}"t|j3d
 D ]V || | |"  d d d d f }#t4|	d |#}$|$D ]}
  |
 |$|
  qq| |"7 } qW 5 Q R X ttD ]B  fdd  D  < t5  }%t6||%d- d  q8d S )!Nr   results)Zlog_pathr   c                    s   g | ]}|  jqS r   r   r   r    r   r   r!   ?  s     z'ModelnetTester.test.<locals>.<listcomp>sampler   r   r   re   r   r   r   r   r   r#   rX   rY   i  FrZ   g{Gz?rc   r_   rk   z(Rotation range in data: {}(avg), {}(max)c                 S   s   g | ]}t tqS r   )r   r>   )r   r   r   r   r   r!   v  s     c                    s$   i | ]}|t j  | d dqS )r   rk   )rp   r   )r   rG   )i_itermetrics_for_iterr   r   
<dictcomp>  s    z'ModelnetTester.test.<locals>.<dictcomp>zEvaluation result (iter {}))r   )7r*   r   r.   r+   pathr   r-   r0   r1   r2   r3   r4   r5   r6   r7   r8   r9   r   r:   r;   r<   r=   r>   r?   r   r   r   r   rp   r   rn   r   r   rA   r@   r[   rq   rr   ro   rs   rt   ru   r   r   r   r   r|   r   
from_numpyrv   floatrz   r   r   r   )&r   _loggerZ	_log_pathr   Ztotal_rotationZall_inlier_ratiosrC   rD   rE   rF   rG   rH   r   Zrotdegr(   rI   rJ   r)   rO   rP   rM   rN   r   r   r   r   r   r   r   r   ra   r   Znum_processedZ	num_totalr4   Zcur_pred_transformsr   r   r   )r   r   r   r   r   +  s     
2(,
 
zModelnetTester.testNrR   r   r   r   r   r   $  s   r   c                 C   s>   | j dkrt| S | j dkr$t| S | j dkr6t| S td S )Nindoorkittimodelnet)r3   r   rW   r   NotImplementedError)r.   r   r   r   get_trainer  s    


r   )Nr   ) Zlib.trainerr   r+   r8   r   ro   rp   lib.benchmark_utilsr   r   r   r   r   open3do3dZcommon.math_torchr	   common.math.so3r
   Zcommon.miscr   collectionsr   Zcoloredlogsr   rW   r   r   r   r   r   r   r   r   r   r   <module>   s$   0g&4
h