§
    uíÙjÊ;  ã                  óè   — d Z ddlmZ ddlmZ ddlZddlZddlZddlm	Z	 ddl
Z
ddl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mZmZmZ dZd„ Zd„ Zd„ Zd„ Zd„ Zd„ Zd„ Z d„ Z! G d„ d¦  «        Z"dS )zGMeasured-point geometry for the independent polygon/planar-pose editor.é    )Úannotations)Ú	lru_cacheN)ÚPathé   )Údigest)Úorient_grid)Úread_pcd)ÚDEFAULTSÚdecompose_roiÚ	svd_planeÚ	valid_xyzzpose-review-geometry-v1c                óV   — t          j        | ddd¬¦  «                             ¦   «         S )NT)ú,ú:F)Ú	sort_keysÚ
separatorsÚ	allow_nan)ÚjsonÚdumpsÚencode)Úvalues    úH/root/private/project/pallet_detection_v2/pallet/pose_review_geometry.pyÚ	canonicalr      s'   € ÝŒ:�e t¸
ÈeÐTÑTÔT×[Ò[Ñ]Ô]Ð]ó    c           
     óÀ  ‡‡	‡
‡— t          j        | t          ¬¦  «        }|j        dk    sV|j        dd …         dk    sCdt          |¦  «        cxk    rdk    r)n n&t          j        |¦  «                             ¦   «         st          d¦  «        ‚|dk      	                    ¦   «         sD|d d …df         |k     	                    ¦   «         s"|d d …df         |k     	                    ¦   «         rt          d	¦  «        ‚d
„ }t          t          |¦  «        ¦  «        D �]_}||         ||dz   t          |¦  «        z           cŠŠ	t           j                             ‰‰	z
  ¦  «        dk     rt          d¦  «        ‚t          |dz   t          |¦  «        ¦  «        D ]â}||dz   k    s|dk    r|t          |¦  «        dz
  k    rŒ(||         ||dz   t          |¦  «        z           cŠ
Š |‰‰	‰
¦  «         |‰‰	‰¦  «         |‰
‰‰¦  «         |‰
‰‰	¦  «        f}|d         |d         z  dk    rO|d         |d         z  dk    r:t          ˆˆ	ˆ
ˆfd„t          d¦  «        D ¦   «         ¦  «        rt          d¦  «        ‚Œã�Œat          t          j        |                     t           j        ¦  «        ¦  «        ¦  «        dk     rt          d¦  «        ‚|                     ¦   «         S )N©Údtypeé   r   )r   é   é€   u+   å¤šè¾¹å½¢é¡»æœ‰3â€“128ä¸ªæœ‰é™�å��æ ‡é¡¶ç‚¹r   u   å¤šè¾¹å½¢è¶…å‡ºåŽŸå›¾è¾¹ç•Œc                óR   — t          t          j        || z
  || z
  ¦  «        ¦  «        S ©N)ÚfloatÚnpÚcross)ÚaÚbÚcs      r   r%   zpolygon_value.<locals>.cross    s$   € Ý•R”X˜a ™c 1 Q¡3Ñ'Ô'Ñ(Ô(Ð(r   ç{®Gáz„?u   å¤šè¾¹å½¢ç›¸é‚»é¡¶ç‚¹é‡�å¤�c           
   3  ó2  •K  — | ]‘}t          t          ‰|         ‰|         ¦  «        t          ‰|         ‰|         ¦  «        ¦  «        t          t          ‰|         ‰|         ¦  «        t          ‰|         ‰|         ¦  «        ¦  «        d z   k    V — Œ’dS )ç•Ö&è.>N)ÚmaxÚmin)Ú.0Úkr&   r'   r(   Úds     €€€€r   ú	<genexpr>z polygon_value.<locals>.<genexpr>+   sœ   øè è € ð 9'ð 9'àõ �˜A˜aœD  1¤™œ¥s¨1¨Q¬4°°!´¡~¤~Ñ6Ô6½#½cÀ!ÀAÄ$ÀqÈÄt¹n¼nÍSÐQRÐSTÔQUÐVWÐXYÔVZÉ^Ì^Ñ:\Ô:\Ð]aÑ:aÒað9'ð 9'ð 9'ð 9'ð 9'ð 9'r   u!   å¤šè¾¹å½¢è‡ªäº¤ï¼Œè¯·è°ƒæ•´é¡¶ç‚¹u   å¤šè¾¹å½¢é�¢ç§¯å¤ªå°�)r$   Úasarrayr#   ÚndimÚshapeÚlenÚisfiniteÚallÚ
ValueErrorÚanyÚrangeÚlinalgÚnormÚabsÚcv2ÚcontourAreaÚastypeÚfloat32Útolist)r   ÚwidthÚheightÚpr%   ÚiÚjÚvr&   r'   r(   r0   s           @@@@r   Úpolygon_valuerI      sð  øøøø€ Ý
Œ
�5¥Ð&Ñ&Ô&€AØ„v�‚{€{�a”g˜a˜b˜b”k TÒ)Ð)°µc¸!±f´fÐ1CÐ1CÒ1CÐ1CÀÒ1CÐ1CÐ1CÐ1CÐ1CÍ2Ì;ÐWXÉ>Ì>×K]ÒK]ÑK_ÔK_Ð1CÝÐFÑGÔGÐGØ	ˆAŠ‡{‚{�}„}ð 8˜˜1˜1˜1˜a˜4œ 5š×-Ò-Ñ/Ô/ð 8°A°a°a°a¸°d´G¸fÒ4D×3IÒ3IÑ3KÔ3Kð 8ÝÐ6Ñ7Ô7Ð7ð)ð )ð )å•3�q‘6”6‰]Œ]ð Fñ FˆØ�Œt�Q˜˜!™�s 1™vœv‘~Ô&ˆˆˆ1ÝŒ9�>Š>˜!˜A™#ÑÔ Ò$Ð$ÝÐ:Ñ;Ô;Ð;Ý�q˜‘s�C ™FœFÑ#Ô#ð 	Fð 	FˆAØ�A�a‘CŠxˆx˜A šF˜F q­C°©F¬F°1©H¢} }ØØ�Q”4˜˜A˜a™C¥3 q¡6¤6™>Ô*ˆDˆAˆqØ��q˜˜1‘”˜u˜u Q q¨™|œ|¨U¨U°1°Q°q©\¬\¸5¸5ÀÀ1ÀQ¹<¼<ÐHˆAØ�Œt�A�a”D‰y˜AŠ~ˆ~ ! A¤$ q¨¤t¡)¨q¢. .µSð 9'ð 9'ð 9'ð 9'ð 9'ð 9'ð 9'å" 1™XœXð9'ñ 9'ô 9'ñ 6'ô 6' .õ !Ð!DÑEÔEÐEøñ	Fõ �3Œ?˜1Ÿ8š8¥B¤JÑ/Ô/Ñ0Ô0Ñ1Ô1°AÒ5Ð5ÝÐ0Ñ1Ô1Ð1Ø�8Š8‰:Œ:Ðr   c                ó  — t          j        | t           j        ¬¦  «        }t           j        d|…d|…f         \  }}|dz   |dz   }}t          j        ||ft
          ¦  «        }t          |t          j        |dd¬¦  «        ¦  «        D ]�\  }	}
t          |	d         |
d         z
  ¦  «        dk    rZ||	d         |k    |
d         |k    k    ||
d         |	d         z
  ||	d         z
  z  |
d         |	d         z
  z  |	d         z   k     z  z  }Œ‚|S )	zIPixel-center inclusion, image-edge polygon coordinates, reproducible RLE.r   Nç      à?éÿÿÿÿr   ©Úaxisr   gVçž¯Ò<)	r$   r2   Úfloat64ÚmgridÚzerosÚboolÚzipÚrollr=   )ÚpolygonrC   rD   rE   ÚyyÚxxÚxÚyÚinsider&   r'   s              r   Úraster_maskr[   4   s  € å
Œ
�7¥"¤*Ð-Ñ-Ô-€AÝŒX�g�v�g˜v ˜v�oÔ&�F€BˆØˆb‰5�"�R‘%€q€AÝŒX�v˜u�o¥tÑ,Ô,€FÝ�A•r”w˜q "¨1Ð-Ñ-Ô-Ñ.Ô.ð _ð _‰ˆˆ1Ýˆq�Œt�A�a”D‰y‰>Œ>˜EÒ!Ð!Ø˜˜!œ˜qš a¨¤d¨Q¢hÒ/°A¸¸1¼¸aÀ¼d¹ÀQÀqÈÄtÁVÑ8LÈaÐPQÌdÐSTÐUVÔSWÉiÑ8XÐYZÐ[\ÔY]Ñ8]Ò4]Ñ^Ñ^ˆFøØ€Mr   c           	     ób  — | pi } t          |                      d|d                              d¦  «        pd¦  «        ¦  «        }t          |                      dd¦  «        ¦  «        }|dvs
|dvs||k    rt          d¦  «        ‚|                      d	d
¦  «        |                      dd
¦  «        }}|dvs|dvrt          d¦  «        ‚|d                              d¦  «        }|                      d|�|dz  nd ¦  «        }t          |                      dd¦  «        ¦  «                             ¦   «         d d…         }|�at          |t          ¦  «        s=t          j        t          |¦  «        ¦  «        rdt          |¦  «        cxk    rdk    sn t          d¦  «        ‚|�)|�t          |¦  «        |dz  k    r|st          d¦  «        ‚|                      dd¦  «        }	t          |	¦  «        t          urt          d¦  «        ‚|||||�t          |¦  «        nd |�||dz  k    rdn|�dnd|	|dœS )NÚforward_axisÚsensor_policyÚ
depth_axisr   Úup_axisr   )r   r   r   u   å‰�å�‘è½´å’Œé«˜åº¦è½´é¡»ä¸�å�ŒÚforward_signr   Úup_sign©rL   r   u   æ— æ•ˆè½´æ–¹å�‘Úscale_to_mmÚ
scale_to_méè  ÚevidenceÚ r+   g    €„.Au   å�•ä½�æ¯”ä¾‹æ— æ•ˆu$   æ‰‹å·¥å�•ä½�æ¯”ä¾‹å¿…é¡»å¡«å†™ä¾�æ�®Úaxes_confirmedFu   è½´å�‘ç¡®è®¤å­—æ®µæ— æ•ˆÚsource_registeredÚhuman_suppliedÚunknown_native_units)r]   r`   ra   rb   re   Úunit_originri   rg   )ÚintÚgetr8   ÚstrÚstripÚ
isinstancerR   Úmathr6   r#   Útype)
r   ÚsourceÚforwardÚupÚfsÚusÚ
registeredÚscalerg   Úverifieds
             r   Úsensor_valuer}   @   s`  € ØˆK�R€EÝ�%—)’)˜N¨F°?Ô,C×,GÒ,GÈÑ,UÔ,UÐ,ZÐYZÑ[Ô[Ñ\Ô\€GÝ	ˆU�YŠY�y !Ñ$Ô$Ñ	%Ô	%€BØ�gÐÐ ¨7Ð!2Ð!2°b¸G²m°mÝÐ9Ñ:Ô:Ð:Ø�YŠY�~ qÑ)Ô)¨5¯9ª9°YÀÑ+BÔ+Bˆ€BØ	�ÐÐ˜2 VÐ+Ð+ÝÐ*Ñ+Ô+Ð+Ø˜Ô(×,Ò,¨]Ñ;Ô;€JØ�IŠI�l°zÐ7M J¨t¡O OÐSWÑXÔX€EÝ�5—9’9˜Z¨Ñ,Ô,Ñ-Ô-×3Ò3Ñ5Ô5°e°t°eÔ<€HØÐ�j¨µÑ5Ô5Ð½T¼]Í5ÐQVÉ<Ì<Ñ=XÔ=XÐÐ`dÕhmÐnsÑhtÔhtÐ`{Ð`{Ò`{Ð`{Ðx{Ò`{Ð`{Ð`{Ð`{ÝÐ-Ñ.Ô.Ð.ØÐ˜jÐ0µE¸%±L´LÀJÈtÁOÒ4SÐ4SÐ]eÐ4SÝÐ?Ñ@Ô@Ð@Ø�yŠyÐ)¨5Ñ1Ô1€HÝˆH�~„~�TÐ!Ð!ÝÐ3Ñ4Ô4Ð4Ø#°ÀBÐSUØ&+Ð&7•e˜E‘l”l�l¸TØ.8Ð.DÈÐR\Ð]aÑRaÒIaÐIaÐ*Ð*Ø+0Ð+<Ð'Ð'ÐBXØ"°ð	:ð :ð :r   c                óP  — t          j        d¦  «        |d                  |d         z  }t          j        d¦  «        |d                  |d         z  }t          j        | t          ¬¦  «        |t          j        | |¦  «        z  z
  }t           j                             |¦  «        dk     r|                     ¦   «         }|t           j                             |¦  «        z  }t          j        ||¦  «        dk     r| }t          j        ||¦  «        }t          j	        |||f¦  «        S )	Nr   r`   rb   r]   ra   r   gš™™™™™¹?r   )
r$   Úeyer2   r#   Údotr;   r<   Úcopyr%   Úcolumn_stack)ÚnormalÚsensorÚhÚfr0   Úws         r   Ú	basis_forrˆ   Z   së   € Ý
Œˆq‰	Œ	�&˜Ô#Ô$ V¨IÔ%6Ñ6€AÝ
Œˆq‰	Œ	�&˜Ô(Ô)¨&°Ô*@Ñ@€AÝ
Œ
�6¥Ð'Ñ'Ô'¨­"¬&°¸Ñ*:Ô*:Ñ(:Ñ:€AÝ	„y‡~‚~�aÑÔ˜2ÒÐØ�FŠF‰HŒHˆØ�Œ�Š˜Ñ	Ô	Ñ€AÝ	„vˆa��{„{�Q‚€ØˆBˆÝ
Œ��1‰Œ€AÝŒ?˜A˜a ˜7Ñ#Ô#Ð#r   c                ó6  — t          j        | d         t          ¬¦  «        t          j        | d         t          ¬¦  «        }}|j        dk    sW|j        dk    sLt          j        |¦  «                             ¦   «         r&t          j        |¦  «                             ¦   «         st          d¦  «        ‚t          j        |j        |z  t          j	        d¦  «        d¬	¦  «        r#t           j
                             |¦  «        d
k     rt          d¦  «        ‚||fS )NÚoriginr   Úbasis)r   )r   r   u   è§†å›¾å��æ ‡æ— æ•ˆr   ç�íµ ÷Æ°>©Úatolg+‡ÙÎ÷ï?u'   è§†å›¾å��æ ‡åŸºå¿…é¡»ä¸ºæ­£äº¤å�³æ‰‹ç³»)r$   r2   r#   r4   r6   r7   r8   ÚallcloseÚTr   r;   Údet)r   Úor'   s      r   Úvalidate_viewr“   g   sã   € ÝŒ:�e˜H”o­UÐ3Ñ3Ô3µR´ZÀÀgÄÕV[Ð5\Ñ5\Ô5\€q€AØ„w�$‚€˜!œ' UÒ*Ð*µ"´+¸a±.´.×2DÒ2DÑ2FÔ2FÐ*ÍbÌkÐZ[ÉnÌn×N`ÒN`ÑNbÔNbÐ*ÝÐ-Ñ.Ô.Ð.ÝŒ;�q”s˜1‘u�bœf Q™iœi¨dÐ3Ñ3Ô3ð Dµr´y·}²}ÀQÑ7GÔ7GÈ$Ò7NÐ7NÝÐBÑCÔCÐCØˆaˆ4€Kr   c                óF  — t          j        t          | ¦  «        t          ¦  «        }t	          |t
          ¦  «        rt          |¦  «        dk    rt          d¦  «        ‚|D �]>}t          |¦  «        \  }}dddœ                     |                     d¦  «        ¦  «        }t          j	        |                     d¦  «        t          ¬¦  «        }|�1|j        d	k    s&t          j        |¦  «                             ¦   «         st          d
¦  «        ‚| |z
  |z  }|\  }	}
}}||d d …df         t          |	|¦  «        k    |d d …df         t          |	|¦  «        k    z  |d d …|f         t          |
|¦  «        k    z  |d d …|f         t          |
|¦  «        k    z   z  }�Œ@|S )Né(   u   å‰”é™¤åŒºåŸŸè¿‡å¤šr   r   )ÚwdÚwhÚplaneÚboundsr   )é   u   å‰”é™¤åŒºåŸŸæ— æ•ˆr   )r$   Úonesr5   rR   rr   Úlistr8   r“   ro   r2   r#   r4   r6   r7   r-   r,   )ÚpointsÚ
exclusionsÚkeepÚitemr’   r'   rG   r™   ÚlocalÚx0Úy0Úx1Úy1s                r   Úretainedr¦   p   sœ  € ÝŒ7•3�v‘;”;¥Ñ%Ô%€DÝ�j¥$Ñ'Ô'ð /­3¨z©?¬?¸RÒ+?Ð+?ÝÐ-Ñ.Ô.Ð.Øð 	Kñ 	KˆÝ˜TÑ"Ô"‰ˆˆ1Ø˜AÐÐ×"Ò" 4§8¢8¨GÑ#4Ô#4Ñ5Ô5ˆÝ”˜DŸHšH XÑ.Ô.µeÐ<Ñ<Ô<ˆØˆ9˜œ¨Ò,Ð,µB´KÀÑ4GÔ4G×4KÒ4KÑ4MÔ4MÐ,ÝÐ1Ñ2Ô2Ð2Ø˜‘˜1‘ˆØ‰ˆˆ2ˆb�Ø�5˜˜˜˜1˜”:¥ R¨¡¤Ò+°°a°a°a¸°c´
½cÀ"ÀR¹j¼jÒ0HÑIØ˜˜˜˜1˜”:¥ R¨¡¤Ò+ñ-Ø05°a°a°a¸°c´
½cÀ"ÀR¹j¼jÒ0HñJð Kñ 	Kˆ‰à€Kr   c                ó  ‡ ‡‡— t          ‰ ¦  «        \  }}t          j        ˆ fd„dD ¦   «         t          ¬¦  «        }t          j        |¦  «                             ¦   «         r%|d         dk    st          |d         ¦  «        dk    rt          d¦  «        ‚t          j        d¦  «        ‰d	                  ‰d
         z  }t          j	        |d d …df         |d¬¦  «        st          d¦  «        ‚|\  }}}}	t          j
        |¦  «        }
t          j        |
¦  «         |d d …df         z  t          j        |
¦  «        |d d …df         z  z   }‰                      dd¦  «        }|dvrt          d¦  «        ‚||z  }|||d d …df         z  z   ||d d …df         z  z   Št          j        d¦  «        ‰d                  ‰d         z  }t          j        ||¦  «        }t          t          j        t          j        | |z  ||z  ¦  «        ¦  «        ¦  «        }‰d         }i d‰d	         dk    rt          ‰d         ¦  «        nd “d‰d	         dk    rt          ‰d         ¦  «        nd “dd “d‰d	         dk    r@t          t          j        t          j        |d         |d         ¦  «        ¦  «        ¦  «        nd “d|“dt          ‰|z  ¦  «        t          ‰|z  ¦  «        g“dddg“d|                     ¦   «         |                     ¦   «         |                     ¦   «         dœ“d ˆˆfd!„t%          d¦  «        D ¦   «         “d"|                     ¦   «         “d#|�d$nd%“d&d'“d(d)“d*d “d+d “d,d “d-d “d.d.|�d/nd0|‰d1         d2œ¥S )3Nc                ó    •— g | ]
}‰|         ‘ŒS © r©   )r.   r/   Úrulers     €r   ú
<listcomp>zpose_value.<locals>.<listcomp>ƒ   s   ø€ ÐJÐJÐJ a˜˜qœÐJÐJÐJr   )r‡   r0   ÚangleÚ
half_widthr   r   r   r   i Œ  u$   æ ‡å°ºä½�ç½®ã€�è§’åº¦æˆ–é•¿åº¦æ— æ•ˆr`   rb   rŒ   r�   u'   æ ‡å°ºé«˜åº¦è½´ä¸Žä¼ æ„Ÿå™¨é…�ç½®ä¸�å�Œr   Údirection_signrc   u   å�‰å…¥æ–¹å�‘æ— æ•ˆr]   ra   re   rX   rY   ÚzÚtheta_degreesÚworkplane_theta_degreesÚworkplane_positionÚworkplane_position_orderrv   rC   Úworkplane_axes)rv   rC   rD   Úsource_positionc                óV   •— g | ]%}|‰d          k    rdnt          ‰|         ¦  «        ‘Œ&S )r`   N)r#   )r.   rF   Úpointr„   s     €€r   r«   zpose_value.<locals>.<listcomp>›   s7   ø€ Ð_Ð_Ð_ÐQR 1 f¨YÔ&7Ò#7Ð#7˜4˜4½UÀ5ÈÄ8¹_¼_Ð_Ð_Ð_r   Údirection_sourceÚposition_unitÚmÚnative_unknownÚreference_pointÚfork_face_width_centerÚheight_reference_not_annotatedTrƒ   rT   ÚpitchÚ
quaternionFÚsource_axes_scaledÚsource_nativeri   )Úfull_six_dofÚrobot_command_eligibleÚcoordinate_frameÚsource_scale_to_mri   )r“   r$   r2   r#   r6   r7   r=   r8   r   r�   Údeg2radÚsinÚcosro   r%   ÚdegreesÚarctan2rB   r:   )rª   r„   r’   r'   ÚvaluesÚ
expected_hr‡   r0   r¬   Ú_ÚphiÚ	directionÚsignr†   Úw0Úthetar{   r·   s   ``               @r   Ú
pose_valuerÔ   �   s  øøø€ Ý˜ÑÔ�D€A€qÝŒZÐJÐJÐJÐJÐ+IÐJÑJÔJÕRWÐXÑXÔX€FÝŒ;�vÑÔ×"Ò"Ñ$Ô$ð A¨¨q¬	°Qª¨½#¸fÀQ¼i¹.¼.È5Ò:PÐ:PÝÐ?Ñ@Ô@Ð@Ý”˜‘”˜6 )Ô,Ô-¨f°YÔ.?Ñ?€JÝŒ;�q˜˜˜˜1˜”v˜z°Ð5Ñ5Ô5ð DÝÐBÑCÔCÐCØ�K€A€aˆˆaÝ
Œ*�UÑ
Ô
€CÝ”˜‘”�˜Q˜q˜q˜q ˜sœVÑ#¥B¤F¨3¡K¤K°°!°!°!°A°#´Ñ$6Ñ6€IØ�9Š9Ð% qÑ)Ô)€DØ�6ÐÐÝÐ-Ñ.Ô.Ð.Ø�Ñ€IØˆa��!�!�!�A�#”‰h‰J�q˜˜1˜1˜1˜Q˜3œ‘xÑ€EÝ
Œˆq‰	Œ	�&˜Ô(Ô)¨&°Ô*@Ñ@€AÝ	Œ�!�JÑ	Ô	€BÝ•"”*�RœZ¨¨
°2©°yÀ±{ÑCÔCÑDÔDÑEÔE€EØ�<Ô €EðPˆC F¨9Ô$5¸Ò$:Ð$:•�u˜Q”x‘”�Àð PØ  yÔ 1°QÒ 6Ð 6�U�5˜”8‰_Œ_ˆ_¸DðPØBEÀtðPàÐU[Ð\eÔUfÐjkÒUkÐUk��rœz­"¬*°Y¸q´\À)ÈAÄ,Ñ*OÔ*OÑPÔPÑQÔQÐQÐquðPð 	" 5ðPð +?ÅÀuÈQÁwÁÄÕPUÐV[Ð\^ÑV^ÑP_ÔP_Ð@`ðPð 	# I¨gÐ#6ð	Pð
 	 A§H¢H¡J¤J°r·y²y±{´{ÈJ×L]ÒL]ÑL_ÔL_Ð`Ð`ðPð 	Ð_Ð_Ð_Ð_Ð_ÕV[Ð\]ÑV^ÔV^Ð_Ñ_Ô_ðPð 	˜I×,Ò,Ñ.Ô.ðPð 1@ÈÐHYÀÀÐ_oðPð 	Ð3ðPð 6VÐW[ðPð 	�$ðPð  ðPð '.¨tðPð 6BÀ4ðPð ¸Ø49Ð4EÐ0Ð0È?Ø"°fÐ=MÔ6NðPð Pð Pð Pr   c                  óP   — e Zd Zd„ Z ed¬¦  «        d„ ¦   «         Zd„ Zd„ Zd„ ZdS )	ÚGeometryEnginec                óÔ  — t          |¦  «        t          |¦  «        c| _        | _        | j                             dd¬¦  «         t	          j        d¦  «        | _        t          j        t          t          ¦  «         
                    ¦   «         t          t          ¦  «                             d¦  «         
                    ¦   «         z   ¦  «                             ¦   «         | _        d S )NT)ÚparentsÚexist_okr   zfork_geometry.py)r   ÚdatasetÚcacheÚmkdirÚ	threadingÚBoundedSemaphoreÚ	semaphoreÚhashlibÚsha256Ú__file__Ú
read_bytesÚ	with_nameÚ	hexdigestÚ	code_hash)ÚselfrÚ   rÛ   s      r   Ú__init__zGeometryEngine.__init__¥   s°   € Ý#'¨¡=¤=µ$°u±+´+Ð ˆŒ�d”jØŒ
×Ò °ÐÑ5Ô5Ð5Ý"Ô3°AÑ6Ô6ˆŒÝ œ­­X©¬×(AÒ(AÑ(CÔ(CÅDÍÁNÄN×D\ÒD\Ð]oÑDpÔDp×D{ÒD{ÑD}ÔD}Ñ(}Ñ~Ô~÷  Iò  Iñ  Kô  KˆŒˆˆr   é   )Úmaxsizec                ó   — t          j        |¦  «        }t          |d         d         ¦  «        }t          |¦  «        |d         k    rt	          d¦  «        ‚| j        |d         z  }t          |¦  «        |d         |d                  k    rt	          d¦  «        ‚t          |¦  «        \  }}|d         \  }}t          |¦  «        ||z  k    s|d	         d
         ||gk    rt	          d¦  «        ‚t          |d         d         ¦  «        t          |d         d         ¦  «        f||fd||z  ffvrt	          d¦  «        ‚|d         |d	          	                    dd¦  «        k    rt	          d¦  «        ‚t          t          j        t          |¦  «        t          j        ¬¦  «                             ||¦  «        |d	         ¦  «        }	||	fS )NÚsource_pathsÚpcdÚsource_anchor_hashu'   åŽŸå§‹PCDå“ˆå¸Œä¸�åŒ¹é…�ï¼Œæ‹’ç»�è¯»å�–ÚimageÚfilesu   åŽŸå›¾å“ˆå¸Œä¸�åŒ¹é…�Úsizer^   Úgridu   ç‚¹äº‘ä¸Žå›¾åƒ�ç½‘æ ¼ä¸�åŒ¹é…�ÚHEIGHTr   ÚWIDTHr   u   PCDå¤´ç½‘æ ¼ä¸�åŒ¹é…�Úpixel_rotation_degreesu   å›¾åƒ�æ—‹è½¬é…�ç½®ä¸�åŒ¹é…�r   )r   Úloadsr   r   r8   rÚ   r	   r5   rn   ro   r   r$   ÚarangeÚint32Úreshape)
rç   Úsource_jsonru   rí   rï   Úheaderr�   r‡   r…   Úmappings
             r   rz   zGeometryEngine.registered«   s´  € å”˜KÑ(Ô(ˆÝ�6˜.Ô)¨%Ô0Ñ1Ô1ˆÝ�#‰;Œ;˜&Ð!5Ô6Ò6Ð6ÝÐFÑGÔGÐGØ”˜V Gœ_Ñ,ˆÝ�%‰=Œ=˜F 7œO¨F°7¬OÔ<Ò<Ð<ÝÐ4Ñ5Ô5Ð5Ý! #™œ‰ˆ�Ø�VŒn‰ˆˆ!Ýˆv‰;Œ;˜!˜A™#ÒÐ ¨Ô!8¸Ô!@ÀQÀqÀEÒ!IÐ!IÝÐ=Ñ>Ô>Ð>Ý��xÔ  Ô#Ñ$Ô$¥S¨°¬¸Ô);Ñ%<Ô%<Ð=ÀqÈÀeÈQÈqÐQRÉsÈGÀ_ÐTÐTÝÐ4Ñ5Ô5Ð5ØÐ*Ô+¨v°oÔ/F×/JÒ/JÐKcÐdeÑ/fÔ/fÒfÐfÝÐ:Ñ;Ô;Ð;Ý�bœi­¨F©¬½"¼(ÐCÑCÔC×KÒKÈAÈaÑPÔPÐQWÐXgÔQhÑiÔiˆØ�wˆÐr   c                ó„  ‡2‡3— |d         \  }}t          |||¦  «        }t          ||¦  «        }t          | j        ||d         ||dœ}t	          j        t          |¦  «        ¦  «                             ¦   «         }| j        |dz   z  }	| j        |dz   z  }
| j	        5  |	 
                    ¦   «         re|
 
                    ¦   «         rQ| j        |dz   z   
                    ¦   «         r2t          j        |
                     ¦   «         ¦  «        cd d d ¦  «         S |                      t          |¦  «                             ¦   «         ¦  «        \  }}t!          |||¦  «        }t#          j        |¦  «        \  }}t'          |¦  «        st)          d¦  «        ‚|                     ¦   «         |                     ¦   «         dz   |                     ¦   «         |                     ¦   «         dz   f\  }}}}||||…||…f         d d	…f                              t"          j        ¦  «        }t"          j        ||||…||…f          <   |d
         }t5          t6          dd¬¦  «        }t9          |||�|dz  nd ¬¦  «        }|d         t#          j        ||g¦  «        z   }||d d …df         |d d …df         f         }||d d	…f                              t"          j        ¦  «        |�|ndz  }t#          j        t'          |¦  «        t"          j        ¦  «        }g }t#          j         d	¦  «        |d                  |d         z  Š3t#          j         d	¦  «        |d                  |d         z  Š2tC          |d         ˆ2ˆ3fd„¬¦  «        }tE          |d d…         d¦  «        D �]Ž\  }}|d         } ||| <   t#          j        |d         d d	…         ¦  «        }!tG          |!|¦  «        }"||          }#|#|"z  }$t#          j$        |$ddgd¬¦  «        \  }%}&|"t#          j        |%d         |&d         z   d z  t#          j%        |$d d …df         ¦  «        t#          j%        |$d d …d f         ¦  «        g¦  «        z  }'| &                    |t'          | ¦  «        |! '                    ¦   «         |' '                    ¦   «         |" '                    ¦   «         t-          tQ          |&d         |%d         z
  d z  ¦  «        d!¦  «        tQ          |d"         |�dndz  ¦  «        |d#         |d$         |d%         d&œ
¦  «         �Œ�tG          ‰2|¦  «        }"t'          |¦  «        ret#          j$        ||"z  ddgd¬¦  «        \  }%}&t#          j%        |d¬¦  «        }'t-          tQ          |&d         |%d         z
  d z  ¦  «        d!¦  «        }(nt#          j        d	¦  «        d'}(}'dt'          |¦  «        ‰2 '                    ¦   «         |' '                    ¦   «         |" '                    ¦   «         |(d(d)œ})tS          |d d …d d	…f         ¦  «        }*t#          j*        |*¦  «        }+t'          |+¦  «        d*k    rH|+t"          j+         ,                    d+¦  «         -                    t'          |+¦  «        d*d,¬-¦  «                 }+|+t#          j.        |+|¦  «                  }+t#          j/        t'          |¦  «        ¦  «        },t'          |,¦  «        d.k    rTt#          j0        t"          j+         ,                    d/¦  «         -                    t'          |¦  «        d.d,¬-¦  «        ¦  «        },t#          j1        ||+d d	…f         |�|ndz  t#          j2        t'          |+¦  «        d0¦  «        |+f¦  «        }-t#          j3        |-t#          j1        ||,         ||,         ||,         f¦  «        f¦  «                             d1¦  «        }.|	 4                    d2¦  «        }/|/ 5                    d3¦  «        5 }0t#          j6        |0|                     t"          j        ¦  «        |                     t"          j        ¦  «        ||t#          j7        |¦  «        |¬4¦  «         d d d ¦  «         n# 1 swxY w Y   |/ 8                    |	¦  «         | j        |dz   z   9                    |. :                    ¦   «         ¦  «         ||||||)t'          |¦  «        |d#         t'          |.¦  «        d5d6t          tw          |	¦  «        d7d8œ}1|
 9                    t          |1¦  «        ¦  «         |1cd d d ¦  «         S # 1 swxY w Y   d S )9Nrñ   rî   )ÚversionÚcodeÚkeyrí   rU   r„   ú.npzú.jsonz.binu   åŒºåŸŸä¸�åŒ…å�«åƒ�ç´ ä¸­å¿ƒr   r   re   é´   ré   )Úransac_max_iterationsÚmaximum_planes_per_regionrf   )Úconfigrd   Úuvr   ç      ð?r`   rb   r]   ra   Úfacesc                ó$  •— | d          t          dt          t          j        | d         d d…         ‰¦  «        ¦  «        ¦  «        dz  z  | d         dk    s4t          t          j        | d         d d…         ‰¦  «        ¦  «        dk    rd	nd
z  S )Nr�   r)   r˜   r   r   ÚstatusÚselected_candidateg      è?gš™™™™™©?r  )r,   r=   r$   r€   )r†   Úf_axisÚh_axiss    €€r   ú<lambda>z(GeometryEngine.compute.<locals>.<lambda>Þ   s�   ø€ ¸A¸h¼K¸<Ý�C��BœF 1 W¤:¨b¨q¨b¤>°&Ñ9Ô9Ñ:Ô:Ñ;Ô;¸QÑ>ñ<?à˜(œÐ';Ò;Ð;½sÅ2Ä6È!ÈGÌ*ÐUWÐVWÐUWÌ.ÐY_ÑC`ÔC`Ñ?aÔ?aÐdgÒ?gÐ?g��Ðmoñ<q€ r   )r   é   Úindicesr˜   r)   g®Gáz®ï?rM   r   g:Œ0âŽyE>Úresidual_medianÚregionsr  Úreasons)
Úidr�   rƒ   Úcenterr‹   r­   r  r  r  ÚflagsrK   Úmanual_all_roi)r  r�   rƒ   r  r‹   r­   r  iPF  é*   F)Úreplacei0u  é+   rL   z<f4z.tmpÚwb)Úsource_indicesr  ÚxyzÚface_idsrU   Úmaské   Úlittle_endian_float32T)ÚtokenÚ
target_keyrU   r„   Ú
candidatesÚmanualÚ
roi_pointsr  Úpreview_pointsÚpreview_strideÚpreview_dtypeÚgeometry_versionÚfull_resolution_artifact_sha256Úno_pose_GT_used)<rI   r}   ÚVERSIONræ   rà   rá   r   rå   rÛ   rß   Úexistsr   rö   Ú	read_textrz   Údecoder[   r$   Únonzeror5   r8   r-   r,   r@   rO   ÚnanÚdictr
   r   ÚarrayrQ   rø   r   ÚsortedÚ	enumeraterˆ   ÚquantileÚmedianÚappendrB   r#   r   ÚflatnonzeroÚrandomÚdefault_rngÚchoiceÚisinr÷   Úsortr‚   ÚfullÚvstackÚwith_suffixÚopenÚsavez_compressedr2   r  Úwrite_bytesÚtobytesr   )4rç   r   ru   rU   r„   r‡   r…   Úbindingr#  ÚpathÚ	meta_pathr�   rü   r   rV   rW   r¢   r¤   r£   r¥   rò   r{   r  Úresultr  Úidsr  Úlabelsr%  ÚrankedÚnumberÚfaceÚixrƒ   r‹   rE   r¡   ÚlowÚhighr  Úhalfr&  ÚgoodÚcontext_idsÚsampledÚcontextÚpreviewÚ	temporaryÚstreamÚmetar  r  s4                                                     @@r   ÚcomputezGeometryEngine.compute¿   st	  øø€ Ø�VŒn‰ˆˆ!Ý ¨¨!Ñ,Ô,ˆÝ˜f VÑ,Ô,ˆÝ$¨D¬NÀØÐ-Ô.¸È&ðRð Rˆå”�y¨Ñ1Ô1Ñ2Ô2×<Ò<Ñ>Ô>ˆØŒz˜5 ™<Ñ(ˆØ”J  g¡Ñ.ˆ	ØŒ^ð F	ð F	Ø�{Š{‰}Œ}ð 9 ×!1Ò!1Ñ!3Ô!3ð 9¸¼ÀUÈ6Á\Ñ9R×8ZÒ8ZÑ8\Ô8\ð 9Ý”z )×"5Ò"5Ñ"7Ô"7Ñ8Ô8ðF	ð F	ð F	ð F	ñ F	ô F	ð F	ð F	ð #Ÿošo­i¸Ñ.?Ô.?×.FÒ.FÑ.HÔ.HÑIÔI‰OˆF�GÝ˜w q¨Ñ+Ô+ˆDÝ”J˜tÑ$Ô$‰EˆBˆrÝ�r‘7”7ð @Ý Ð!>Ñ?Ô?Ð?ØŸ&š&™(œ( 2§6¢6¡8¤8¨A¡:¨b¯fªf©h¬h°r·v²v±x´xÀ±zÐA‰KˆBˆr�"�RØ˜' " R %¨¨2¨ +Ô.¨r°¨rÐ1Ô2×9Ò9½"¼*ÑEÔEˆDÝ')¤vˆD�$�r˜"�u˜R ˜U�{Ô#Ð#Ñ$Ø˜<Ô(ˆEÝ�(¸#ÐYZÐ[Ñ[Ô[ˆFÝ" 4°ÐRWÐRcÀEÈ$ÁJÀJÐimÐnÑnÔnˆFØ˜”�bœh¨¨2 wÑ/Ô/Ñ/ˆBØ˜"˜Q˜Q˜Q˜q˜Sœ' " Q Q Q q S¤'˜/Ô*ˆCØ˜˜R˜a˜R˜”.×'Ò'­¬
Ñ3Ô3¸eÐ>O°U°UÐUWÑXˆCÝ”X�c #™hœh¥r¤xÑ0Ô0ˆFØˆJÝ”V˜A‘Y”Y˜v iÔ0Ô1°&¸Ô2CÑCˆFÝ”V˜A‘Y”Y˜v nÔ5Ô6°v¸nÔ7MÑMˆFå˜F 7œOð 2qð 2qð 2qð 2qð 2qð rñ rô rˆFõ !*¨&°°"°¬+°aÑ 8Ô 8ð `ñ `‘�˜Ø˜)”_�°6 f¨R¡jÝœ $ w¤-°°°Ô"3Ñ4Ô4�Ý! &¨Ñ0Ô0�Ø˜”G� Q u¡W˜UÝœ; u¨c°#¨Y¸AÐ>Ñ>Ô>‘��DØ�rœx¨#¨a¬&°°a´©.¸!Ñ);½B¼IÀeÈAÈAÈAÈaÈCÄjÑ<QÔ<QÕRTÔR[Ð\aÐbcÐbcÐbcÐdeÐbeÔ\fÑRgÔRgÐ(hÑiÔiÑi�Ø×!Ò!¨½¸B¹¼ÈÏÊÉÌØ#Ÿ]š]™_œ_°U·\²\±^´^Ý!$¥U¨D°¬G°C¸´F©N¸AÑ+=Ñ%>Ô%>¸tÑ!DÔ!DÝ&+¨DÐ1BÔ,CÈUÐM^ÀTÀTÐdeÑ,fÑ&gÔ&gØ" 9œo°t¸H´~ÈdÐS\Ìoð	#_ð #_ñ `ô `ð `ñ `õ
 ˜f VÑ,Ô,ˆEÝ�3‰xŒxð -Ýœ; s¨5¡y°#°c°ÀÐBÑBÔB‘��DÝœ 3¨AÐ.Ñ.Ô.�Ý�5 $ q¤'¨#¨a¬&¡.°!Ñ!3Ñ4Ô4°TÑ:Ô:��å œh q™kœk¨"�t�Ø¥c¨#¡h¤h¸¿º¹¼ÐQW×Q^ÒQ^ÑQ`ÔQ`Ø#Ÿlšl™nœn¸$ÐHXðZð ZˆFõ ˜V A A A b q b Dœ\Ñ*Ô*ˆDÝœ.¨Ñ.Ô.ˆKÝ�;ÑÔ Ò%Ð%Ø)­"¬)×*?Ò*?ÀÑ*CÔ*C×*JÒ*JÍ3È{ÑK[ÔK[Ð\aÐjoÐ*JÑ*pÔ*pÔq�Ø%¥r¤w¨{¸3Ñ'?Ô'?Ð&?Ô@ˆKÝ”i¥ C¡¤Ñ)Ô)ˆGÝ�7‰|Œ|˜EÒ!Ð!Ýœ'¥"¤)×"7Ò"7¸Ñ";Ô";×"BÒ"BÅ3ÀsÁ8Ä8ÈEÐZ_Ð"BÑ"`Ô"`ÑaÔa�Ý”o v¨k¸"¸1¸"¨nÔ'=ÈÐHY¸u¸uÐ_aÑ'bÝ')¤w­s°;Ñ/?Ô/?ÀÑ'CÔ'CÀKð'Qñ Rô RˆGå”i ­¬¸#¸g¼,ÀvÈgÄÐWZÐ[bÔWcÐ9dÑ)eÔ)eÐ fÑgÔg×nÒnÐotÑuÔuˆGØ×(Ò(¨Ñ0Ô0ˆIØ—’ Ñ%Ô%ð S¨ÝÔ# F¸#¿*º*ÅRÄXÑ:NÔ:NÐRT×R[ÒR[Õ\^Ô\dÑReÔReØ VµB´J¸wÑ4GÔ4GÈTðSñ Sô Sð SðSð Sð Sñ Sô Sð Sð Sð Sð Sð Sð Søøøð Sð Sð Sð Sð ×Ò˜dÑ#Ô#Ð#ØŒZ˜˜v™Ñ&×3Ò3°G·O²OÑ4EÔ4EÑFÔFÐFØ!¨s¸WÈfØ'°ÅSÈÁXÄXØ  Ô+½SÀ¹\¼\Ð[\Ø 7Í7Ý28¸±,´,ÐQUð	Wð WˆDð
 ×!Ò!¥)¨D¡/¤/Ñ2Ô2Ð2ØðMF	ð F	ð F	ð F	ñ F	ô F	ð F	ð F	ð F	ð F	ð F	ð F	øøøð F	ð F	ð F	ð F	ð F	ð F	s@   ÂA-d5Ä[&d5ß<A(a0á$d5á0a4	á4d5á7a4	á8B0d5ä5d9ä<d9c                óˆ  — t          |t          ¦  «        r,t          |¦  «        dk    st          d„ |D ¦   «         ¦  «        rt	          d¦  «        ‚| j        |dz   z  }|                     ¦   «         st	          d¦  «        ‚t          j        | 	                    ¦   «         ¦  «        }|d         |k    rt	          d¦  «        ‚|S )Né@   c              3  ó   K  — | ]}|d vV — Œ	dS )Ú0123456789abcdefNr©   ©r.   r(   s     r   r1   z&GeometryEngine.load.<locals>.<genexpr>  s)   è è € Ð=iÐ=iÐ^_¸aÐGYÐ>YÐ=iÐ=iÐ=iÐ=iÐ=iÐ=ir   u   æ— æ•ˆç‚¹äº‘ç‰ˆæœ¬r  u'   ç‚¹äº‘ç‰ˆæœ¬ä¸�å­˜åœ¨ï¼Œè¯·é‡�æ–°æ��å�–r$  u!   ç‚¹äº‘ç‰ˆæœ¬ä¸�å±žäºŽå½“å‰�ç›®æ ‡)
rr   rp   r5   r9   r8   rÛ   r/  r   rö   r0  )rç   r#  r   rI  r\  s        r   ÚloadzGeometryEngine.load  sº   € Ý˜%¥Ñ$Ô$ð 	3­¨E©
¬
°Bª¨½#Ð=iÐ=iÐchÐ=iÑ=iÔ=iÑ:iÔ:i¨ÝÐ1Ñ2Ô2Ð2ØŒz˜5 ™=Ñ)ˆØ�{Š{‰}Œ}ð 	HÝÐFÑGÔGÐGÝŒz˜$Ÿ.š.Ñ*Ô*Ñ+Ô+ˆØ�Ô Ò$Ð$ÝÐ@ÑAÔAÐAØˆr   c                ó0  ‡‡— d„ |d         D ¦   «         dhz  Št          |t          ¦  «        r|rt          ˆfd„|D ¦   «         ¦  «        rt          d¦  «        ‚t	          j        | j        |d         dz   z  d¬	¦  «        5 Šˆfd
„‰j        D ¦   «         }d d d ¦  «         n# 1 swxY w Y   d|v r-t	          j        t          |d         ¦  «        t          ¦  «        nt	          j        |d         |¦  «        }|t          |d         |¦  «        z  }||d<   |S )Nc                ó   — h | ]
}|d          ’ŒS )r  r©   rb  s     r   ú	<setcomp>z*GeometryEngine.selected.<locals>.<setcomp>  s   € Ð5Ð5Ð5˜Q��4”Ð5Ð5Ð5r   r%  r   c              3  óL   •K  — | ]}t          |¦  «        t          up|‰vV — Œd S r"   )rt   rn   )r.   rF   Úvalids     €r   r1   z*GeometryEngine.selected.<locals>.<genexpr>  s9   øè è € Ð5hÐ5hÐ_`µd¸1±g´gÅSÐ6HÐ6ZÈAÐUZÈNÐ5hÐ5hÐ5hÐ5hÐ5hÐ5hr   u6   è¯·é€‰æ‹©æœ‰æ•ˆå�‰å�–é�¢ï¼Œæˆ–é€‰æ‹©æ‰‹åŠ¨å…¨éƒ¨åŒºåŸŸr#  r  F)Úallow_picklec                ó"   •— i | ]}|‰|         “ŒS r©   r©   )r.   r/   r¯   s     €r   ú
<dictcomp>z+GeometryEngine.selected.<locals>.<dictcomp>   s   ø€ Ð,Ð,Ð,˜q�A�a˜”dÐ,Ð,Ð,r   r  r  Úselected)rr   rœ   r9   r8   r$   rc  rÛ   rð   r›   r5   rR   r?  r¦   )rç   r\  rL  rž   ÚdatarŸ   rh  r¯   s         @@r   rl  zGeometryEngine.selected  sh  øø€ Ø5Ð5 $ |Ô"4Ð5Ñ5Ô5¸¸Ñ;ˆÝ˜#�dÑ#Ô#ð 	W¨3ð 	Wµ#Ð5hÐ5hÐ5hÐ5hÐdgÐ5hÑ5hÔ5hÑ2hÔ2hð 	WÝÐUÑVÔVÐVÝŒW�T”Z  g¤¨vÑ!5Ñ6ÀEÐJÑJÔJð 	-ÈaØ,Ð,Ð,Ð, A¤GÐ,Ñ,Ô,ˆDð	-ð 	-ð 	-ñ 	-ô 	-ð 	-ð 	-ð 	-ð 	-ð 	-ð 	-øøøð 	-ð 	-ð 	-ð 	-à12°c°°�rŒw•s˜4 œ;Ñ'Ô'­Ñ-Ô-Ð-½r¼wÀtÈJÔGWÐX[Ñ?\Ô?\ˆØ•˜˜eœ ZÑ0Ô0Ñ0ˆØˆˆZÑØˆs   ÂB!Â!B%Â(B%N)	Ú__name__Ú
__module__Ú__qualname__rè   r   rz   r]  rc  rl  r©   r   r   rÖ   rÖ   ¤   s€   € € € € € ðKð Kð Kð €Y�qÐÑÔðð ñ Ôðð&Oð Oð Oðb	ð 	ð 	ð	ð 	ð 	ð 	ð 	r   rÖ   )#Ú__doc__Ú
__future__r   Ú	functoolsr   rà   r   rs   Úpathlibr   rÝ   r>   Únumpyr$   Úcluster_targetsr   Údata.orientationr   Údata.pcdr	   Úfork_geometryr
   r   r   r   r.  r   rI   r[   r}   rˆ   r“   r¦   rÔ   rÖ   r©   r   r   ú<module>rz     s—  ðØ MÐ MØ "Ð "Ð "Ð "Ð "Ð "à Ð Ð Ð Ð Ð Ø €€€Ø €€€Ø €€€Ø Ð Ð Ð Ð Ð Ø Ð Ð Ð à 
€
€
€
Ø Ð Ð Ð à #Ð #Ð #Ð #Ð #Ð #Ø )Ð )Ð )Ð )Ð )Ð )Ø Ð Ð Ð Ð Ð Ø HÐ HÐ HÐ HÐ HÐ HÐ HÐ HÐ HÐ HÐ HÐ Hà
#€ð^ð ^ð ^ðð ð ð4	ð 	ð 	ð:ð :ð :ð4
$ð 
$ð 
$ðð ð ðð ð ð" Pð  Pð  PðF@ð @ð @ð @ð @ñ @ô @ð @ð @ð @r   