§
    iÎ¸j¤  ã                  ó”   — d Z ddlmZ ddlmZ ddlm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
„ Zd„ Zd„ Zd„ Zddœd„Zd„ ZdS )zLEvidence-based, per-collection sensor policies; never infer units per frame.é    )Úannotations)Údefaultdict)ÚreplaceN)ÚPathé   )Ú	_pcd_grid)Úread_pcdc                ó¦   — | j         rt          | j         ¦  «        nd }d| j        v rdnd}| j                             d¦  «        d         › d|› d|› �S )NÚnone_rectifiedÚprimaryú/r   ú|)Úpcdr   ÚvariantÚframe_idÚsplit)ÚrecordÚgridr   s      ú@/root/private/project/pallet_detection_v2/pallet/data/sensors.pyÚ
policy_keyr      se   € Ø$*¤JÐ8�9�V”ZÑ Ô Ð °D€DØ"2°f´nÐ"DÐ"DÐÐÈ)€GØŒo×#Ò# CÑ(Ô(¨Ô+Ð>Ð>¨dÐ>Ð>°WÐ>Ð>Ð>ó    c                óø  ‡— |\  }}t          j        dt          | ¦  «        dz
  t          dt          | ¦  «        ¦  «        t          ¬¦  «        }| |dd…f                              t           j        ¦  «        }t          j        ||z  t          |dz
  d¦  «        z  dz  dz
  ||z  t          |dz
  d¦  «        z  dz  dz
  t          j	        t          |¦  «        ¦  «        f¦  «        }g }t          d¦  «        D �]kŠˆfd„t          d¦  «        D ¦   «         }t          j        |¦  «                             d¬	¦  «        |dd…‰f         d
k    z  }	|	                     ¦   «         dk     rŒp||	         dd…|f         ||	‰‰dz   …f         z  }
t          j        |
¦  «        dk                          d¬	¦  «        }||	         |         |
|         }
}t          |¦  «        dk     rŒèt           j                             ||
d¬¦  «        d         }|
||z  z
  dz                       d¬	¦  «        }|
|
                     d¬	¦  «        z
  dz                       d¬	¦  «        }t%          t          j        d|t          j        |d¦  «        z  z
  ¦  «        ¦  «        }t           j                             |dd…         d¬	¦  «        }|dk    r£t          |¦  «        dk    r�t           j                             |dd…         ¦  «        dk     re|                     ‰||                     ¦   «         t          j        dt          j        |¦  «        z  ¦  «                             ¦   «         dœ¦  «         �Œm|rt          |d„ ¬¦  «        ndS )zDIdentify a forward coordinate only if pinhole ratios fit pixel rays.r   r   i   ©ÚdtypeNé   é   c                ó    •— g | ]
}|‰k    ¯|‘ŒS © r   )Ú.0ÚiÚaxiss     €r   ú
<listcomp>z'projection_evidence.<locals>.<listcomp>   s   ø€ Ð2Ð2Ð2�q¨¨Tª	¨	�¨	¨	¨	r   ©r!   çH¯¼šò×z>é@   é   )Úrcondgê-�™—q=g×£p=
×ï?g{®Gáz”?é   )r!   Úr2ÚcoefficientsÚfov_estimatec                ó   — | d         S )Nr)   r   )Úxs    r   ú<lambda>z%projection_evidence.<locals>.<lambda>0   s
   €  a¨¤g€ r   )Úkey)ÚnpÚlinspaceÚlenÚminÚintÚastypeÚfloat64Úcolumn_stackÚmaxÚonesÚrangeÚisfiniteÚallÚsumÚabsÚlinalgÚlstsqÚmeanÚfloatÚmaximumÚnormÚcondÚappendÚtolistÚdegreesÚarctan)ÚpointsÚshapeÚhÚwÚindicesÚxyzÚuvÚchoicesÚotherÚvalidÚvaluesÚfiniter-   ÚfitÚresidualÚtotalÚscoreÚslopesr!   s                     @r   Úprojection_evidencer[      s?  ø€ à�D€A€qÝŒk˜!�S ™[œ[¨1™_­c°$½¸F¹¼Ñ.DÔ.DÍCÐPÑPÔP€GØ
�˜"˜1˜"�Ô
×
$Ò
$¥R¤ZÑ
0Ô
0€CÝ	Œ˜7 Q™;­#¨a°!©e°Q©-¬-Ñ7¸!Ñ;¸aÑ?Ø" a™<­3¨q°1©u°a©=¬=Ñ8¸1Ñ<¸qÑ@Å"Ä'Í#ÈgÉ,Ì,ÑBWÔBWðYñ 
Zô 
Z€Bà€GÝ�a‘”ð Yñ YˆØ2Ð2Ð2Ð2�E !™HœHÐ2Ñ2Ô2ˆÝ”˜CÑ Ô ×$Ò$¨!Ð$Ñ,Ô,°°A°A°A°t°G´¸tÒ0CÑDˆØ�9Š9‰;Œ;˜ÒÐØØ�U”˜A˜A˜A˜u˜HÔ%¨¨E°4¸¸q¹°=Ð,@Ô(AÑAˆÝ”&˜‘.”. 1Ò$×)Ò)¨qÐ)Ñ1Ô1ˆØ�u”I˜fÔ% v¨f¤~ˆ6ˆÝˆq‰6Œ6�BŠ;ˆ;ØÝŒi�oŠo˜a ¨tˆoÑ4Ô4°QÔ7ˆØ˜a #™gÑ%¨!Ñ+×0Ò0°aÐ0Ñ8Ô8ˆØ˜6Ÿ;š;¨A˜;Ñ.Ô.Ñ.°1Ñ4×9Ò9¸qÐ9ÑAÔAˆÝ•b”f˜Q ­B¬J°u¸eÑ,DÔ,DÑ!DÑDÑEÔEÑFÔFˆÝ”—’  B Q B¤¨a�Ñ0Ô0ˆØ�EŠ>ˆ>�c &™kœk¨DÒ0Ð0µR´Y·^²^ÀCÈÈÈÄGÑ5LÔ5LÈrÒ5QÐ5QØ�NŠN D°ÀsÇzÂzÁ|Ä|Ý,.¬J°q½2¼9ÀVÑ;LÔ;LÑ7LÑ,MÔ,M×,TÒ,TÑ,VÔ,VðXð Xñ Yô Yð Yùà29ÐC�3ˆwÐ-Ð-Ð.Ñ.Ô.Ð.¸tÐCr   c                ó0  — t          j        t          | ¦  «        t           j        ¦  «        }	 |                     ¦   «         r&|                     ¦   «                              ¦   «         nd 	 |                     ¦   «          S # |                     ¦   «          w xY w)N)Úcv2ÚFileStorageÚstrÚFILE_STORAGE_READÚisOpenedÚgetFirstTopLevelNodeÚmatÚrelease)ÚpathÚfss     r   Ú_matrixrg   3   sr   € Ý	Œ�˜T™œ¥CÔ$9Ñ	:Ô	:€BðØ24·+²+±-´-ÐIˆr×&Ò&Ñ(Ô(×,Ò,Ñ.Ô.Ð.ÀTÐIà
�
Š
‰Œˆˆøˆ�
Š
‰Œˆˆøøøs   ®;A? Á?Bc                óÖ  — g }d| j         v r¥t          j        t          | j         d         ¦  «        t          j        ¦  «        }|�l|j        |k    ra|j        t          j        k    rL| 	                    d| 
                    ¦   «                              t          ¦  «        |dd…|f         f¦  «         d| j         v rßt          | j         d         ¦  «        }|�Ã|j        g |¢d‘R k    r³|j        t          j        k    ržt          j        t          j        |¦  «        ¦  «        dk    rt| 	                    d|                     dd¦  «         
                    ¦   «                              t          ¦  «        |dd…dd…f          
                    ¦   «         f¦  «         |D �]L\  }}}	t          j        |	¦  «        t          j        |¦  «        z  t          j        |	¦  «        d	k    z  t          j        |¦  «        dk    z  }
|
                     ¦   «         d
k     rŒ{dD ]Î}t          j        |	|
         |z  ||
         z
  ¦  «        }|t          j        t          j        ||
         ¦  «        d¦  «        z  }t          t          j        |d¦  «        ¦  «        dk     rP||t-          |
                     ¦   «         ¦  «        t          t          j        |d¦  «        ¦  «        | j        dœc c S ŒÏ�ŒNdS )zHFind a documented metric encoding and verify its numeric correspondence.ÚdepthNÚuint16_depth_mmrO   r   r   Úint16_xyz_mméÿÿÿÿr$   r%   )g      ð?g     @�@éd   gffffffî?g{®Gáz„?)Úscale_to_mmÚsourceÚmatched_pixelsÚrelative_p95r   )Úpathsr]   Úimreadr_   ÚIMREAD_UNCHANGEDrK   r   r0   Úuint16rF   Úravelr5   rB   rg   Úint16r8   r>   Úreshaper;   r=   rC   Úquantiler4   r   )r   rO   rK   r!   Úsourcesri   rc   ro   ÚtargetÚrawÚmaskÚscaleÚerrorÚrelatives                 r   Úmetric_anchorr�   ;   sº  € à€GØ�&”,ÐÐÝ”
�3˜vœ|¨GÔ4Ñ5Ô5µsÔ7KÑLÔLˆØÐ ¤°Ò!5Ð!5¸%¼+ÍÌÒ:RÐ:RØ�NŠNÐ-¨u¯{ª{©}¬}×/CÒ/CÅEÑ/JÔ/JÈCÐPQÐPQÐPQÐSWÐPWÌLÐYÑZÔZÐZØ�”ÐÐÝ�f”l 5Ô)Ñ*Ô*ˆØˆ?˜sœy¨K¨e¨K°Q¨K¨KÒ7Ð7¸C¼IÍÌÒ<QÐ<QÕVXÔV\Õ]_Ô]cÐdgÑ]hÔ]hÑViÔViÐlmÒVmÐVmà�NŠN˜N¨C¯KªK¸¸AÑ,>Ô,>×,DÒ,DÑ,FÔ,F×,MÒ,MÍeÑ,TÔ,TÐVYÐZ[ÐZ[ÐZ[Ð]_Ð^_Ð]_ÐZ_ÔV`×VfÒVfÑVhÔVhÐiÑjÔjÐjØ&ð 
5ñ 
5Ñˆ�˜ÝŒ{˜3ÑÔ¥"¤+¨fÑ"5Ô"5Ñ5½¼À¹¼ÀtÒ9KÑLÕPRÔPVÐW]ÑP^ÔP^ÐabÒPbÑcˆØ�8Š8‰:Œ:˜Š?ˆ?ØØ"ð 	5ð 	5ˆEÝ”F˜3˜tœ9 uÑ,¨v°d¬|Ñ;Ñ<Ô<ˆEØ�rœz­"¬&°¸´Ñ*>Ô*>ÀÑDÔDÑDˆHÝ•R”[ ¨4Ñ0Ô0Ñ1Ô1°DÒ8Ð8Ø',¸Ý*-¨d¯hªh©j¬j©/¬/Í5ÕQSÔQ\Ð]eÐgkÑQlÔQlÑKmÔKmØ$*¤Oð5ð 5ð 5ð 5ð 5ð 5ð 5ð 9ñ	5ð ˆ4r   r   )Úsamples_per_policyc               óV  — t          t          ¦  «        }| D ]E}|j        r<t          |j        ¦  «        r(|t	          |¦  «                                      |¦  «         ŒFi }t          |                     ¦   «         ¦  «        D �]Q\  }}t          t          t          j
        dt          |¦  «        dz
  t          |t          |¦  «        ¦  «        t          ¬¦  «        ¦  «        ¦  «        }g g g }}
}	|D �]}||         }	 t          |j        ¦  «        \  }}t          |j        ¦  «        }t          |¦  «        |d         |d         z  k    rt          d¦  «        ‚t!          ||¦  «        }|rM|	                     d|j        i|¥¦  «         t%          ||||d         ¦  «        }|r|
                     |¦  «         ŒÈ# t&          $ r5}|                     |j        t)          |¦  «        dœ¦  «         Y d }~�Œd }~ww xY wt          |d         j        ¦  «        }t          |	¦  «        t          |¦  «        k    ot          d„ |	D ¦   «         ¦  «        dk    }t+          |
¦  «        ot          d	„ |
D ¦   «         ¦  «        dk    }t          j        d
„ |	D ¦   «         ¦  «        }|o+t          j        |d¬¦  «                             ¦   «         dk     }t          |¦  «        r(|                     d¬¦  «                             ¦   «         nd }|sdn!dd                     d„ |D ¦   «         ¦  «        z   }i d|“dt          |¦  «        “d|d         › d|d         › d|› �“dt+          |o|o|¦  «        “dd“dd“dd“d|r|	d         d         nd “d|r|
d         d         nd “d|	“d|
“d |“d!t          |¦  «        “d"t          |¦  «        “d#|r|r|rd nd$“d%d&“||<   �ŒSt9          |¦  «        }|j                             d'd'¬(¦  «         |                     tA          j!        |d¬)¦  «        d*¬+¦  «         |S ),Nr   r   r   z,PCD point count does not match restored gridr   r!   )r   r   c                ó   — h | ]
}|d          ’ŒS r#   r   ©r   Úes     r   ú	<setcomp>z!build_registry.<locals>.<setcomp>n   s   € Ð?\Ð?\Ð?\ÈaÀÀ&Ä	Ð?\Ð?\Ð?\r   c                ó   — h | ]
}|d          ’ŒS )rn   r   r…   s     r   r‡   z!build_registry.<locals>.<setcomp>o   s   € Ð-PÐ-PÐ-PÀ1¨a°Ô.>Ð-PÐ-PÐ-Pr   c                ó   — g | ]
}|d          ‘ŒS )r+   r   r…   s     r   r"   z"build_registry.<locals>.<listcomp>p   s   € Ð=Ð=Ð=¨q˜˜>Ô*Ð=Ð=Ð=r   r#   r   Ú
unverifiedÚfov_Ú_c           	   3  ót   K  — | ]3}t          t          t          |d z  ¦  «        d z  ¦  «        ¦  «        V — Œ4dS )r&   N)r_   r4   Úround)r   Úvs     r   ú	<genexpr>z!build_registry.<locals>.<genexpr>s   sK   è è € ÐUzÐUzÐqrÕVYÕZ]Õ^cÐdeÐfgÑdgÑ^hÔ^hÐijÑ^jÑZkÔZkÑVlÔVlÐUzÐUzÐUzÐUzÐUzÐUzr   Ú	policy_idr   Ú	domain_idr-   Údepth_availableÚdepth_sourcer   Ú
depth_kindÚaxialÚoutput_unitÚmmÚ
depth_axisrn   r[   Úunit_evidenceÚerrorsÚsample_countÚframesÚunavailable_reasonz@No consistent sampled projection and paired metric-unit evidenceÚsource_notezqProjection fit identifies configurations, not factory calibration; metric anchors follow dataset report encoding.T)ÚparentsÚexist_ok)Úindentzutf-8)Úencoding)"r   Úlistr   r   r   rF   ÚsortedÚitemsÚsetr0   r1   r2   r3   r4   r	   Ú
ValueErrorr[   r   r�   Ú	Exceptionr_   ÚboolÚarrayÚptpr8   rA   rG   Újoinr   ÚparentÚmkdirÚ
write_textÚjsonÚdumps)ÚrecordsÚoutputr‚   Úgroupsr   Úregistryr/   ÚmembersÚ
sample_idsÚevidenceÚanchorsr›   r    rŒ   rJ   rK   Ú
projectionÚanchorÚexcr   Ústable_axisÚstable_scaleÚfovsÚstable_projectionÚfovÚprojection_tags                             r   Úbuild_registryrÄ   U   sá  € Ý�ÑÔ€FØð 6ð 6ˆØŒ:ð 	6�) F¤JÑ/Ô/ð 	6Ø•:˜fÑ%Ô%Ô&×-Ò-¨fÑ5Ô5Ð5øØ€HÝ˜vŸ|š|™~œ~Ñ.Ô.ð $Pñ $P‰ˆˆWÝ�C¥¤¨A­s°7©|¬|¸A©~½sÐCUÕWZÐ[bÑWcÔWcÑ?dÔ?dÕloÐ pÑ pÔ pÑqÔqÑrÔrˆ
Ø$&¨¨B˜6�'ˆØð 	Pñ 	PˆAØ˜Q”ZˆFðPÝ$ V¤ZÑ0Ô0‘	��6Ý! &¤*Ñ-Ô-�Ý�v‘;”; %¨¤(¨U°1¬XÑ"5Ò5Ð5Ý$Ð%SÑTÔTÐTÝ0°¸Ñ?Ô?�
Øð /Ø—O’O Z°´Ð$OÀJÐ$OÑPÔPÐPÝ*¨6°6¸5À*ÈVÔBTÑUÔU�FØð /ØŸš vÑ.Ô.Ð.øøÝð Pð Pð PØ—’¨6¬?ÅSÈÁXÄXÐNÐNÑOÔOÐOÐOÐOÐOÐOÑOøøøøðPøøøå˜ œœÑ(Ô(ˆÝ˜(‘m”m¥s¨:¡¤Ò6Ðb½3Ð?\Ð?\ÐS[Ð?\Ñ?\Ô?\Ñ;]Ô;]ÐabÒ;bˆÝ˜G‘}”}ÐV­Ð-PÐ-PÈÐ-PÑ-PÔ-PÑ)QÔ)QÐUVÒ)VˆÝŒxÐ=Ð=°HÐ=Ñ=Ô=Ñ>Ô>ˆØ'ÐJ­B¬F°4¸aÐ,@Ñ,@Ô,@×,DÒ,DÑ,FÔ,FÈÒ,JÐÝ,/°©I¬IÐ?ˆd�iŠi˜QˆiÑÔ×&Ò&Ñ(Ô(Ð(¸4ˆØ->Ðz˜˜ÀFÈSÏXÊXÐUzÐUzÐvyÐUzÑUzÔUzÑMzÔMzÑDzˆðPØ˜ðPØ$¥d¨4¡j¤jðPà˜D œGÐ@Ð@ d¨1¤gÐ@Ð@°Ð@Ð@ðPð �t KÐ$VÐ4EÐ$VÈ,ÑWÔWðPð ˜Eð	Pð $0°ð	Pð ;HÈð	Pð
 °ÐF˜( 1œ+ fÔ-Ð-À$ðPð ¸ÐN˜7 1œ: mÔ4Ð4È$ðPð " 8ðPð .=¸gðPð �fðPð -­c°*©o¬oðPð @HÍÈWÉÌðPð !¨+ð #TÐ:Kð #TÐP\ð #T $ $ØSðPð ð  OðPˆ�‰‰õ �&‰\Œ\€FØ
„M×Ò ¨tÐÑ4Ô4Ð4Ø
×Ò•d”j °!Ð4Ñ4Ô4¸wÐÑGÔGÐGØ€Os   Ã<B;F8Æ8
G7Ç*G2Ç2G7c                ó    ‡— ˆfd„| D ¦   «         S )Nc                ó|   •— g | ]8}t          |‰                     t          |¦  «        d dddœ¦  «        ¬¦  «        ‘Œ9S )ÚunknownFzNo organized sensor grid)r’   r“   rž   )Úsensor_config)r   Úgetr   )r   r   r¶   s     €r   r"   z#attach_registry.<locals>.<listcomp>‡   so   ø€ ð ð ð àõ �F¨(¯,ª,µzÀ&Ñ7IÔ7IØ°5ÐPjðLlð Llñ +mô +mð nñ nô nð ð ð r   r   )r³   r¶   s    `r   Úattach_registryrÊ   †   s.   ø€ ðð ð ð àðñ ô ð r   )Ú__doc__Ú
__future__r   Úcollectionsr   Údataclassesr   r±   Úpathlibr   r]   Únumpyr0   Úmanifestr   r   r	   r   r[   rg   r�   rÄ   rÊ   r   r   r   ú<module>rÒ      s  ðØ RÐ RØ "Ð "Ð "Ð "Ð "Ð "à #Ð #Ð #Ð #Ð #Ð #Ø Ð Ð Ð Ð Ð Ø €€€Ø Ð Ð Ð Ð Ð à 
€
€
€
Ø Ð Ð Ð à Ð Ð Ð Ð Ð Ø Ð Ð Ð Ð Ð ð?ð ?ð ?ðDð Dð Dð:ð ð ðð ð ð4 ;<ð .ð .ð .ð .ð .ðbð ð ð ð r   