def rotMatOfExpMap_orig(expMap):
"""
Original rotMatOfExpMap, used for comparison to optimized version
"""
if isinstance(expMap, ndarray):
if expMap.ndim != 2:
if expMap.ndim == 1 and len(expMap) == 3:
numObjs = 1
expMap = expMap.reshape(3, 1)
else:
raise RuntimeError("input is the wrong dimensionality")
elif expMap.shape[0] != 3:
raise RuntimeError(
"input is the wrong shape along the 0-axis; "
+ "Yours is %d when is should be 3"
% (expMap.shape[0])
)
else:
numObjs = expMap.shape[1]
elif isinstance(expMap, list) or isinstance(expMap, tuple):
if len(expMap) != 3:
raise RuntimeError(
"for list/tuple input only one exponential map "
+ "vector is allowed"
)
else:
if not isscalar(expMap[0]) or not isscalar(expMap[1]) \
or not isscalar(expMap[2]):
raise RuntimeError(
"for list/tuple input only one exponential map "
+ "vector is allowed"
)
else:
numObjs = 1
expMap = asarray(expMap).reshape(3, 1)
phi = columnNorm(expMap) # angles of rotation from exponential maps
W = skewMatrixOfVector(expMap) # skew matrices of exponential maps
# Find tiny angles to avoid divide-by-zero and apply limits in expressions
zeroIndex = phi < cnst.epsf
phi[zeroIndex] = 1
# first term
C1 = sin(phi) / phi
C1[zeroIndex] = 1
# second term
C2 = (1 - cos(phi)) / phi**2
C2[zeroIndex] = 1
if numObjs == 1:
rmat = I3 + C1 * W + C2 * dot(W, W)
else:
rmat = zeros((numObjs, 3, 3))
for i in range(numObjs):
rmat[i, :, :] = \
I3 + C1[i] * W[i, :, :] + C2[i] * dot(W[i, :, :], W[i, :, :])
return rmat
def rotMatOfExpMap_opt(expMap):
"""Optimized version of rotMatOfExpMap
"""
if expMap.ndim == 1:
expMap = expMap.reshape(3, 1)
# angles of rotation from exponential maps
phi = atleast_1d(columnNorm(expMap))
# skew matrices of exponential maps
W = skewMatrixOfVector(expMap)
# Find tiny angles to avoid divide-by-zero and apply limits in expressions
zeroIndex = phi < cnst.epsf
phi[zeroIndex] = 1
# first term
C1 = sin(phi) / phi
C1[zeroIndex] = 1 # is this right? might be OK since C1 multiplies W
# second term
C2 = (1 - cos(phi)) / phi**2
C2[zeroIndex] = 0.5 # won't matter because W^2 is small
numObjs = expMap.shape[1]
if numObjs == 1: # case of single point
W = np.reshape(W, [1, 3, 3])
pass
C1 = np.tile(
np.reshape(C1, [numObjs, 1]),
[1, 9]).reshape([numObjs, 3, 3])
C2 = np.tile(
np.reshape(C2, [numObjs, 1]),
[1, 9]).reshape([numObjs, 3, 3])
W2 = np.zeros([numObjs, 3, 3])
for i in range(3):
for j in range(3):
W2[:, i, j] = np.sum(W[:, i, :]*W[:, :, j], 1)
pass
pass
rmat = C1*W + C2 * W2
rmat[:, 0, 0] += 1.
rmat[:, 1, 1] += 1.
rmat[:, 2, 2] += 1.
return rmat.squeeze()
def rotMatOfExpMap_orig(expMap):
"""Original rotMatOfExpMap, used for comparison to optimized version
"""
if isinstance(expMap, ndarray):
if expMap.ndim != 2:
if expMap.ndim == 1 and len(expMap) == 3:
numObjs = 1
expMap = expMap.reshape(3, 1)
else:
raise RuntimeError, "input is the wrong dimensionality"
elif expMap.shape[0] != 3:
raise RuntimeError(
"input is the wrong shape along the 0-axis. Yours is %d when is should be 3" % (expMap.shape[0])
)
else:
numObjs = expMap.shape[1]
elif isinstance(expMap, list) or isinstance(expMap, tuple):
if len(expMap) != 3:
raise RuntimeError, "for list/tuple input only one exponential map vector is allowed"
else:
if not isscalar(expMap[0]) or not isscalar(expMap[1]) or not isscalar(expMap[2]):
raise RuntimeError, "for list/tuple input only one exponential map vector is allowed"
else:
numObjs = 1
expMap = asarray(expMap).reshape(3, 1)
phi = columnNorm(expMap) # angles of rotation from exponential maps
W = skewMatrixOfVector(expMap) # skew matrices of exponential maps
# Find tiny angles to avoid divide-by-zero and apply limits in expressions
zeroIndex = phi < tinyRotAng
phi[zeroIndex] = 1
# first term
C1 = sin(phi) / phi
C1[zeroIndex] = 1
# second term
C2 = (1 - cos(phi)) / phi ** 2
C2[zeroIndex] = 1
if numObjs == 1:
rmat = I3 + C1 * W + C2 * dot(W, W)
else:
rmat = zeros((numObjs, 3, 3))
for i in range(numObjs):
rmat[i, :, :] = I3 + C1[i] * W[i, :, :] + C2[i] * dot(W[i, :, :], W[i, :, :])
return rmat
def rotMatOfExpMap_opt(expMap):
"""Optimized version of rotMatOfExpMap
"""
if expMap.ndim == 1:
expMap = expMap.reshape(3, 1)
phi = atleast_1d(columnNorm(expMap)) # angles of rotation from exponential maps
W = skewMatrixOfVector(expMap) # skew matrices of exponential maps
# Find tiny angles to avoid divide-by-zero and apply limits in expressions
zeroIndex = phi < tinyRotAng
phi[zeroIndex] = 1
# first term
C1 = sin(phi) / phi
C1[zeroIndex] = 1 # is this right? might be OK since C1 multiplies W
# second term
C2 = (1 - cos(phi)) / phi**2
C2[zeroIndex] = 0.5 # won't matter because W^2 is small
numObjs = expMap.shape[1]
if numObjs == 1: # case of single point
W = numpy.reshape(W, [1, 3, 3])
pass
C1 = numpy.tile(numpy.reshape(C1, [numObjs, 1]), [1, 9]).reshape([numObjs,3,3])
C2 = numpy.tile(numpy.reshape(C2, [numObjs, 1]), [1, 9]).reshape([numObjs,3,3])
W2 = numpy.zeros([numObjs, 3, 3])
for i in range(3):
for j in range(3):
W2[:, i, j] = numpy.sum(W[:, i, :]*W[:, :, j], 1)
pass
pass
rmat = C1*W + C2 * W2
rmat[:, 0, 0] += 1.
rmat[:, 1, 1] += 1.
rmat[:, 2, 2] += 1.
return rmat.squeeze()
