§
    ?'×j%  ã                   óØ   — d 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 ddlmZ ddlmZmZmZ ddlmZmZ dd	lmZmZmZ d
„ Z G d„ d¦  «        Zd„ Zedk    r e¦   «          dS dS )zIInference for the best learned v3 benchmark models, without label access.é    N)ÚPathé   )Úpose_benchmark_models)ÚEndToEnd)Úfeature_valuesÚdecodedÚ	REFERENCE)Úraster_maskÚ	basis_for)Údecompose_roiÚDEFAULTSÚ	valid_xyzc                 ó¬  — t          j        | ¦  «        } t          j        |¦  «        }| j        dk    s@| j        d         dk    s/|j        dk    s$t          j        |j        t           j        ¦  «        st          d¦  «        ‚|                     ¦   «         dk     s%| 	                    ¦   «         t          | ¦  «        k    rt          d¦  «        ‚|�)t          j        |¦  «        r|dk    rt          d¦  «        ‚|j        \  }}t          j        |t          ¦  «        \  }}}	}
t          j        |¦  «                             ¦   «         rt          |	|
¦  «        dk    rt          d¦  «        ‚t          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„ ¬¦  «        }|�|nd}|||f         }t/          | |d d…f         ¦  «        }||         }t          j        ||f¦  «        |         }| |                              t          ¦  «        }|d d …d d…fxx         |z  cc<   t          |¦  «        dk     rt          d¦  «        ‚| p$t3          |d         d         d         ¦  «        dk    }|rð|dk    rt          d¦  «        ‚t          j        |d d …d d…f         d¬¦  «        }t          t          t          j        |d d …df         d¦  «        t          j        |d d …df         d¦  «        z
  ¦  «        d¦  «        }t          j        ddg¦  «        }t          j        g d ¢g d!¢g d"¢f¦  «        }t          ||dz  |¬#¦  «        } t;          ||||g|| d$d%¦  «        S |d         }!|d&         t          j        ||g¦  «        z   }"||"d d …df         |"d d …df         f         }| |                              t          ¦  «        }#|#d d …d d…fxx         |z  cc<   t=          |!d         d d…         t          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          t          |&d         |%d         z
  dz  ¦  «        d¦  «        ¬)¦  «        } t;          |#|"||g|| d$d%¦  «        \  }'}(})}}|dk    rt;          ||||g|| d$d%¦  «        \  }'}*}*}*}*|'|(|)||fS )*Né   r   é   z@Expected points_xyzi[N,4] and integer pixel_to_source_index[H,W]r   zInvalid point registrationzInvalid scalezInvalid box)ÚxÚyÚwidthÚheightz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    ú"pallet/pose_benchmark_inference.pyú<lambda>z!automatic_input.<locals>.<lambda>#   sj   € °°(´¨|½CÀÅCÈÈ'Ì
ÐSTÌÑDVÔDVÑ<WÔ<WÐYZÑ<ZÑ/ZØ˜Hœ+Ð';Ò;Ð;½sÀ1ÀWÄ:ÈaÄ=Ñ?QÔ?QÐRUÒ?UÐ?U�3�3Ð[]ñ0_€ ó    )Úkeyr#   é   z'Fewer than 16 valid original-box pointsr    g333333ë?ÚroizNo valid automatic fork plane)Úaxisg®Gáz®ï?r   g:Œ0âŽyE>g        )r   r   r   )r   r   r   )r   r   r   )ÚcenterÚ
half_widthÚbasisé*   i   Úuv)Úup_axisÚup_signÚforward_axisÚforward_signÚindices)r.   r0   r/   )ÚnpÚasarrayÚndimÚshapeÚ
issubdtypeÚdtypeÚintegerÚ
ValueErrorÚminr$   ÚlenÚisfiniteÚfloatÚallÚdictÚzipr
   ÚnonzeroÚastypeÚnanr   r   Úsortedr   Úcolumn_stackr%   ÚmedianÚquantileÚarrayr   r   )+r   ÚmappingÚboxÚ
scale_to_mÚrouter   r   r   r   ÚwÚhÚbÚmaskÚvvÚuuÚx0Úx1Úy0Úy1ÚgridÚresultÚrankedÚfactorÚidsÚgoodÚfull_uvÚfullÚfallbackr.   ÚspanÚ	directionr0   Ú	candidateÚfacer2   ÚxyziÚlocalÚlowÚhighr&   ÚgÚbaseÚ_s+                                              r'   Úautomatic_inputrq      sQ  € ÝŒ:�fÑÔ€F¥b¤j°Ñ&9Ô&9˜gØ„{�A‚~€~˜œ aœ¨!Ò+Ð+¨w¬|¸Qª¨ÅbÄmÐT[ÔTaÕbdÔblÑFmÔFm¨ÝÐ[Ñ\Ô\Ð\Ø‡{‚{�}„}�Q‚€˜'Ÿ+š+™-œ-­¨V©¬Ò4Ð4½:ÐFbÑ;cÔ;cÐ5cØÐ¥r¤{°:Ñ'>Ô'>ÐÀ*ÈaÂ-À-ÕWaÐbqÑWrÔWrÐQrØ”�L€Fˆ5ÝŒJ�s�5Ñ!Ô!�G€A€aˆˆ!ÝŒ;�sÑÔ×ÒÑ!Ô!ÐP¥S¨¨1¡X¤X¨q¢[ [µzÀ-Ñ7PÔ7PÐ1PÝ
�3Ð)¨#Ñ.Ô.Ñ/Ô/€AÝ	�q˜�e˜Q˜q™S ˜G Q q¡S¨¨1© I¨q°°1±¨gÐ6°u¸VÑ	DÔ	D€DÝ
Œ*�TÑ
Ô
�E€B€rÝˆr‰7Œ7Ð=�Ð$<Ñ=Ô=Ð=Ø—’‘”˜Ÿš™œ !™ B§F¢F¡H¤H¨R¯VªV©X¬X°a©ZÐ7�K€B€rˆ"ˆRØ	�˜˜2˜˜b ˜e˜Ô$ R a RÐ'Ô	(×	/Ò	/µÑ	6Ô	6€DÝœV€Dˆ$ˆr�"ˆu�R˜�Uˆ{Ô
Ð	ÑÝ˜�d¥8À#Ð`aÐbÑbÔbØ8BÐ8N Z°¡_ _ÐTXðZñ Zô Z€Få�&˜”/ð '_ð '_ð `ñ `ô `€Fà#Ð/ˆ:ˆ:°R€FØ��2�Œ€C�I f¨S°°!°¨V¤nÑ5Ô5�t¸#¸d¼)°cÝŒO˜R ˜GÑ$Ô$ TÔ*€GØ	�Œ×	Ò	�EÑ	"Ô	"€D 4¨¨¨¨"¨1¨"¨ : :¤:¨vÑ#5 : :¡:Ý
ˆ4�y„y�‚|€|�*Ð%NÑOÔOÐOØˆZÐ9�3˜v aœy¨Ô1°!Ô4Ñ5Ô5°cÒ9€HØð NØ�%Š<ˆ<�jÐ)HÑIÔIÐIÝŒy˜˜a˜a˜a   ˜dœ¨Ð+Ñ+Ô+ˆÝ••r”{ 4¨¨¨¨!¨¤9¨SÑ1Ô1µ"´+¸dÀ1À1À1ÀQÀ3¼iÈÑ2LÔ2LÑLÑMÔMÈdÑSÔSˆÝ”(˜B˜r˜7Ñ#Ô#ˆ	­"¬/¸7¸7¸7À7À7À7È7È7È7Ð:SÑ*TÔ*T EÝ˜f°°Q±¸UÐCÑCÔCˆ	Ý˜d 7¨E°&¨>¸!¸IÀbÈÑMÔMÐMØ	�Œ€D�f˜T”l¥2¤8¨R°¨GÑ#4Ô#4Ñ4�2¸ÀÀAÀAÀAÀaÀCÄÈÈAÈAÈAÈaÈCÌÀÔ9Q°SØ	�Œ×	Ò	�EÑ	"Ô	"€D 4¨¨¨¨"¨1¨"¨ : :¤:¨vÑ#5 : :¡:Ý
�D˜”M " 1 "Ô%¥d°1¸QÈAÐ[\Ð&]Ñ&]Ô&]Ñ
^Ô
^€EØ
ˆt�IŒ˜r ˜rÐ!Ô
" 5Ñ
(€Eµ"´+¸eÀSÈÀIÐSTÐ2UÑ2UÔ2U©¨¨TØ•”˜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Ñ[Ô[Ñ[€FÝ˜& u½½EÀ4ÈÄ7È3ÈqÌ6Á>ÐSTÑBTÑ<UÔ<UÐVZÑ8[Ô8[Ð\Ñ\Ô\€IÝ*¨4°°E¸&°>À!ÀIÈbÐQTÑUÔUÑ€A€aˆˆT�)Øˆe‚|€|Ý   g¨u°V¨n¸Q¸yÈÈCÑPÔP‰	ˆˆ!ˆAˆa�ØˆQˆt�D˜Ð"Ð"r)   c                   ó(   — e Zd Zdd„Zd„ Zddœd„ZdS )	ÚBenchmarkPosePredictorÚcudaFc                 óÂ  — t          |¦  «                             ¦   «         | _        || _        || _        t          j        | j        dz                       ¦   «         ¦  «        | _        | j        d          	                    ¦   «         D ]‰\  }}| j        |z                       ¦   «         }| 
                    | j        ¦  «        r<t          j        |                     ¦   «         ¦  «                             ¦   «         |k    rt          d|z   ¦  «        ‚ŒŠ| j        t           _        | j        dz  t           _        i | _        | j        d          	                    ¦   «         D �]�\  }}t)          j        | j        |d         z  dd¬	¦  «        }	|	d
         rÅt!          j        |d         ¦  «                             |¦  «                             ¦   «         }
|	d         }t!          j        t5          |d         ¦  «        |d         |d         ¦  «        }|                     |d¬¦  «         |
|                     |¦  «                             ¦   «         f| j        |<   Œøt9          |d         t;          j        dt:          j        ¦  «        |d         ¬¦  «        }|                     |	d         d¬¦  «         d |                     |¦  «                             ¦   «         f| j        |<   �Œƒd S )Nzbundle_config.jsonÚfileszBundle hash mismatch: ÚupstreamÚprofilesÚ
checkpointÚcpuT)Úmap_locationÚweights_onlyÚfrozen_encoderÚfamilyÚmodelÚmeanÚstd)Ústrict)r   r+   Útune)rƒ   ) r   ÚresolveÚrootÚdeviceÚallow_nativeÚjsonÚloadsÚ	read_textÚconfigÚitemsÚis_relative_toÚhashlibÚsha256Ú
read_bytesÚ	hexdigestr?   ÚarchitecturesÚROOTÚUPSTREAMÚmodelsÚtorchÚloadÚBackboneÚtoÚevalÚPoseHeadrA   Úload_state_dictr   r8   ÚzerosÚfloat32)ÚselfÚbundler†   r‡   ÚnameÚexpectedÚpathÚbranchÚspecÚstateÚbackboneÚweightsÚheadr   s                 r'   Ú__init__zBenchmarkPosePredictor.__init__?   s‹  € Ý�v‘,”,×&Ò&Ñ(Ô(ˆŒ	°V¨¬Èl¸DÔ<MÝ”J ¤	Ð*>Ñ >×IÒIÑKÔKÑLÔLˆŒØ!œ[¨Ô1×7Ò7Ñ9Ô9ð 	@ð 	@‰MˆD�Ø”)˜D‘.×)Ò)Ñ+Ô+ˆDØ×&Ò& t¤yÑ1Ô1ð @µW´^ÀDÇOÂOÑDUÔDUÑ5VÔ5V×5`Ò5`Ñ5bÔ5bÐdlÒ5lÐ5lÝ Ð!9¸$Ñ!>Ñ?Ô?Ð?ð 6màœ9�ÔØ#œy¨Ñ3�ÔØˆŒØœ; zÔ2×8Ò8Ñ:Ô:ð 
	Cñ 
	C‰KˆF�4Ý”*˜TœY t¨LÔ'9Ñ9ÀuÐZ^Ð_Ñ_Ô_ˆEØÐ%Ô&ð CÝ&Ô/°°X´Ñ?Ô?×BÒBÀ6ÑJÔJ×OÒOÑQÔQ�Ø˜gœ�­MÔ,BÅ3ÀwÈvÄÑCWÔCWÐX_Ð`fÔXgÐhoÐpuÔhvÑ,wÔ,w tØ×$Ò$ W°DÐ$Ñ9Ô9Ð9Ø%-¨d¯gªg°f©o¬o×.BÒ.BÑ.DÔ.DÐ$E�”˜FÑ#Ð#å˜t Hœ~­b¬h°v½b¼jÑ.IÔ.IÈtÐTZÌ|Ð\Ñ\Ô\�Ø×%Ò% e¨G¤n¸DÐ%ÑAÔAÐAØ%)¨%¯(ª(°6Ñ*:Ô*:×*?Ò*?Ñ*AÔ*AÐ$B�”˜FÑ#Ñ#ð
	Cð 
	Cr)   c                 ó�  — | j         |         \  }}t          j        ¦   «         5  t          j        |t          j        | j        ¬¦  «                             d¦  «        }t          j        |t          j        | j        ¬¦  «                             d¦  «        }|�) |t          j         ||¦  «        |fd¦  «        ¦  «        n |||¦  «        }d d d ¦  «         n# 1 swxY w Y   |                     ¦   «          	                    ¦   «         d         }	t          j        |	¦  «                             ¦   «         st          d¦  «        ‚|	S )N)r=   r†   r   éÿÿÿÿzNonfinite prediction)r•   r–   Úno_gradÚ	as_tensorrž   r†   Ú	unsqueezeÚcatrz   Únumpyr8   rB   rD   r?   )
rŸ   r&   rn   r¤   r§   r   ÚftÚgtÚpredÚvalues
             r'   ÚresidualzBenchmarkPosePredictor.residualU   sg  € Ø”{ 6Ô*‰ˆ�ÝŒ]‰_Œ_ð 	bð 	bÝŒ˜q¥u¤}¸D¼KÐHÑHÔH×RÒRÐSTÑUÔUˆBÝŒ˜q¥u¤}¸D¼KÐHÑHÔH×RÒRÐSTÑUÔUˆBØ;CÐ;O��•u”y ( (¨2¡,¤,¨rÐ!2°2Ñ6Ô6Ñ7Ô7Ð7ÐUZÐUZÐ[]Ð^`ÑUaÔUaˆDð	bð 	bð 	bñ 	bô 	bð 	bð 	bð 	bð 	bð 	bð 	bøøøð 	bð 	bð 	bð 	bð �hŠh‰jŒj×ÒÑ Ô  Ô#ˆÝŒ{˜5Ñ!Ô!×%Ò%Ñ'Ô'ÐP­jÐ9OÑ.PÔ.PÐ(PØˆs   ¤B*CÃCÃ!CN)rQ   c                ó¾  — |€| j         st          d¦  «        ‚|�dnd}| j        d         |         }||d         vrt          d¦  «        ‚t          |||||d         ¦  «        \  }}	}
}}|                      ||	|¦  «        }t          ||
t          j        |¦  «        |¦  «        }t          d i dt          |d	         ¦  «        “d
t          |d         ¦  «        “dt          t          j
        t          j        |d         |d         ¦  «        ¦  «        ¦  «        “dd “dd “dd “dd “dd “dt          “d|d         “d|d         “d|dk    rdnd“d|“dd“dd“dd“dd“ŽS )!Nz$Unknown units require --allow-nativeÚmetricÚnative_unknownrx   Údomainsz&Domain not covered by this unit branchrR   r   r   r   r   Útheta_degreesr   r   ÚzÚnormalÚrollÚpitchÚ
quaternionÚreference_pointÚmodel_familyr~   Úposition_unitÚmÚsource_scale_to_mÚhuman_pose_confirmedFÚreview_requiredTÚfull_six_dofÚrobot_command_eligible© )r‡   r?   r‹   rq   r¶   r   r8   r9   rE   rC   ÚdegreesÚarctan2r	   )rŸ   r   rO   rP   rQ   Údomainr¤   r¥   r&   rn   ro   rf   rg   r´   rµ   s                  r'   ÚposezBenchmarkPosePredictor.pose_   sò  € ØÐ dÔ&7Ð½jÐIoÑ>pÔ>pÐ8pØ%Ð1ˆxˆxÐ7GˆØŒ[˜Ô$ VÔ,ˆØ˜˜iœÐ(Ð(­zÐ:bÑ/cÔ/cÐ)cÝ /°°w¸sÀ:ÈdÐSZÌmÑ \Ô \Ñˆˆ!ˆD��iØ�]Š]˜1˜Q˜vÑ&Ô&ˆÝ�d˜4¥¤
¨4Ñ 0Ô 0°Ñ;Ô;ˆÝð Eð Eð E•e˜E !œH‘o”o�oð E­¨e°A¬h©¬¨ð EÅeÍBÌJÕWYÔWaÐbgÐhiÔbjÐkpÐqrÔksÑWtÔWtÑLuÔLuÑFvÔFvÐFvð EØ�dðEØ"& $ðEØ,0¨DðEØ7;°tðEØGKÀtðEÝ\eÐ\eðEà!% h¤ ðEà6:¸7´m°mðEàY_ÐaiÒYiÐYiÐRUÐRUÐoøðEð '1 jðEð GLÀeðEð ]aÐ\`ðEð "' ð	Eð ?D¸eð	Eð 	Er)   )rt   F)Ú__name__Ú
__module__Ú__qualname__rª   r¶   rÎ   rÊ   r)   r'   rs   rs   >   s`   € € € € € ðCð Cð Cð Cð,ð ð ð 37ð Eð Eð Eð Eð Eð Eð Er)   rs   c                  ó†  — t          j        t          ¬¦  «        } |                      ddt          ¬¦  «         |                      ddt          ¬¦  «         |                      dddt
          d¬	¦  «         |                      d
d¬¦  «         |                      dt
          ¬¦  «         |                      dd¬¦  «         |                      dd¬¦  «         |                      ddt          ¬¦  «         |                      ¦   «         }t          |j        |j	        |j
        ¦  «        }t          j        |j        d¬¦  «        5 }|                     |d         |d         |j        |j        |j        ¬¦  «        }d d d ¦  «         n# 1 swxY w Y   |j                             t)          j        |ddd¬¦  «        dz   ¦  «         d S )N)Údescriptionz--bundleT)ÚrequiredÚtypez--cloudz--boxr   )ÚXÚYÚWÚH)rÔ   ÚnargsrÕ   Úmetavarz--domain)rÔ   z--scale-to-m)rÕ   z--allow-nativeÚ
store_true)Úactionz--devicert   )Údefaultz--outputF)Úallow_pickleÚpoints_xyziÚpixel_to_source_index)rQ   rÍ   r   )Úensure_asciiÚindentÚ	allow_nanú
)ÚargparseÚArgumentParserÚ__doc__Úadd_argumentr   rC   Ú
parse_argsrs   r    r†   r‡   r8   r—   ÚcloudrÎ   rP   rQ   rÍ   ÚoutputÚ
write_textrˆ   Údumps)ÚparserÚargsÚ	predictorr¼   r^   s        r'   Úmainrò   n   sû  € ÝÔ"­wÐ7Ñ7Ô7€FØ
×Ò˜
¨DµdÐÑ;Ô;Ð;Ø
×Ò˜	¨4µTÐÑ:Ô:Ð:Ø
×Ò˜¨°A½5ÐIZÐÑ[Ô[Ð[Ø
×Ò˜
¨DÐÑ1Ô1Ð1Ø
×Ò˜­EÐÑ2Ô2Ð2Ø
×ÒÐ(°ÐÑ=Ô=Ð=Ø
×Ò˜
¨6ÐÑ2Ô2Ð2Ø
×Ò˜
¨DµdÐÑ;Ô;Ð;Ø	×	Ò	Ñ	Ô	€DÝ$ T¤[°´¸TÔ=NÑOÔO€IÝ	Œ�”¨Ð	/Ñ	/Ô	/ð M°1Ø�~Š~˜a Ô.¨qÐ1HÔ/IÈ$Ì(Ø)-¬ÀÄð ñ Mô MˆðMð Mð Mñ Mô Mð Mð Mð Mð Mð Mð Møøøð Mð Mð Mð Mð 	„K×Ò�4œ: f¸%ÀqÐSXÐYÑYÔYÐZ^Ñ^Ñ_Ô_Ð_Ð_Ð_s   Å6FÆFÆ
FÚ__main__)rè   ræ   rŽ   rˆ   Úpathlibr   r±   r8   r–   Ú r   r’   Úpose_benchmarkr   Úpose_refinementr   r   r	   Úpose_review_geometryr
   r   Úfork_geometryr   r   r   rq   rs   rò   rÏ   rÊ   r)   r'   ú<module>rú      s9  ðØ OÐ OØ €€€Ø €€€Ø €€€Ø Ð Ð Ð Ð Ð à Ð Ð Ð Ø €€€à 4Ð 4Ð 4Ð 4Ð 4Ð 4Ø $Ð $Ð $Ð $Ð $Ð $Ø ?Ð ?Ð ?Ð ?Ð ?Ð ?Ð ?Ð ?Ð ?Ð ?Ø 8Ð 8Ð 8Ð 8Ð 8Ð 8Ð 8Ð 8Ø =Ð =Ð =Ð =Ð =Ð =Ð =Ð =Ð =Ð =ð*#ð *#ð *#ðZ-Eð -Eð -Eð -Eð -Eñ -Eô -Eð -Eð``ð `ð `ð$ ˆZÒÐ˜˜™œ˜˜˜ÐÐr)   