Author: Not specified Language: python
Description: Not specified Timestamp: 2018-06-03 21:58:09 +0000
View raw paste Reply
  1. def ICP(self, Pv):
  2.         Uvc, Pvc = self.getCorresponding(Pv, self.tree)
  3.            
  4.         um = self.getCenterOfMass(Uvc)
  5.         pm = self.getCenterOfMass(Pvc)
  6.        
  7.         Uvc = Uvc - um
  8.         Pvc = Pvc - pm
  9.  
  10.         W = self.computeW(Uvc, Pvc)
  11.  
  12.         U, S, V = scipy.linalg.svd(np.transpose(W), full_matrices=True)
  13.  
  14.         R = U.dot(V)
  15.         t = um - R.dot(pm)
  16.  
  17.  
  18.         Pv = multiply(R, Pv) + t
  19.        
  20.         return Pv
View raw paste Reply