§
    MÂÓj«  ã                  ó¼   — d 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mZmZmZmZmZ d„ Zd„ Zd	„ Z G d
„ d¦  «        Zd„ Zedk    r e¦   «          dS dS )a0  Native-image detection -> registered XYZI ROI -> partial-pose prediction.

No ground-truth box, pose, source timestamp, group ID or semantic class label
is accepted as an inference input. Outputs are research estimates, not robot
commands. The caller explicitly chooses the partial task and unit policy.
é    )ÚannotationsN)ÚPathé   )ÚTASKSÚPointNetResidualÚRidgeResidualÚbase_predictionÚroi_featuresc                ó  ‡— t          j        ¦   «         }t          | ¦  «                             d¦  «        5 Št	          ˆfd„d¦  «        D ]}|                     |¦  «         Œ	 d d d ¦  «         n# 1 swxY w Y   |                     ¦   «         S )NÚrbc                 ó.   •— ‰                       d¦  «        S )Ni  @ )Úread)Ústreams   €úpallet/pose_inference.pyú<lambda>zdigest_file.<locals>.<lambda>   s   ø€  §¢¨[Ñ!9Ô!9€ ó    r   )ÚhashlibÚsha256r   ÚopenÚiterÚupdateÚ	hexdigest)ÚpathÚshaÚblockr   s      @r   Údigest_filer      sÎ   ø€ ÝŒÑÔ€CÝ	ˆd‰Œ�Š˜Ñ	Ô	ð Q &ÝÐ9Ð9Ð9Ð9¸#Ñ>Ô>ÐPÐPˆE¸s¿zºzÈ%Ñ?PÔ?PÐ?PÐ?PÐPðQð Qð Qñ Qô Qð Qð Qð Qð Qð Qð Qøøøð Qð Qð Qð Qà�=Š=‰?Œ?Ðs   ·,A0Á0A4Á7A4c                óâ   — | |z                        ¦   «         }t          |¦  «                             ¦   «         s'|                     |                       ¦   «         ¦  «        st	          d¦  «        ‚|S )NzUnsafe model bundle path)Úresolver   Úis_absoluteÚis_relative_toÚ
ValueError)ÚrootÚnamer   s      r   Úbundle_pathr$      sb   € Ø
ˆt‰)×	Ò	Ñ	Ô	€DÝˆD�z„z×ÒÑÔð 5 t×':Ò':¸4¿<º<¹>¼>Ñ'JÔ'Jð 5ÝÐ3Ñ4Ô4Ð4Ø€Kr   c                ó„  — t          j        | t          ¬¦  «        }t          j        |¦  «                             ¦   «         st          d¦  «        ‚t          |d         ¦  «        t          |d         ¦  «        d d d d d d d ddddœ}|t          d         k    rDt          t          j        t          j        |d         |d	         ¦  «        ¦  «        ¦  «        |d
<   ne|t          d         k    rE| 	                    t          |d	         ¦  «        |dd…          
                    ¦   «         ¬¦  «         nt          d¦  «        ‚|S )N)ÚdtypezNonfinite model outputr   r   Ú3source_annotation_reference_not_unified_robot_frameF)ÚxÚyÚzÚtheta_degreesÚnormalÚrollÚpitchÚyawÚ
quaternionÚcoordinate_frameÚfull_six_dofÚrobot_command_eligibleé   é   r+   é   )r*   r,   zUnknown partial task)ÚnpÚasarrayÚfloatÚisfiniteÚallr!   r   Úrad2degÚarctan2r   Útolist)Ú
predictionÚtaskÚpÚdocuments       r   Úpartial_documentrC   "   s  € Ý„j�¥%Ð(Ñ(Ô(€AÝŒ;�q‰>Œ>×ÒÑÔÐJ¥jÐ1IÑ&JÔ&JÐ JÝ˜˜!œ‘+”+¥%¨¨!¬¡+¤+°$ÀtØ˜T¨$°TÀtØPØ°eð=ð =€Hð �U�1ŒX‚~€~µµb´jÅÄÈAÈaÌDÐQRÐSTÔQUÑAVÔAVÑ6WÔ6WÑ0XÔ0X�h˜Ñ/Ð/Ø	�u�QŒxŠˆ˜Ÿš­%°°!´©+¬+¸Q¸qÀ¸s¼V¿]º]¹_¼_˜ÑMÔMÐMÐMÝÐ0Ñ1Ô1Ð	1Ø€Or   c                  óX   — e Zd Zdddœd„Zed         dddœd„Zd	„ Zed         dd
œd„ZdS )ÚPalletPosePredictorÚcpuF©Úallow_unverified_unitsc          	     ó‚  — t          |¦  «                             ¦   «         | _        t          j        | j        dz                       ¦   «         ¦  «        | _        t          j        |¦  «        | _        || _	        | j        d          
                    ¦   «         D ]:\  }}t          t          | j        |¦  «        ¦  «        |k    rt          d¦  «        ‚Œ;ddlm}  |t!          t          | j        | j        d         ¦  «        ¦  «        ¦  «        | _        i | _        | j        d          
                    ¦   «         D ]¨\  }}t          j        t          | j        |¦  «        dd	¬
¦  «        }|d         |k    rt          d¦  «        ‚t)          |¦  «        }	|	                     |d         ¦  «         |	                     | j        ¦  «                             ¦   «         | j        |<   Œ©t1          j        t          j        t          | j        | j        d         ¦  «                             ¦   «         ¦  «        ¦  «        | _        d S )Nzinference_config.jsonÚfileszModel bundle hash mismatchr   )ÚYOLOÚdetector_weightsÚpointnet_weightsrF   T)Úmap_locationÚweights_onlyr@   z Partial task/checkpoint mismatchÚmodelÚridge_weights)r   r   r"   ÚjsonÚloadsÚ	read_textÚspecÚtorchÚdevicerH   Úitemsr   r$   r!   ÚultralyticsrK   ÚstrÚdetectorÚnetworksÚloadr   Úload_state_dictÚtoÚevalr   Úfrom_documentÚridge)
ÚselfÚbundlerW   rH   r#   ÚexpectedrK   r@   ÚsavedÚnets
             r   Ú__init__zPalletPosePredictor.__init__0   sí  € Ý�v‘,”,×&Ò&Ñ(Ô(ˆŒ	Ý”*˜dœiÐ(?Ñ?×JÒJÑLÔLÑMÔMˆŒ	Ý”L Ñ(Ô(ˆŒØ$:ˆÔ#Ø!œY wÔ/×5Ò5Ñ7Ô7ð 	qð 	q‰MˆD�Ý�; t¤y°Ñ6Ô6Ñ7Ô7¸ÒAÐAÍ
ÐSoÑHpÔHpÐBpÐAØ$Ð$Ð$Ð$Ð$Ð$Ø�d�3�{¨4¬9°T´YÐ?QÔ5RÑSÔSÑTÔTÑUÔUˆŒØˆŒØœÐ#5Ô6×<Ò<Ñ>Ô>ð 	;ð 	;‰IˆD�Ý”*�[¨¬°4Ñ8Ô8ÀeÐY]Ð^Ñ^Ô^ˆEØ�VŒ}˜dÒ"Ð"­Ð4VÑ)WÔ)WÐ#WÝ  Ñ&Ô&ˆC s×':Ò':¸5À¼>Ñ'JÔ'JÐ'JØ #§¢ t¤{Ñ 3Ô 3× 8Ò 8Ñ :Ô :ˆDŒM˜$ÑÐÝ Ô.­t¬z½+ÀdÄiÐPTÔPYÐZiÔPjÑ:kÔ:k×:uÒ:uÑ:wÔ:wÑ/xÔ/xÑyÔyˆŒ
ˆ
ˆ
r   r   N)r@   Ú
scale_to_mÚfamilyc          
     ó  — |t           vrt          d¦  «        ‚|€| j        st          d¦  «        ‚t          |||||| j        d         |d¬¦  «        \  }	}
}|p|t           d         k    r| j        d         nd}|d	k    rt          |
d          |¦  «        d         }�n/|d
k    rG|t           d         k    rt          d¦  «        ‚| j                             |
d          ¦  «        d         }nâ|dk    rÍt          j	        ¦   «         5   | j
        |         t          j        |	d          ¦  «                             | j        ¦  «        t          j        |
d          ¦  «                             | j        ¦  «        ¦  «        d                              ¦   «                              ¦   «         }d d d ¦  «         n# 1 swxY w Y   nt          d¦  «        ‚|                     ¦   «         t#          ||¦  «        ||||t           d         k    rdnd ddœS )Nz%Specify a supported partial-pose taskzXPoint units unverified; provide scale_to_m or explicitly allow research-only assumptionsÚpose_featuresé*   )ri   Úseedr   Úselected_planar_familyÚpointnetÚgeometryrb   zNo trained normal ridge modelzUnknown model familyr   F)r?   ÚposeÚmethodr@   Úinput_qualityÚnormal_generalization_evaluatedÚ!global_robot_calibration_verified)r   r!   rH   r
   rU   r	   rb   ÚpredictrV   Úinference_moder\   Ú
from_numpyr_   rW   rF   Únumpyr>   rC   )rc   ÚpointsÚmappingÚboxÚwidthÚheightr@   ri   rj   ÚfÚgÚmetadatars   Úpreds                 r   rr   zPalletPosePredictor.poseA   sM  € Ø•uÐÐ¥:Ð.UÑ#VÔ#VÐVØÐ dÔ&AÐÝÐwÑxÔxÐxÝ! &¨°°U¸6À$Ä)ÈOÔB\Ø! rð+ñ +ô +‰ˆˆ!ˆHàÐ`ÀÅuÈQÄxÂÀ˜$œ)Ð$<Ô=Ð=ÐU_ˆØ�:ÒÐ¥?°1°T´7¸4Ñ#@Ô#@ÀÔ#C˜d™dØ�WŠ_ˆ_Ø•U˜1”XŠ~ˆ~¥JÐ/NÑ$OÔ$OÐOØ”×#Ò# A d¤GÑ,Ô,¨QÔ/ˆDˆDØ�ZÒÐÝÔ%Ñ'Ô'ð Pð PØ(�T”] 4Ô(­Ô)9¸!¸D¼'Ñ)BÔ)B×)EÒ)EÀdÄkÑ)RÔ)RÝÔ$ Q t¤WÑ-Ô-×0Ò0°´Ñ=Ô=ñ?ô ?Ø?@ôBßBEÂ#Á%Ä%ÏÊÉÌð ðPð Pð Pñ Pô Pð Pð Pð Pð Pð Pð Pøøøð Pð Pð Pð Pøõ Ð4Ñ5Ô5Ð5Ø!Ÿ[š[™]œ]Õ2BÀ4ÈÑ2MÔ2MÐW]Ø¨Ð\`ÕbgÐhiÔbjÒ\jÐ\jÐSXÐSXÐptØ05ð7ð 7ð 	7s   ÄB"F1Æ1F5Æ8F5c           
     ó`  — |j         dvrt          d¦  «        ‚|j         dk    r$t          j        |d d …d d …d f         dd¬¦  «        }| j                             || j        d         | j        d         | j        d         | j        d	         t          | j        ¦  «        d
¬¦  «        d         }|j	        €g S |j	        j
                             ¦   «                              ¦   «         }|j	        j                             ¦   «                              ¦   «         }d„ t          ||¦  «        D ¦   «         S )N)r5   r4   zExpected native image gridr5   r4   )ÚaxisÚdetector_imgszÚdetector_confÚdetector_nms_iouÚdetector_max_detF)ÚimgszÚconfÚiouÚmax_detrW   Úverboser   c           
     óü   — g | ]y\  }}t          |d          ¦  «        t          |d         ¦  «        t          |d         |d          z
  ¦  «        t          |d         |d         z
  ¦  «        gt          |¦  «        dœ‘ŒzS )r   r   r5   r4   )Úbox_xywhÚ
confidence)r9   )Ú.0ÚbÚscores      r   ú
<listcomp>z.PalletPosePredictor.detect.<locals>.<listcomp>]   s�   € ð Ið Ið IÙ+2¨1¨Uõ # 1 Q¤4™[œ[­¨q°¬t©¬µU¸1¸Q¼4ÀÀ!Ä¹9Ñ5EÔ5EÅeÈAÈaÌDÐQRÐSTÔQUÉIÑFVÔFVÐWÝ˜u™œð'ð 'ð Ið Ið Ir   )Úndimr!   r7   Úrepeatr[   rw   rU   rZ   rW   ÚboxesÚxyxyrF   rz   r‹   Úzip)rc   ÚimageÚresultr˜   Úscoress        r   ÚdetectzPalletPosePredictor.detectU   s2  € ØŒ:˜UÐ"Ð"­Ð4PÑ)QÔ)QÐ#QØŒ:�qŠ=ˆ=�rœy¨¨q¨q¨q°°°°4¨x¬¸ÀÐBÑBÔB˜ØŒ}×$Ò$ U°´Ð;KÔ1LØ”˜?Ô+°´	Ð:LÔ0MØ”IÐ0Ô1½¸T¼[Ñ9IÔ9IÐRWð %ñ Yô YàYZô\ˆð Œ<Ð r 	ØŒlÔ×#Ò#Ñ%Ô%×+Ò+Ñ-Ô-ˆ°V´\Ô5F×5JÒ5JÑ5LÔ5L×5RÒ5RÑ5TÔ5T¨fðIð IÝ69¸%ÀÑ6GÔ6GðIñ Iô Ið 	Ir   ©r@   ri   c          
     óî  — t          |j        ¦  «        t          |j        d d…         ¦  «        k    rt          d¦  «        ‚|j        d d…         \  }}g }|                      |¦  «        D ]ƒ}		 |                      |||	d         ||||¬¦  «        }
|                     i |	¥|
¥dd i¥¦  «         ŒC# t          $ r4}|                     i |	¥d d t          |¦  «        dœ¥¦  «         Y d }~Œ|d }~ww xY w||ddddœS )	Nr5   z!Image/cloud registration mismatchr�   rŸ   Úerror)rr   r?   r¡   F)Ú
detectionsr@   r2   r3   Úground_truth_used_for_inference)ÚtupleÚshaper!   rž   rr   ÚappendrZ   )rc   r›   r{   r|   r@   ri   r   r~   Úpredictionsr}   rœ   r¡   s               r   rw   zPalletPosePredictor.predict`   sF  € Ý�”ÑÔ¥ u¤{°2°A°2¤Ñ!7Ô!7Ò7Ð7½jÐIlÑ>mÔ>mÐ8mØ”[  ! ”_‰ˆˆuØˆØ—;’;˜uÑ%Ô%ð 	]ð 	]ˆCð]Ø—y’y ¨°°J´ÀÀfÐRVÐbl�yÑmÔm�Ø×"Ò"Ð#@ cÐ#@¨FÐ#@°7¸4Ð#@Ð#@ÑAÔAÐAÐAøÝð ]ð ]ð ]Ø×"Ò"Ð#[ cÐ#[°À4ÕPSÐTYÑPZÔPZÐ#[Ð#[Ð#[Ñ\Ô\Ð\Ð\Ð\Ð\Ð\Ð\øøøøð]øøøà(°ÀEØ%*ÈUðTð Tð 	Ts   Á.?B.Â.
C,Â8*C'Ã'C,)rF   )Ú__name__Ú
__module__Ú__qualname__rh   r   rr   rž   rw   © r   r   rE   rE   /   sž   € € € € € ðzÀ5ð zð zð zð zð zð" :?¸q¼ÈTÐY]ð 7ð 7ð 7ð 7ð 7ð(	Ið 	Ið 	Ið 27°q´ÀTð Tð Tð Tð Tð Tð Tð Tr   rE   c                 ó  — t          j        t          ¬¦  «        } |                      dt          d¬¦  «         |                      dt          d¬¦  «         |                      dt          dd¬¦  «         |                      d	t          d¬¦  «         |                      d
t
          t
          d         ¬¦  «         |                      dd¬¦  «         |                      dt          ¬¦  «         |                      dd¬¦  «         |                      ¦   «         }dd l}| 	                    t          |j        ¦  «        |j        ¦  «        }|€t          d¦  «        ‚t          j        |j        d¬¦  «        5 }|d         |d         }}d d d ¦  «         n# 1 swxY w Y   t#          |j        |j        |j        ¬¦  «        }|                     ||||j        |j        ¬¦  «        }|j                             ¦   «         rt          d¦  «        ‚|j        j                             dd¬¦  «         |j                             t;          j        |ddd¬¦  «        d z   ¦  «         t?          t;          j        |d¬!¦  «        ¦  «         d S )"N)Údescriptionz--bundleT)ÚtypeÚrequiredz--imagez--cloudz.NPZ with points_xyzi and pixel_to_source_index)r®   r¯   Úhelpz--outputz--taskr   )ÚchoicesÚdefaultz--devicerF   )r²   z--scale-to-m)r®   z--allow-unverified-unitsÚ
store_true)ÚactionzUnable to read input imageF)Úallow_pickleÚpoints_xyziÚpixel_to_source_indexrG   rŸ   z2Refusing to overwrite an existing inference output)ÚparentsÚexist_okr5   )Úensure_asciiÚindentÚ	allow_nanú
)rº   ) ÚargparseÚArgumentParserÚ__doc__Úadd_argumentr   r   r9   Ú
parse_argsÚcv2ÚimreadrZ   r›   ÚIMREAD_GRAYSCALEr!   r7   r]   ÚcloudrE   rd   rW   rH   rw   r@   ri   ÚoutputÚexistsÚparentÚmkdirÚ
write_textrR   ÚdumpsÚprint)	ÚparserÚargsrÃ   r›   r*   r{   r|   Ú	predictorrœ   s	            r   ÚmainrÑ   n   sh  € ÝÔ"­wÐ7Ñ7Ô7€FØ
×Ò˜
­°dÐÑ;Ô;Ð;Ø
×Ò˜	¥t°TÐÑ:Ô:Ð:Ø
×Ò˜	¥t°TÐ?oÐÑpÔpÐpØ
×Ò˜
­°dÐÑ;Ô;Ð;Ø
×Ò˜­µu¸Q´xÐÑ@Ô@Ð@Ø
×Ò˜
¨5ÐÑ1Ô1Ð1Ø
×Ò˜­EÐÑ2Ô2Ð2Ø
×ÒÐ2¸,ÐÑGÔGÐGØ	×	Ò	Ñ	Ô	€DØ€J€J€JØ
�*Š*•S˜œ‘_”_ SÔ%9Ñ
:Ô
:€EØ€}�:Ð&BÑCÔCÐCÝ	Œ�”¨Ð	/Ñ	/Ô	/Ðo°1ÀAÀmÔDTÐUVÐWnÔUo¸G°VÐoÐoÐoÑoÔoÐoÐoÐoÐoÐoÐoøøøÐoÐoÐoÐoÝ! $¤+¨d¬kÐQUÔQlÐmÑmÔm€IØ×Ò˜U 6¨'°t´yÈDÌOÐÑ\Ô\€FØ„{×ÒÑÔÐb¥*Ð-aÑ"bÔ"bÐbØ„KÔ×Ò T°4ÐÑ8Ô8Ð8Ø„K×Ò�4œ: f¸%ÀqÐSXÐYÑYÔYÐZ^Ñ^Ñ_Ô_Ð_Ý	�$Œ*�V¨Ð
/Ñ
/Ô
/Ñ0Ô0Ð0Ð0Ð0s   Å3FÆFÆFÚ__main__)rÀ   Ú
__future__r   r¾   r   rR   Úpathlibr   rz   r7   rV   Ú
pose_modelr   r   r   r	   r
   r   r$   rC   rE   rÑ   r¨   r«   r   r   ú<module>rÖ      s  ððð ð #Ð "Ð "Ð "Ð "Ð "à €€€Ø €€€Ø €€€Ø Ð Ð Ð Ð Ð à Ð Ð Ð Ø €€€à ]Ð ]Ð ]Ð ]Ð ]Ð ]Ð ]Ð ]Ð ]Ð ]Ð ]Ð ]Ð ]Ð ]ðð ð ðð ð ð
ð 
ð 
ð<Tð <Tð <Tð <Tð <Tñ <Tô <Tð <Tð~1ð 1ð 1ð. ˆZÒÐ˜˜™œ˜˜˜ÐÐr   