
    l0j*                        d Z ddlZddlmZmZmZ ddlmZ ddl	m
Z
mZ ddlmZ ddlmZmZmZ dd	lmZmZmZ 	 ddlmZ dd
lmZ n8# e$ r0ZddlmZ  ej        e          Z ej        e          ZY dZ[ndZ[ww xY w	 	 	 	 d*dedededefdZ	 	 	 	 	 d+dededee         dedededefdZd,dZ d  Z!d! Z"	 	 	 	 	 	 	 	 	 	 d-d%Z#	 	 	 	 	 d.d&Z$	 	 	 d/d'Z%	 	 	 	 	 	 	 	 	 d0d)Z&dS )1za
registration.py
---------------

Functions for registering (aligning) point clouds with meshes.
    N   )boundstransformationsutil)weighted_vertex_normals)
PointCloud	plane_fit)transform_points)anglescrossnormals)	ArrayLikeIntegerOptional)cKDTree)
exceptions  F
   2   samplesscale	icp_first	icp_finalc                 d   d }t          j        | d          st          d          d}| }	t          j        |d          rit          | j                  t          |j                  k    r|}	d} || |          }
| }n|} |||          }
|j        r|j        }nN|j        j        }nAt          j        |d          r|}
t          j
        |
          d         }nt          d	          |	j        r|	j        }n|	j        j        }t          j        t          j                            |          |          }t          j        d
 g dg dg dg dg dg dg dg dfD                       }t          j        t          |                    t          j        z  }dgt          |          z  }|	j        }t'          |          D ]w\  }}t          j        t)          j        ||          t          j                            |                    }t-          d|
|	|t/          |          |d|\  }}}|||<   |||<   xt-          d|
|	|t          j        |                   t/          |          |d|\  }}}|t          |
          z  }|r t          j                            |          }n|}||fS )a  
    Align a mesh with another mesh or a PointCloud using
    the principal axes of inertia as a starting point which
    is refined by iterative closest point.

    Parameters
    ------------
    mesh : trimesh.Trimesh object
      Mesh to align with other
    other : trimesh.Trimesh or (n, 3) float
      Mesh or points in space
    samples : int
      Number of samples from mesh surface to align
    scale : bool
      Allow scaling in transform
    icp_first : int
      How many ICP iterations for the 9 possible
      combinations of sign flippage
    icp_final : int
      How many ICP iterations for the closest
      candidate from the wider search
    kwargs : dict
      Passed through to `icp`, which passes through to `procrustes`

    Returns
    -----------
    mesh_to_other : (4, 4) float
      Transform to align mesh to the other object
    cost : float
      Average squared distance per point
    c           	          t          | j                  |dz  k     rCt          j        | j        |                     |t          | j                  z
            f          S |                     |          S )z
        Return a combination of mesh vertices and surface samples
        with vertices chosen by likelihood to be important
        to registration.
           )lenverticesnpvstacksamplemcounts     W/home/wildlama/miniconda3/envs/lam/lib/python3.11/site-packages/trimesh/registration.py
key_pointszmesh_other.<locals>.key_pointsF   s[     qz??eai((9aj!((53qz??3J*K*KLMMM88E??"    Trimeshzmesh must be Trimesh object!TFr"      r   z$other must be mesh or (n, 3) points!c                 b    g | ],}t          j        d           t          j        |d          z  -S )   r   )r   eyeappend).0diags     r%   
<listcomp>zmesh_other.<locals>.<listcomp>   sA     	
 	
 	
 F1II	$***	
 	
 	
r'   )r   r   r   )r   r   r*   )r   r*   r   )r*   r   r   )r*   r*   r   )r*   r   r*   )r   r*   r*   )r*   r*   r*   N)abinitialmax_iterationsr    )r   is_instance_named
ValueErrorr   r   	is_volumeprincipal_inertia_transformbounding_box_orientedis_shaper   oriented_boundsr   dotlinalginvarrayonesinfcentroid	enumerater   transform_aroundicpintargmin)meshotherr   r   r   r   kwargsr&   inversesearchpointspoints_mesh
points_PIT
search_PITsearch_to_pointscubescosts
transformsrE   iflipa_to_bmatrix_junkcostmesh_to_others                             r%   
mesh_otherr_      s9   R	# 	# 	# !$	22 97888GFeY// At}EN 3 333 FGZ$g666FKKKZ%w777F  	W$@JJ$:VJJ	ug	&	& A+F33A6

?@@@  N7

1M

 vbimmJ77DD H	
 	
 		





		
 	
 	
 E" GCJJ"&(E#e**$JHU##  4 ,T8<<IMM*++
 
 " 
y>>
 
 
 
t 
1a  

29U++,9~~   FE4 	CKKD
  	f--$r'   Tr3   r4   weights
reflectiontranslationreturn_costc                 v   t          j        | t           j                  } t          j        |t           j                  }t          j        | d          rt          j        |d          st          d          t          |           t          |          k    rt          d          |yt          j        |t           j                  }t          |          t          |           k    rt          d          ||                                z                      d          }|rI|| 	                    d	          }	n| |z                      d	          }	|	                    d	          }
n>t          j
        | j        d
                   }	t          j
        |j        d
                   }
|r|=t          j        | |	z
  dz                                  t          |           z            }n/t          j        | |	z
  dz  |z                                            }t          j        ||
z
  dz                                  t          |          z            }nd
}d
}|'t          j        ||
z
  |z  j        | |	z
  |z            }n<t          j        ||
z
  |z  j        | |	z
  |z  |                    d          z            }t           j                            |          \  }}}|rt          j        ||          }nmt          j        t          j        |t          j        d
d
t           j                            t          j        ||                    g                    |          }|
||z  t          j        ||	          z  z
  }t          j        ||z  |z  |                    dd
          f          }t          j        |t          j        dg| j        d
         z  dgz                                 d
d          f          }|r/t-          | |          }||z
  dz  	                                }|||fS |S )aj  
    Perform Procrustes' analysis to quickly align two corresponding
    point clouds subject to constraints. This is much cheaper than
    any other registration method but only applies if the two inputs
    correspond in order.

    Finds the transformation T mapping a to b which minimizes the
    square sum distances between Ta and b, also called the cost.

    Optionally specify different weights for the points in a to minimize
    the weighted square sum distances between Ta and b, which can
    improve transformation robustness on noisy data if the points'
    probability distribution is known.

    Parameters
    ----------
    a : (n,3) float
      List of points in space
    b : (n,3) float
      List of points in space
    weights : (n,) float
      List of floats representing how much weight is assigned to each point of a
    reflection : bool
      If the transformation is allowed reflections
    translation : bool
      If the transformation is allowed translation and rotation.
    scale : bool
      If the transformation is allowed scaling
    return_cost : bool
      Whether to return the cost and transformed a as well

    Returns
    ----------
    matrix : (4,4) float
      The transformation matrix sending a to b
    transformed : (n,3) float
      The image of a under the transformation
    cost : float
      The cost of the transformation
    dtyper)   points must be (n,3)!z+a and b must contain same number of points!Nz)weights must have same length as a and b!)r*   r   r   axisr   r   r*                 ?)r   
asanyarrayfloat64r   r=   r9   r   sumreshapemeanzerosshapesqrtr?   Tr@   svdr1   dethstackr    rB   r
   )r3   r4   r`   ra   rb   r   rc   ww_normacenterbcenterascalebscaletargetu_svhRr[   transformedr]   s                        r%   
procrustesr      s   d 	arz***A
arz***A=G$$ 2DM!W,E,E 20111
1vvQFGGGM'444q66SVVHIIIaeegg+&&w//  	'?ff!fnnGG6z&&A&..G&&a&..(171:&&(171:&&  ?Wq7{q05577#a&&@AAFF
 WG1V;@@BBCCF1w;1,1133c!ff<== !g+/2a'kV5KMM'kV#&!g+)?199WCUCU(U
 
 	f%%IAr2 QF1bMM F26!RWaBIMM"&B--,H,H%IJJKKRPP
 Vf_q'0B0BBBKY!+[-@-@Q-G-GHIIFY#!'!*)=)E F F N NqRT U UVWWF &q&11[Q&,,..{D((r'   h㈵>   c                    t          j        | t           j                  } t          j        | d          st          d          |t          j        d          }t          j        |d          }|sSt          j        |t           j                  }t          j        |d          st          d          t          |          }t          | |          } |}t           j
        }	t          |          D ]{}
|r|j                            |           \  }}}n!|                    | d          \  }}||         }t          d	| |d|\  }}}|} t          j        ||          }|	|z
  |k     r n|}	||||fS )
a  
    Apply the iterative closest point algorithm to align a point cloud with
    another point cloud or mesh. Will only produce reasonable results if the
    initial transformation is roughly correct. Initial transformation can be
    found by applying Procrustes' analysis to a suitable set of landmark
    points (often picked manually).

    Parameters
    ----------
    a : (n,3) float
      List of points in space.
    b : (m,3) float or Trimesh
      List of points in space or mesh.
    initial : (4,4) float
      Initial transformation.
    threshold : float
      Stop when change in cost is less than threshold
    max_iterations : int
      Maximum number of iterations
    kwargs : dict
      Args to pass to procrustes

    Returns
    ----------
    matrix : (4,4) float
      The transformation matrix sending a to b
    transformed : (n,3) float
      The image of a under the transformation
    cost : float
      The cost of the transformation
    re   r)   rg   Nr-   r(   r   )r3   r4   r7   )r   rl   rm   r   r=   r9   r.   r8   r   r
   rD   rangenearest
on_surfacequeryr   r?   )r3   r4   r5   	thresholdr6   rM   is_meshbtreetotal_matrixold_cost_closest	_distance_faces
_distancesixr[   r   r]   s                      r%   rH   rH   ?  s   B 	arz***A=G$$ 20111&))$Q	22G M!2:...}Q(( 	64555

 	G$$AL vH >""   	)*)=)=a)@)@&GY"[[A..NJeG %/$Hg$H$H$H$H!T vfl33d?Y&&EHHd**r'   c                 \   t          j        |d          s8t          |t                    s#t	          j        |          }t          |          }| j        | j        }}| j        |d d d f         z
  |z  | _        |j        |d d d f         z
  |z  |_        |||d d d f         z
  |z  }||||fS )Nr(   )	r   r8   
isinstancer   r   rl   rE   r   r   )source_meshtarget_geometrytarget_positionsr   rE   r   s         r%   _normalize_by_sourcer     s     !/9== /jG G / =11$X..!*K,=eH'08D!!!G3DDMK / 88D!!!G;L LPUUO#,xaaa/@@EI,h==r'   c                    | j         z  d d d f         z   | _         |j         z  d d d f         z   |_         ||z  d d d f         z   }t          |t                    rfd|D             }n|z  d d d f         z   }|S )Nc                 4    g | ]}|z  d d d f         z   S )Nr7   )r0   xrE   r   s     r%   r2   z*_denormalize_by_source.<locals>.<listcomp>  s/    @@@A%!)htQQQw//@@@r'   )r   r   list)r   r   r   resultrE   r   s       ``r%   _denormalize_by_sourcer     s     !;#77(47:KKK$'??(4QRQRQR7BSSO# #33htQQQw6GG&$ 4@@@@@@@@(47"33Mr'   -C6?皙?   c                    d }d }d }d }d }t          | ||          \  }}}}t          | j                  }t          | j                  }| j                                        } || d          }t          j        ddd|g          }t          j        ||          } || j                  } || j	                  } ||          } ||| ||          \  }}|g d	g d
g dg dg}|r|g}|D ]\  }} }!}"|	s|dk     rd}!t          j
        t
          j                  j        }#t          j
        t
          j                  j        }$d}%|#|$z
  |k    rK|"|%|"k     rBt          |||	 |!dk    |!dk    o|
|          }&t          j        |          }'d|'|&d         |k    <   |!dk    rhd|&v rd|&d         }(|
rd|&v r|&d         }(||z  })t!          j        |)|(          }*|	rt          j        |*dd          nt          j        |*          }*|'|*|!z  z  }' ||||'|&d         |||||| 
  
        }||z  }|$}#t
          j                            |&d         |z
  d          }+|+|'z                                  }$|r|                    |           |%dz  }%|#|$z
  |k    r
|";|%|"k     B|r|},n|},t1          | |||,||          },|,S )a  
    Non Rigid Iterative Closest Points

    Implementation of "Amberg et al. 2007: Optimal Step
    Nonrigid ICP Algorithms for Surface Registration."
    Allows to register non-rigidly a mesh on another or
    on a point cloud. The core algorithm is explained
    at the end of page 3 of the paper.

    Comparison between nricp_amberg and nricp_sumner:
    * nricp_amberg fits to the target mesh in less steps
    * nricp_amberg can generate sharp edges
      * only vertices and their neighbors are considered
    * nricp_sumner tend to preserve more the original shape
    * nricp_sumner parameters are easier to tune
    * nricp_sumner solves for triangle positions whereas
      nricp_amberg solves for vertex transforms
    * nricp_sumner is less optimized when wn > 0

    Parameters
    ----------
    source_mesh : Trimesh
        Source mesh containing both vertices and faces.
    target_geometry : Trimesh or PointCloud or (n, 3) float
        Target geometry. It can contain no faces or be a PointCloud.
    source_landmarks : (n,) int or ((n,) int, (n, 3) float)
        n landmarks on the the source mesh.
        Represented as vertex indices (n,) int.
        It can also be represented as a tuple of triangle
        indices and barycentric coordinates ((n,) int, (n, 3) float,).
    target_positions : (n, 3) float
        Target positions assigned to source landmarks
    steps : Core parameters of the algorithm
        Iterable of iterables (ws, wl, wn, max_iter,).
        ws is smoothness term, wl weights landmark importance, wn normal importance
        and max_iter is the maximum number of iterations per step.
    eps : float
        If the error decrease if inferior to this value, the current step ends.
    gamma : float
        Weight the translation part against the rotational/skew part.
        Recommended value : 1.
    distance_threshold : float
        Distance threshold to account for a vertex match or not.
    return_records : bool
        If True, also returns all the intermediate results. It can help debugging
        and tune the parameters to match a specific case.
    use_faces : bool
        If True and if target geometry has faces, use proximity.closest_point to find
        matching points. Else use scipy's cKDTree object.
    use_vertex_normals : bool
        If True and if target geometry has faces, interpolate the normals of the target
        geometry matching points.
        Else use face normals or estimated normals if target geometry has no faces.
    neighbors_count : int
        number of neighbors used for normal estimation. Only used if target geometry has
        no faces or if use_faces is False.

    Returns
    ----------
    result : (n, 3) float or List[(n, 3) float]
        The vertices positions of source_mesh such that it is registered non-rigidly
        onto the target geometry.
        If return_records is True, it returns the list of the vertex positions at each
        iteration.
    c
                    ||d d d f         z  }
|d uo|d u}|| z  |                     |d d d f                   g}d|z  |z   df}|r0|                    |	|z             d|z  |z   |j        d         z   df}t          j        t          j        |                    }t          j        |t          j                  }|
|d|z  d|z  |z   d d f<   |r(||	z  |d|z  |z   d|z  |z   |j        d         z   d d f<   t          j	        
                    |j        |z  |j        |z                                            }|S )Nr-   r+   r   re   )multiplyr/   rr   sparse
csr_matrixr    
lil_matrixr   float32r@   spsolvert   toarray)M_kron_GDvertices_weightr   wsnEnVDlUlwlUuse_landmarksA_stackB_shapeABXs                    r%   _solve_systemz#nricp_amberg.<locals>._solve_system  sh   oaaag..$92T>=!**_QQQW-E"F"FGr6B;" 	5NN27###2v{RXa[0!4GfmG4455gRZ888'(!b&AFRK
 !!!
#$ 	F>@2gAa"frkQVb[28A;67:;M!!!#'13733;;==r'   c                 X   | j                                         dz   }t          | j                   }t          j        t          j        |          d          }| j                                         }t          j        d|z  t          j                  }d|dd d<   |rst          j	        
                    | j        | j         d d df                  | j        | j         d d df                  z
  d          }|t          j        d|z  d          z  }t          j        |||ff||f          S )Nr   r   r*   r   rh   rr   )edgesmaxr   r   repeatarangeflattenrC   r   r@   normr   r   
coo_matrix)rK   	do_weightr   r   rowscolsdataedge_lengthss           r%   _node_arc_incidencez)nricp_amberg.<locals>._node_arc_incidence  s   Z^^!__y2**z!!##wq2vrz**QTT
 	39>>djA./$-
111a4@P2QQXZ *  L BIa,.222D $t!5b"XFFFFr'   c                 V   t          |           }t          j        t          j        |          d          }t          j        d|z            }t          j        | t          j        |df          fd                                          }t          j        |||ff|d|z  f          S )Nr-   r   r*   rh   r   )	r   r   r   r   concatenaterC   r   r   r   )vertex_3d_datar   r   r   r   s        r%   	_create_Dznricp_amberg.<locals>._create_D  s      y2**yR  ~~rwAw/?/?@rJJJRRTT $t!5b!b&\JJJJr'   c                     t          j        t          j        d          t          j        g dg          fd          }t          j        || df          S )Nr+   )r   r   r   r   rh   r   )r   r   r.   rB   tile)r   X_s     r%   	_create_Xznricp_amberg.<locals>._create_X'  sI    ^RVAYY)))(=(=>QGGGwrB7###r'   c                 @   d\  }}||||fS t          |t                    rj|\  }}|j        |         }| |                                d d f         }|xj        |                                                    t          j        |j                            z  c_        |j	        |d d df                  }	|j	        |d d df                  }
|j	        |d d df                  }||
|d d dd f         z  z
  ||d d dd f         z  z
  }||	|d d dd f         z  z
  ||d d dd f         z  z
  }||	|d d dd f         z  z
  |
|d d dd f         z  z
  }t          j
        |j        d         dz  df          }||dd d<   ||dd d<   ||dd d<   n| |d d f         }|}||fS )N)NNr   r   r   r+   )r   tuplefacesr   r   r   r   diffindptrr   rq   rr   )r   r   source_landmarksr   r   r   source_tidssource_baryssource_tri_vidsx0x1x2Ul0Ul1Ul2s                  r%   _create_Dl_Ulz#nricp_amberg.<locals>._create_Dl_Ul,  s#   B#'7'?r6M&.. 	"(8%K)/<O?**,,aaa/0BGG|++--44RWRY5G5GHHHGG%oaaad&;<B%oaaad&;<B%oaaad&;<B |AAAq$J//0|AAAq$J//0  !|AAAq$J//0|AAAq$J//0  !|AAAq$J//0|AAAq$J//0 
 39Q<!+Q/00BBqt!tHBqt!tHBqt!tHH#QQQ&'B!B2vr'   Tr   N){Gz?r         ?r   )g{Gz?   r   r   )gQ?g      @r   r   )r   r   rj   r   r+   r   from_vertices_onlyreturn_normalsreturn_interpolated_normalsneighbors_count	distancesr   interpolated_normalsr   r*   rh   )r   r   r   r   copyr   r1   r   kronvertex_normalsfinfor   r   float16
_from_meshrC   r   diagonal_dotclipabsr@   r   rp   r/   r   )-r   r   r   r   stepsepsgammadistance_thresholdreturn_records	use_facesuse_vertex_normalsr   r   r   r   r   r   rE   r   r   r   transformed_verticesMGr   r   DNr   r   r   recordsr   r   wnmax_iter
last_errorerrorcpt_iterqresr   target_normalssource_normalsr?   	error_vecr   s-                                                r%   nricp_ambergr    s   `  "G G GK K K$ $ $
' ' 'R :N_&6: :6O%x
 
[		B	[!	"	"B '/4466K..A
Aq% !!A{1a  H	+&''A	;-	.	.B	"A]1k+;=MNNFB }   	
  )'( !& . .BH  	_q00BXbj))-
$$( 5 3&&H,<8@S@S$'0=!Av,.F,I7I /  D !gbkkOFGOD-0BBCAvv)t++!%i% B*@D*H*H%)*@%AN!#a'GG,5Fbgc1a(((26#;;"1CG"; !_d9or2r2rSU A $%q5 J	tI9M'MTVWWI06688E 53444MHG 5 3&&H,<8@S@SJ  &%#_&6% F Mr'   c                 D   t          j        |          }t          |t          | j                            }|st          | j                  dk    rt          | j        || j        ||          S i }ddlm	}	 ddl
m}
  |	| |          \  |d<   |d<   |d<   |r| j        |d                  |d	<   |s|rn |
| j        | j        |d                           |d                   |d
<   |r;t          j        d|d
         | j        | j        |d                                     |d<   |S )a  
    Find the the closest points and associated attributes from a Trimesh.

    Parameters
    -----------
    mesh : Trimesh
        Trimesh from which the query is performed
    input_points : (m, 3) float
        Input query points
    from_vertices_only : bool
        If True, consider only the vertices and not the faces
    return_barycentric_coordinates : bool
        If True, return the barycentric coordinates
    return_normals : bool
        If True, compute the normals at each closest point
    return_interpolated_normals : bool
        If True, return the interpolated normal at each closest point
    neighbors_count : int
        The number of closest neighbors to query
    kwargs : dict
        Dict to accept other key word arguments (not used)
    Returns
    ----------
    qres : Dict
      Dictionary containing :
       - nearest points (m, 3) with key 'nearest'
       - distances to nearest point (m,) with key 'distances'
       - support triangle indices of the nearest points (m,) with key 'tids'
       - [optional] normals at nearest points (m,3) with key 'normals'
       - [optional] barycentric coordinates in support triangles (m,3) with key
         'barycentric_coordinates'
       - [optional] interpolated normals (m,3) with key 'interpolated_normals'
    r   )r   r   r   )closest_point)points_to_barycentricr   r   tidsr   barycentric_coordinatesz
ij,ijk->ikr   )r   rl   minr   r   r   _from_pointskdtree	proximityr  	trianglesr  face_normalseinsumr   )rK   input_pointsr   return_barycentric_coordinatesr   r   r   rM   r  r  r  s              r%   r   r     sk   V =..L/3t}+=+=>>O 
S__11MK)+
 
 
 	
 D((((((0000007D}T<7X7X4DOT+&V :+DL9Y% )D *?*?M$*T&\23T)_+
 +
&' ' 	+-9./#DJtF|$<=, ,D'(
 Kr'   c                    t          j        |           } t          j        |          }t          |t          |                     }i }|t	          |           }|rt|dk    sJ |                    ||          \  }}| |ddf         }	t          |	          d         |d<   |	dddf         |d<   |dddf         |d<   |dddf         |d	<   n3|                    |          \  |d<   |d	<   | |d	         ddf         |d<   |S )
a  
    Find the the closest points and associated attributes
    from a set of 3D points.

    Parameters
    -----------
    target_points : (n, 3) float
      Points from which the query is performed
    input_points : (m, 3) float
      Input query points
    kdtree : scipy.cKDTree
      KDTree used for query. Computed if not provided
    return_normals : bool
      If True, compute the normals at each nearest point
    neighbors_count : int
      The number of closest neighbors to query
    kwargs : dict
      Dict to accept other key word arguments (not used)

    Returns
    ----------
    qres : Dict
      Dictionary containing :
       - nearest points (m, 3) with key 'nearest'
       - distances to nearest point (m,) with key 'distances'
       - vertex indices of the nearest points (m,) with key 'vertex_indices'
       - [optional] normals at nearest points (m,3) with key 'normals'
    Nr+   )kr   r   r   r   r   vertex_indices)r   rl   r  r   r   r   r	   )
target_pointsr  r  r   r   rM   r  r   indicesr   s
             r%   r  r    s5   J M-00M=..L/3}+=+=>>OD~'' 
C!#####\\,/\JJ	7
+#G,,Q/Y!!!!Q$-Y%aaadO[!(A4:LL4N4N1[4 01'-=(>(ABYKr'   vertexc           	        45 4fd4d }4fd}4fd}d }d }5fd}t          | ||          \  }}}}t          | j                  5|duo|du}|g d	g d	g d
g dg} ||           \  }}}t          j                            |          }|
dk    r| j        }n| j        } ||||          \  }} |||||          \  }} ||| |          \  }}|                                }|r|d5         g} t          |          D ][\  }!\  }"}#}$}%}&||#z  ||$z  g}'||#z  ||$z  g}(|r0|'
                    ||%z             |(
                    ||%z             |!dk    s|sM|"dk    rFt          ||d5         | |&dk    |o|&dk    |	          }) ||)d         |t          |                    \  }*}+t          j        5          },d|,|)d         |k    <   |&dk    sd|)v rp|)d         }-|rd|)v r|)d         }- ||| j                  }.t          j        |.|-          }/|rt          j        |/dd          nt          j        |/          }/|,|/|&z  z  },|*xj        |,|         |*j                 z  c_        |+|,|df         z  }+|'
                    |*|"z             |(
                    |+|"z             t)          j        |'d          }0|0                                 t          j        |(          }1t(          j                            |0j        |0z                                            }2|2                    |0j        |1z            }|r| 
                    |d5                    ]|r| }3n
|d5         }3t9          | |||3||          }3|3S )ao  
    Non Rigid Iterative Closest Points

    Implementation of the correspondence computation part of
    "Sumner and Popovic 2004: Deformation Transfer for Triangle Meshes"
    Allows to register non-rigidly a mesh on another geometry.

    Comparison between nricp_amberg and nricp_sumner:
    * nricp_amberg fits to the target mesh in less steps
    * nricp_amberg can generate sharp edges (only vertices and their
        neighbors are considered)
    * nricp_sumner tend to preserve more the original shape
    * nricp_sumner parameters are easier to tune
    * nricp_sumner solves for triangle positions whereas nricp_amberg solves for
        vertex transforms
    * nricp_sumner is less optimized when wn > 0

    Parameters
    ----------
    source_mesh : Trimesh
        Source mesh containing both vertices and faces.
    target_geometry : Trimesh or PointCloud or (n, 3) float
        Target geometry. It can contain no faces or be a PointCloud.
    source_landmarks : (n,) int or ((n,) int, (n, 3) float)
        n landmarks on the the source mesh.
        Represented as vertex indices (n,) int.
        It can also be represented as a tuple of triangle indices and barycentric
        coordinates ((n,) int, (n, 3) float,).
    target_positions : (n, 3) float
        Target positions assigned to source landmarks
    steps : Core parameters of the algorithm
        Iterable of iterables (wc, wi, ws, wl, wn).
        wc is the correspondence term (strength of fitting), wi is the identity term
        (recommended value : 0.001), ws is smoothness term, wl weights the landmark
        importance and wn the normal importance.
    distance_threshold : float
        Distance threshold to account for a vertex match or not.
    return_records : bool
        If True, also returns all the intermediate results. It can help debugging
        and tune the parameters to match a specific case.
    use_faces : bool
        If True and if target geometry has faces, use proximity.closest_point to find
        matching points. Else use scipy's cKDTree object.
    use_vertex_normals : bool
        If True and if target geometry has faces, interpolate the normals of the target
        geometry matching points.
        Else use face normals or estimated normals if target geometry has no faces.
    neighbors_count : int
        number of neighbors used for normal estimation. Only used if target geometry has
        no faces or if use_faces is False.
    face_pairs_type : str 'vertex' or 'edge'
        Method to determine face pairs used in the smoothness cost. 'vertex' yields
        smoother results.


    Returns
    ----------
    result : (n, 3) float or List[(n, 3) float]
        The vertices positions of source_mesh such that it is registered non-rigidly
        onto the target geometry.
        If return_records is True, it returns the list of the vertex positions at each
        iteration.
    c                    t          j        g ddz            	_        t          |          }t          j        	j        |          dt          j        t          j        |          d          z  z   }t          j        | j        d          }|                    d           }|	                    |d          }t          j
        ||fd                                          }t          j        |||ffd|z  |ft          	          S )
N)r   r   r   r-   r+      r   rh   	   r*   )rr   rf   )r   rB   _rowr   r   r   r   flatrn   ro   r   r   r   r   float)
r   Vinvsizer   r   r   minus_inv_sum	Vinv_flatr   _construct_transform_matrixs
            r%   r/  z1nricp_sumner.<locals>._construct_transform_matrix  s    +-8IIIM+B+B#(YYw27<<q29IbMM2D
 D
 @
 
 yQ''q)))LLQ''	~}i8rBBBJJLL $t!5a"fd^SXYYYYr'   c                    | j         }| j        d d df         }| j        d d df         }| j        d d df         }||z   }t          j        | j        |f          }t          | j                  t          | j                  }}t          j        |||z             d d d f         }	t          j        | j        |	fd          }
t          j        ||z
  d         ||z
  d         |d         fd          }||
|fS )Nr   r   r   r*   rh   ).N)r  r  r   r   r   r   r   r   )rK   v4_vecv1v2v3v4r   r   nT
v4_indicestetrahedronsframess               r%   _build_tetrahedronsz)nricp_sumner.<locals>._build_tetrahedrons  s   "^AAAqD!^AAAqD!^AAAqD!&[>4=""566T]##S__BYr27++AAAtG4
~tz:&>RHHH2gy!BGY#7	9JKRT
 
 
 v--r'   c                      ||t          |                                                     }t          j        t          j        dt
                    t          |          df          }||fS )Nr+   re   r   )r   tocsrr   r   identityr*  )vtettetr+  AEiBir/  s        r%   _construct_identity_costz.nricp_sumner.<locals>._construct_identity_cost  sg    ))II
 
 %''	 	
 WR[%0003s88Q-@@Bwr'   c                     ||d d df                  ||d d df                  t          |                                                     } ||d d df                  ||d d df                  t          |                                                     }||z
                                  }|                                 t	          j        t          |          dz  df          }||fS )Nr   r   r+   )r   r<  tocsceliminate_zerosr   rq   )	r>  r?  r+  
face_pairsAEs_rAEs_lAEsBsr/  s	           r%   _construct_smoothness_costz0nricp_sumner.<locals>._construct_smoothness_cost  s    ++
111a4 !4
111a4(8#93t99
 

%'' 	 ,+
111a4 !4
111a4(8#93t99
 

%'' 	 u}##%%Xs:*A.//Bwr'   c                    |/d t          j        t          |j                  t                    fS t          |t                    r|\  }}|j        |         }t          |          t          |           }}t          j        t          j	        |          d          }|j
        }	|j
        }
t          j        |
||	ff||f          }||t          j        t           j                  j        k             }t          j        t          |j                  t                    }d||<   nt          |          t          |           }}t          j	        |          }|j
        }	t          j        |          }
t          j        |
||	ff||f          }t          j        t          |j                  t                    }d||<   ||fS )Nre   r+   r   F)r   rC   r   r   boolr   r   r   r   r   r)  r   r   r   r   r   )r>  r   r   source_landmarks_tidssource_landmarks_baryssource_landmarks_vidsnLnVTr   r   r   AElmarker_vidsnon_markers_masks                 r%   _construct_landmark_costz.nricp_sumner.<locals>._construct_landmark_cost  s   #[%9!:!:$GGGGG&.. 	7<L9!#9$/$56K$L!/00#d))B9RYr]]A..D(-D).D#TD$<$8S	JJJC/&"*)=)=)AAK  "ws;+?'@'@MMM,1[)) *++SYYB9R==D#(D72;;D#TD$<$8S	JJJC!ws;+?'@'@MMM16-.$$$r'   c                     t          j        |t          d          d t          |                   }||         }| |         }||fS )Ncsc)rf   format)r   r=  r*  r   )rP   rU  r,  AEcBcs        r%   _construct_correspondence_costz4nricp_sumner.<locals>._construct_correspondence_cost  sK    od%>>>?VEUAVAV?VW"#$%Bwr'   c                     | |         }t          |          }t          ||          d         }t          |          }t          |||          }|S )N)r  crossesr   )vertex_countr   r  face_angles)r   r   r   r   )r   r   mesh_trianglesmesh_triangles_crossmesh_face_normalsmesh_face_anglesmesh_normalsr   s          r%   _compute_vertex_normalsz-nricp_sumner.<locals>._compute_vertex_normals  su    !%$^44#$.B
 
 

 ".11.*(	
 
 
 r'   N)r   MbP?rk     r   )r   rg  rk   rh  r   )d   rg  rk   rh  r   r#  r   r   r   r   r   r   r   rX  )rY  )r   r   r   r   r@   rA   face_neighborhoodface_adjacencyr   rF   r/   r   rC   r   r   r   r   r   r   r"  r   r    rE  r   splurt   rD  solver   )6r   r   r   r   r   r   r   r   r   r   face_pairs_typer:  rB  rK  rV  r\  rf  rE   r   r   source_vtet
source_tetVr+  rF  r@  rA  rI  rJ  rS  rU  r  r  rX   wcwir   r   r  AstackBstackr  rZ  r[  r   r  r  r?   r   r4   LUr   r/  r   s6                                                       @@r%   nricp_sumnerrw  @  s   ZZ Z Z Z Z. . .         % % %>      " <P_&6< <8_&% 
[!	"	"B$D0Q5ET5QM} %$$$$$%%%&&&
 "5!4[!A!AKQ9==D ("" 2

 /
 '&{JEEGC((j$
SSGC44["2 C	 '++-- .',- $-U#3#3 76 76BBB(C"H%r'27# 	1MM#(###MM*R/000EEEBFF$SbS)'0=!Av-?-JBF /  D 54Y!13{3C3C GC !gbkkOFGOD-0BBCAvvd**!%i% B*@D*H*H%)*@%AN!8!8(+*;" " 'GG,5Fbgc1a(((26#;;"1CG"; HH(89#+FFHH/"2D"899BMM#(###MM"r'""" M&///	N6""]q 1 122!xxa00  	6NN/4555 +%crc*#_&6% F Mr'   )r   Fr   r   )NTTTT)Nr   r   )
NNNr   r   r   FTTr   )FFFFr   )NFr   )	NNNr   FTTr   r#  )'__doc__numpyr    r   r   r   geometryr   rP   r   r	   r
   r  r   r   r   typedr   r   r   scipy.sparser   scipy.spatialr   BaseExceptionEr   ExceptionWrapperrM  r_   r   rH   r   r   r  r   r  rw  r7   r'   r%   <module>r     s        + + + + + + + + + + - - - - - - ) ) ) ) ) ) ) ) - - - - - - - - - - - - - - - - / / / / / / / / / /	,!!!!!!%%%%%%% , , , )j)!,,G(Z(++FFFFFF, b b b 	b
 b b b b bP $(z zzz i z 	z
 z z z z z zzK+ K+ K+ K+\> > >"  ( 

D D D DT #( %L L L Ld 8 8 8 8| 
` ` ` ` ` `s   A A<&A77A<