在MATLAB這一功能(由張皓)自稱遞歸調用函數中的功能在Python(試圖複製MATLAB行爲)
function r=rotmat2expmap(R)
% Software provided by Hao Zhang
% http://www.cs.berkeley.edu/~nhz/software/rotations
r=quat2expmap(rotmat2quat(R));
作爲參數傳遞給函數現在如果
function [r]=quat2expmap(q)
% Software provided by Hao Zhang
% http://www.cs.berkeley.edu/~nhz/software/rotations
%
% function [r]=quat2expmap(q)
% convert quaternion q into exponential map r
%
% denote the axis of rotation by unit vector r0, the angle by theta
% q is of the form (cos(theta/2), r0*sin(theta/2))
% r is of the form r0*theta
if (abs(norm(q)-1)>1E-3)
error('quat2expmap: input quaternion is not norm 1');
end
sinhalftheta=norm(q(2:4));
coshalftheta=q(1);
r0=q(2:4)/(norm(q(2:4))+eps);
theta=2*atan2(sinhalftheta,coshalftheta);
theta=mod(theta+2*pi,2*pi);
%if (theta>pi), theta=2*pi-theta; r0=-r0; end
if (theta>pi)
theta=2*pi-theta;
r0=-r0;
end
r=r0*theta;
我們將一個旋轉矩陣傳遞給第一個函數
R =
0.9940 0.0773 -0.0773
-0.0713 0.9945 0.0769
0.0828 -0.0709 0.9940
它遞歸地計算出正確的結果(在這種情況下)簡單地說:
r =
-0.0741 -0.0803 -0.0745
唉,這是在MATLAB和正常工作(原作者知道自己在做什麼)。我還沒有完全成功地獲得相同的功能在Python的工作(我有效地翻譯的代碼),我錯了地方:
def rotmat2expmap(R):
"""
Convert rotation matrix to exponential mapping.
Based on G.W. Taylor's MATLAB equivalent.
"""
r = quat2expmap(rotmat2expmap(R))
return r
def quat2expmap(q):
"""Converts quaternion q (rotation matrix) into exponential map r.
Provided by Hao Zhang and G.W. Taylor.
Denote the axis of rotation by unit vector r0, the angle by theta
q is of the form (cos(theta/2), r0*sin(theta/2))
r is of the form r0*theta
"""
if abs(np.linalg.norm(q,2)-1) > 1e-3:
print('quat2expmap: input quaternion is not norm 1')
# Implement to simulate MATLAB like linear array structure
temp = q.T.flatten()
sinhalftheta = np.linalg.norm(temp[1:4],2)
coshalftheta = temp[0]
r0 = temp[1:4]/(np.linalg.norm(temp[1:4],2) + np.spacing(1))
theta = 2*math.atan2(sinhalftheta,coshalftheta)
theta = fmod(theta+2*pi,2*pi) # Remainder after division (modulo operation)
if theta > pi:
theta = 2*pi-theta
r0 = -r0
r = r0*theta
return r
如果我嘗試運行此(使用相同的例子R),然後循環次數最多,整個事件崩潰。
任何人有任何奇特的想法?
沒有正確讀取代碼的情況。感謝您指出我的錯誤。 – Astrid 2014-11-01 13:25:57