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()