
    Hj                     |    d Z ddlZddlmZmZ ddlmZ ddl	m
Z
 ddlZddZd ZddZdd	Zdd
ZddZddZd Zy)u   
weld_seam.py — 블럭: STL→포인트클라우드 + 심검출 + 경로 생성
(welding_pipeline.py에서 2026-07-04 분할 — 로직 원문 그대로, 수정 없음)
출처: romi-lab/robotic-welding-demo (demo_all.py)
    N)splprepsplev)Rotation)KDTreec                 X   ddl }|j                  |       }|j                  j                  ||      \  }}|j                  |   }t
        j                  j                         }t
        j                  j                  |      |_
        t
        j                  j                  |      |_        |S )u:   trimesh로 STL 샘플링 → open3d 포인트 클라우드r   N)trimeshloadsamplesample_surfaceface_normalso3dgeometry
PointCloudutilityVector3dVectorpointsnormals)stl_pathsample_pointsr   meshptsface_idxr   pcds           U   E:\도진팩토리\3D스캔및티칭시스템\02_TOOLS\welding_auto_path\weld_seam.pystl_to_pointcloudr      s    <<!DNN11$FMC)G
,,
!
!
#C++C0CJ++,,W5CKJ    c                    t        |j                        }t        j                  |j                        }t
        j                  j                  |      }d}|dz  }t        j                  |      }| dk(  rt        |      D ]  }	|j                  |j                  |	   |      \  }
}}|
dk  r,t        j                  |t        |      ddf   d      }t        j                  |||	ddf         }t        j                  j                  ||	ddf         }|dk  r|||z  z  ||	ddf   z  }t        j                  j                  ||z
        ||	<    |S )	u9   법선벡터 비대칭도(asymmetry)로 홈 엣지 검출      	asymmetry   Nr   axis&.>)lenr   npasarrayr   r   r   KDTreeFlannzerosrangesearch_radius_vector_3dmeanlistdotlinalgnorm)featurer   
voxel_size	pc_numbern_listpcd_treeneighborradiusfeature_valueindexkidx_vectorr.   r0   projs                    r   find_feature_valuer?   !   s-   CJJIZZ$F||'',HH!^FHHY'M+9%E"::3::e;LfUKQQ1uWWVDIqL1:F&&q!12C99>>&"23Dd{$+&q)99D#%99>>&4-#@M%  & r   c                    t        j                  |      |z  }t        j                  ||kD        d   }t        j                  | j                        |   }t
        j                  j                         }t
        j                  j                  |      |_        t        j                  |j                  ||            }	t        |	      dk(  s|	j                         dk  r|S t        j                  t        j                  |	|	dk\                 }
|	|
k(  }t
        j                  j                         }t
        j                  j                  ||         |_        |S )u8   비대칭도 높은 포인트 → DBSCAN 클러스터링r   )eps
min_points)r&   maxwherer'   r   r   r   r   r   r   arraycluster_dbscanr%   argmaxbincount)r   r8   threshold_ratiorA   rB   	thresholdhigh_idxr   
groove_pcdlabelsbestmaskresults                r   cluster_grooverQ   ;   s   }%7Ixx	1215H
**SZZ
 
*C((*J2237JXXj//CJ/OPF
6{a6::<!+ 99R[[!!456DT>D\\$$&FKK..s4y9FMMr   c                     t        j                  | d      }| |z
  }t         j                  j                  |      \  }}}|d   }|j	                  |      }t        j
                  ||      |z   }||fS )uC   SVD로 포인트들을 회귀선 위로 투영 → 중심선 추출r   r"   )r&   r,   r/   svdr.   outer)	r   	thicknesscentroidcenteredr<   vh	directiont_vals	projecteds	            r   	thin_liner\   P   sm    wwvA&H Hyy}}X&HAq"1I \\)$F+h6Iir   c           	      z   t        t        t        |                   }| d   g}|j                  d       t	        |       }|rf|d   }|j                  |t        dt        |      dz               \  }}|D ]-  }||v s|j                  | |          |j                  |        n |rft        j                  |      S )u   KDTree로 순차 정렬r   
      )r:   )
r-   r*   r%   remover   queryminappendr&   rE   )	r   step	remaining
sorted_ptstreelastdistsidxsr;   s	            r   sort_pointsrl   ]   s    U3v;'(I)JQ&>D
"~jjRY!1C)DjEtCi!!&+.  %	   88Jr   c           
          | j                   }t        ||t        dt        |       dz
              \  }}t	        j
                  dd|      }t	        j                  t        ||            j                   }|S )u6   B-스플라인 보간으로 부드러운 경로 생성r!   r`   )sr:   r   )Tr   rc   r%   r&   linspacerE   r   )sorted_pointssmooth_factor
num_outputr   tckuu_newsmooths           r   generate_trajectoryrx   p   s`    
//CSMSC4F4J-KLFCKK1j)EXXeE3'(**FMr   c                     | j                  ddd      \  }}t        j                  |dd       }|t        j                  j	                  |      z  }|S )u+   RANSAC 평면 피팅으로 면법선 추출{Gz?r!   i  )distance_thresholdransac_nnum_iterationsN)segment_planer&   rE   r/   r0   )r   methodplane_modelr<   normals        r   find_normalr   y   sV    && ' NK
 XXk"1o&F
biinnV$$FMr   c                    g }|t         j                  j                  |      z  }t        t	        |       dz
        D ]  }| |dz      | |   z
  }t        j
                  ||      |z  }||z
  }t         j                  j                  |      }|dk  rt        j                  g d      }n||z  }t        j                  ||      }	|	t         j                  j                  |	      z  }	t        j                  ||	|g      }
t        j                  |
      }|j                  | |   |f        |j                  | d   |d   d   f       |S )uL   경로 접선 + 면법선 → 6DOF 토치 방향 (romi-lab find_orientation)r`   r$   )g      ?        r   r^   )r&   r/   r0   r*   r%   r.   rE   crosscolumn_stackRfrom_matrixrd   )trajectory_ptssurface_normalposesz_diripos_diffr>   x_dirr0   y_dir
rot_matrixrs               r   find_orientationr      s"   ERYY^^N;;E3~&*+!!a%(>!+<<vvh&.4yy~~e$$;HH_-ETME&&&__eUE%:;
MM*%nQ'+, ,$ 
LL.$eBil34Lr   )i )gffffff?rz   r_   )g      ?)g{Gzt?)r   d   )ransac)__doc__numpyr&   scipy.interpolater   r   scipy.spatial.transformr   r   scipy.spatialr   open3dr   r   r?   rQ   r\   rl   rx   r   r    r   r   <module>r      sC   
  , 1   	 4*
  &	r   