§
    ½/ÎjÃ;  ã                  óø   — d Z ddlmZ ddlZddlZddlZddlZddlm	Z	 ddl
mZ i dd“dd	“d
d“dd“dd“dd“dd“dd“dd“dd“dd“dd“dd“d d“d!d"“d#d$“d%d&“d'd(d)œ¥Zd*„ Zd+„ Zd2d,„Zd-„ Zd.„ Zd/„ Zd3d0„Zd4d1„ZdS )5zçOrganized-cloud surface proposals. No interpolation or learned segmentation.

Indices always refer to measured points. Convex support is used only to identify
parallel surfaces behind apertures, never to rasterize the output mask.
é    )ÚannotationsN)Ú
coo_matrix)Úconnected_componentsÚseedé*   Úneighbor_multiplierg      @Únormal_angle_degreesg      9@Útolerance_spacing_ratioç      à?Úminimum_tolerance_mmg       @Úmaximum_tolerance_mmg      4@Úminimum_region_pointsé   Úminimum_face_pointsé    Úransac_max_iterationsiè  Úransac_probabilityg+‡ÙÎ÷ï?Úhypothesis_pointsi    Úmaximum_planes_per_regioné   Úcoplanar_angle_degreesg       @Ú!background_parallel_angle_degreesÚbackground_fractiongffffffæ?Úownership_residual_marginçš™™™™™É?Únormal_window_radiusé   é   g      Ð?)Únormal_minimum_neighborsÚnormal_maximum_eigenvalue_ratioc                óÂ   — t          j        | ¦  «                             d¬¦  «        t           j                             t          j        | ¦  «        d¬¦  «        dk    z  S )Néÿÿÿÿ©Úaxisg:Œ0âŽyE>)ÚnpÚisfiniteÚallÚlinalgÚnormÚ
nan_to_num)Úxyzs    úA/root/private/project/pallet_detection_v2/pallet/fork_geometry.pyÚ	valid_xyzr-      sK   € ÝŒ;�sÑÔ×Ò RÐÑ(Ô(­B¬I¯NªN½2¼=ÈÑ;MÔ;MÐTV¨NÑ,WÔ,WÐZ^Ò,^Ñ_Ð_ó    c           	     ó†  — t          t          d| ¦  «        t          | | |z
  ¦  «        ¦  «        t          t          d| ¦  «        t          |||z
  ¦  «        ¦  «        f}t          t          d|¦  «        t          | | |z   ¦  «        ¦  «        t          t          d|¦  «        t          |||z   ¦  «        ¦  «        f}||fS )Nr   )ÚsliceÚmaxÚmin)ÚhÚwÚdyÚdxÚaÚbs         r,   Ú_pair_slicesr9   #   s›   € Ý	�s�1�r�c‰{Œ{�C  1 R¡4™LœLÑ	)Ô	)­5µ°Q¸¸±´½cÀ!ÀQÀrÁT¹l¼lÑ+KÔ+KÐL€AÝ	�s�1�b‰zŒz�3˜q ! B¡$™<œ<Ñ	(Ô	(­%µ°A°r±
´
½CÀÀ1ÀRÁ4¹L¼LÑ*IÔ*IÐJ€AØˆaˆ4€Kr.   c                óZ  — t          j        | t           j        ¬¦  «        } t          | ¦  «        }| |�|ndz  }|j        \  }}t          j        d||ft           j        ¦  «        }t          d¦  «        D ]Œ\  }	\  }
}t          |||
|¦  «        \  }}t           j	         
                    ||         ||         z
  d¬¦  «        }t          j        ||         ||         z  |dk    z  |t           j        ¦  «        ||	         |<   Œ�t          j        ¦   «         5  t          j        d	t          ¦  «         t          j        |d
¬¦  «        }ddd¦  «         n# 1 swxY w Y   |t          j        |¦  «        |z  |d
k    z           }t%          |¦  «        r!t'          t          j        |¦  «        ¦  «        nd}t          j        t          j        |¦  «        |d
k    z  ||¦  «        }||d         z  }|�"t          j        ||d         |d         ¦  «        }t          j        |d¦  «        }t          j        ||ft           j        ¦  «        }t          j        ||dft           j        ¦  «        }t          j        ||ddft           j        ¦  «        }|d         }t3          | |dz   ¦  «        D �]}
t3          | |dz   ¦  «        D ]ì}t          |||
|¦  «        \  }}||         ||         z
  }||         ||         z  t           j	         
                    |d¬¦  «        |d         t          j        ||         ||         ¦  «        z  k    z  }t          j        |d         |d¦  «        }||xx         |z  cc<   ||xx         |z  cc<   ||xx         |ddd…df         |dddd…f         z  z  cc<   Œí�Œ|t          j        |d         d¦  «        z  }|t          j        |d         d¦  «        z  |ddd…df         |dddd…f         z  z
  }t           j	                             |¦  «        \  }}|ddd…d
f         }|||d         k    z  |d         t          j        |dz  dz  d¦  «        k    z  |d         |d         |d         z  k    z  }|€t          j        d¦  «        nt          j        |¦  «        |�|ndz  }||z
  } | t          j        t           j	         
                    | d¬¦  «        d         d¦  «        z  } t          ||dd
¦  «        \  }}| |         | |         z
  }!||         ||         z  }"|"                     ¦   «         rt          j        |!|"         d
¬¦  «        nt          j        d¦  «        }#|#t;          t           j	         
                    |#¦  «        d¦  «        z  }$||||||| |$fS )z=8-neighbor spacing and discontinuity-bounded tangent normals.©ÚdtypeNç      ð?é   ))r"   r"   )r"   r   )r"   é   )r   r"   ©r   r?   ©r?   r"   ©r?   r   ©r?   r?   r"   r#   çê-�™—q=Úignorer   r
   r   r   g»½×Ùß|Û=é   r   r?   r   ).Nç        .).NNr   ).r?   r   gš™™™™™™?ç›+¡†›„=).r   r    g#B’¡œÇ;)r%   ÚasarrayÚfloat64r-   ÚshapeÚfullÚnanÚ	enumerater9   r(   r)   ÚwhereÚwarningsÚcatch_warningsÚsimplefilterÚRuntimeWarningÚ	nanmedianr&   ÚlenÚfloatÚmedianÚclipÚmaximumÚzerosÚint16ÚrangeÚminimumÚeighÚanyr1   )%r+   ÚconfigÚscale_to_mmÚoriginÚgoodÚpr3   r4   Ú	distancesÚir5   r6   r7   r8   ÚdÚspacingÚsamplesÚfallbackÚ	toleranceÚcountsÚfirstÚsecondÚradiusÚdeltaÚkeepÚ
safe_deltaÚmeanÚ
covarianceÚvaluesÚvectorsÚnormalsÚreliableÚraysÚ	ray_deltaÚpairÚvectorÚverticals%                                        r,   Úorganized_featuresr~   )   sÚ  € å
Œ*�S¥¤
Ð
+Ñ
+Ô
+€CÝ�S‰>Œ>€DØ˜kÐ5ˆ{ˆ{¸2Ñ>€AØŒ:�D€A€qÝ”˜˜A˜q˜	¥2¤6Ñ*Ô*€IÝ Ð!XÑYÔYð Oð O‰ˆ‰8ˆB�Ý˜A˜q " bÑ)Ô)‰ˆˆ1ÝŒI�NŠN˜1˜Qœ4  !¤™9¨2ˆNÑ.Ô.ˆÝœ( 4¨¤7¨T°!¬WÑ#4¸¸Eº	Ñ#BÀAÅrÄvÑNÔNˆ	�!Œ�Q‰ˆÝ	Ô	 Ñ	"Ô	"ð 2ð 2ÝÔ˜h­Ñ7Ô7Ð7Ý”,˜y¨qÐ1Ñ1Ô1ˆð2ð 2ð 2ñ 2ô 2ð 2ð 2ð 2ð 2ð 2ð 2øøøð 2ð 2ð 2ð 2ð •b”k 'Ñ*Ô*¨TÑ1°W¸q²[ÑAÔB€GÝ,/°©L¬LÐ@�u•R”Y˜wÑ'Ô'Ñ(Ô(Ð(¸b€HÝŒh•r”{ 7Ñ+Ô+¨w¸ª{Ñ;¸WÀhÑOÔO€GØ˜&Ð!:Ô;Ñ;€IØÐÝ”G˜I vÐ.DÔ'EÀvÐNdÔGeÑfÔfˆ	Ý”
˜9 eÑ,Ô,€Iõ ŒX�q˜�e�BœHÑ%Ô%€FÝŒH�a˜˜!�W�RœZÑ(Ô(€EÝŒX�q˜˜1˜Q�i¥¤
Ñ+Ô+€FØÐ*Ô+€FÝ�V�G˜F 1™HÑ%Ô%ð Fñ FˆÝ˜˜  q¡Ñ)Ô)ð 	Fð 	FˆBÝ˜q  2 bÑ)Ô)‰CˆAˆaØ�a”D˜˜1œ‘IˆEØ˜”7˜4 œ7‘?¥B¤I§N¢N°5¸b NÑ$AÔ$AÀ6ÐJ_ÔC`ÕacÔakÐlsÐtuÔlvÐw~ð  @Aô  xBñ  bCô  bCñ  DCò  %Cñ  DˆDÝ”x  X¤¨u°RÑ8Ô8ˆJØ�1ˆIˆIŒI�t‰OˆIˆI‰IØ�!ˆHˆHŒH�jÑ ˆHˆH‰HØ�1ˆIˆIŒI�z # q q q¨ +Ô.¨z¸#¸dÀ1À1À1¸*Ô/EÑEÑEˆIˆI‰IˆIñ	Fð 
�rŒz˜& Ô*¨1Ñ-Ô-Ñ	-€DØ•b”j ¨Ô!6°qÑ9Ô9Ñ9¸$¸sÀAÀAÀAÀd¸{Ô:KÈDÐQTÐUYÐZ[ÐZ[ÐZ[ÐQ[ÔL\Ñ:\Ñ\€JÝ”9—>’> *Ñ-Ô-�N€Fˆ7Ø�C˜˜˜˜1�HÔ€GØ�F˜FÐ#=Ô>Ò>Ñ?Ø�u”�bœj¨°!©°D©¸Ñ?Ô?Ò?ñAà�u”˜vÐ&GÔHÈÐPUÌÑVÒVñX€Hð #˜N�RŒX�a‰[Œ[ˆ[µ´
¸6Ñ0BÔ0BÐT_ÐTkÀ[À[ÐqsÑ0t€FØˆV‰8€DØ�BŒJ•r”y—~’~ d°�~Ñ4Ô4°XÔ>ÀÑFÔFÑF€DÝ˜˜1˜a Ñ#Ô#�D€A€qØ�Q”˜˜Qœ‘€IØ�Œ7�T˜!”WÑ€DØ37·8²8±:´:ÐN�RŒY�y ”¨QÐ/Ñ/Ô/Ð/Å2Ä8ÈAÁ;Ä;€FØ•c�"œ)Ÿ.š.¨Ñ0Ô0°%Ñ8Ô8Ñ8€HØˆd�G˜Y¨°¸4ÀÐIÐIs   Ä1EÅEÅEc           	     ó8  — |j         \  }}t          j        ||fdt          j        ¬¦  «        }t          j        |                     ¦   «         t          j        ¬¦  «        ||<   g g }
}	t          j        t          j        |d         ¦  «        ¦  «        }dD �] \  }}t          ||||¦  «        \  }}||         ||         z  }|t          j
                             | |         | |         z
  d¬¦  «        |d         t          j        ||         ||         ¦  «        z  k    z  }t          j        t          j        ||         ||         z  d¬¦  «        ¦  «        }|||         ||         z   ||k    z  z  }|	                     ||         |         ¦  «         |
                     ||         |         ¦  «         �Œ"t          |                     ¦   «         ¦  «        }|s!t          j        dt          j        ¦  «        dfS t          j        |	¦  «        t          j        |
¦  «        }}t%          t          j        t)          |¦  «        t          j        ¦  «        ||ff||f¬¦  «                             ¦   «         }t/          |d	d
¬¦  «        \  }}|t          |¦  «        fS )Nr"   r;   r	   )r@   rA   rB   rC   r#   r   r   )rK   FT)ÚdirectedÚreturn_labels)rK   r%   rL   Úint32ÚarangeÚsumÚmathÚcosÚradiansr9   r(   r)   r]   ÚabsÚappendÚintÚemptyÚconcatenater   ÚonesrU   Úuint8Útocsrr   )rd   rc   rh   rw   rx   r`   r3   r4   ÚidsÚedges_aÚedges_bÚ	cos_limitr5   r6   r7   r8   rq   ÚcosineÚnÚaaÚbbÚgraphÚcountÚlabelss                           r,   Úneighbor_regionsr›   _   sC  € ØŒ:�D€A€qÝ
Œ'�1�a�&˜"¥B¤HÐ
-Ñ
-Ô
-€CÝ”	˜$Ÿ(š(™*œ*­B¬HÐ5Ñ5Ô5€Cˆ�IØ˜2ˆW€GÝ”�œ fÐ-CÔ&DÑEÔEÑFÔF€IØ,ð Cñ C‰ˆˆBÝ˜A˜q " bÑ)Ô)‰ˆˆ1Ø�AŒw˜˜aœÑ ˆØ•”	—’˜q œt A a¤D™y¨r�Ñ2Ô2°fÐ=RÔ6SÕTVÔT^Ð_fÐghÔ_iÐkrÐstÔkuÑTvÔTvÑ6vÒvÑvˆÝ”�œ˜w qœz¨'°!¬*Ñ4¸2Ð>Ñ>Ô>Ñ?Ô?ˆØ�(˜1”+ ¨¤Ñ+Ð,°¸)Ò0CÑDÑDˆØ�Š�s˜1”v˜d”|Ñ$Ô$Ð$ g§n¢n°S¸´V¸D´\Ñ&BÔ&BÐ&BÑ&BÝˆD�HŠH‰JŒJ‰Œ€AØð (ÝŒx˜�2œ8Ñ$Ô$ aÐ'Ð'ÝŒ^˜GÑ$Ô$¥b¤n°WÑ&=Ô&=ˆ€BÝ�œ¥ B¡¤­¬Ñ2Ô2°R¸°HÐ=ÀaÈÀUÐKÑKÔK×QÒQÑSÔS€EÝ(¨¸ÈdÐSÑSÔS�M€Eˆ6Ø•3�u‘:”:ÐÐr.   c                ól  — t          j        | d¬¦  «        }| |z
  }t           j                             |j        |z  t          t          | ¦  «        d¦  «        z  ¦  «        \  }}|d         t          |d         dz  |d         dz   ¦  «        k    rd S |d d …df         }t           j        || |z  f         S )Nr   r#   r?   g)\�Âõ(ð?rH   )r%   rs   r(   r^   ÚTr1   rU   Úr_)ÚpointsÚcenterÚcenteredru   rv   r•   s         r,   Ú	svd_planer¢   u   s¨   € ÝŒW�V !Ð$Ñ$Ô$€FØ�f‰}€HÝ”i—n’n X¤Z°Ñ%8½½SÀ¹[¼[È!Ñ9LÔ9LÑ%LÑMÔM�O€FˆGØˆa„y•C˜˜qœ	 $™¨¨q¬	°%©Ñ8Ô8Ò8Ð8ØˆtØ����1�Œ€AÝŒ5��V�G˜A‘I�ÔÐr.   c                ó	  — t          | ¦  «        }||d         k     rdt          j        |t          ¦  «        fS t          j        |¦  «        }||d         k    r0t          j        |                     ||d         d¬¦  «        ¦  «        }| |         ||         }}d\  }}	}
|d         }|
t          ||d         ¦  «        k     �r¯t          d|d         |
z
  ¦  «        }|                     d	t          |¦  «        |d
f¬¦  «        }||dd…d	f                  ||dd…df                  ||dd…df                  }}}t          j	        ||z
  ||z
  ¦  «        }t          j
                             |d¬¦  «        }t          j
                             ||z
  d¬¦  «        t          j
                             ||z
  d¬¦  «        z  }|dk    ||dz  k    z  }|                     ¦   «         �ro||         ||df         z  }t          j        |||         z  d¬¦  «         }t          j        ||j        z  |d         z   ¦  «        |dd…df         k                         d	¬¦  «        }t!          t          j        |¦  «        ¦  «        }t!          ||         ¦  «        |	k    r»t!          ||         ¦  «        }	t          j        ||         ||         f         }t          |	t          |¦  «        z  d¦  «        }t'          dt)          j        t)          j        d|d         z
  ¦  «        t)          j        t'          d|d
z  z
  d¦  «        ¦  «        z  ¦  «        ¦  «        }|
|z  }
|
t          ||d         ¦  «        k     �°¯|€dt          j        |t          ¦  «        fS t          j        | |dd
…         z  |d
         z   ¦  «        |k    }t/          d
¦  «        D ]º}|                     ¦   «         |d         k     rdt          j        |t          ¦  «        fc S t1          | |         ¦  «        }|€dt          j        |t          ¦  «        fc S t          j        | |dd
…         z  |d
         z   ¦  «        |k    }|}t          j        ||¦  «        r n|}Œ»||fS )zESample proposals on bounded data, refine and classify on every point.r   Nr   F)Úreplace)Nr   r   r   r   r   rF   )Úsizer?   r   r#   g¼‰Ø—²Òœ<g�íµ ÷Æ°>gÑÜÿÿÿÿï?r   rD   )rU   r%   rZ   Úboolrƒ   ÚsortÚchoicer2   ÚintegersÚcrossr(   r)   r_   r„   rˆ   r�   rŠ   Úargmaxrž   r1   r…   ÚceilÚlogr\   r¢   Úarray_equal)rŸ   Ú
tolerancesÚrngr`   r•   ÚsampleÚqÚtÚbestÚ
best_countÚtrialsÚrequiredÚbatchÚpicksÚxÚyÚzrª   ÚnormsÚedgesÚusablerw   ÚoffsetsÚscoresÚkÚfractionrq   Ú_ÚrefinedÚnew_keeps                                 r,   Úransac_planerÇ      sD  € åˆF‰Œ€AØˆ6Ð)Ô*Ò*Ð*Ø•R”X˜a¥Ñ&Ô&Ð&Ð&ÝŒY�q‰\Œ\€FØˆ6Ð%Ô&Ò&Ð&Ý”˜Ÿš A vÐ.AÔ'BÈE˜ÑRÔRÑSÔSˆØ�&Œ>˜: fÔ-€q€AØ)Ñ€Dˆ*�fØÐ-Ô.€HØ
•3�x Ð(?Ô!@ÑAÔAÒ
AÑ
AÝ�B˜Ð6Ô7¸Ñ>Ñ?Ô?ˆØ—’˜Q¥ A¡¤¨e°A¨Y�Ñ7Ô7ˆØ�E˜!˜!˜!˜A˜#”J”-  5¨¨¨¨1¨¤:¤°°%¸¸¸¸!¸´*´ˆaˆ1ˆÝ”˜˜1™˜Q˜q™SÑ!Ô!ˆÝ”	—’˜u¨1�Ñ-Ô-ˆÝ”	—’˜q ™s¨�Ñ*Ô*­2¬9¯>ª>¸!¸A¹#À1¨>Ñ+EÔ+EÑEˆØ˜%’- E¨E°$©JÒ$6Ñ7ˆØ�:Š:‰<Œ<ñ 	{Ø˜F”m E¨&°¨+Ô$6Ñ6ˆGÝ”v˜g a¨¤iÑ/°QÐ7Ñ7Ô7Ð7ˆGÝ”f˜Q˜wœy™[¨°¬Ñ6Ñ7Ô7¸1¸Q¸Q¸Q¸t¸V¼9ÒD×IÒIÈqÐIÑQÔQˆFÝ•B”I˜fÑ%Ô%Ñ&Ô&ˆAÝ�6˜!”9‰~Œ~ 
Ò*Ð*Ý  ¨¤™^œ^�
µB´E¸'À!¼*ÀWÈQÄZÐ:OÔ4P¨TÝ˜z­#¨a©&¬&Ñ0°Ñ9Ô9�Ý˜r¥4¤9­T¬X°a¸Ð?SÔ8TÑ6TÑ-UÔ-UÕVZÔV^Õ_bÐcdÐemÐopÑepÑcpÐqvÑ_wÔ_wÑVxÔVxÑ-xÑ#yÔ#yÑzÔz�Ø�%‰ˆð# •3�x Ð(?Ô!@ÑAÔAÒ
AÑ
Að$ €|Ø•R”X˜a¥Ñ%Ô%Ð%Ð%ÝŒ6�&˜˜b˜q˜bœ‘/ $ q¤'Ñ)Ñ*Ô*¨jÒ8€DÝ�1‰XŒXð 
ð 
ˆØ�8Š8‰:Œ:˜Ð6Ô7Ò7Ð7Ø�œ !¥DÑ)Ô)Ð)Ð)Ð)Ð)Ý˜F 4œLÑ)Ô)ˆØˆ?Ø�œ !¥DÑ)Ô)Ð)Ð)Ð)Ð)Ý”6˜& ¨¨!¨¤Ñ,¨W°Q¬ZÑ7Ñ8Ô8¸JÒFˆØˆÝŒ>˜$˜xÑ(Ô(ð 	ØˆEØˆˆØ�ˆ:Ðr.   c                ó’  ‡;— t          |€t          n|¦  «        }t          | |||¦  «        \  }}}}}	}
}}t          j        |¦  «        \  }}t          j        ||g¦  «                             t          j        ¦  «        }||         ||         ||         }}}t          ||||	|
|¦  «        \  }}g }t          j	         
                    |¦  «        }t          j        |€t          j        d¦  «        n|¦  «        |�|ndz  }t          j        ||¬¦  «        }t          j        |d¬¦  «        }t          j        dt          j        |¦  «        f         }d}t          j        ||d         k    ¦  «        D �]}|||         ||dz            …         }t%          |d	         ¦  «        D ]É}t'          ||         ||         ||¦  «        \  }} |�|                      ¦   «         |d         k     r n£||          }!|d d…         t          j        ||!         d¬
¦  «        |z
  z  dk    r| }|                     |!|t/          |¦  «        gdœ¦  «         ||           }t1          |¦  «        |d         k     r nŒÊt1          |¦  «        |d         k    r|dz  }�Œg }"t3          |d„ ¬¦  «        D �]P}#d }$|"D �]0}%t5          t7          |#d         d d…         |%d         d d…         z  ¦  «        ¦  «        }&|&t9          j        t9          j        |d         ¦  «        ¦  «        k     rŒo|#d         |%d         }(}'dt7          t          j        t          j        ||'         ||(         f         ¦  «        ¦  «        z  })t          j        t          j        ||'         |%d         d d…         z  |%d         d         z   ¦  «        ¦  «        |)k    �rt          j        t          j        ||(         |#d         d d…         z  |#d         d         z   ¦  «        ¦  «        |)k    rÎt          j        |(|'f         }*tA          ||*         ¦  «        }|�£t          j!        t          j        ||*         |d d…         z  |d         z   ¦  «        ||*         z  d¦  «        dk    rU|d d…         t          j        ||*         d¬
¦  «        |z
  z  dk    r| }|%}$|*|%d<   ||%d<   |%dxx         |#d         z  cc<    n�Œ2|$€|"                     |#¦  «         �ŒRg }+|"D �]�}#|#d         }!|#d         }|!t          j        ||!         |d d…         z  |d         z   ¦  «        ||!         k             }!t1          |!¦  «        |d         k     rŒkt          j"        |!¦  «        |#d<   |# #                    ddgt7          t          j        ||!         ¦  «        ¦  «        t7          t          j        ||!         ¦  «        ¦  «        t7          t          j        t          j$         %                    ||!         |z
  d¬
¦  «        ¦  «        ¦  «        ¬¦  «         t7          t          j        t          j        ||         |!         |d d…         z  ¦  «        ¦  «        ¦  «        },t5          t7          |d d…         |z  ¦  «        ¦  «        }-|# #                    |,|-¬¦  «         |,dk     r|#d                              d¦  «         |-dk    r&|,dk     r d|#d <   |#d                              d!¦  «         t7          |
|         |!                              ¦   «         ¦  «        d"k     r|#d                              d#¦  «         |€|#d                              d$¦  «         |+                     |#¦  «         �Œƒ|+ "                    d%„ ¬¦  «         tM          |+¦  «        D �]Ï\  }(}.|+d |(…         D �]½Š;‰;d          dk    rŒt5          t7          ‰;d         d d…         |.d         d d…         z  ¦  «        ¦  «        t9          j        t9          j        |d&         ¦  «        ¦  «        k     rŒz|‰;d                                       t          j'        ¦  «        }/t          j(        |/d d …df         ¦  «        d'tS          | j*        d         dz
  d¦  «        z  k     rŒçtW          j,        |/¦  «         -                    d(d¦  «        }0t1          |0¦  «        dk     r�Œ$||.d                                       t          j.        ¦  «        }1t          j/        |0d(d¬
¦  «        |0z
  }2|1d d …d d d …f         |0d d d …d d …f         z
  }3|2d d d …df         |3d d …d d …df         z  |2d d d …df         |3d d …d d …df         z  z
  }4|4d)k     0                    d¬
¦  «        |4d*k     0                    d¬
¦  «        z  }5||.d                  ‰;d         d d…         z  ‰;d         d         z   }6tS          d‰;d+         z  d‰;d,         z  ¦  «        }7|6|7 k     |5z  }8|8                     ¦   «         |d-         k    rMd.|.d <   |.d                              d/¦  «         tc          ˆ;fd0„tM          |+¦  «        D ¦   «         ¦  «        |.d1<    n�Œ¿�ŒÑtM          |+¦  «        D ]ý\  }9}#t          j        ||#d                  |#d         d d…         z  |#d         d         z   ¦  «        }:|# #                    |9t1          |#d         ¦  «        t7          t          j        |:¦  «        ¦  «        t7          t          j!        |:d2¦  «        ¦  «        t7          ||#d                   )                    ¦   «         ¦  «        t3          te          |#d         ¦  «        ¦  «        ¬3¦  «         Œþ|||||+||| 3                    ¦   «         r't7          |
|                              ¦   «         ¦  «        nd4|�d5nd6d7œ	S )8NrF   r=   )Ú	minlengthÚstable©Úkindr   r   r?   r   r#   )ÚindicesÚplaneÚregionsc                ó|   — t          | d         ¦  «         t          | d                              ¦   «         ¦  «        fS )NrÍ   ©rU   rŠ   r2   ©Úfs    r,   ú<lambda>zdecompose_roi.<locals>.<lambda>Í   s2   € µ°Q°y´\Ñ1BÔ1BÐ0BÅ3ÀqÈÄ|×GWÒGWÑGYÔGYÑCZÔCZÐ/[€ r.   )ÚkeyrÎ   r   rÍ   r   éZ   g      ø?rÏ   r   Úselected_candidateÚforkability_not_confirmed)ÚstatusÚreasonsrk   rh   Úmedian_range)Úview_normal_cosineÚimage_vertical_normal_cosiner   rÚ   Úgrazing_viewgš™™™™™é?g333333ã?Údiagnostic_onlyrÙ   Úpossible_top_or_ground_facer   Úlimited_reliable_normalsÚ unit_unknown_relative_thresholdsc                óŠ   — | d         t          | d         ¦  «         t          | d                              ¦   «         ¦  «        fS )NrÛ   rÍ   rÑ   rÒ   s    r,   rÔ   zdecompose_roi.<locals>.<lambda>ñ   s9   € ˜Q˜~Ô.µ°A°i´LÑ0AÔ0AÐ/AÅ#ÀaÈ	Äl×FVÒFVÑFXÔFXÑBYÔBYÐZ€ r.   r   g333333Ó?r"   gñhãˆµøä¾gñhãˆµøä>rk   rh   r   Úrejected_backgroundÚbehind_parallel_front_supportc              3  ó*   •K  — | ]\  }}|‰u ¯	|V — Œd S )N© )Ú.0rÂ   rÓ   Úfronts      €r,   ú	<genexpr>z decompose_roi.<locals>.<genexpr>  s3   øè è € Ð1^Ð1^¹¸¸!ÐSTÐX]ÐS]ÐS]°!ÐS]ÐS]ÐS]ÐS]Ð1^Ð1^r.   Úoccluding_face_indexé_   )Úcandidate_indexrŸ   Úresidual_medianÚresidual_p95Úmaximum_point_tolerancerÏ   rG   ÚmmÚraw_unknown)	ÚuvÚxyz_valid_maskÚpoints_workr¯   ÚfacesrÏ   Úregions_plane_limit_reachedÚnormal_valid_fractionÚgeometry_units)4ÚdictÚDEFAULTSr~   r%   ÚnonzeroÚcolumn_stackÚastyper‚   r›   ÚrandomÚdefault_rngrI   rZ   ÚbincountÚargsortrž   ÚcumsumÚflatnonzeror\   rÇ   r„   rs   r‰   rŠ   rU   Úsortedrˆ   rV   r…   r†   r‡   rW   r¢   Ú
percentiler§   Úupdater(   r)   rN   Úfloat32Úptpr1   rK   Úcv2Ú
convexHullÚreshaperJ   Úrollr'   ÚnextÚsetr_   )<r+   r`   ra   rb   r   rd   rc   rh   rk   rw   rx   ry   r}   ÚvvÚuuró   rŸ   ÚtsÚssrÏ   r™   Ú
candidatesr°   Úorigin_workÚsizesÚorderÚ
boundariesÚexhausted_regionsÚregionÚ	remainingrÄ   rÎ   rq   ÚindsÚmergedÚfaceÚ
compatibleÚpriorÚanglerf   ÚjÚlimitÚtogetherrö   r”   Úvertical_cosineÚbackÚfuÚhullÚbur¾   rp   ÚsignsÚinsideÚsignedÚ
separationÚbehindrÂ   Úresidualré   s<                                                              @r,   Údecompose_roir0  ­   s²  ø€ Ý˜f˜n•(�(°&Ñ9Ô9€FÝEWÐX[Ð\bÐcnÐouÑEvÔEvÑB€A€tˆW�i ¨(°D¸(ÝŒZ˜ÑÔ�F€BˆÝ	Œ˜"˜R˜Ñ	!Ô	!×	(Ò	(­¬Ñ	2Ô	2€BØ�t”W˜i¨œo¨w°t¬}�ˆB€FÝ% a¨¨W°W¸XÀfÑMÔM�N€GˆUØ€JÝ
Œ)×
Ò
 Ñ
%Ô
%€CÝ”*¨F¨N�RœX a™[œ[˜[ÀÑGÔGÐXcÐXoÈÈÐuwÑx€KÝŒK˜¨%Ð0Ñ0Ô0€EåŒJ�w HÐ-Ñ-Ô-€E½B¼EÀ!ÅBÄIÈeÑDTÔDTÐBTÔ<U¨zØÐÝ”. ¨&Ð1HÔ*IÒ!IÑJÔJð 'ñ 'ˆØ˜* VÔ,¨Z¸¸q¹Ô-AÐAÔBˆ	Ý�vÐ9Ô:Ñ;Ô;ð 	'ð 	'ˆAÝ& v¨iÔ'8¸¸I¼ÀsÈ6ÑRÔR‰KˆE�4Øˆ} §¢¡
¤
¨VÐ4KÔ-LÒ LÐ LØ�Ø˜T”?ˆDØ�R�a�RŒy�"œ' &¨¤,°AÐ6Ñ6Ô6°{ÑBÑCÀAÒEÐEØ˜�Ø×Ò¨°eÅsÈ6Á{Ä{ÀmÐTÐTÑUÔUÐUØ! 4 %Ô(ˆIÝ�9‰~Œ~ Ð'>Ô ?Ò?Ð?Ø�ð @õ �9‰~Œ~ Ð(?Ô!@Ò@Ð@Ø! QÑ&Ð!ùà€FÝ�zÐ&[Ð&[Ð\Ñ\Ô\ð 2ñ 2ˆØˆ
Øð 	<ñ 	<ˆEÝ�˜d 7œm¨B¨Q¨BÔ/°°g´¸rÀ¸rÔ0BÑBÑCÔCÑDÔDˆEØ•t”x¥¤¨VÐ4LÔ-MÑ NÔ NÑOÔOÒOÐOØØ�Y”  iÔ 0ˆaˆAØ•E�"œ)¥B¤E¨"¨Q¬%°°1´¨+Ô$6Ñ7Ô7Ñ8Ô8Ñ8ˆEÝ”	�"œ& ¨¤¨5°¬>¸"¸1¸"Ô+=Ñ!=¸eÀG¼nÈQÔ>OÑ!OÑPÔPÑQÔQÐUZÒZÑZÝœ	¥"¤&¨°¬°4¸´=ÀÀ!ÀÔ3DÑ)DÀTÈ'Ä]ÐSTÔEUÑ)UÑ"VÔ"VÑWÔWÐ[`Ò`Ð`Ýœ˜q ˜sœ�Ý  xÔ 0Ñ1Ô1�ØÐ$­¬µr´v¸fÀXÔ>NÈuÐUWÐVWÐUWÌyÑ>XÐY^Ð_`ÔYaÑ>aÑ7bÔ7bÐceÐfnÔcoÑ7oÐprÑ)sÔ)sÐuxÒ)xÐ)xØ˜R˜a˜R”y¥"¤'¨&°Ô*:ÀÐ"BÑ"BÔ"BÀ;Ñ"NÑOÐPQÒQÐQÐY^ÐX^ÐRWØ$�J°h U¨9Ñ%5Èe¸uÀW¹~Ø˜)Ð$Ð$Ô$ d¨9¤oÑ5Ð$Ð$Ñ$°e°eùØÐ˜fŸmšm¨DÑ1Ô1Ð1ùØ
€EØð ñ ˆØ�)Œ_ˆ 4¨¤=˜Uà•"”&˜ œ e¨B¨Q¨B¤iÑ/°°a´Ñ8Ñ9Ô9¸RÀ¼XÒEÔFˆÝˆt‰9Œ9�VÐ1Ô2Ò2Ð2°8Ýœ ™œˆˆY‰Ø�ŠÐ/Ð9TÐ8UÝ#¥B¤I¨b°¬hÑ$7Ô$7Ñ8Ô8ÅÅrÄyÐQSÐTXÔQYÑGZÔGZÑA[ÔA[Ý!&¥r¤yµ´·²ÀÀtÄÈ[Ñ@XÐ^_°Ñ1`Ô1`Ñ'aÔ'aÑ!bÔ!bð 	ñ 	dô 	dð 	dõ •R”Y�rœv d¨4¤j°Ô&6°u¸R¸a¸R´yÑ&@ÑAÔAÑBÔBÑCÔCˆÝ�E %¨¨¨¤)¨HÑ"4Ñ5Ô5Ñ6Ô6ˆØ�Š vÈOˆÑ\Ô\Ð\Ø�"Š9ˆ9�T˜)”_×+Ò+¨NÑ;Ô;Ð;Ø˜2ÒÐ &¨¢) )Ø,ˆD�‰N¨T°)¬_×-CÒ-CÐDaÑ-bÔ-bÐ-bÝ�˜$” Ô%×*Ò*Ñ,Ô,Ñ-Ô-¨bÒ0Ð0°°i´×1GÒ1GÐHbÑ1cÔ1cÐ1cØÐ˜t Iœ×5Ò5Ð6XÑYÔYÐYØ�Š�TÑÔÐÑØ	‡J‚JÐZÐZ€JÑ[Ô[Ð[Ý˜EÑ"Ô"ð ñ ‰ˆˆ$Ø˜2˜A˜2”Yð 	ñ 	ˆEØ�XŒÐ 4Ò4Ð4°XÝ•5˜˜wœ¨¨¨Ô+¨D°¬M¸"¸1¸"Ô,=Ñ=Ñ>Ô>Ñ?Ô?ÅÄÍÌÐV\ð  ^Aô  WBñ  JCô  JCñ  ADô  ADò  Dð  Dð  EMØ�%˜	Ô"Ô#×*Ò*­2¬:Ñ6Ô6ˆBÝŒv�b˜˜˜˜1˜”g‰Œ˜r¥# c¤i°¤l°1¡n°QÑ"7Ô"7Ñ7Ò7Ð7¸Ý” Ñ#Ô#×+Ò+¨B¨qÑ1Ô1ˆDÝ�4‰yŒy˜Š{ˆ{™8à�$�y”/Ô"×)Ò)­"¬*Ñ5Ô5ˆBÝ”'˜$˜r qÐ)Ñ)Ô)¨$Ñ.ˆEØ�Q�Q�Q�t˜A˜A˜A�X”,˜t D¨¨¨¨1¨1¨1 Hœ~Ñ-ˆEØ˜˜Q˜Q˜Q˜q˜”/ %¨¨¨¨!¨!¨!¨A¨¤,Ñ.¨u°T¸!¸!¸!¸A°X¬¸uÀQÀQÀQÀqÀqÀqÈÀU¼|Ñ/KÑKˆEØ˜5’L×%Ò%¨1Ð%Ñ-Ô-¨u°dª{×.?Ò.?ÀQÐ.?Ñ.GÔ.GÑGˆFØ˜$˜yœ/Ô*¨5°¬>¸"¸1¸"Ô+=Ñ=¸eÀG¼nÈQÔ>OÑOˆFÝ˜1˜U ;Ô/Ñ/°°%¸	Ô2BÑ0BÑCÔCˆJØ˜j˜[Ò(¨&Ñ0ˆFØ�{Š{‰}Œ}˜fÐ%:Ô;Ò;Ð;Ø4��X‘°T¸)´_×5KÒ5KÐLkÑ5lÔ5lÐ5lÝ-1Ð1^Ð1^Ð1^Ð1^½yÈÑ?OÔ?OÐ1^Ñ1^Ô1^Ñ-^Ô-^�Ð+Ñ,Ø�ñ <ùõ ˜EÑ"Ô"ð :ð :‰ˆˆ$Ý”˜˜t IœÔ/°°W´¸b¸q¸bÔ0AÑAÀ$ÀwÄ-ÐPQÔBRÑRÑSÔSˆØ�Š A­S°°i´Ñ-AÔ-AÕRWÕXZÔXaÐbjÑXkÔXkÑRlÔRlÝ!&¥r¤}°X¸bÑ'AÔ'AÑ!BÔ!BÝ,1°"°T¸)´_Ô2E×2IÒ2IÑ2KÔ2KÑ,LÔ,LÝ"¥3 t¨I¤Ñ#7Ô#7Ñ8Ô8ð 	ñ 	:ô 	:ð 	:ð 	:ð  T¸ÈBÐW\ØÐ:KØDHÇHÂHÁJÄJÐ$V¥E¨(°4¬.×*=Ò*=Ñ*?Ô*?Ñ$@Ô$@Ð$@ÐTVØ%0Ð%<˜T˜TÀ-ðQð Qð Qr.   c                ó  — t          j        |t           j        ¬¦  «                             ¦   «         }t          j        |dk    ¦  «        }t          |¦  «        s!|t          j        dt           j        ¦  «        fS t          j        | ¦  «        }t          j        ||         ddd¬¦  «        \  }}}	t          j        |d¬¦  «        }
t           j	        dt          j
        |	¦  «        f         }g }t          j        |	dk    ¦  «        D ]Ñ}||
||         ||dz            …                  }t          t          ||         ¦  «        ¦  «        dk    rŒH|t          j        t          j        |¦  «        |         d¬¦  «                 }||d                  ||d                  z
  |k    rd||dd	…         <   Œ«d
||<   |                     ||d                  ¦  «         ŒÒ|t          j        |t           j        ¬¦  «                             d
d¦  «        fS )zGFace ID >0: unique owner; -1: unresolved conflict; 0: no selected face.r;   r   )r   r   T)r$   Úreturn_inverseÚreturn_countsrÊ   rË   r?   Nr"   r   )r%   rI   r‚   Úcopyr  rU   r‹   Úuniquer  rž   r  r  r‰   r  )Úpoint_uvÚface_assignmentsÚnormalized_residualsÚmarginÚassignedÚactiveró   rÄ   Úgroupsrl   r  ÚboundsÚ	conflictsÚgr  Úrankeds                   r,   Úresolve_ownershiprA    sÓ  € åŒZÐ(­r¬xÐ8Ñ8Ô8×=Ò=Ñ?Ô?€HÝŒ>˜( 1š*Ñ%Ô%€FÝˆv‰;Œ;Ð?˜h¥r¤x°µb´hÑ'?Ô'?Ð?Ð?Ý	„z�(ÑÔ€BÝ”I˜b œj¨a¸tÐRVÐWÑWÔW�O€A€fˆVÝ
Œ*�V Ð
*Ñ
*Ô
*€Eµ"´%¸½"¼)ÀFÑ:KÔ:KÐ8KÔ2L¨6Ø€IÝŒ^˜F 1šHÑ%Ô%ð <ð <ˆØ�E˜& œ) F¨1¨Q©3¤KÐ/Ô0Ô1ˆÝ�s�8˜D”>Ñ"Ô"Ñ#Ô# QÒ&Ð& xØ•B”J�rœzÐ*>Ñ?Ô?ÀÔEÈ8ÐTÑTÔTÔUˆØ  q¤	Ô*Ð+?ÀÀqÄ	Ô+JÑJÈ6ÒQÐQØ!"ˆH�V˜A˜B˜B”ZÑ Ð àˆH�T‰N˜i×.Ò.¨r°$°q´'¬{Ñ;Ô;Ð;Ð;Ø•B”J˜y­r¬xÐ8Ñ8Ô8×@Ò@ÀÀAÑFÔFÐFÐFr.   )NN)NNNr   )r   )Ú__doc__Ú
__future__r   r…   rP   r
  Únumpyr%   Úscipy.sparser   Úscipy.sparse.csgraphr   rû   r-   r9   r~   r›   r¢   rÇ   r0  rA  rç   r.   r,   ú<module>rG     sø  ððð ð
 #Ð "Ð "Ð "Ð "Ð "à €€€Ø €€€à 
€
€
€
Ø Ð Ð Ð Ø #Ð #Ð #Ð #Ð #Ð #Ø 5Ð 5Ð 5Ð 5Ð 5Ð 5ðØ
ˆBðØ% sðØ,BÀDðà˜sðà$:¸Cðð ˜Dðð #:¸2ðð ˜2ð	ð  7¸ð	ð
 ˜%ðð
 "5°dðð   ðð &>¸sðð (¨ðð 0EÀcðð   ðð ˜Aðð <=Ø'+ðð ð €ð`ð `ð `ðð ð ð3Jð 3Jð 3Jð 3Jðlð ð ð,ð ð ð+ð +ð +ð\cQð cQð cQð cQðLGð Gð Gð Gð Gð Gr.   