parameters: (model,data)
return_statement: return rot,trans,trans_error
docstring: Align two trajectories using the method of Horn (closed-form).
      匹配误差计算====
      Input:
      model -- first trajectory (3xn)    估计值
      data -- second trajectory (3xn)    真值=
      
      Output:
      rot -- rotation matrix (3x3)    两数据的旋转平移矩阵
      trans -- translation vector (3x1)
      trans_error -- translational error per point (1xn) 匹配误差
docstring_summary: Align two trajectories using the method of Horn (closed-form).
      匹配误差计算====
      Input:
      model -- first trajectory (3xn)    估计值
      data -- second trajectory (3xn)    真值=
      
      Output:
      rot -- rotation matrix (3x3)    两数据的旋转平移矩阵
      trans -- translation vector (3x1)
      trans_error -- translational error per point (1xn) 匹配误差
docstring_tokens: Align two trajectories using the method of Horn ( closed - form ) . 匹配误差计算 ==== Input : model -- first trajectory ( 3xn ) 估计值 data -- second trajectory ( 3xn ) 真值 = Output : rot -- rotation matrix ( 3x3 ) 两数据的旋转平移矩阵 trans -- translation vector ( 3x1 ) trans_error -- translational error per point ( 1xn ) 匹配误差
function: def align(model,data):
      """Align two trajectories using the method of Horn (closed-form).
      匹配误差计算====
      Input:
      model -- first trajectory (3xn)    估计值
      data -- second trajectory (3xn)    真值=
      
      Output:
      rot -- rotation matrix (3x3)    两数据的旋转平移矩阵
      trans -- translation vector (3x1)
      trans_error -- translational error per point (1xn) 匹配误差
      
      """
      numpy.set_printoptions(precision=3,suppress=True)
      model_zerocentered = model - model.mean(1) # 去均值===
      data_zerocentered = data - data.mean(1)
      
      W = numpy.zeros( (3,3) )# 
      for column in range(model.shape[1]):
          W += numpy.outer(model_zerocentered[:,column],data_zerocentered[:,column])
          # outer() 前一个参数表示 后一个参数扩大倍数
          # https://blog.csdn.net/hqh131360239/article/details/79064592
      U,d,Vh = numpy.linalg.linalg.svd(W.transpose())# 奇异值分解
      S = numpy.matrix(numpy.identity( 3 ))# 单位阵
      if(numpy.linalg.det(U) * numpy.linalg.det(Vh)<0):
          S[2,2] = -1
      rot = U*S*Vh
      trans = data.mean(1) - rot * model.mean(1)
      
      model_aligned = rot * model + trans
      alignment_error = model_aligned - data
      # err = sqrt((x-x')^2 + (y-y')^2 + (z-z')^2) 
      trans_error = numpy.sqrt(numpy.sum(numpy.multiply(alignment_error,alignment_error),0)).A[0]
          
      return rot,trans,trans_error
function_tokens: def align ( model , data ) : numpy . set_printoptions ( precision = 3 , suppress = True ) model_zerocentered = model - model . mean ( 1 ) # 去均值=== data_zerocentered = data - data . mean ( 1 ) W = numpy . zeros ( ( 3 , 3 ) ) #  for column in range ( model . shape [ 1 ] ) : W += numpy . outer ( model_zerocentered [ : , column ] , data_zerocentered [ : , column ] ) # outer() 前一个参数表示 后一个参数扩大倍数 # https://blog.csdn.net/hqh131360239/article/details/79064592 U , d , Vh = numpy . linalg . linalg . svd ( W . transpose ( ) ) # 奇异值分解 S = numpy . matrix ( numpy . identity ( 3 ) ) # 单位阵 if ( numpy . linalg . det ( U ) * numpy . linalg . det ( Vh ) < 0 ) : S [ 2 , 2 ] = - 1 rot = U * S * Vh trans = data . mean ( 1 ) - rot * model . mean ( 1 ) model_aligned = rot * model + trans alignment_error = model_aligned - data # err = sqrt((x-x')^2 + (y-y')^2 + (z-z')^2)  trans_error = numpy . sqrt ( numpy . sum ( numpy . multiply ( alignment_error , alignment_error ) , 0 ) ) . A [ 0 ] return rot , trans , trans_error