for consistency, I want to reimplement the conversion from a rotation matrix to quaternions of the Cpp Eigen library in python.
The Cpp implementation can be found here and below you can find my python implementation:
def rotationMatrixToQuaternion3(m):
#q0 = qw
t = np.matrix.trace(m)
q = np.asarray([0.0, 0.0, 0.0, 0.0], dtype=np.float64)
if(t > 0):
t = np.sqrt(t + 1)
q[0] = 0.5 * t
t = 0.5/t
q[1] = (m[2,1] - m[1,2]) * t
q[2] = (m[0,2] - m[2,0]) * t
q[3] = (m[1,0] - m[0,1]) * t
else:
i = 0
if (m[1,1] > m[0,0]):
i = 1
if (m[2,2] > m[i,i]):
i = 2
j = (i+1)%3
k = (j+1)%3
t = np.sqrt(m[i,i] - m[j,j] - m[k,k] + 1)
q[i] = 0.5 * t
t = 0.5 / t
q[0] = (m[k,j] - m[j,k]) * t
q[j] = (m[j,i] + m[i,j]) * t
q[k] = (m[k,i] + m[i,k]) * t
return q
Here are some example results:
rotation matrix:
[[-0.00998882 0.01194957 -0.99987871]
[ 0.49223613 -0.87032691 -0.01531875]
[-0.8704044 -0.49232944 0.00281153]]
python implemnation - qw, qx, qy, qz:
[-0.68145553 -0.18496647 0.68613542 0. ]
eigen:
-0.686135 -0.174997 -0.681456 0.184966
rotation matrix:
[[ 0.01541426 0.02293597 -0.9996181 ]
[ 0.49081359 -0.87117607 -0.01242048]
[-0.87112825 -0.49043469 -0.02468582]]
python implemnation - qw, qx, qy, qz:
[-0.17288173 0.18580601 -0.67658628 0. ]
eigen:
-0.686135 -0.174997 -0.681456 0.184966
rotation matrix:
[[ 0.03744363 -0.01068005 -0.99924167]
[ 0.48694091 -0.87299945 0.02757743]
[-0.87263195 -0.48760425 -0.02748771]]
python implemnation - qw, qx, qy, qz:
[-0.18503815 0.17105894 -0.67232212 0. ]
eigen:
-0.672322 -0.185038 -0.696048 0.171059
Help would be highly appreciated!
Thanks,
Johannes
It seems that the order used by Eigen is [x, y, z, w], see same source file that you base your implementation on.
So the indices that you use should be changed in the following way:
def rotationMatrixToQuaternion3(m):
#q0 = qw
t = np.matrix.trace(m)
q = np.asarray([0.0, 0.0, 0.0, 0.0], dtype=np.float64)
if(t > 0):
t = np.sqrt(t + 1)
q[3] = 0.5 * t
t = 0.5/t
q[0] = (m[2,1] - m[1,2]) * t
q[1] = (m[0,2] - m[2,0]) * t
q[2] = (m[1,0] - m[0,1]) * t
else:
i = 0
if (m[1,1] > m[0,0]):
i = 1
if (m[2,2] > m[i,i]):
i = 2
j = (i+1)%3
k = (j+1)%3
t = np.sqrt(m[i,i] - m[j,j] - m[k,k] + 1)
q[i] = 0.5 * t
t = 0.5 / t
q[3] = (m[k,j] - m[j,k]) * t
q[j] = (m[j,i] + m[i,j]) * t
q[k] = (m[k,i] + m[i,k]) * t
return q
And the returned quaternion is [x, y, z, w].
Running the modified code did not produce the same results that you report for Eigen. Unfortunately, I do not know how to reproduce the Eigen results.
However, there is a scipy implementation of quaternion-to-matrix, which gives the same results as the above implementation (up to multiplication by of the vector by -1 which is an inherent ambiguity of the quaternion and is thus implementation-dependent):
from scipy.spatial import transform
mat = np.array([[-0.00998882, 0.01194957, -0.99987871],
[ 0.49223613, -0.87032691, -0.01531875,],
[-0.8704044, -0.49232944, 0.00281153]])
quat_sp = transform.Rotation.from_matrix(mat).as_quat()
So I think the above implementation is correct, and the problem was in the invocation of Eigen.
I have this code:
import numpy as np
from skimage.util import img_as_ubyte
from skimage.feature import canny
import math
image = img_as_ubyte(sf_img)
edges = np.flipud(canny(image, sigma=3, low_threshold=10, high_threshold=25))
non_zeros = np.nonzero(edges)
true_rows = non_zeros[0]
true_col = non_zeros[1]
plt.imshow(edges)
plt.show()
N_im = 256
x0 = 0
y0 = -0.25
Npx = 129
Npy = 60
delta_py = 0.025
delta_px = 0.031
Nr = 9
delta_r = 0.5
rho = 0.063
epsilon = 0.75
r_k = np.zeros((1, Nr))
r_min = 0.5
for k in range(0, Nr):
r_k[0, k] = k * delta_r + r_min
a = np.zeros((Npy, Npx, Nr))
#FOR LOOP TO BE TIME OPTIMIZED
for i in range(0, np.size(true_col, 0)): #true_col and true_rows has the same size so it doesn't matter
for m in range(0, Npy):
for l in range(0, Npx):
d = math.sqrt(math.pow(
(((true_col[i] - math.floor((N_im + 1) / 2)) / (N_im + 1) / 2) - (
l * delta_px - (Npx * delta_px / 2) + x0)),
2) + math.pow(
(((true_rows[i] - math.floor((N_im + 1) / 2)) / (N_im + 1) / 2) - (
m * delta_py - (Npy * delta_py / 2) + y0)),
2))
min_idx = np.argmin(np.abs(d - r_k))
rk_hat = r_k[0, min_idx]
if np.abs(d - rk_hat) < rho:
a[m, l, min_idx] = a[m, l, min_idx] + 1
#ANOTHER LOOP TO BE OPTIMIZED
# for m in range(0, Npy):
# for l in range(0, Npx): #ORIGINAL
# for k in range(0, Nr):
# if a[m, l, k] < epsilon * np.max(a):
# a[m, l, k] = 0
a[np.where(a[:, :, :] < epsilon * np.max(a))] = 0 #SUBSTITUTED
a_prime = np.sum(a, axis=2)
acc_x = np.zeros((Npx, 1))
acc_y = np.zeros((Npy, 1))
for l in range(0, Npx):
acc_x[l, 0] = l * delta_px - (Npx * delta_px / 2) + x0
for m in range(0, Npy):
acc_y[m, 0] = m * delta_py - (Npy * delta_py / 2) + y0
prod = 0
for m in range(0, Npy):
for l in range(0, Npx):
prod = prod + (np.array([acc_x[l, 0], acc_y[m, 0]]) * a_prime[m, l])
points = prod / np.sum(a_prime)
Based on comment to an answer:
true_rows = np.random.randint(0,256,10)
true_col = np.random.randint(0,256,10)
Which, briefly, scans a 256x256 image that has been previously processed through the Canny Edge detection.
The For Loop so must scan every pixel of the resulting image and must also compute 2 nested for loops which does some operations depending on the value of l and m indexes of the 'a' matrix.
Since the edge detection returns an image with zeros and ones (in correspondence of edges) and since the inside operations has to be done only for the one-valued points, I've used
non_zeros = np.nonzero(edges)
to obtain only the indexes I'm interested in. Indeed, previously the code was in this way
for i in range(0, N_im):
for j in range(0, N_im):
if edges[i, j] == 1:
for m in range(0, Npy):
for l in range(0, Npx):
d = math.sqrt(math.pow(
(((i - math.floor((N_im + 1) / 2)) / (N_im + 1) / 2) - (
l * delta_px - (Npx * delta_px / 2) + x0)),
2) + math.pow(
(((j - math.floor((N_im + 1) / 2)) / (N_im + 1) / 2) - (
m * delta_py - (Npy * delta_py / 2) + y0)),
2))
min_idx = np.argmin(np.abs(d - r_k))
rk_hat = r_k[0, min_idx]
if np.abs(d - rk_hat) < rho:
a[m, l, min_idx] = a[m, l, min_idx] + 1
It seems like I managed to optimize the first two loops, but my script needs to be faster than that.
It takes roughly 6~7 minutes to run and I need to execute it for like 1000 times. Can you help me optimize even further those for loops of this script? Thank you!
You can use Numba JIT to speed up the computation (since the default CPython interpreter is very bad for such computation). Moreover, you can rework the loops so that the code can run in parallel.
Here is the resulting code:
import numba as nb
# Assume you work with 64-bits integer,
# feel free to change it to 32-bit integers if this is not the case.
# If you encounter type issue, let Numba choose with: #nb.njit(parallel=True)
# However, note that the first run will be slower if you let Numba choose.
#nb.njit('int64[:,:,::1](bool_[:,:], float64[:,:], int64, int64, int64, int64, float64, float64, float64, float64, float64)', parallel=True)
def fasterImpl(edges, r_k, Npy, Npx, Nr, N_im, delta_px, delta_py, rho, x0, y0):
a = np.zeros((Npy, Npx, Nr), dtype=nb.int64)
# Find all the position where edges[i,j]==1
validEdgePos = np.where(edges == 1)
for m in nb.prange(0, Npy):
for l in range(0, Npx):
# Iterate over the i,j value where edges[i,j]==1
for i, j in zip(validEdgePos[0], validEdgePos[1]):
d = math.sqrt(math.pow(
(((i - math.floor((N_im + 1) / 2)) / (N_im + 1) / 2) - (
l * delta_px - (Npx * delta_px / 2) + x0)),
2) + math.pow(
(((j - math.floor((N_im + 1) / 2)) / (N_im + 1) / 2) - (
m * delta_py - (Npy * delta_py / 2) + y0)),
2))
min_idx = np.argmin(np.abs(d - r_k))
rk_hat = r_k[0, min_idx]
if np.abs(d - rk_hat) < rho:
a[m, l, min_idx] += 1
return a
On my machine, with inputs described in your question (including the provided sf_img), this code is 616 times faster.
Reference time: 109.680 s
Optimized time: 0.178 s
Note that results are exactly the same than the reference implementation.
Based on your script, you have little experience with numpy in general. Numpy is optimized with SIMD instructions and your code kinda defeats it. I would advise you to review the basics on how to write numpy code
Please review this cheat sheet. https://s3.amazonaws.com/assets.datacamp.com/blog_assets/Numpy_Python_Cheat_Sheet.pdf
For instance, this code can be changed from
r_k = np.zeros((1, Nr))
for k in range(0, Nr):
r_k[0, k] = k * delta_r + r_min
### to a simple np.arange assignment
r_k = np.zeros((1, Nr))
r_k[0,:] = np.arange(Nr) * delta_r + r_min
### or you can do everything in one line
r_k = np.expand_dims(np.arange(Nr) * delta_r + r_min,axis=0)
This code is a little awkward because you are creating a np.array while looping through each element. You can probably simplify this code too. Are you changing the data type from int to a np.array of two here?
prod = 0
for m in range(0, Npy):
for l in range(0, Npx):
prod = prod + (np.array([acc_x[l, 0], acc_y[m, 0]]) * a_prime[m, l])
For this loop, you can slowly separate out dependent and independent elements.
#FOR LOOP TO BE TIME OPTIMIZED
for i in range(0, np.size(true_col, 0)): #true_col and true_rows has the same size so it doesn't matter
for m in range(0, Npy):
for l in range(0, Npx):
d = math.sqrt(math.pow(
(((true_col[i] - math.floor((N_im + 1) / 2)) / (N_im + 1) / 2) - (
l * delta_px - (Npx * delta_px / 2) + x0)),
2) + math.pow(
(((true_rows[i] - math.floor((N_im + 1) / 2)) / (N_im + 1) / 2) - (
m * delta_py - (Npy * delta_py / 2) + y0)),
2))
min_idx = np.argmin(np.abs(d - r_k))
rk_hat = r_k[0, min_idx]
if np.abs(d - rk_hat) < rho:
a[m, l, min_idx] = a[m, l, min_idx] + 1
The outer loop for i in range(0, np.size(true_col, 0)) is fine
You do not need a loop to compute this. For index multiplication, you can allocate an extra matrix array such that you have the desired 1:1 format.
for m in range(0, Npy):
for l in range(0, Npx):
d = math.sqrt(math.pow(
(((true_col[i] - math.floor((N_im + 1) / 2)) / (N_im + 1) / 2) - (
l * delta_px - (Npx * delta_px / 2) + x0)),
2) + math.pow(
(((true_rows[i] - math.floor((N_im + 1) / 2)) / (N_im + 1) / 2) - (
m * delta_py - (Npy * delta_py / 2) + y0)),
2))
To emulate m and l behavior, you can create an Npx by Npy index matrix. Although this pattern may seem odd, Numpy inherited tricks from MATLAB ecosystem because the goal of MATLAB/numpy is to simplify code and allow you to spend more fixing your logic.
## l matrix
[[0,1,2,3,4,5,6,7,8....Npx],
[0,1,2,3,4,5,6,7,8....Npx],
.....
[0,1,2,3,4,5,6,7,8....Npx]]
##m matrix
[[0,0,0,0,0,0,0,0,0,0,0,0],
[1,1,1,1,,1,1,1,1,1,1,1,1],
.....
[Npx,Npx,Npx.....,Npx]]
## You can create both with one command
l_mat, m_mat = np.meshgrid(np.arange(Npx), np.arange(Npy))
>>> l_mat
array([[ 0, 1, 2, ..., 147, 148, 149],
[ 0, 1, 2, ..., 147, 148, 149],
[ 0, 1, 2, ..., 147, 148, 149],
...,
[ 0, 1, 2, ..., 147, 148, 149],
[ 0, 1, 2, ..., 147, 148, 149],
[ 0, 1, 2, ..., 147, 148, 149]])
>>> m_mat
array([[ 0, 0, 0, ..., 0, 0, 0],
[ 1, 1, 1, ..., 1, 1, 1],
[ 2, 2, 2, ..., 2, 2, 2],
...,
[97, 97, 97, ..., 97, 97, 97],
[98, 98, 98, ..., 98, 98, 98],
[99, 99, 99, ..., 99, 99, 99]])
With those two matrices, you can multiply it to create the result.
d = np.sqrt(np.pow( true_col[i] - np.floor((N_im + 1)/2)) / (N_im + l_mat).....
For these two lines of code, you seem to be setting up an argmin matrix.
min_idx = np.argmin(np.abs(d - r_k))
rk_hat = r_k[0, min_idx]
https://numpy.org/doc/stable/reference/generated/numpy.vectorize.html
vfunc = np.vectorize(lambda x: np.argmin(np.abs(x - r_k))
min_idx = vfunc(d)
vfunc2 = np.vectorize(lambda x: r_k[0, x])
rk_hat = vfunc2(min_idx)
For the last two lines, d and rk_hat should be Npx by Npy matrixes. You can use matrix slicing or np.where to create a matrix mask.
if np.abs(d - rk_hat) < rho:
points = np.where( np.abs(d-rk_hat) < rho )
https://numpy.org/doc/stable/reference/generated/numpy.where.html
I give up the last line, it probably doesn't matter if you put it in a loop
a[m, l, min_idx] = a[m, l, min_idx] + 1
for xy in points:
a[xy[0],xy[1], min_idx[xy[0],xy[1]]] += 1
New answer which optimizes the the nested loop,
....
for i in range(0, np.size(true_col, 0)): #true_col and true_rows has the same size so it doesn't matter
for m in range(0, Npy):
for l in range(0, Npx):
There is a substantial improvement in processing time. For true_col and true_rows lengths of 2500 it takes about 3 seconds on my machine. It is in a function for testing purposes.
def new():
a = np.zeros((Npy, Npx, Nr),dtype=int)
# tease out and separate some of the terms
# used in the calculation of the distance - d
bb = N_im + 1
cc = (Npx * delta_px / 2)
dd = (Npy * delta_py / 2)
l, m = np.meshgrid(np.arange(Npx), np.arange(Npy))
q = (true_col - math.floor(bb / 2)) / bb / 2 # shape (true_col length,)
r = l * delta_px - cc + x0 # shape(Npy,Npx)
s = np.square(q - r[...,None]) # shape(Npy,Npx,true_col length)
# - last dimension is the outer loop of the original
t = (true_rows - math.floor(bb / 2)) / bb / 2 # shape (len(true_rows),)
u = m * delta_py - dd + y0 # shape(60,129) ... (Npx,Npy)
v = np.square(t - u[...,None]) # shape(Npy,Npx,true_col length)
d = np.sqrt(s + v) # shape(Npy,Npx,true_col length)
e1 = np.abs(d[...,None] - r_k.squeeze()) # shape(Npy,Npx,true_col length,len(r_k[0,:]))
min_idx = np.argmin(e1,-1) # shape(Npy,Npx,true_col length)
rk_hat = r_k[0,min_idx] # shape(Npy,Npx,true_col length)
zz = np.abs(d-rk_hat) # shape(Npy,Npx,true_col length)
condition = zz < rho # shape(Npy,Npx,true_col length)
# seemingly unavoidable for loop needed to perform
# a bincount along the last dimension (filtered)
# while retaining the 2d position info
# this will be pretty fast though,
# nothing really going on other than indexing and assignment
for iii in range(Npy*Npx):
row,col = divmod(iii,Npx)
filter = condition[row,col]
one_d = min_idx[row,col]
counts = np.bincount(one_d[filter])
a[row,col,:counts.size] = counts
return a
I could not figure out how to use Numpy methods to get rid of the final loop which filters for less then rho AND does a bincount - if I figure this out, I will update
Data from you question and comments
import math
import numpy as np
np.random.seed(5)
n_ = 2500
true_col = np.random.randint(0,256,n_)
true_rows = np.random.randint(0,256,n_)
N_im = 256
x0 = 0
y0 = -0.25
Npx = 129
Npy = 60
# Npx = 8
# Npy = 4
delta_py = 0.025
delta_px = 0.031
Nr = 9
delta_r = 0.5
rho = 0.063
epsilon = 0.75
r_min = 0.5
r_k = np.arange(Nr) * delta_r + r_min
r_k = r_k.reshape(1,Nr)
Your original nested loops in a function - with some diagnostic additions.
def original(writer=None):
'''writer should be a csv.Writer object.'''
a = np.zeros((Npy, Npx, Nr),dtype=int)
for i in range(0, np.size(true_col, 0)): #true_col and true_rows has the same size so it doesn't matter
for m in range(0, Npy):
for l in range(0, Npx):
d = math.sqrt(math.pow((((true_col[i] - math.floor((N_im + 1) / 2)) / (N_im + 1) / 2) - (l * delta_px - (Npx * delta_px / 2) + x0)),2) +
math.pow((((true_rows[i] - math.floor((N_im + 1) / 2)) / (N_im + 1) / 2) - (m * delta_py - (Npy * delta_py / 2) + y0)),2))
min_idx = np.argmin(np.abs(d - r_k)) # scalar
rk_hat = r_k[0, min_idx] # scalar
if np.abs(d - rk_hat) < rho:
# if (m,l) == (0,0):
if writer:
writer.writerow([i,m,l,d,min_idx,rk_hat,a[m, l, min_idx] + 1])
# print(f'condition satisfied: i:{i} a[{m},{l},{min_idx}] = {a[m, l, min_idx]} + 1')
a[m, l, min_idx] = a[m, l, min_idx] + 1
return a