U ÿî¹`Õ2ã@s¨dZddlZddlZddlZddlZddlZddlZddlm Z ddl m Z dd„Z dd„Zdd „Zdd d „Zddd„Zdd„Zddd„Zdd„Zddd„Zdd„ZdS)zp Script for benchmarking the 3DMatch test dataset. Author: Zan Gojcic, Shengyu Huang Last modified: 30.11.2020 éN)Ú defaultdictcsŠt | dd¡|¡‰tj‡fdd„tˆjdƒDƒdd d¡}tj|dddd }t |¡}t  t j g¡}d ||  |j ¡ |j¡}|S) aÊ Torch batch implementation of the rotation error between the estimated and the ground truth rotatiom matrix. Rotation error is defined as r_e = rccos( rac{Trace(\mathbf{R}_{ij}^{T}\mathbf{R}_{ij}^{\mathrm{GT}) - 1}{2}) Args: R1 (torch tensor): Estimated rotation matrices [b,3,3] R2 (torch tensor): Ground truth rotation matrices [b,3,3] Returns: ae (torch tensor): Rotation error in angular degreees [b,1] ééc s0g|](}t ˆ|dd…dd…f¡dd‘qS)Nrr)ÚtorchÚtrace)Ú.0Ú_©ZR_©ú+/workspace/OverlapPredator/lib/benchmark.pyÚ sz"rotation_error..r©ÚdiméÿÿÿÿN)Úoutg€f@)rÚmatmulÚ transposeÚstackÚrangeÚshapeÚ unsqueezeÚclampÚacosÚTensorÚmathÚpiÚtoÚdeviceÚtypeÚdtype)ÚR1ÚR2ÚeZaerr r r Úrotation_error s , r#cCstj||ddS)aÇ Torch batch implementation of the rotation error between the estimated and the ground truth rotatiom matrix. Rotation error is defined as r_e = rccos( rac{Trace(\mathbf{R}_{ij}^{T}\mathbf{R}_{ij}^{\mathrm{GT}) - 1}{2}) Args: t1 (torch tensor): Estimated translation vectors [b,3,1] t2 (torch tensor): Ground truth translation vectors [b,3,1] Returns: te (torch tensor): translation error in meters [b,1] )rrr )rÚnorm)Út1Út2r r r Útranslation_error's r'cCst|dd…df}|dd…dd…f}t |¡}tj||dd…gdd}| dd¡|| dd¡|d}| ¡S)a Computer the transformation error as an approximation of the RMSE of corresponding points. More informaiton at http://redwood-data.org/indoor/registration.html Args: trans (numpy array): transformation matrices [n,4,4] info (numpy array): covariance matrices of the gt transformation paramaters [n,4,4] Returns: p (float): transformation error Nérr©Úaxisé)rr)ÚnqZmat2quatÚnpÚ concatenateÚreshapeÚitem)ÚtransÚinfoÚtÚrÚqZerÚpr r r ÚcomputeTransformationErr6s  $r7éc Cs"t|ƒ}| ¡}|dd|d…}g}tt|ƒƒD] }| || d¡dd…¡q6g}tt|ƒƒD]8}| ||d ¡||d ¡||d ¡g¡qhg}tt|ƒƒD],}|ddkr²| || d¡d|…¡q²tj|tj d  d ||¡}t |¡}||fW5QR£SQRXdS) a. Function that reads a trajectory saved in the 3DMatch/Redwood format to a numpy array. Format specification can be found at http://redwood-data.org/indoor/fileformat.html Args: filename (str): path to the '.txt' file containing the trajectory data dim (int): dimension of the transformation matrix (4x4 for 3D data) Returns: final_keys (dict): indices of pairs with more than 30% overlap (only this ones are included in the gt file) traj (numpy array): gt pairwise transformation matrices for n pairs[n,dim, dim] rNrú r(ré©rr) ÚopenÚ readlinesrÚlenÚappendÚsplitÚstripr-ÚasarrayÚfloatr/) ÚfilenamerÚfÚlinesÚkeysZ temp_keysÚiZ final_keysÚtrajr r r Úread_trajectoryKs  6  rJr+c CsÒt|ƒ}| ¡}W5QRXt|ƒd}t|ƒd|ks.cSs"g|]}tj|dd dd¡‘qS)r9)Úseprr)r-Ú fromstringr/rMr r r r Œsrr)r;r) r<r=r>ÚAssertionErrorrrAr@r-r.r?rBrCr/) rDrÚfidÚcontentsÚn_pairsZ info_listZn_framerHZ frame_idx0Z frame_idx1Z info_matrixZ cov_matrixr r r Úread_trajectory_infoss   $$ÿ rTcCs|t t|ƒddf¡}t|ƒD]Z\}}|dd|d<t ||kjdd¡d}||dd…dd…f||dd…dd…f<q|S)a Extract only those transformation matrices from the ground truth trajectory that are also in the estimated trajectory. Args: est_pairs (numpy array): indices of point cloud pairs with enough estimated overlap [m, 3] gt_pairs (numpy array): indices of gt overlaping point cloud pairs [n,3] gt_traj (numpy array): 3d array of the gt transformation parameters [n,4,4] Returns: ext_traj (numpy array): gt transformation parameters for the point cloud pairs from est_pairs [m,4,4] r8rrrr)N)r-Úzerosr>Ú enumerateÚwhereÚall)Ú est_pairsÚgt_pairsÚgt_trajZext_trajZest_idxÚpairÚgt_idxr r r Ú extract_corresponding_trajectors“s *r^c sžt|dƒŠ}t|jdƒD]t}||dr||dd…dd…f ¡‰| d tt||ƒ¡d¡| d ‡fdd„t|ƒDƒ¡¡| d¡qW5QRXdS) aÙ Writes the trajectory into a '.txt' file in 3DMatch/Redwood format. Format specification can be found at http://redwood-data.org/indoor/fileformat.html Args: traj (numpy array): trajectory for n pairs[n,dim, dim] metadata (numpy array): file containing metadata about fragment numbers [n,3] filename (str): path where to save the '.txt' file containing trajectory data dim (int): dimension of the transformation matrix (4x4 for 3D data) ÚwrrNr9Ú c3s$|]}d tdjˆ|ƒ¡VqdS)r9z{0:.12f}N)ÚjoinÚmapÚformat)rrH©r6r r Ú »sz#write_trajectory..)r<rrÚtolistÚwriterarbÚstr)rIÚmetadatarDrrEÚidxr rdr Úwrite_trajectory©s  "rkc Csät |¡}t |¡}|d|d}}|d|d}}|jd|krbtj |jd¡d|…} ntj |jd|¡} |jd|kr tj |jd¡d|…} ntj |jd|¡} || || }}|| || }}||||fS)NÚcoordsÚfeatsr)rÚloadrr-ÚrandomÚ permutationÚchoice) Úsrc_pathÚtgt_pathZn_pointsÚsrcÚtgtÚsrc_pcdZ src_embeddingÚtgt_pcdZ tgt_embeddingZ src_permuteZ tgt_permuter r r Ú read_pairs¿s  rxçš™™™™™É?c Cs”|d}tj||ftjd}g}t|jdƒD]<} t|| dfƒ} t|| dfƒ} | | dkr.| || | f<q.t |dk¡} d} d}t|jdƒD]Î} t|| dfƒ} t|| dfƒ} || dd…dd…f}|| | fdkrT|d7}|| | f}ttj ||dd…dd…f¡|||dd…dd…fƒ}||krH| d7} |  d¡n |  d¡q|  d¡q|dkrr|d7}| d|}| d| }|||fS)aa Evaluates the performance of the registration algorithm according to the evaluation protocol defined by the 3DMatch/Redwood datasets. The evaluation protocol can be found at http://redwood-data.org/indoor/registration.html Args: num_fragment (int): path to the '.txt' file containing the trajectory information data result (numpy array): estimated transformation matrices [n,4,4] result_pairs (numpy array): indices of the point cloud for which the transformation matrix was estimated (m,3) gt_pairs (numpy array): indices of the ground truth overlapping point cloud pairs (n,3) gt (numpy array): ground truth transformation matrices [n,4,4] gt_cov (numpy array): covariance matrix of the ground truth transfromation parameters [n,6,6] err2 (float): threshold for the RMSE of the gt correspondences (default: 0.2m) Returns: precision (float): mean registration precision over the scene (not so important because it can be increased see papers) recall (float): mean registration recall over the scene (deciding parameter for the performance of the algorithm) rr;rrNg€„.Agð?) r-rUrLrrÚsumr7ÚlinalgÚinvr?)Z num_fragmentÚresultZ result_pairsrZÚgtZgt_infoÚerr2Zgt_maskÚflagsrjrHÚjZn_gtÚgoodZn_resÚposer]r6Ú precisionÚrecallr r r Úevaluate_registrationÕs:  :       r†c s¾tt ˆ¡ƒ}‡fdd„|Dƒ}ttƒ}ttƒ}ggggf\}}}} g} ddddddd d g} t|›d d ƒ>} |  d ¡t|ƒD]†\} }ttj   |d¡ƒ\}}d}|D],}t t |dƒt |dƒƒ}||dk7}qª|   |¡ttj   |d¡ƒ\}}ttj   ||| d¡ƒ\}}t||||||ƒ\}}}t|||ƒ}tt |dd…dd…dd…f¡t |dd…dd…dd…f¡ƒ ¡ ¡t |¡dk}tt |dd…dd…dd…f¡t |dd…dd…dd…f¡ƒ ¡ ¡t |¡dk}|d  t |¡¡|d  t |¡¡|d  t |¡¡|d  t |¡¡|d  t |¡¡|d  t |¡¡|d  t |¡¡|d  t |¡¡| | d¡ ¡¡| | d¡ ¡¡|  |¡|   |¡|  d  | | ||t |¡t |¡|¡¡t !|›d|| ›d|¡q‚t | ¡t |¡ "¡t "| ¡}|  d  t |¡t #|¡¡¡|  d  |¡¡|  d  t |d¡t #|d¡¡¡|  d   t |d¡t #|d¡¡¡W5QRX|  $¡dS)!Ncsg|]}tj ˆ|¡‘qSr )ÚosÚpathra)rÚele©Ú gt_folderr r r szbenchmark..ZKitchenzHome 1zHome 2zHotel 1zHotel 2zHotel 3ZStudyzMIT Labz/resultr_u1Scene ¦ prec. ¦ rec. ¦ re ¦ te ¦ samples ¦ zgt.logrrzgt.infozest.logr(r8ÚmeanÚmedianÚminÚmaxru6{} ¦ {:.3f} ¦ {:.3f} ¦ {:.3f} ¦ {:.3f} ¦ {:3d}¦ ú/z /flag.npyz"Mean precision: {:.3f}: +- {:.3f} zWeighted precision: {:.3f} z#Mean median RRE: {:.3f}: +- {:.3f} z#Mean median RTE: {:.3F}: +- {:.3f} )%Úsortedr‡ÚlistdirrÚlistr<rgrVrJrˆraÚabsrLr?rTr†r^r#rÚ from_numpyÚcpuÚnumpyr-Úarrayr'rŒrrŽrÚextendr/rfrcÚsaverzÚstdÚclose)Z est_folderr‹ZscenesÚ scene_namesZ re_per_sceneZ te_per_sceneZre_allZte_allr„r…Zn_validsZ short_namesrErjZscenerZr[Zn_validr‰ÚdiffZ n_fragmentsZ gt_traj_covrYÚest_trajZtemp_precisionZ temp_recallZc_flagZ ext_gt_trajÚreÚteZweighted_precisionr rŠr Ú benchmarksT   XX  *"&0r¢)r8)r+)r8)ry)Ú__doc__r—r-r‡ÚsysÚglobrrÚ collectionsrZnibabel.quaternionsZ quaternionsr,r#r'r7rJrTr^rkrxr†r¢r r r r Ús(   (   :