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],
])
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 want to implement the forward kinematic of a robot with TensorFlow; mainly to gain automatic differentiation and to plug this module into larger network architectures.
In general I have a bunch of 4x4 transformation matrices, defined by the dh-parameters (d, theta, a, alpha) and the joint angle q:
[[ cos(theta+q), -sin(theta+q), 0, a],
[sin(theta+q)*cos(alpha), cos(theta+q)*cos(alpha), -sin(alpha), -sin(alpha)*d],
[sin(theta+q)*sin(alpha), cos(theta+q)*sin(alpha), cos(alpha), cos(alpha)*d],
[ 0, 0, 0, 1]])
My robot has 10 different joints, all connected sequentially.
I thought it would be smart to precompute sine and cosine.
q = tf.keras.layers.Input((10,))
sin_q = tf.sin(q)
cos_q = tf.cos(q)
Lets look at the transformation at the first joint with the specific set of dh-parameters (d=0.1055, theta=0, a=0, alpha=0):
m0 = [[cos(q0), -sin(q0), 0, 0],
[sin(q0), cos(q0), 0, 0],
0, 0, 1, 0.10550],
0, 0, 0, 1]]
My first problem is how to build something like this with TensorFlow?
In numpy I would initialize the matrix and fill in the nonzero values.
m_shape = tf.TensorShape((batch_size,4,4))
m0 = tf.zeros(m_shape)
m0[..., 0, 0] = cos_q[..., 0]
m0[..., 0, 1] = -sin_q[..., 0]
m0[..., 1, 0] = cos_q[..., 0]
m0[..., 1, 1] = sin_q[..., 0]
m0[..., 2, 3] = 0.10550
m0[..., 3, 3] = 1
Error -> 'Tensor' object does not support item assignment
But Tensorflow doesn't allow assignment for tensors.
It seems that the way to go is via tf.stack(). I need to create a vector of ones of the same size as my not specified batch_size, stack and reshape.
(Note: In the general case there are less zero values)
e = tf.ones_like(q[..., 0])
m0 = tf.stack([cos_q[..., 0], -sin_q[..., 0], 0*e, 0*e,
sin_q[..., 0], cos_q[..., 0], 0*e, 0*e,
0*e, 0*e, 1*e, 0.10550*e,
0*e, 0*e, 0*e, 1*e], axis=-1)
m0 = tf.keras.layers.Reshape((4, 4))(m0)
Is this correct or is there a smarter way to build such general transformations in TensorFlow?
As final result I am interested in the transformation at the end of the kinematic chain. I want to put in an array of different joint configurations (?, 10) and get the transformation at the end effector (?, 4, 4).
m_end = m0 # m1 # m2 # ... # m10
forward_net = tf.keras.Model(inputs=[q], outputs=[m_end]
result = forward_net.predict(np.random.random((100, 10)))
This works but its neither elegant nor fast.
The speed is my bigger problem; the same implementation in numpy is 150x faster.
How can I improve the speed? I thought TensorFlow should excel at tasks like this.
Should I build it as Model and use predict to calculate the results; there is nothing to learn here, so I am not sure what to use.
If you want to build 4x4 rotation matrices from an angle, or from the sine and cosine of an angle, you can do it like this:
import tensorflow as tf
def make_rotation(alpha, axis):
return make_rotation_sincos(tf.math.sin(alpha), tf.math.cos(alpha), axis)
def make_rotation_sincos(sin, cos, axis):
axis = axis.strip().lower()
zeros = tf.zeros_like(sin)
ones = tf.ones_like(sin)
if axis == 'x':
rot = tf.stack([
tf.stack([ ones, zeros, zeros], -1),
tf.stack([zeros, cos, -sin], -1),
tf.stack([zeros, sin, cos], -1),
], -2)
elif axis == 'y':
rot = tf.stack([
tf.stack([ cos, zeros, sin], -1),
tf.stack([zeros, ones, zeros], -1),
tf.stack([ -sin, zeros, cos], -1),
], -2)
elif axis == 'z':
rot = tf.stack([
tf.stack([ cos, -sin, zeros], -1),
tf.stack([ sin, cos, zeros], -1),
tf.stack([zeros, zeros, ones], -1),
], -2)
else:
raise ValueError('Invalid axis {!r}.'.format(axis))
last_row = tf.expand_dims(tf.stack([zeros, zeros, zeros], -1), -2)
last_col = tf.expand_dims(tf.stack([zeros, zeros, zeros, ones], -1), -1)
return tf.concat([tf.concat([rot, last_row], -2), last_col], -1)
About computing the forward kinematic chain, you can do that with tf.scan. For example, assuming the initial shape (?, 10):
# Make rotation matrices
rots = make_rotation(...)
rots_t = tf.transpose(rots, (1, 0, 2, 3))
out = tf.scan(tf.matmul, rots_t)[-1]
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
This is the Matlab code I want to replicate in OpenCv
e[~, threshold] = edge(I, 'sobel');
fudgeFactor = .5;
BWs = edge(I,'sobel', threshold * fudgeFactor);
figure, imshow(BWs), title('binary gradient mask');
This is my test image:
Cell image
I have tried things like
blurred_gray = cv2.GaussianBlur(gray_image,(3,3),0)
sobelx = cv2.Sobel(blurred_gray,cv2.CV_8U,1,0,ksize=3)
sobely = cv2.Sobel(blurred_gray,cv2.CV_8U,0,1,ksize=3)[2]
And the output I got is:
sobelx
sobely
I tried adding sobelx and sobely because I read they're partial derivatives, but the result image looks same as the above and varying the ksize didn't help.
This is the output I need:
edge image
Could someone please tell me what I'm doing wrong and what I should do to get the same result image?
The MATLAB implementation of the sobel edge detection isn't visible so we can only guess exactly what is happening. The only hint we get is from the documentation on edge states that when the 'sobel' option is used then
Finds edges at those points where the gradient of the image I is
maximum, using the Sobel approximation to the derivative.
It's not stated, but taking the maximum of the gradient is more complicated than simply taking the local maximums in the image. Instead we want to find local maximums with respect to the gradient direction. Unfortunately the actual code used by MATLAB for this operation is hidden.
Looking at the code that is available in edge it appears that they use 4*mean(magnitude) for the threshold in the thinning operation so I'm using this combined with your fudge factor. The orientated_non_max_suppression function is far from optimal but I wrote it for readability over performance.
import cv2
import numpy as np
import scipy.ndimage.filters
gray_image = cv2.imread('cell.png', cv2.IMREAD_GRAYSCALE).astype(dtype=np.float32)
def orientated_non_max_suppression(mag, ang):
ang_quant = np.round(ang / (np.pi/4)) % 4
winE = np.array([[0, 0, 0],
[1, 1, 1],
[0, 0, 0]])
winSE = np.array([[1, 0, 0],
[0, 1, 0],
[0, 0, 1]])
winS = np.array([[0, 1, 0],
[0, 1, 0],
[0, 1, 0]])
winSW = np.array([[0, 0, 1],
[0, 1, 0],
[1, 0, 0]])
magE = non_max_suppression(mag, winE)
magSE = non_max_suppression(mag, winSE)
magS = non_max_suppression(mag, winS)
magSW = non_max_suppression(mag, winSW)
mag[ang_quant == 0] = magE[ang_quant == 0]
mag[ang_quant == 1] = magSE[ang_quant == 1]
mag[ang_quant == 2] = magS[ang_quant == 2]
mag[ang_quant == 3] = magSW[ang_quant == 3]
return mag
def non_max_suppression(data, win):
data_max = scipy.ndimage.filters.maximum_filter(data, footprint=win, mode='constant')
data_max[data != data_max] = 0
return data_max
# compute sobel response
sobelx = cv2.Sobel(gray_image, cv2.CV_32F, 1, 0, ksize=3)
sobely = cv2.Sobel(gray_image, cv2.CV_32F, 0, 1, ksize=3)
mag = np.hypot(sobelx, sobely)
ang = np.arctan2(sobely, sobelx)
# threshold
fudgefactor = 0.5
threshold = 4 * fudgefactor * np.mean(mag)
mag[mag < threshold] = 0
# non-maximal suppression
mag = orientated_non_max_suppression(mag, ang)
# alternative but doesn't consider gradient direction
# mag = skimage.morphology.thin(mag.astype(np.bool)).astype(np.float32)
# create mask
mag[mag > 0] = 255
mag = mag.astype(np.uint8)
Results on the Cell
Python
MATLAB
Results on MATLAB's peppers.png (built-in)
Python
MATLAB
The MATLAB implementation must use something a little different but it looks like this gets pretty close.
My goal is to transform an image in such a way that three source points are mapped to three target points in an empty array. I have solved the finding of the correct affine matrix, however I cannot apply an affine transformation on a color image.
More specifically, I am struggling with the correct use of the scipy.ndimage.interpolation.affine_transform method. As this question and it's anwers point out, the affine_transform-method can be somewhat unintuitive (especially regarding offset calculation), however, user timday shows how apply a rotation and a shearing on an image and position it in another array, while user geodata gives more background information.
My problem is to generalize the approach shown there (1) to color images and (2) to an arbitrary transformation which I calculated myself.
This is my code (which should run as is on your computer):
import numpy as np
from scipy import ndimage
import matplotlib.pyplot as plt
def calcAffineMatrix(sourcePoints, targetPoints):
# For three source- and three target points, find the affine transformation
# Function works correctly, not part of the question
A = []
b = []
for sp, trg in zip(sourcePoints, targetPoints):
A.append([sp[0], 0, sp[1], 0, 1, 0])
A.append([0, sp[0], 0, sp[1], 0, 1])
b.append(trg[0])
b.append(trg[1])
result, resids, rank, s = np.linalg.lstsq(np.array(A), np.array(b))
a0, a1, a2, a3, a4, a5 = result
# Ignoring offset here, later use timday's suggested offset calculation
affineTrafo = np.array([[a0, a1, 0], [a2, a3, 0], [0, 0, 1]], 'd')
# Testing the correctness of transformation matrix
for i, _ in enumerate(sourcePoints):
src = sourcePoints[i]
src.append(1.)
trg = targetPoints[i]
trg.append(1.)
at = affineTrafo.copy()
at[2, 0:2] = [a4, a5]
assert(np.array_equal(np.round(np.array(src).dot(at)), np.array(trg)))
return affineTrafo
# Prepare source image
sourcePoints = [[162., 112.], [130., 112.], [162., 240.]]
targetPoints = [[180., 102.], [101., 101.], [190., 200.]]
image = np.empty((300, 300, 3), dtype='uint8')
image[:] = 255
# Mark border for better visibility
image[0:2, :] = 0
image[-3:-1, :] = 0
image[:, 0:2] = 0
image[:, -3:-1] = 0
# Mark source points in red
for sp in sourcePoints:
sp = [int(u) for u in sp]
image[sp[1] - 5:sp[1] + 5, sp[0] - 5:sp[0] + 5, :] = np.array([255, 0, 0])
# Show image
plt.subplot(3, 1, 1)
plt.imshow(image)
# Prepare array in which the image is placed
array = np.empty((400, 300, 3), dtype='uint8')
array[:] = 255
a2 = array.copy()
# Mark target points in blue
for tp in targetPoints:
tp = [int(u) for u in tp]
a2[tp[1] - 2:tp[1] + 2, tp[0] - 2:tp[0] + 2] = [0, 0, 255]
# Show array
plt.subplot(3, 1, 2)
plt.imshow(a2)
# Next 5 program lines are actually relevant for question:
# Calculate affine matrix
affineTrafo = calcAffineMatrix(sourcePoints, targetPoints)
# This follows the c_in-c_out method proposed in linked stackoverflow issue
# extended for color channel (no translation here)
c_in = np.array([sourcePoints[0][0], sourcePoints[0][1], 0])
c_out = np.array([targetPoints[0][0], targetPoints[0][1], 0])
offset = (c_in - np.dot(c_out, affineTrafo))
# Affine transform!
ndimage.interpolation.affine_transform(image, affineTrafo, order=2, offset=offset,
output=array, output_shape=array.shape,
cval=255)
# Mark blue target points in array, expected to be above red source points
for tp in targetPoints:
tp = [int(u) for u in tp]
array[tp[1] - 2:tp[1] + 2, tp[0] - 2:tp[0] + 2] = [0, 0, 255]
plt.subplot(3, 1, 3)
plt.imshow(array)
plt.show()
Other approaches I tried include working with the inverse, transpose or both of affineTrafo:
affineTrafo = np.linalg.inv(affineTrafo)
affineTrafo = affineTrafo.T
affineTrafo = np.linalg.inv(affineTrafo.T)
affineTrafo = np.linalg.inv(affineTrafo).T
In his answer, geodata shows how to calculate the matrix that affine_trafo needs to do a scaling and rotation:
If one wants a scaling S first and then a rotation R it holds that T=R*S and therefore T.inv=S.inv*R.inv (note the reversed order).
Which I tried to copy using matrix decomposition (decomposing my affine transformation into a rotation, a shearing and another rotation):
u, s, v = np.linalg.svd(affineTrafo[:2,:2])
uInv = np.linalg.inv(u)
sInv = np.linalg.inv(np.diag((s)))
vInv = np.linalg.inv(v)
affineTrafo[:2, :2] = uInv.dot(sInv).dot(vInv)
Again, without success.
For all of my results, it's not (only) an offset problem. It is clearly visible from the pictures that the relative positions of source and target points do not correspond.
I searched the web and stackoverflow and did not find an answer for my problem. Please help me! :)
I finally got it working thanks to AlexanderReynolds hint to use another library. This is of course a workaround; I could not get it working using scipy's affine_transform, so I used OpenCVs cv2.warpAffine instead. In case this is helpful to anyone else, this is my code:
import numpy as np
import matplotlib.pyplot as plt
import cv2
# Prepare source image
sourcePoints = [[162., 112.], [130., 112.], [162., 240.]]
targetPoints = [[180., 102.], [101., 101.], [190., 200.]]
image = np.empty((300, 300, 3), dtype='uint8')
image[:] = 255
# Mark border for better visibility
image[0:2, :] = 0
image[-3:-1, :] = 0
image[:, 0:2] = 0
image[:, -3:-1] = 0
# Mark source points in red
for sp in sourcePoints:
sp = [int(u) for u in sp]
image[sp[1] - 5:sp[1] + 5, sp[0] - 5:sp[0] + 5, :] = np.array([255, 0, 0])
# Show image
plt.subplot(3, 1, 1)
plt.imshow(image)
# Prepare array in which the image is placed
array = np.empty((400, 300, 3), dtype='uint8')
array[:] = 255
a2 = array.copy()
# Mark target points in blue
for tp in targetPoints:
tp = [int(u) for u in tp]
a2[tp[1] - 2:tp[1] + 2, tp[0] - 2:tp[0] + 2] = [0, 0, 255]
# Show array
plt.subplot(3, 1, 2)
plt.imshow(a2)
# Calculate affine matrix and transform image
M = cv2.getAffineTransform(np.float32(sourcePoints), np.float32(targetPoints))
array = cv2.warpAffine(image, M, array.shape[:2], borderValue=[255, 255, 255])
# Mark blue target points in array, expected to be above red source points
for tp in targetPoints:
tp = [int(u) for u in tp]
array[tp[1] - 2:tp[1] + 2, tp[0] - 2:tp[0] + 2] = [0, 0, 255]
plt.subplot(3, 1, 3)
plt.imshow(array)
plt.show()
Comments:
Interesting how it worked almost immediately after changing the library. After having spent more than a day trying to get it work with scipy, this is a lesson for myself to change libraries faster.
In case someone wants to find an (least squares) approximation for an affine transformation based on more than three points, this is how you get the matrix that works with cv2.warpAffine:
Code:
def calcAffineMatrix(sourcePoints, targetPoints):
# For three or more source and target points, find the affine transformation
A = []
b = []
for sp, trg in zip(sourcePoints, targetPoints):
A.append([sp[0], 0, sp[1], 0, 1, 0])
A.append([0, sp[0], 0, sp[1], 0, 1])
b.append(trg[0])
b.append(trg[1])
result, resids, rank, s = np.linalg.lstsq(np.array(A), np.array(b))
a0, a1, a2, a3, a4, a5 = result
affineTrafo = np.float32([[a0, a2, a4], [a1, a3, a5]])
return affineTrafo