
    (h                     8    d dl Zd dlmZ d dlmZ d ZddZd Z	y)    N)KDTreec                    | j                   |j                   k(  sJ | j                   d   }t        j                  | d      }t        j                  |d      }| |z
  }||z
  }t        j                  |j                  |      }t        j
                  j                  |      \  }}	}
t        j                  |
j                  |j                        }t        j
                  j                  |      dk  r>|
|dz
  d d fxx   dz  cc<   t        j                  |
j                  |j                        }|t        j                  ||      z
  }t        j                  |dz         }||d |d |f<   ||d ||f<   |||fS )N   r   axis)	shapenpmeandotTlinalgsvddetidentity)ABm
centroid_A
centroid_BAABBHUSVtRtr   s                 B/home/developers/rajanand/mypropertyqr-fmb-refixing-v2/pydep/v2.pybest_fit_transformr       s6   77agg	
A#J#J	
ZB	
ZB
rttRAyy}}QHAq"
rttQSSA	yy}}Q!
1Q36
b
FF244RVVAz**A
AEAAbqb"1"fIAbqb!eHa7N    c                 `   | j                   d   }t        j                  |dz   | j                   d   f      }| j                  |d |d d f<   |t        j                  ||      }t        |      }d}t        j                  |dz         }	t        |      D ]  }
|d |d d f   j                  }|j                  |      \  }}||   }t        ||      \  }}}t        j                  ||      }t        j                  |      }t        ||z
        |k  r |	S |}t        j                  ||	      }	 |	S )Nr   r   )r	   r
   onesr   r   r   eyerangequeryr    r   abs)source_pointstarget_points	init_posemax_iterations	tolerancer   srctree
prev_errorT_cumulativeicurrent_src	distancesindicesmatched_tgtT_step_
mean_errors                     r   icpr9      s1   AA
''1q5---a01
2CCAJffY$- DJ66!a%=L>" 
4"1"a%jll!ZZ4	7#G,)+{C1ffVS!WWY'
zJ&')3   
vvfl3
4 r!   c                    dd} || d      } ||d      }t        ||      }t        j                  dt        |      f      }|j                  |d dd d f<   t        j
                  ||      }|d dd d f   j                  }t        j                  |t        j                        j                  t        |      df      }	|	S )Nc                    t        j                  |       } t        |       }t        j                  dd|d      }t        j                  | dd      }|| z
  }| d d d d d f   |d d d d f   |d d d d d f   z  z   }|j                  dd      S )Nr   r   F)endpointr   r      )r
   asarraylenlinspacerollreshape)pointssamples_per_edgenr   p2diffsdenses          r   densify_polygonztest.<locals>.densify_polygon0   s    F#KKK1.?WWVRa(Vq$z"QtQ}%5aqj8I%II}}R##r!         r=   )dtype)
   )	r9   r
   r#   r?   r   r   arrayint32rB   )
points_Apoints_BrI   dense_Adense_Br   points_B_homaligned_B_hom	aligned_Baligned_B_cvs
             r   testrX   .   s    $ h+Gh+G 	GWA 77As8}-.L"**L!QFF1l+Mbqb!e$&&I  88IRXX6>>IPQ?RSLr!   )N2   g-C6?)
numpyr
   matplotlib.pyplotpyplotpltscipy.spatialr   r    r9   rX    r!   r   <module>r`      s       &,&r!   