Ë
    Ó~¢jt  ã                   ó^   — d Z ddl­ ddlmZ ddlZd„ Zd„ Zd„ Zd„ Zd	„ Z	e
d
k(  r e	«        yy)zãCompare rigid rotations and separately labelled reflection diagnostics.

No scaling, nonrigid warping, or deletion of unmatched geometry is permitted.
Use Euclidean residuals over the full spatially sampled scan to rank poses.
é    )Ú*)ÚcKDTreeNc                 ó(  — | j                  d«      }t        j                  j                  t        j	                  | j
                  «      «      \  }}|d d …d d d…f   }t        j                  j                  |«      dk  r|d d …dfxx   dz  cc<   ||fS )Nr   éÿÿÿÿ)ÚmeanÚnpÚlinalgÚeighÚcovÚTÚdet)ÚpÚcÚ_Úvs       õG   /Users/bsk/Downloads/á„†á…©á†¯á„�á…¦á†¨á„‰á…³/analysis/register_ac28.pyÚframer   
   sn   € Ø‡f�fˆQƒi€A”R—Y‘Y—^‘^¤B§F¡F¨1¯3©3£KÓ0‘��1°A²a¹¸"¸°f±I°!Ü	‡y�y‡}�}�QÓ˜Ò˜!šA˜b˜D›' 2™+›'ØˆQˆ3€Jó    c                 óF   — | |d d…d d…f   j                   z  |d d…df   z   S )Né   )r   )r   Úts     r   Ú	transformr      s0   € ˜1˜Q˜r ˜r " 1 "˜u™XŸZ™Z™<¨¨"¨1¨"¨Q¨$©Ñ/Ð/r   c                 óÖ  — | j                  d«      }|j                  d«      }t        j                  j                  | |z
  j                  ||z
  z  «      \  }}}|j                  |j                  z  }t        j                  j                  |«      dk  r&|dxx   dz  cc<   |j                  |j                  z  }t        j                  d«      }||d d…d d…f<   |||z  z
  |d d…df<   |S )Nr   r   é   r   )r   r   r	   Úsvdr   r   Úeye)	ÚaÚbÚacÚbcÚur   ÚvtÚrr   s	            r   Úfitr$      sÂ   € Ø‡v�vˆaƒy€B�A—F‘F˜1“I�¤R§Y¡Y§]¡]°A°b±D·8±8¸Q¸r¹T±?Ó%C™f˜a  "Ø‡d�dˆ1�3‰3�h€AÜ	‡y�y‡}�}�QÓ˜Ò˜"˜R›& "™*›& r§t¡t¨A¯C©C¡x QÜ‡f�fˆQƒi€A˜��"�1�"�R�a�R�%‘ 2 a¨¡d¡7˜1˜R˜a˜R ˜T™7Ø€Hr   c           
      ó&  — t        |«      }|j                  «       }dD ]ó  }t        d«      D ]ã  }t        | |«      }|j	                  |d¬«      \  }}	|t        |t        j                  |d«      «      k  }
|
j                  «       dk  r Œkt        ||
   ||	|
      «      }||z  }t        j                  j                  |d d…df   «      dk  sŒ¡t        j                  j                  |d d…d d…f   t        j                  d«      z
  «      d	k  sŒã Œó Œõ |j	                  t        | |«      d¬«      d
   }|t        t        j                  t        j                  |d«      dz  «      dz  «      t        j                  |g d¢«      j!                  «       fS )N)é–   é<   é   é   é   é-   é   )Úworkersg\�Âõ(\ï?é   r   gü©ñÒMb`?g�íµ ÷Æ°>r   r'   ç      à?)r/   çÍÌÌÌÌÌì?çffffffî?)r   ÚcopyÚranger   ÚqueryÚminr   ÚquantileÚsumr$   r	   Únormr   Úfloatr   ÚminimumÚtolist)r   ÚcpÚinitialÚtreer   Ú	thresholdr   ÚqÚdÚidxÚmaskÚdeltas               r   ÚicprE      sV  € Ü	�‹€D˜Ÿ™›�aÛ%ˆ	Ü�r–ˆAÜ˜˜!‹nˆA D§J¡J¨q¸ JÓ$;™e˜a Ø”3�y¤§¡¨Q¨sÓ!3Ó4Ñ4ˆDØ�x‰x‹z˜"Š}™UÜ�a˜‘g˜b  T¡™mÓ,ˆE¨u°Q©w¨QÜ�y‰y�~‰~˜e B Q B q D™kÓ*¨4Ó/´B·I±I·N±NÀ5ÈÈ!ÈÈBÈQÈBÈÁ<ÔPR×PVÑPVÐWXÓPYÑCYÓ4ZÐ[_Ó4_Ñ`eñ ð &ð ‡j�j”˜1˜Q“¨€jÓ*¨1Ñ-€AØŒU”2—7‘7œ2Ÿ:™: a¨Ó+¨QÑ.Ó/°Ñ3Ó4´R·[±[ÀÂ;Ó5O×5VÑ5VÓ5XÐXÐXr   c                  óÊ  ‡#— t         j                  t        dz  «      d   } t         j                  t        dz  «      }t         j                  t	        | «      j                  d«      j                  «      }t         j                  t	        |«      j                  d«      j                  «      }t        |«      \  }}t        |«      \  }}g }t        j                  «       }	t        j                  t        d«      «      D �]n  }
t        j                  dd¬«      D ]ð  }t         j                  d«      d d …|
f   t         j                  |«      z  }||z  |j                  z  }t         j                  d«      }||d d…d d…f<   |||z  z
  |d d…df<   t!        |||«      \  }}}t#        t%        t         j&                  j)                  |d d…d d…f   «      «      «      |||j+                  «       t-        |
|f«      d	œ}|j/                  |«       Œò t1        |d
„ ¬«      d d }t3        dt%        t        j                  «       |	z
  «      d|
d|D �cg c]  }|d   t%        |d   d«      f‘Œ c}d¬«       �Œq |j5                  d„ ¬«       t        dz  j7                  t8        j;                  |d¬«      «       t<        j>                  jA                  t-        tB        dz  «      «      }|jE                  «        t<        jF                  jH                  jK                  «       }|jM                  t<        jF                  jH                  jN                  jQ                  |«      «       i }dD �]�  \  }Š#tS        ˆ#fd„|D «       «      }t         jU                  |d   «      }t        d«      D ]Ç  }tW        | |«      }|jY                  t<        jZ                  j]                  |j_                  d«      «      «      d   ja                  «       }t         j&                  jc                  ||z
  d¬«      }|dk  }te        ||   ||   «      }||z  }t         j&                  jc                  |d d…df   «      d k  sŒÇ n tW        | |«      }|jY                  t<        jZ                  j]                  |j_                  d«      «      «      d   ja                  «       }t         j&                  jc                  ||z
  d¬«      }|j+                  «       ‰#tg        t         ji                  |«      «      tg        t         jk                  |d!«      «      tg        t         jk                  |d"«      «      tg        t         jm                  |d#k  «      «      tg        t         jm                  |d$k  «      «      tg        t         jm                  t         jo                  |d%«      dz  «      d&z  «      d'œ||<   t         jq                  t        d(|› d)�z  |||¬*«       �Œ’ t        d+z  j7                  t8        j;                  |d¬«      «       tr        ju                  ddd,¬-«      \  }}tw        |«      D ]Ð  \  }}t         j                  t        d(|› d)�z  «      d   }ty        ||   g d.¢«      D ]”  \  } \  }!}"| j{                  |d d d…|!f   |d d d…|"f   d/d0d1¬2«       | j{                  |d d …|!f   |d d …|"f   d/d3d4¬2«       | j}                  d5«       | j                  |› d6d7|!   › d8d7|"   › �«       | j�                  d9¬:«       Œ– ŒÒ |jƒ                  «        |j…                  t        d;z  d<¬=«       t3        t8        j;                  |d¬«      d¬«       y c c}w )>Nz!16h_all_merge_pointcloud_full.npzÚpointszcad_sample.npyr*   r   r   )r   é   )Úrepeat)r   ÚscoreÚ	quantilesr   Úseedc                 ó   — | d   S ©NrJ   © ©Úxs    r   Ú<lambda>zmain.<locals>.<lambda>2   s   € ¨A¨gªJr   )Úkeyr,   ÚelapsedÚpermÚbestr   rJ   T)Úflushc                 ó   — | d   S rN   rO   rP   s    r   rR   zmain.<locals>.<lambda>4   s   €   7¢r   zpose_candidates.json)Úindentzviewer/part.stl))ÚrigidrH   )Úreflection_diagnosticr   c              3   ó4   •K  — | ]  }|d    ‰k(  sŒ|–— Œ y­w)r   NrO   )Ú.0rQ   r   s     €r   Ú	<genexpr>zmain.<locals>.<genexpr>:   s   øè ø€ Ð9™Z˜¨1¨U©8°S«=”!™Zùs   ƒ‘r   é2   Úfloat32rH   )Úaxisr)   gü©ñÒMb@?r0   r1   é   é
   r'   r/   )r   r   Ú	median_mmÚp90_mmÚp95_mmÚwithin5Úwithin10Úcapped_rmse60Ú16h_z.npz)rG   Údistancer   zregistration_comparison.json)é   é	   )Úfigsize))r   r,   )r   rH   )rH   r,   g333333Ó?z#546578ÚDesign)Úsr   Úlabelz#e17437ÚScanÚequalz: ÚXYZz / é   )Úmarkerscalezrotation_vs_reflection.pngéŒ   )Údpi)Cr   ÚloadÚOUTÚasarrayÚcloudÚvoxel_down_samplerG   r   ÚtimeÚ	itertoolsÚpermutationsr3   Úproductr   Údiagr   rE   ÚintÚroundr	   r   r;   ÚstrÚappendÚsortedÚprintÚsortÚ
write_textÚjsonÚdumpsÚo3dÚioÚread_triangle_meshÚDATAÚremove_duplicated_verticesr   ÚgeometryÚRaycastingSceneÚadd_trianglesÚTriangleMeshÚfrom_legacyÚnextÚarrayr   Úcompute_closest_pointsÚcoreÚTensorÚastypeÚnumpyr8   r$   r9   Úmedianr6   r   r:   Úsavez_compressedÚpltÚsubplotsÚ	enumerateÚzipÚscatterÚ
set_aspectÚ	set_titleÚlegendÚtight_layoutÚsavefig)$Úp0Úcp0r   r<   ÚcsÚvsÚccÚvcÚ
candidatesÚstartrU   Úsignsr   r#   r   rJ   ÚquantÚrowrV   rQ   ÚmeshÚsceneÚresultsÚkindr   r@   ÚclosestrA   rC   rD   ÚfigÚaxesÚaxÚiÚjr   s$                                      @r   Úmainr¿   $   sî  ø€ Ü	‡w�wŒsÐ6Ñ6Ó7¸ÑA€BÜ
�‰”Ð$Ñ$Ó%€CÜ‡j�j”�r“×,Ñ,¨QÓ/×6Ñ6Ó7€AÜ	‡z�z”%˜“*×.Ñ.¨qÓ1×8Ñ8Ó9€BÜ
�‹(�E€B€rœ˜r›‘5�2�bØ€JœŸ	™	›�%Ü×&Ñ&¤u¨Q£x×0ˆÜ×&Ñ& v°Q×7ˆEÜ�f‰f�Q‹iš˜$˜Ñ¤§¡¨£Ñ.ˆAØ�‰d�2—4‘4‰iˆAœ"Ÿ&™& ›)˜¨Q A b q b¨¨!¨ e¡H°r¸!¸B¹$±w¨q°°!°°A°©wÜ˜a  1›+‰MˆAˆe�EÜœ5¤§¡§¡¨q°°!°°B°Q°B°©xÓ!8Ó9Ó:À5ÐUZÐgh×goÑgoÓgqÔy|ð  Cð  DIð  ~Jó  zKñ  LˆCØ×Ñ˜cÕ"ð 8ô �JÑ#6Ô7¸¸Ð;ˆÜˆiœœdŸi™i›k¨%Ñ/Ó0°¸¸VÑmqÓDrÑmqÐhiÀaÈÁhÌuÐUVÐW^ÑU_Ð`aÓObÒEcÐmqÑDrÐy}×~ð 1ð ‡O�OÑ+€OÔ,ÜÐ	Ñ×+Ñ+¬D¯J©J°zÈ¨JÓ,KÔLÜ	�‰×	"Ñ	"¤3¤tÐ,=Ñ'=Ó#>Ó	?€DÀ×@_Ñ@_Ô@aÜ
�%‰%�.‰.×
(Ñ
(Ó
*€E¨5×+>Ñ+>¼s¿u¹u¿~¹~×?ZÑ?Z×?fÑ?fÐgkÓ?lÔ+mØ€GÜ>‰ˆˆSÜÓ9™ZÓ9Ó9ˆ¼B¿H¹HÀTÈ+ÑEVÓ<W¸!Ü�r–ˆAÜ˜˜1‹oˆA e×&BÑ&BÄ3Ç8Á8Ç?Á?ÐST×S[ÑS[Ð\eÓSfÓCgÓ&hÐiqÑ&r×&xÑ&xÓ&z˜gÜ�i‰i�n‰n˜Q˜w™Y¨AˆnÓ.ˆA°A°b±D¨tÜ�a˜‘g˜g d™mÓ,ˆE¨u°Q©w¨QÜ�y‰y�~‰~˜e B Q B q D™kÓ*¨5Ó0±ð	 ô
 �B�q‹/ˆ %×">Ñ">¼s¿x¹x¿¹ÈqÏxÉxÐXaÓObÓ?cÓ"dÐemÑ"n×"tÑ"tÓ"v˜'Ü
�)‰)�.‰.˜˜7™¨ˆ.Ó
*ˆØ#$§8¡8£:°CÄEÌ"Ï)É)ÐTUË,ÓDWÔafÔgi×grÑgrÐstÐuwÓgxÓayô  DIô  JL÷  JUñ  JUð  VWð  X[ó  J\ó  D]ô  hmô  np÷  nuñ  nuð  vwð  xyñ  vyó  nzó  h{ô  GLô  MO÷  MTñ  MTð  UVð  WYñ  UYó  MZó  G[ô  lqô  rt÷  ryñ  ryô  z|÷  zDñ  zDð  EFð  GIó  zJð  LMñ  zMó  rNð  PRñ  rRó  lSñ  Tˆ�‰Ü
×ÑœC $ t f¨DÐ 1Ñ1¸ÀAÐPQÐÖRð ?ô 	Ð	'Ñ'×3Ñ3´D·J±J¸wÈa°JÓ4PÔQÜ�\‰\˜!˜A fˆ\Ó-�H€CˆÜ˜gÖ&‰ˆˆDÜ
�'‰'”#˜˜T˜F $Ð'Ñ'Ó
(¨Ñ
2ˆÜ˜D ™IÒ&9Ö:‰HˆB‰u��!Ø�J‰J�s™3˜Q˜3˜q˜5‘z #¡c¨ c¨! e¡*¨r°IÀHˆJÔMØ�J‰J�qš˜1˜‘v˜a¢ ! ™f r¨I¸FˆJÔCØ�M‰M˜'Ô" 2§<¡<°4°&¸¸5À¹8¸*ÀCÈÈaÉÀzÐ0RÔ#SÐTV×T]ÑT]ÐjkÐT]ÕTlñ ;ð 'ð ×ÑÔ�s—{‘{¤3Ð'CÑ#CÈ�{ÔLÜ	Œ$�*‰*�W Aˆ*Ó
&¨TÖ2ùò5 Ess   É] Ú__main__)Ú__doc__Ú
ac28_auditÚscipy.spatialr   r   r   r   r$   rE   r¿   Ú__name__rO   r   r   Ú<module>rÅ      sA   ðñô
 Ý !Û òò
 0òò
Yò)3ðV ˆZÒ™�Ðr   