Question

在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），则循环次数最大化并且整个事情崩溃。

任何人都有任何奇特的想法？

Answer 1

您似乎误读了原始函数定义。它没有递归地调用自身，而是调用rotmat2quat（而不是rotmat2expmap）。您可能需要实施rotmat2quat（请参阅例如https://github.com/gwtaylor/imCRBM/blob/master/Motion/rotmat2quat.m）。

你在Python中以递归方式调用函数是正确的。但是，以任何语言递归调用函数而不先应用一些减少（使输入变小）将导致无限递归。这就是你的Python代码中发生的事情以及为什么它会达到递归深度限制。如果MatLab代码是您最初怀疑的那样，它也会发生什么。那就是你基本上有f（R） - ＆gt; f（R） - > f（R） - > f（R） - > f（R） - > ......输入在递归调用之前永远不会改变，因此每次它进行另一次递归调用并且永远不会结束。希望这很清楚。

递归调用Python中函数内的函数（尝试复制MATLAB行为）

1 个答案: