I have a task to check if a matrix is a rotation matrix, I write code as follow:
import numpy as np
def isRotationMatrix(R):
# some code here
# return True or False
R = np.array([
[0, 0, 1],
[1, 0, 0],
[0, 1, 0],
])
print(isRotationMatrix(R)) # Should be True
R = np.array([
[-1, 0, 0],
[0, 1, 0],
[0, 0, 1],
])
print(isRotationMatrix(R)) # Should be False
I don't know how to implement the function isRotationMatrix.
My naive implement, it only works for a 3x3 matrix:
def isRotationMatrix(R_3x3):
should_be_norm_one = np.allclose(np.linalg.norm(R_3x3, axis=0), np.ones(shape=3))
x = R_3x3[:, 0].ravel()
y = R_3x3[:, 1].ravel()
z = R_3x3[:, 2].ravel()
should_be_perpendicular = \
np.allclose(np.cross(x, y), z) \
and np.allclose(np.cross(y, z), x) \
and np.allclose(np.cross(z, x), y)
return should_be_perpendicular and should_be_norm_one
I am using this definition of rotation matrix. A rotation matrix should satisfy the conditions M (M^T) = (M^T) M = I and det(M) = 1. Here M^T denotes transpose of M, I denotes identity matrix and det(M) represents determinant of matrix M.
You can use the following python code to check if the matrix is a rotation matrix.
import numpy as np
''' I have chosen `M` as an example. Feel free to put in your own matrix.'''
M = np.array([[0,-1,0],[1,0,0],[0,0,1]])
def isRotationMatrix(M):
tag = False
I = np.identity(M.shape[0])
if np.all((np.matmul(M, M.T)) == I) and (np.linalg.det(M)==1): tag = True
return tag
if(isRotationMatrix(M)): print 'M is a rotation matrix.'
else: print 'M is not a rotation matrix.'
A rotation matrix is a orthonormal matrix and its determinant should be 1.
My implement:
import numpy as np
def isRotationMatrix(R):
# square matrix test
if R.ndim != 2 or R.shape[0] != R.shape[1]:
return False
should_be_identity = np.allclose(R.dot(R.T), np.identity(R.shape[0], np.float))
should_be_one = np.allclose(np.linalg.det(R), 1)
return should_be_identity and should_be_one
if __name__ == '__main__':
R = np.array([
[0, 0, 1],
[1, 0, 0],
[0, 1, 0],
])
print(isRotationMatrix(R)) # True
R = np.array([
[-1, 0, 0],
[0, 1, 0],
[0, 0, 1],
])
print(isRotationMatrix(R)) # True
print(isRotationMatrix(np.zeros((3, 2)))) # False
Related
I'm following the code presented here:
Compute homography for a virtual camera with opencv
As a note, I made a tiny modification to the code: in the translation matrix, I'm left-multiplying the first 3 rows of the last column by -R to get the translation in the global frame. I also changed the translation matrix definition to use "-dist" because in the global frame, movement toward the camera would be in the negative z direction.
When I turn the X rotation to 0, I get a weird... aliased version of the loaded image that appears ABOVE the horizon line, where there should be nothing.
My question:
Why? Is this just a weird artifact of how the homography is calculated? How can I get rid of it? I know for x=0 (in the presented code) I can just ignore/erase anything above the horizon line, but my use case the x rotation might be -10 to 10 degrees or so - how can I calculate where the horizon line would be in those cases (so I can ignore image data above it) - or is there a mathematical solution the computing the homography that will bypass this problem all together?
Thanks!
EDIT: Adding in code/image in question:
import cv2
import numpy as np
rotXdeg = 90
rotYdeg = 90
rotZdeg = 90
f = 500
dist = 500
def onRotXChange(val):
global rotXdeg
rotXdeg = val
def onRotYChange(val):
global rotYdeg
rotYdeg = val
def onRotZChange(val):
global rotZdeg
rotZdeg = val
def onFchange(val):
global f
f=val
def onDistChange(val):
global dist
dist=val
if __name__ == '__main__':
#Read input image, and create output image
src = cv2.imread('/path/to/image.jpg')
dst = np.ndarray(shape=src.shape,dtype=src.dtype)
#Create user interface with trackbars that will allow to modify the parameters of the transformation
wndname1 = "Source:"
wndname2 = "WarpPerspective: "
cv2.namedWindow(wndname1, 1)
cv2.namedWindow(wndname2, 1)
cv2.createTrackbar("Rotation X", wndname2, rotXdeg, 180, onRotXChange)
cv2.createTrackbar("Rotation Y", wndname2, rotYdeg, 180, onRotYChange)
cv2.createTrackbar("Rotation Z", wndname2, rotZdeg, 180, onRotZChange)
cv2.createTrackbar("f", wndname2, f, 2000, onFchange)
cv2.createTrackbar("Distance", wndname2, dist, 2000, onDistChange)
#Show original image
cv2.imshow(wndname1, src)
h , w = src.shape[:2]
while True:
rotX = (rotXdeg - 90)*np.pi/180
rotY = (rotYdeg - 90)*np.pi/180
rotZ = (rotZdeg - 90)*np.pi/180
#Projection 2D -> 3D matrix
A1= np.matrix([[1, 0, -w/2],
[0, 1, -h/2],
[0, 0, 0 ],
[0, 0, 1 ]])
# Rotation matrices around the X,Y,Z axis
RX = np.matrix([[1, 0, 0, 0],
[0,np.cos(rotX),-np.sin(rotX), 0],
[0,np.sin(rotX),np.cos(rotX) , 0],
[0, 0, 0, 1]])
RY = np.matrix([[ np.cos(rotY), 0, np.sin(rotY), 0],
[ 0, 1, 0, 0],
[ -np.sin(rotY), 0, np.cos(rotY), 0],
[ 0, 0, 0, 1]])
RZ = np.matrix([[ np.cos(rotZ), -np.sin(rotZ), 0, 0],
[ np.sin(rotZ), np.cos(rotZ), 0, 0],
[ 0, 0, 1, 0],
[ 0, 0, 0, 1]])
#Composed rotation matrix with (RX,RY,RZ)
R = RX * RY * RZ
#Translation matrix on the Z axis change dist will change the height
T = np.matrix([[1,0,0,0],
[0,1,0,0],
[0,0,1,-dist],
[0,0,0,1]])
extractT = T[:3, 3:4]
solveT = -R[:3, :3]#extractT
T[:3, 3:4] = solveT
#Camera Intrisecs matrix 3D -> 2D
A2= np.matrix([[f, 0, w/2,0],
[0, f, h/2,0],
[0, 0, 1,0]])
# Final and overall transformation matrix
H = A2 * (T * (R * A1))
# Apply matrix transformation
cv2.warpPerspective(src, H, (w, h), dst, cv2.INTER_CUBIC)
#Show the image
cv2.imshow(wndname2, dst)
if (cv2.waitKey(1) == ord('q')):
break
Image:
I have an array a of length N and need to implement the following operation:
With p in [0..1]. This equation is a lossy sum, where the first indexes in the sum are weighted by a greater loss (p^{n-i}) than the last ones. The last index (i=n) is always weigthed by 1. if p = 1, then the operation is a simple cumsum.
b = np.cumsum(a)
If if p != 1, I can implement this operation in a cpu-inefficient way:
b = np.empty(np.shape(a))
# I'm using the (-1,-1,-1) idiom for reversed ranges
p_vec = np.power(p, np.arange(N-1, 0-1, -1))
# p_vec[0] = p^{N-1}, p_vec[-1] = 1
for n in range(N):
b[n] = np.sum(a[:n+1]*p_vec[-(n+1):])
Or in a memory-inefficient but vectorized way (IMO is cpu inefficient too, since a lot of work is wasted):
a_idx = np.reshape(np.arange(N+1), (1, N+1)) - np.reshape(np.arange(N-1, 0-1, -1), (N, 1))
a_idx = np.maximum(0, a_idx)
# For N=4, a_idx looks like this:
# [[0, 0, 0, 0, 1],
# [0, 0, 0, 1, 2],
# [0, 0, 1, 2, 3],
# [0, 1, 2, 3, 4]]
a_ext = np.concatenate(([0], a,), axis=0) # len(a_ext) = N + 1
p_vec = np.power(p, np.arange(N, 0-1, -1)) # len(p_vec) = N + 1
b = np.dot(a_ext[a_idx], p_vec)
Is there a better way to achieve this 'lossy' cumsum?
What you want is a IIR filter, you can use scipy.signal.lfilter(), here is the code:
Your code:
import numpy as np
N = 10
p = 0.8
np.random.seed(0)
x = np.random.randn(N)
y = np.empty_like(x)
p_vec = np.power(p, np.arange(N-1, 0-1, -1))
for n in range(N):
y[n] = np.sum(x[:n+1]*p_vec[-(n+1):])
y
the output:
array([1.76405235, 1.81139909, 2.42785725, 4.183179 , 5.21410119,
3.19400307, 3.50529088, 2.65287549, 2.01908154, 2.02586374])
By using lfilter():
from scipy import signal
y = signal.lfilter([1], [1, -p], x)
print(y)
the output:
array([1.76405235, 1.81139909, 2.42785725, 4.183179 , 5.21410119,
3.19400307, 3.50529088, 2.65287549, 2.01908154, 2.02586374])
I've been tinkering with this python snippet which is supposed to demonstrate four different approaches of calculating PageRank.
the code:
from numpy import *
def powerMethodBase(A,x0,iter):
""" basic power method """
for i in range(iter):
x0 = dot(A,x0)
x0 = x0/linalg.norm(x0,1)
return x0
def powerMethod(A,x0,m,iter):
""" power method modified to compute
the maximal real eigenvector
of the matrix M built on top of the input matrix A """
n = A.shape[1]
delta = m*(array([1]*n,dtype='float64')/n) # array([1]*n is [1 1 ... 1] n times
for i in range(iter):
x0 = dot((1-m),dot(A,x0)) + delta
return x0
def maximalEigenvector(A):
""" using the eig function to compute eigenvectors """
n = A.shape[1]
w,v = linalg.eig(A)
return abs(real(v[:n,0])/linalg.norm(v[:n,0],1))
def linearEquations(A,m):
""" solving linear equations
of the system (I-(1-m)*A)*x = m*s """
n = A.shape[1]
C = eye(n,n)-dot((1-m),A)
b = m*(array([1]*n,dtype='float64')/n)
return linalg.solve(C,b)
def getTeleMatrix(A,m):
""" return the matrix M
of the web described by A """
n = A.shape[1]
S = ones((n,n))/n
return (1-m)*A+m*S
A = array([ [0, 0, 0, 1, 0, 1],
[1/2.0, 0, 0, 0, 0, 0],
[0, 1/2.0, 0, 0, 0, 0],
[0, 1/2.0, 1/3.0, 0, 0, 0],
[0, 0, 1/3.0, 0, 0, 0],
[1/2.0, 0, 1/3.0, 0, 1, 0 ] ])
n = A.shape[1] # A is n x n
m = 0.15
M = getTeleMatrix(A,m)
x0 = [1]*n
x1 = powerMethod(A,x0,m,130)
x2 = powerMethodBase(M,x0,130)
x3 = maximalEigenvector(M)
x4 = linearEquations(A,m)
# comparison of the four methods
labels = range(1,6)
print array([labels, x1, x2, x3, x4]).T
The expected output:
[[ 1. 0.32954577 0.32954577 0.32954577 0.32954577]
[ 2. 0.16505695 0.16505695 0.16505695 0.16505695]
[ 3. 0.0951492 0.0951492 0.0951492 0.0951492 ]
[ 4. 0.12210815 0.12210815 0.12210815 0.12210815]
[ 5. 0.05195894 0.05195894 0.05195894 0.05195894]
[ 6. 0.23618099 0.23618099 0.23618099 0.23618099]]
After modifying line 59 to labels = range(1,7), I ran the script locally and got the expected output above. Note how each line in the output repeats the same page rank four times to demonstrate that all four methods of calculating pagerank work and return the same results.
However, when I swap out the A array for a different one:
A = array([ [0, 0, 1/2.0, 0],
[1, 0, 1/2.0, 0],
[0, 0, 0, 0],
[0, 1, 0, 0] ])
It gives me this output:
[[ 1. 0.0534375 0.11790211 0.11790211 0.0534375 ]
[ 2. 0.09885937 0.29698454 0.29698454 0.09885937]
[ 3. 0.0375 0.06701063 0.06701063 0.0375 ]
[ 4. 0.12153047 0.51810272 0.51810272 0.12153047]]
note how the first and fourth methods return the same result and the second and third return the same results, but all four are returning different answers. What am I doing wrong here? Are the four methods supposed to return different results at times?
I tried modifying the getTeleMatrix() function but it didn't solve the problem.
Any help is greatly appreciated.
I want to solve for the extrinsics by using direct linear transformation on corresponding 3D LIDAR points and 2D camera points. I already have the intrinsics.
Problem is, points behind the camera gets re-projected as well (see picture below).
So I constrain to only points "in front of the camera", i.e z > 0. The problem is, on different trials where different sets of points are used, the produced extrinsic matrix produces differing axes. Sometimes, constraining z > 0 gives the right results (centre part of image), whereas other times I need z < 0, which I believe to be the z-axis going into the camera. So the question is, how do I constrain the Z axes of the camera to be sticking out of the camera?
def with_intrinsic(points2d, points3d, intrinsic):
cam1_K_inverse = np.linalg.inv(intrinsic)
#direct linear transformation calibration, assumes no intrinsic matrix
assert points2d.shape[0] >= 3
assert points3d.shape[0] == points2d.shape[0]
A = []
points2d_homo = []
for u,v in points2d:
points2d_homo.append([u, v, 1])
points2d_homo = np.array(points2d_homo).T #columns to be data points
points2d_inv = np.dot(cam1_K_inverse, points2d_homo).T
assert points2d_inv.shape == (points2d.shape[0], 3)
assert points2d_inv[0, 2] == 1
for idx in range(points2d.shape[0]):
x3d, y3d, z3d = points3d[idx]
u, v, _ = points2d_inv[idx]
A.append([x3d, y3d, z3d, 1, 0, 0, 0, 0, -u * x3d, -u * y3d, -u * z3d, -u])
A.append([0, 0, 0, 0, x3d, y3d, z3d, 1, -v * x3d, -v * y3d, -v * z3d, -v])
A = np.array(A)
U, D, VT = np.linalg.svd(A)
M = VT.T[:, -1].reshape((3, 4))
error = get_reprojection_error(points2d, points3d, intrinsic, M)
logging.debug("error with_intrinsic: %s", error)
return M
update: I tried to check if re-projecting 1 of the "training" points will yield me z < 0. If so, I do a np.dot(R, extrinsic) to rotate the point about PI radians around 1 of the axis. I've tried all 3 axes but that still don't seem to yield the correct result.
R1 = np.array([
[1, 0, 0],
[0, np.cos(pi), -np.sin(pi)],
[0, np.sin(pi), np.cos(pi)],
])
R2 = np.array([
[np.cos(pi), 0, np.sin(pi)],
[0, 1, 0],
[-np.sin(pi), 0, np.cos(pi)],
])
R3 = np.array([
[np.cos(pi), -np.sin(pi), 0],
[np.sin(pi), np.cos(pi), 0],
[0, 0, 1],
])
I am doing some matrix computation with sympy 0.7.6 in python 2.7.10. For example,
M =
[cos(q1), -6.12323399573677e-17*sin(q1), -1.0*sin(q1), 150*sin(q1)]
[sin(q1), 6.12323399573677e-17*cos(q1), 1.0*cos(q1), 150*sin(q1)]
[ 0, -1.0, 6.12323399573677e-17, 445]
[ 0, 0, 0, 1]
Then I apply simplify to M and it results in:
M =
[cos(q1), 0, -1.0*sin(q1), 150*sin(q1)]
[sin(q1), 0, 1.0*cos(q1), 150*sin(q1)]
[ 0, -1.0, 6.12323399573677e-17, 445]
[ 0, 0, 0, 1]
It's clear that -6.12323399573677e-17*sin(q1) is simplified to 0 but 6.12323399573677e-17 is not. Is it possible to simplify the pure number item with simplify?
Sympy’s nsimplify function with the rational=True argument converts floats within an expression to rational numbers (within a given tolerance). Something like 6.12323399573677e-17 will be converted to 0 if below the threshold. So, in your case:
from sympy import Symbol, Matrix, sin, cos, nsimplify
q1 = Symbol("q1")
M = Matrix([
[cos(q1), -6.12323e-17*sin(q1), 1.0*sin(q1), 150*sin(q1)],
[sin(q1), 6.12323e-17*cos(q1), 1.0*cos(q1), 150*sin(q1)],
[ 0, -1.0, 6.123233e-17, 445],
[ 0, 0, 0, 1],
])
nsimplify(M,tolerance=1e-10,rational=True)
# Matrix([
# [cos(q1), 0, -sin(q1), 150*sin(q1)],
# [sin(q1), 0, cos(q1), 150*sin(q1)],
# [ 0, -1, 0, 445],
# [ 0, 0, 0, 1]])
Note how this also converted -1.0 to -1.
If you are using a Matrix (sympy.matrices.dense.MutableDenseMatrix), including one with sybolic elements, the conversion can be done with the following function:
def round2zero(m, e):
for i in range(m.shape[0]):
for j in range(m.shape[1]):
if (isinstance(m[i,j], Float) and m[i,j] < e):
m[i,j] = 0
For example:
from sympy import *
e = .0000001 # change according to your definition of small
x, y, z = symbols('x y z')
mlist = [[0.0, 1.0*cos(z)], [x*y, 1.05000000000000], [0, 6.12323399573677e-17]]
m = Matrix(mlist)
m
Out[4]:
Matrix([
[0.0, 1.0*cos(z)],
[x*y, 1.05],
[ 0, 6.12323399573677e-17]])
round2zero(m,e)
m
Matrix([
[ 0, 1.0*cos(z)],
[x*y, 1.05],
[ 0, 0]])
Some fuzzing can help detect more values that can be set to zero:
import sympy
import random
def fuzz_simplify(matrix, min=-1.0, max=1.0, iterations=1000, tolerance=0.005):
m = sympy.Matrix(matrix)
free_sym = range(len(J.free_symbols))
f = sympy.lambdify(m.free_symbols,m)
sum = f(*[0 for i in free_sym])
for i in range(0, iterations):
rand_params = [random.uniform(min,max) for i in free_sym]
sum += f(*rand_params)
for i in range(0, J.shape[0]):
for j in range(0, J.shape[1]):
if sum[i,j] < tolerance:
m[i,j] *= 0
return m