Exemplo n.º 1
0
def SO3_distance(R0, R1):
    """
  Return distance between the given two rotation matrices in SO3 space.

  @type  R0: numpy.ndarray
  @param R0: Rotation matrix.
  @type  R1: numpy.ndarray
  @param R1: Rotation matrix.

  @rtype: float
  @return: Distance.
  """
    dR = lie.logvect(dot(R0.T, R1))
    return np.sqrt(dot(dR, dR))
Exemplo n.º 2
0
def SO3Distance(R0, R1):  # bi-invariance
    return linalg.norm(lie.logvect(dot(R0.T, R1)))
Exemplo n.º 3
0
def SO3Distance(R0, R1): # bi-invariance
    return linalg.norm(lie.logvect(dot(R0.T,R1)))
Exemplo n.º 4
0
def SO3Distance(R0, R1): # bi-invariance
    return np.linalg.norm(Lie.logvect(np.dot(R0.T, R1)))