本文整理汇总了Python中Transform.quat2rot方法的典型用法代码示例。如果您正苦于以下问题：Python Transform.quat2rot方法的具体用法？Python Transform.quat2rot怎么用？Python Transform.quat2rot使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Transform的用法示例。

在下文中一共展示了Transform.quat2rot方法的2个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的Python代码示例。

示例1: icp

# 需要导入模块: import Transform [as 别名]
# 或者: from Transform import quat2rot [as 别名]
def icp(data, model, thres=.001, maxIter=1000):
  '''
    compute icp on two point sets

    matrix: DxN

    return:
      qr - quaterion rotation
      qt - translation
  '''
  # augment if needed
  if data.shape[0] == 3:
    data = np.vstack((data, np.ones(data.shape[1])));
  if model.shape[0] == 3:
    model = np.vstack((model, np.ones(model.shape[1])));

  # initialize registration
  qr = np.array([1.0, 0.0, 0.0, 0.0]);
  qt = np.array([0.0, 0.0, 0.0]);
  dm = np.Inf;
  count = 0;

  # compute initial closest points
  di = np.arange(data.shape[1]);
  (imin, d) = closest_points(data, model);
  dk = np.sum(d[di, imin]);

  # loop until the change in error is below the threshold
  while (dm > thres and count < maxIter):
  
    # compute registration
    (qr, qt) = compute_registration(data, model[:,imin]);

    # transform data
    T = np.dot(tr.trans(qt), tr.quat2rot(qr));
    tdata = np.dot(T, data);

    # compute closest points
    (imin, d) = closest_points(tdata, model);
    dk1 = np.sum(d[di, imin]);

    dm = np.abs(dk1 - dk);
    dk = dk1;
    count += 1;

  return (qr, qt, dk);

示例2: compute_registration

# 需要导入模块: import Transform [as 别名]
# 或者: from Transform import quat2rot [as 别名]
def compute_registration(P, X):
  '''
    compute the registration between the two data sets
    Pi should correspond to Xi (distance)

    matrix: 4xN

    return:
      qr - quaterion rotation
      qt - translation
  '''

  # compute mean
  uP = np.array([np.sum(P, axis=1)/P.shape[1]]);
  uX = np.array([np.sum(X, axis=1)/X.shape[1]]);

  # cross-covariance matrix
  cc = np.dot(P[:3,:], X[:3,:].T)/P.shape[1] - np.dot(uP[:,:3].T, uX[:,:3]);

  A = cc - cc.T;
  delta = np.array([[A[1,2], A[2,0], A[0,1]]]).T;

  trace = np.trace(cc);
  t0 = np.hstack(([[trace]], delta.T));
  t1 = np.hstack((delta, cc + cc.T - trace * np.eye(3)));
  Q = np.vstack((t0, t1));

  # comute quaternion
  # main eigenvector of Q
  (U, s, Vh) = np.linalg.svd(Q);
  qr = Vh[0];

  # compute translation 
  R  = tr.quat2rot(qr);
  qt = uX - np.dot(R,uP.T).T;

  return (qr, qt[0,:3]);

本文标签属性：

示例：示例是什么意思

代码：代码大全可复制

Python：python怎么读