本文整理汇总了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]);