§
    æÖj�  ã                  óÒ   — d Z 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	Z	ddl
mZ ddlmZmZ ddlmZmZmZmZmZmZ dd	lmZmZ  G d
„ d¦  «        Zd„ Zedk    r e¦   «          dS dS )zïPortable box/registered-cloud or detector->cloud fork-face-center inference.

No human polygon, face selection, final ruler or pose target is accepted. Unknown
units require explicit --allow-native and never generate metric_pose_estimate.
é    )ÚannotationsN)ÚPathé   )Údigest)ÚDEFAULTSÚdecompose_roi)ÚSCHEMAÚ	REFERENCEÚdecodedÚfeature_valuesÚload_predictorsÚpredict_residual)Ú	basis_forÚraster_maskc                  ó.   — e Zd Zdd„Zddœd„Zddœd„ZdS )	ÚRefinedPosePredictorÚcpuFc                ód  — t          |¦  «                             ¦   «         | _        || _        || _        t          j        | j        dz                       ¦   «         ¦  «        | _        | j        d         t          k    rt          d¦  «        ‚| j        d                              ¦   «         D ]]\  }}| j        |z                       ¦   «         }|                     | j        ¦  «        rt          |¦  «        |k    rt          d¦  «        ‚Œ^t          | j        | j        d         |¦  «        | _        d S )Nzbundle_config.jsonÚschemazUnsupported refinement bundleÚfileszInvalid bundle file/hashÚprofiles)r   ÚresolveÚrootÚdeviceÚallow_nativeÚjsonÚloadsÚ	read_textÚspecr	   Ú
ValueErrorÚitemsÚis_relative_tor   r   Úmodels)ÚselfÚbundler   r   ÚnameÚshaÚpaths          ú#pallet/pose_refinement_inference.pyÚ__init__zRefinedPosePredictor.__init__   s  € Ý�v‘,”,×&Ò&Ñ(Ô(ˆŒ	°V¨¬Èl¸DÔ<MÝ”*˜dœiÐ(<Ñ<×GÒGÑIÔIÑJÔJˆŒ	ØŒ9�XÔ¥Ò&Ð&­ZÐ8WÑ-XÔ-XÐ'XØœ	 'Ô*×0Ò0Ñ2Ô2ð 	tð 	t‰HˆD�Ø”)˜D‘.×)Ò)Ñ+Ô+ˆDØ×&Ò& t¤yÑ1Ô1ÐsµV¸D±\´\À3Ò5FÐ5FÍZÐXrÑMsÔMsÐGsÐ5FÝ# D¤I¨d¬i¸
Ô.CÀFÑKÔKˆŒˆˆó    N)Ú
scale_to_mc               óD  — |€| j         st          d¦  «        ‚|�)t          j        |¦  «        r|dk    rt          d¦  «        ‚|j        dk    sY|j        d         dk    sH|j        dk    s=|                     ¦   «         dk     s%|                     ¦   «         t          |¦  «        k    rt          d¦  «        ‚|�dnd	}| j	        |         }||d
         d         vrt          d¦  «        ‚t          j
        |t          ¦  «        \  }}	}
}|j        \  }}t          |
|¦  «        dk    s&t          j        |¦  «                             ¦   «         st          d¦  «        ‚t          ||	g||
z   |	g||
z   |	|z   g||	|z   gg||¦  «        }t          j        |¦  «        \  }}t          |¦  «        st          d¦  «        ‚|                     ¦   «         |                     ¦   «         dz   |                     ¦   «         |                     ¦   «         dz   f\  }}}}||||…||…f         d d…f                              t          ¦  «        }t          j        ||||…||…f          <   t#          |t%          t&          dd¬¦  «        |�|dz  nd ¬¦  «        }t)          |d         d„ ¬¦  «        }|st          d¦  «        ‚|d         }t+          |d         d         ¦  «        dk    rt          d¦  «        ‚|d         t          j        ||g¦  «        z   }||d d …df         |d d …df         f         }||                              t          ¦  «        }|d d …d d…fxx         |�|ndz  cc<   t/          |d         d d…         dddddœ¦  «        }||d         d d…f         }||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          | d         |d         z
  dz  ¦  «        d#¦  «        d$œ}"t7          ||||gt%          t9          d%|¦  «        ¦  «        |"| j        d&         | j        d'         ¦  «        \  }#}$}%}&}'t=          ||#d(         |$d(         | j        ¦  «        d         }(t          j          !                    |(d d…         ¦  «        dk    rt          d)¦  «        ‚tE          |(d          |%d          t          j        |&g¦  «        |'d          ¦  «        d         })t          |)d         ¦  «        t          |)d         ¦  «        t          t          j#        t          j$        |)d         |)d         ¦  «        ¦  «        ¦  «        d d d d d tJ          |dk    rd*nd	|d+d+d,œ}*|*|dk    r|*nd ||d
         d-         d+d.d+t          |d         ¦  «        t          |¦  «        d/œ	S )0Nz+Unknown units require explicit allow_nativer   zInvalid scale_to_mé   r   é   zCloud registration invalidÚmetricÚnative_unknownÚprofileÚdomainsz*Sensor domain outside this branch trainingzInvalid original boxzBox contains no pixelsé   é´   é   )Úransac_max_iterationsÚmaximum_planes_per_regioniè  )Úscale_to_mmÚfacesc                óÎ   — | d          t          dt          | d         d         ¦  «        ¦  «        dz  z  | d         dk    st          | d         d         ¦  «        dk    rd	nd
z  S )NÚpointsç{®Gáz„?Úplaner   r.   ÚstatusÚselected_candidateg      è?gš™™™™™©?ç      ð?)ÚmaxÚabs)Úfs    r)   ú<lambda>z+RefinedPosePredictor.pose.<locals>.<lambda>0   si   € °A°h´K°<ÅÀCÍÈAÈgÌJÐWXÌMÑHZÔHZÑ@[Ô@[Ð]^Ñ@^Ñ3^Ø�h”KÐ!5Ò5Ð5½¸Q¸w¼ZÈ¼]Ñ9KÔ9KÈCÒ9OÐ9OˆSˆSÐUWñ4Y€ r+   )ÚkeyzNo automatic plane candidater>   g333333ë?z&Automatic candidate is not a fork faceÚuvrA   )Úup_axisÚup_signÚforward_axisÚforward_signÚindicesr=   g®Gáz®ï?)Úaxisg:Œ0âŽyE>)ÚcenterÚbasisÚ
half_width)ÚxÚyÚwidthÚheightÚseedr<   )NNzResidual outside frozen limitsÚmF)rQ   rR   Útheta_degreesÚzÚnormalÚrollÚpitchÚ
quaternionÚreference_pointÚposition_unitÚsource_scale_to_mÚfull_six_dofÚrobot_command_eligibleÚselected_familyT)	Úpose_estimateÚmetric_pose_estimateÚbranchÚmodel_familyÚhuman_pose_confirmedÚreview_requiredra   Úautomatic_plane_pointsÚautomatic_ROI_points)&r   r    ÚnpÚisfiniteÚndimÚshapeÚminrB   Úlenr#   ÚasarrayÚfloatÚallr   ÚnonzeroÚastypeÚnanr   Údictr   ÚsortedrC   Úarrayr   ÚquantileÚmedianÚtolistr   Úzipr   r   r   ÚlinalgÚnormr   ÚdegreesÚarctan2r
   )+r$   r<   ÚmappingÚboxr,   Údomainre   ÚmodelrQ   rR   ÚwÚhrT   rS   ÚmaskÚvvÚuuÚx0Úx1Úy0Úy1ÚgridÚresultÚrankedÚfacerG   ÚidsÚxyziÚbÚpÚlocalÚlowÚhighrN   Ú	candidaterD   ÚgÚbaseÚspanÚ	directionÚresidualÚvalueÚposes+                                              r)   r¡   zRefinedPosePredictor.pose    s+  € ØÐ dÔ&7Ð½jÐIvÑ>wÔ>wÐ8wØÐ!­2¬;°zÑ+BÔ+BÐ!ÀjÐRSÂmÀmÕ[eÐfzÑ[{Ô[{ÐU{ØŒ;˜Š>ˆ>˜Vœ\¨!œ_¨aÒ/Ð/°7´<À²?°?ÀgÇkÂkÁmÄmÐTUÂoÀoÐY`×YdÒYdÑYfÔYfÕhkÐlrÑhsÔhsÒYsÐYsÝÐ9Ñ:Ô:Ð:Ø%Ð1ˆxˆxÐ7GˆÈdÌkÐZ`ÔNaÈØ˜˜yÔ)¨)Ô4Ð4Ð4½:ÐFrÑ;sÔ;sÐ5sÝ”
˜3�uÑ%Ô%‰ˆˆ!ˆAˆa°7´=¡l f¨UÝˆq�‰8Œ8�QŠ;ˆ;�bœk¨#Ñ.Ô.×2Ò2Ñ4Ô4ˆ;½:ÐF\Ñ;]Ô;]Ð5]Ý˜1˜Q˜%  1¡ Q ¨¨1©¨Q¨q©S¨	°1°Q°q±S°'Ð:¸5ÀÑHÔHˆÝŒj˜ÑÔ‰ˆˆ2Ý�2‰wŒwÐA�ZÐ(@ÑAÔAÐAØ—F’F‘H”H˜RŸVšV™XœX a™Z¨¯ª©¬°·²±´¸!±Ð;‰ˆˆ2ˆb�Ø�G˜B˜r˜E " R %˜KÔ(¨¨!¨Ð+Ô,×3Ò3µEÑ:Ô:ˆÕTVÔTZ¸4ÀÀbÈÀeÈBÈrÈEÀkÔARÐ@RÑ;SÝ˜T¥$¥xÀcÐdeÐ"fÑ"fÔ"fØ+5Ð+A˜
 4™˜ÀtðMñ Mô Mˆå�f˜W”oð +Yð +Yð Zñ Zô ZˆàÐF�JÐ'EÑFÔFÐFØ�AŒYˆÝˆt�GŒ}˜QÔÑ Ô  Ò$Ð$­:Ð6^Ñ+_Ô+_Ð%_Ø�$Œ<�œ " R Ñ)Ô)Ñ)ˆ¨g°b¸¸¸¸1¸´g¸bÀÀÀÀ1À¼g°oÔ.F¨#Ø�CŒ[×Ò¥Ñ&Ô&ˆ t¨A¨A¨A¨b¨q¨b¨D z z¤zÀÐAW°:°:Ð]_Ñ'_ z z¡zÝ
�D˜”M " 1 "Ô%°¸QÈaÐ_`Ð&aÐ&aÑ
bÔ
bˆØ
ˆt�IŒ˜r ˜rÐ!Ô
"ˆ¨¨1© 5µb´kÀ%ÈÈSÈ	ÐWXÐ6YÑ6YÔ6Y©X¨S°Ø•”˜C œF 4¨¤7™N¨AÑ-­b¬i¸¸a¸a¸aÀ¸c¼
Ñ.CÔ.CÅBÄIÈeÐTUÐTUÐTUÐVWÐTWÌjÑDYÔDYÐZÑ[Ô[Ñ[ˆØ"ŸMšM™OœO°A·H²H±J´JÍCÕPUÐW[Ð\]ÔW^Ð_bÐcdÔ_eÑWeÐghÑVhÑPiÔPiÐjnÑLoÔLoÐpÐpˆ	Ý .¨t°B¸¸f°~ÅdÍ3ÐOiÐjmÑKnÔKnÑFoÔFoØ�d”i Ô'¨¬	°(Ô(;ñ!=ô !=Ñˆˆ!ˆD��iå! %¨¨)¬°Q°y´\À$Ä+ÑNÔNÈqÔQˆÝŒ9�>Š>˜( 2 A 2œ,Ñ'Ô'¨Ò)Ð)µ
Ð;[Ñ0\Ô0\Ð*\Ý�h˜t”n T¨$¤Zµ´¸$¸Ñ0@Ô0@ÀÈ4ÄÑQÔQÐRSÔTˆÝ˜˜aœ‘/”/¥e¨E°!¬H¡o¤oÅeÍBÌJÕWYÔWaÐbgÐhiÔbjÐkpÐqrÔksÑWtÔWtÑLuÔLuÑFvÔFvØ˜d¨$°tÈÕ`iØ#)¨8Ò#3Ð#3˜C˜CÐ9IÐ^hØ ¸%ðAð Aˆð !%ÀFÈHÒDTÐDT¸D¸DÐZ^Ø¨5°Ô+;Ð<MÔ+NØ#(¸4ÐY^Ý%(¨¨i¬Ñ%9Ô%9ÕQTÐUXÑQYÔQYð[ð [ð 	[r+   c               ó^  — ddl m} t          j        |d¬¦  «        5 }|d         |d         }}d d d ¦  «         n# 1 swxY w Y   ddlm}	 |	                     |¦  «        5 }
|
j        |j        d         |j        d         fk    rt          d	¦  «        ‚	 d d d ¦  «         n# 1 swxY w Y    |t          | j        d
z  ¦  «        ¦  «        }|                     t          |¦  «        dd| j        d¬¦  «        d         }g }t          |j        j                             ¦   «                              ¦   «         |j        j                             ¦   «                              ¦   «         ¦  «        D ]¸\  }}|\  }}}}t)          |¦  «        t)          |¦  «        t)          ||z
  ¦  «        t)          ||z
  ¦  «        g}|                      |||||¬¦  «        }n*# t          $ r}d t          |¦  «        ddœ}Y d }~nd }~ww xY w|                     |t)          |¦  «        dœ|¥¦  «         Œ¹|t.          ddœS )Nr   )ÚYOLOF)Úallow_pickleÚpoints_xyziÚpixel_to_source_index)ÚImager   zImage/grid dimensions mismatchzdetector.ptg      Ð?i€  )ÚconfÚimgszr   Úverbose©r,   r„   T)rc   Úunavailable_reasonrh   )Úbox_xywhÚdetection_confidence)Ú
detectionsÚpose_referencera   )Úultralyticsr£   rk   ÚloadÚPILr§   ÚopenÚsizern   r    Ústrr   Úpredictr   r}   ÚboxesÚxyxyr   Únumpyr¨   rr   r¡   Úappendr
   )r$   ÚimageÚcloudr,   r„   r£   rX   r<   r‚   r§   ÚoriginalÚdetectorÚ	detectionÚresultsrƒ   Ú
confidencerQ   rR   rŒ   rŽ   Úxywhr    Úerrors                          r)   Údetect_and_posez$RefinedPosePredictor.detect_and_poseI   s�  € Ø$Ð$Ð$Ð$Ð$Ð$ÝŒW�U¨Ð.Ñ.Ô.Ðn°!À1À]ÔCSÐTUÐVmÔTn¸7°FÐnÐnÐnÑnÔnÐnÐnÐnÐnÐnÐnøøøÐnÐnÐnÐnØÐÐÐÐÐØ�ZŠZ˜ÑÔð 	u (ØŒ}˜wœ}¨QÔ/°´¸aÔ0@ÐAÒAÐAÍ
ÐSsÑHtÔHtÐBtÐAð	uð 	uð 	uñ 	uô 	uð 	uð 	uð 	uð 	uð 	uð 	uøøøð 	uð 	uð 	uð 	uà�•c˜$œ) MÑ1Ñ2Ô2Ñ3Ô3ˆØ×"Ò"¥3 u¡:¤:°3¸SÈÌÐ\aÐ"ÑbÔbÐcdÔeˆ	ØˆÝ! )¤/Ô"6×":Ò":Ñ"<Ô"<×"BÒ"BÑ"DÔ"DÀYÄ_ÔEY×E]ÒE]ÑE_ÔE_×EeÒEeÑEgÔEgÑhÔhð 	_ð 	_‰NˆC�
Ø‰IˆAˆa��2¥ a¡¤­¨q©¬µ%¸¸1¹±+´+½eÀBÀqÁD¹k¼kÐJ˜$Ø—i’i  w¨t¸zÐQW�iÑXÔX��øÝÐzÐzÐz¸dÕX[Ð\aÑXbÔXbÐuyÐ-zÐ-z u u u u u uøøøøÐzøøøØ�NŠN tÅ5ÈÑCTÔCTÐ]Ð]ÐW\Ð]Ñ^Ô^Ð^Ð^Ø$µiÐY^Ð_Ð_Ð_s7   �:º>Á>Á 3B Â B$Â'B$Æ7GÇ
G9ÇG4Ç4G9)r   F)Ú__name__Ú
__module__Ú__qualname__r*   r¡   rÅ   © r+   r)   r   r      su   € € € € € ðLð Lð Lð Lð 37ð '[ð '[ð '[ð '[ð '[ðR 7;ð `ð `ð `ð `ð `ð `ð `r+   r   c                 ó4  — t          j        t          ¬¦  «        } |                      dt          d¬¦  «         |                      dt          d¬¦  «         |                      dt          d¬¦  «         |                      dd¬¦  «         |                      d	t
          ¬
¦  «         |                      dd¬¦  «         |                      dd¬¦  «         |                      dt          d¬¦  «         |                      ¦   «         }t          j        d¦  «         t          |j
        |j        |j        ¦  «        }|                     |j        |j        |j        |j        ¬¦  «        }|j                             t)          j        |ddd¬¦  «        dz   ¦  «         d S )N)Údescriptionz--bundleT)ÚtypeÚrequiredz--imagez--cloudz--domain)rÍ   z--scale-to-m)rÌ   z--allow-nativeÚ
store_true)Úactionz--devicer   )Údefaultz--outputr   r«   Fr.   )Úensure_asciiÚindentÚ	allow_nanú
)ÚargparseÚArgumentParserÚ__doc__Úadd_argumentr   rr   Ú
parse_argsÚtorchÚset_num_threadsr   r%   r   r   rÅ   r¼   r½   r,   r„   ÚoutputÚ
write_textr   Údumps)ÚparserÚargsÚ	predictorr�   s       r)   Úmainrâ   Z   s~  € ÝÔ"­wÐ7Ñ7Ô7€FØ
×Ò˜
­°dÐÑ;Ô;Ð;¸F×<OÒ<OÐPYÕ_cÐmqÐ<OÑ<rÔ<rÐ<rØ
×Ò˜	¥t°TÐÑ:Ô:Ð:¸6×;NÒ;NÈzÐcgÐ;NÑ;hÔ;hÐ;hØ
×Ò˜­EÐÑ2Ô2Ð2°6×3FÒ3FÐGWÐ_kÐ3FÑ3lÔ3lÐ3lØ
×Ò˜
¨5ÐÑ1Ô1Ð1°&×2EÒ2EÀjÕVZÐdhÐ2EÑ2iÔ2iÐ2iØ	×	Ò	Ñ	Ô	€D�UÔ2°1Ñ5Ô5Ð5Ý" 4¤;¨t¬{¸4Ô;LÑMÔM€IØ×$Ò$ T¤Z°´
ÀdÄoÐ]aÔ]hÐ$ÑiÔi€FØ„K×Ò�4œ: f¸%ÀqÐSXÐYÑYÔYÐZ^Ñ^Ñ_Ô_Ð_Ð_Ð_r+   Ú__main__)r×   Ú
__future__r   rÕ   r   Úpathlibr   rº   rk   rÚ   Úcluster_targetsr   Úfork_geometryr   r   Úpose_refinementr	   r
   r   r   r   r   Úpose_review_geometryr   r   r   râ   rÆ   rÉ   r+   r)   ú<module>rê      sZ  ððð ð
 #Ð "Ð "Ð "Ð "Ð "à €€€Ø €€€Ø Ð Ð Ð Ð Ð à Ð Ð Ð Ø €€€à #Ð #Ð #Ð #Ð #Ð #Ø 1Ð 1Ð 1Ð 1Ð 1Ð 1Ð 1Ð 1ð&ð &ð &ð &ð &ð &ð &ð &ð &ð &ð &ð &ð &ð &ð &ð &à 7Ð 7Ð 7Ð 7Ð 7Ð 7Ð 7Ð 7ðA`ð A`ð A`ð A`ð A`ñ A`ô A`ð A`ðH	`ð 	`ð 	`ð ˆZÒÐ˜˜™œ˜˜˜ÐÐr+   