#NEWLINE# def fit_point_cloud( src_pts , tgt_pts , rotate = True , translate = True , scale = 0 , x0 = None , leastsq_args = { #NEWLINE# #INDENT# #NEWLINE# #UNINDENT# } , out = 'params' ) : #NEWLINE# #INDENT# "Find a transform that minimizes the squared distance from each source\n    point to its closest target point\n\n    Uses :func:`scipy.optimize.leastsq` to find a transformation involving\n    a combination of rotation, translation, and scaling (in that order).\n\n    Parameters\n    ----------\n    src_pts : array, shape = (n, 3)\n        Points to which the transform should be applied.\n    tgt_pts : array, shape = (m, 3)\n        Points to which src_pts should be fitted. Each point in tgt_pts should\n        correspond to the point in src_pts with the same index.\n    rotate : bool\n        Allow rotation of the ``src_pts``.\n    translate : bool\n        Allow translation of the ``src_pts``.\n    scale : 0 | 1 | 3\n        Number of scaling parameters. With 0, points are not scaled. With 1,\n        points are scaled by the same factor along all axes. With 3, points are\n        scaled by a separate factor along each axis.\n    x0 : None | tuple\n        Initial values for the fit parameters.\n    leastsq_args : dict\n        Additional parameters to submit to :func:`scipy.optimize.leastsq`.\n    out : 'params' | 'trans'\n        In what format to return the estimate: 'params' returns a tuple with\n        the fit parameters; 'trans' returns a transformation matrix of shape\n        (4, 4).\n\n    Returns\n    -------\n    x : array, shape = (n_params, )\n        Estimated parameters for the transformation.\n\n    Notes\n    -----\n    Assumes that the target points form a dense enough point cloud so that\n    the distance of each src_pt to the closest tgt_pt can be used as an\n    estimate of the distance of src_pt to tgt_pts.\n    " #NEWLINE# from scipy.optimize import leastsq #NEWLINE# kwargs = { #NEWLINE# #INDENT# 'epsfcn' : 0.01 , #NEWLINE# #UNINDENT# } #NEWLINE# kwargs . update ( leastsq_args ) #NEWLINE# src_pts = np . atleast_2d ( src_pts ) #NEWLINE# tgt_pts = np . atleast_2d ( tgt_pts ) #NEWLINE# translate = bool ( translate ) #NEWLINE# rotate = bool ( rotate ) #NEWLINE# scale = int ( scale ) #NEWLINE# if translate : #NEWLINE# #INDENT# src_pts = np . hstack ( ( src_pts , np . ones ( ( len ( src_pts ) , 1 ) ) ) ) #NEWLINE# #UNINDENT# try : #NEWLINE# #INDENT# from sklearn.neighbors import BallTree #NEWLINE# tgt_pts = BallTree ( tgt_pts ) #NEWLINE# errfunc = _point_cloud_error_balltree #NEWLINE# #UNINDENT# except ImportError : #NEWLINE# #INDENT# warn ( 'Sklearn could not be imported. Fitting points will be slower. To improve performance, install the sklearn module.' ) #NEWLINE# errfunc = _point_cloud_error #NEWLINE# #UNINDENT# param_info = ( rotate , translate , scale ) #NEWLINE# if ( param_info == ( True , False , 0 ) ) : #NEWLINE# #INDENT# x0 = ( x0 or ( 0 , 0 , 0 ) ) #NEWLINE# def error( x ) : #NEWLINE# #INDENT# ( rx , ry , rz ) = x #NEWLINE# trans = rotation3d ( rx , ry , rz ) #NEWLINE# est = dot ( src_pts , trans . T ) #NEWLINE# err = errfunc ( est , tgt_pts ) #NEWLINE# return err #NEWLINE# #UNINDENT# #UNINDENT# else : #NEWLINE# #INDENT# if ( param_info == ( True , False , 1 ) ) : #NEWLINE# #INDENT# x0 = ( x0 or ( 0 , 0 , 0 , 1 ) ) #NEWLINE# def error( x ) : #NEWLINE# #INDENT# ( rx , ry , rz , s ) = x #NEWLINE# trans = ( rotation3d ( rx , ry , rz ) * s ) #NEWLINE# est = dot ( src_pts , trans . T ) #NEWLINE# err = errfunc ( est , tgt_pts ) #NEWLINE# return err #NEWLINE# #UNINDENT# #UNINDENT# else : #NEWLINE# #INDENT# if ( param_info == ( True , False , 3 ) ) : #NEWLINE# #INDENT# x0 = ( x0 or ( 0 , 0 , 0 , 1 , 1 , 1 ) ) #NEWLINE# def error( x ) : #NEWLINE# #INDENT# ( rx , ry , rz , sx , sy , sz ) = x #NEWLINE# trans = ( rotation3d ( rx , ry , rz ) * [ sx , sy , sz ] ) #NEWLINE# est = dot ( src_pts , trans . T ) #NEWLINE# err = errfunc ( est , tgt_pts ) #NEWLINE# return err #NEWLINE# #UNINDENT# #UNINDENT# else : #NEWLINE# #INDENT# if ( param_info == ( True , True , 0 ) ) : #NEWLINE# #INDENT# x0 = ( x0 or ( 0 , 0 , 0 , 0 , 0 , 0 ) ) #NEWLINE# def error( x ) : #NEWLINE# #INDENT# ( rx , ry , rz , tx , ty , tz ) = x #NEWLINE# trans = dot ( translation ( tx , ty , tz ) , rotation ( rx , ry , rz ) ) #NEWLINE# est = dot ( src_pts , trans . T ) #NEWLINE# err = errfunc ( est [ : , : 3 ] , tgt_pts ) #NEWLINE# return err #NEWLINE# #UNINDENT# #UNINDENT# else : #NEWLINE# #INDENT# raise NotImplementedError ( ( 'The specified parameter combination is not implemented: rotate=%r, translate=%r, scale=%r' % param_info ) ) #NEWLINE# #UNINDENT# #UNINDENT# #UNINDENT# #UNINDENT# ( est , _ , info , msg , _ ) = leastsq ( error , x0 , full_output = True , ** kwargs ) #NEWLINE# logger . debug ( 'fit_point_cloud leastsq (%i calls) info: %s' , info [ 'nfev' ] , msg ) #NEWLINE# if ( out == 'params' ) : #NEWLINE# #INDENT# return est #NEWLINE# #UNINDENT# else : #NEWLINE# #INDENT# if ( out == 'trans' ) : #NEWLINE# #INDENT# return _trans_from_params ( param_info , est ) #NEWLINE# #UNINDENT# else : #NEWLINE# #INDENT# raise ValueError ( ( "Invalid out parameter: %r. Needs to be 'params' or 'trans'." % x0 ) )