def getPoly(B, a, c, f1, f2, size):
Wa = np.diag(a)
BTWa = np.matmul(B.T, Wa)
A1_poly = np.zeros((h, w, 2, 2), dtype=np.float64)
A2_poly = np.zeros((h, w, 2, 2), dtype=np.float64)
b1_poly = np.zeros((h, w, 2), dtype=np.float64)
b2_poly = np.zeros((h, w, 2), dtype=np.float64)
windowSize = (size, size)
for y in range(0, f1.shape[0], 1):
for x in range(0, f1.shape[1], 1):
p_f1 = f1[y:y + windowSize[1], x:x + windowSize[0]].flatten()
p_f2 = f2[y:y + windowSize[1], x:x + windowSize[0]].flatten()
Wc = np.diag(c[y:y + windowSize[1], x:x + windowSize[0]].flatten())
if (p_f1.shape[0] != size * size):
continue
BTWaWc = np.matmul(BTWa, Wc)
BTWaWcB = np.matmul(BTWaWc, B)
#Solve systems of linear equations Ax = B for x
_, r1 = cv.solve(BTWaWcB, np.matmul(BTWaWc, p_f1))
_, r2 = cv.solve(BTWaWcB, np.matmul(BTWaWc, p_f2))
A1 = [[r1[3], r1[5] / 2], [r1[5] / 2, r1[4]]]
A2 = [[r2[3], r2[5] / 2], [r2[5] / 2, r2[4]]]
b1 = [r1[1], r1[2]]
b2 = [r2[1], r2[2]]
A1_poly[y, x, :, :] = A1
A2_poly[y, x, :, :] = A2
b1_poly[y, x, :] = b1
b2_poly[y, x, :] = b2
return A1_poly, A2_poly, b1_poly, b2_poly
def linearlstriangulation(u,P1,u1,P2):
A = np.zeros((4, 3))
A[0][0] = u[0]*P1[2][0]-P1[0][0]
A[0][1] = u[0]*P1[2][1]-P1[0][1]
A[0][2] = u[0]*P1[2][2]-P1[0][2]
A[1][0] = u[1]*P1[2][0]-P1[1][0]
A[1][1] = u[1]*P1[2][1]-P1[1][1]
A[1][2] = u[1]*P1[2][2]-P1[1][2]
A[2][0] = u1[0]*P2[2][0]-P2[0][0]
A[2][1] = u1[0]*P2[2][1]-P2[0][1]
A[2][2] = u1[0]*P2[2][2]-P2[0][2]
A[3][0] = u1[1]*P2[2][0]-P2[1][0]
A[3][1] = u1[1]*P2[2][1]-P2[1][1]
A[3][2] = u1[1]*P2[2][2]-P2[1][2]
B = np.zeros((4, 1))
B[0][0] = -(u[0]*P1[2][3]-P1[0][3])
B[1][0] = -(u[1]*P1[2][3]-P1[1][3])
B[2][0] = -(u1[0]*P2[2][3]-P2[0][3])
B[3][0] = -(u1[1]*P2[2][3]-P2[1][3])
X = np.zeros((4, 1))
AA = cv2.solve(A, B, X, cv2.DECOMP_SVD)
print "========================cv2 solve========================================="
print cv2.solve(A, B, X, cv2.DECOMP_SVD)[1]
print "========================np linalg solve==================================="
print np.linalg.lstsq(A, B)[0]
return cv2.solve(A, B, X, cv2.DECOMP_SVD)[1]
def polyTriangulate(F, projPointsR, projPointsL):
import cv2
import numpy as np
rx = projPointsR[0, i]
ry = projPointsR[1, i]
lx = projPointsL[0, i]
ly = projPointsL[1, i]
T_R = ((1, 0, -rx), (0, 1, -ry), (0, 0, 1))
T_R = np.asarray(T_R)
T_L = ((1, 0, -lx), (0, 1, -ly), (0, 0, 1))
T_L = np.asarray(T_L)
F = np.linalg.inv(T_L).T * F * np.linalg.inv(T_R)
e_R = zeros((3))
e_L = zeros((3))
_, e_R = cv2.solve(F, np.zeros((3)), e_R, cv2.DECOMP_SVD)
_, e_L = cv2.solve(F, np.zeros((3)), e_L, cv2.DECOMP_SVD)
def copy_warp_perspective(image, matrix, dim_out):
image_blank = np.ndarray((dim_out[1], dim_out[0], 3), dtype=float)
rows1, columns1 = (dim_out[0], dim_out[1])
for i in range(rows1):
for j in range(columns1):
image_blank[j, i] = (255, 255, 255)
matrix_in = np.ndarray((2, 1), dtype=float)
valores = np.ndarray((2, 1), dtype=float)
for x in range(1, rows1):
for y in range(1, columns1):
matrix_copy = np.copy(matrix)
matrix_copy[0, :] = matrix_copy[0, :] / float(x)
matrix_copy[0, :2] = matrix_copy[0, :2] - matrix_copy[2, :2]
matrix_copy[1, :] = matrix_copy[1, :] / float(y)
matrix_copy[1, :2] = matrix_copy[1, :2] - matrix_copy[2, :2]
matrix_in[0, 0] = matrix_copy[2, 2] - matrix_copy[0, 2]
matrix_in[1, 0] = matrix_copy[2, 2] - matrix_copy[1, 2]
cv.solve(matrix_copy[:2, :2], matrix_in, valores)
valor_x = valores[0, 0]
valor_y = valores[1, 0]
if 0 <= valor_x < image.shape[1] and 0 <= valor_y < image.shape[0]:
image_blank[y, x] = image[int(valor_y), int(valor_x)]
image_blank[0, 0] = image_blank[1, 1]
for i in range(1, rows1):
image_blank[0, i] = image_blank[1, i]
for i in range(1, columns1):
image_blank[i, 0] = image_blank[i, 1]
return image_blank
def getAffineTransform(pts1, pts2):
A = np.float32(np.concatenate((pts1, [[1], [1], [1]]), axis=1))
pts2 = np.float32(pts2)
b1 = pts2[:, 0]
b2 = pts2[:, 1]
x = cv2.solve(A, b1)
x2 = cv2.solve(A, b2)
return np.float32(np.concatenate((x[1], x2[1])).reshape(2, 3))
def delta_P(P2, P1):
"""
Return P = P2 '-' P1,
where '-' denotes the difference between perspective transformations.
More accurately: P2 = P * P1, solved for "P".
"""
P = np.empty((4, 4))
cv2.solve(P1.T, P2.T, P, cv2.DECOMP_SVD)
P = P.T
P[3, 0:3] = 0 # make sure these are zero
P[3, 3] = 1 # make sure this is one
return P
def matrix_get_affine(pts1, pts2):
answer = np.zeros((2, 3), dtype=np.float64)
matrix_a = np.zeros((3, 3), dtype=np.float64)
matrix_b = np.zeros((3, 1), dtype=np.float64)
for i in range(len(pts1)):
matrix_a[i, :] = np.array([pts1[i][0], pts1[i][1], 1], dtype=np.float64)
matrix_b[i, 0] = pts2[i][0]
value = cv.solve(matrix_a, matrix_b)[1]
answer[0, :] = np.transpose(value)
for i in range(len(pts1)):
matrix_b[i, 0] = pts2[i][1]
value = cv.solve(matrix_a, matrix_b)[1]
answer[1, :] = np.transpose(value)
return answer
def FitPoints(self, points):
if points is None:
return (False, None)
length = len(points)
if length < 3:
return (False, None)
A = np.ones((length, 3), np.float)
b = np.zeros((length, 1), np.float)
for ind in range(length):
A[ind, 0] = points[ind, 1] * points[ind, 1]
A[ind, 1] = points[ind, 1]
b[ind, 0] = points[ind, 0]
x_sys = cv2.solve(A, b, flags=cv2.DECOMP_LU | cv2.DECOMP_NORMAL)
if x_sys[0]:
m = LaneLineModel()
m.a = x_sys[1][0]
m.b = x_sys[1][1]
m.c = x_sys[1][2]
return (True, m)
return False, None
def GetPerspectiveTransform(pi,ps): x1,y1 = pi[0] x2,y2 = pi[1] x3,y3 = pi[2] x4,y4 = pi[3] xx1,yy1 = ps[0] xx2,yy2 = ps[1] xx3,yy3 = ps[2] xx4,yy4 = ps[3] A = np.array([ [x1,y1,1,0,0,0, -x1*xx1, -xx1*y1 ], [0,0,0,x1,y1,1, -x1*yy1, -y1*yy1], [x2,y2, 1,0,0,0, -x2*xx2, -xx2*y2], [0,0,0,x2,y2,1, -x2*yy2, -y2*yy2], [x3,y3,1,0,0,0, -x3*xx3, -xx3*y3], [0,0,0,x3,y3,1, -x3*yy3, -y3*yy3], [x4,y4,1,0,0,0, -x4*xx4, -xx4*y4], [0,0,0,x4,y4,1, -x4*yy4, -y4*yy4] ],np.float32) #print(A) ps = ps.flatten() x = cv2.solve(A,ps) x = x[1].flatten() x = np.append(x,np.float32(1)) M = x.reshape(3,3) return M
def multi_triangulate(cams, pts, views, max_iter=1, stop_epsilon=1e-6):
A = np.zeros((len(views) * 2, 4))
prev_weights = np.ones((len(views)))
weights = np.ones((len(views)))
assert (len(views) == len(pts))
for it in range(0, max_iter):
for i in range(0, len(views)):
pt = pts[i]
vi = views[i]
M = cams[vi]
r = 2 * i
A[r, :] = (pt[0] * M[2, :] - M[0, :]) / weights[i]
A[r + 1, :] = (pt[1] * M[2, :] - M[1, :]) / weights[i]
A_ = A[:, 0:3]
B_ = -A[:, 3:4]
# B_ *= -1
# out = np.linalg.lstsq(A_, B_)[0] # alternative
out = cv2.solve(A_, B_, flags=cv2.DECOMP_SVD)[1]
for i in range(0, len(views)):
vi = views[i]
M = cams[vi]
# compute w-component of projected point
weights[i] = np.dot(M[2, 0:3], out) + M[2, 3]
tmp = weights - prev_weights
lo = np.min(tmp)
hi = np.max(tmp)
hi = max(-lo, hi)
if hi <= stop_epsilon:
break
prev_weights, weights = weights, prev_weights
return out
def compute_affine_transform(points, refpoints, w=None):
'''
compute the affine tranform matrix
'''
if w == None:
w = [1] * (len(points) * 2)
assert (len(w) == 2 * len(points))
y = []
for n, p in enumerate(refpoints):
y += [p[0] / w[n * 2], p[1] / w[n * 2 + 1]]
A = []
for n, p in enumerate(points):
A.extend([[p[0] / w[n * 2], p[1] / w[n * 2], 0, 0, 1 / w[n * 2], 0],
[
0, 0, p[0] / w[n * 2 + 1], p[1] / w[n * 2 + 1], 0,
1 / w[n * 2 + 1]
]])
lstsq = cv2.solve(np.array(A), np.array(y), flags=cv2.DECOMP_SVD)
h11, h12, h21, h22, dx, dy = lstsq[1]
#R = np.array([[h11, h12, dx], [h21, h22, dy]])
# The row above works too - but creates a redundant dimension
R = np.array([[h11[0], h12[0], dx[0]], [h21[0], h22[0], dy[0]]])
return R
def get_ball_center(normalize_3D):
'''
求四个特征点的球心
由4个点的坐标在球面上的关系式可转换成线性方程组的求解
（x-xi)^2+(y-yi)^2+(z-zi)^2=r^2,最终可转化成AX=b的求解，A为3*3,b为3*1
:param normalize_3D:
:return: 球心坐标(x,y,z)
'''
_ = 0
if normalize_3D.shape[0] != 4:
return -1
A = np.array([(normalize_3D[0] - normalize_3D[1]),
(normalize_3D[3] - normalize_3D[3]),
(normalize_3D[1] - normalize_3D[2])])
tmp = np.array([
np.sum(normalize_3D[0]**2),
np.sum(normalize_3D[1]**2),
np.sum(normalize_3D[2]**2),
np.sum(normalize_3D[3]**2)
])
b = 1 / 2 * np.array([(tmp[0] - tmp[1]), (tmp[2] - tmp[3]),
(tmp[1] - tmp[2])])
_, X = cv2.solve(A, b, _, cv2.DECOMP_SVD) # 奇异值分解
ball_center = np.array([X[0][0], X[1][0], X[2][0]])
return ball_center
def multi_triangulate(cams, pts, views, max_iter=1, stop_epsilon=1e-6):
A = np.zeros((len(views) * 2, 4))
prev_weights = np.ones((len(views)))
weights = np.ones((len(views)))
assert len(views) == len(pts)
for it in range(0, max_iter):
for i in range(0, len(views)):
pt = pts[i]
vi = views[i]
M = cams[vi]
r = 2 * i
A[r, :] = (pt[0] * M[2, :] - M[0, :]) / weights[i]
A[r + 1, :] = (pt[1] * M[2, :] - M[1, :]) / weights[i]
A_ = A[:, 0:3]
B_ = -A[:, 3:4]
# B_ *= -1
# out = np.linalg.lstsq(A_, B_)[0] # alternative
out = cv2.solve(A_, B_, flags=cv2.DECOMP_SVD)[1]
for i in range(0, len(views)):
vi = views[i]
M = cams[vi]
# compute w-component of projected point
weights[i] = np.dot(M[2, 0:3], out) + M[2, 3]
tmp = weights - prev_weights
lo = np.min(tmp)
hi = np.max(tmp)
hi = max(-lo, hi)
if hi <= stop_epsilon:
break
prev_weights, weights = weights, prev_weights
return out
def get_3D_point(u1, P1, u2, P2):
"""Solves for 3D point using homogeneous 2D points and the respective camera matrices"""
A = np.array([[
u1[0] * P1[2, 0] - P1[0, 0], u1[0] * P1[2, 1] - P1[0, 1],
u1[0] * P1[2, 2] - P1[0, 2]
],
[
u1[1] * P1[2, 0] - P1[1, 0], u1[1] * P1[2, 1] - P1[1, 1],
u1[1] * P1[2, 2] - P1[1, 2]
],
[
u2[0] * P2[2, 0] - P2[0, 0], u2[0] * P2[2, 1] - P2[0, 1],
u2[0] * P2[2, 2] - P2[0, 2]
],
[
u2[1] * P2[2, 0] - P2[1, 0], u2[1] * P2[2, 1] - P2[1, 1],
u2[1] * P2[2, 2] - P2[1, 2]
]])
B = np.array([
-(u1[0] * P1[2, 3] - P1[0, 3]), -(u1[1] * P1[2, 3] - P1[1, 3]),
-(u2[0] * P2[2, 3] - P2[0, 3]), -(u2[1] * P2[2, 3] - P2[1, 3])
])
X = cv2.solve(A, B, flags=cv2.DECOMP_SVD)
return X[1]
def cal_X(l, delta):
B = np.zeros_like(l)
for i in range(l.shape[1]):
B[:, i] = l[:, i] * delta
A = l.T.dot(l)
_, X = cv2.solve(A, B.T, flags=cv2.DECOMP_NORMAL)
return X
def encode_iris(norm_img, mask_img, order=16, eps_lb=0.25, eps_ub=1.0):
# getting image dimensions
height, width = norm_img.shape
# computing Mf matrix
A = ComputeMfAnnular(width, height, order, eps_lb, eps_ub)
# reshaping image (as a column vector)
b = norm_img.reshape((height * width, ))
# changing datatype to float64
b = b.astype(np.float64)
# # applying mask to the Mf matrix and the normalized image
# mask_img = mask_img.reshape((height * width,))
# # creating the filter
# mask = mask_img[:] == 1
#
# # filtering Mf and normalized image
# A = A[mask] # getting only "good" rows
# b = b[mask] # getting only "good" rows
# solving the corresponding SEL (Ax = b) by min squares
result, x = cv2.solve(src1=A, src2=b, flags=cv2.DECOMP_QR)
# if there was an error of some kind
if not result:
return UNKNOWN_FAIL, None, None
# returning the solution of the SEL if any
return SUCCESS, x, None
def get_P_matrix(normalize_3D, normalize_2D):
num = normalize_2D.shape[0]
_ = 0 # _是要舍弃的值
if num < 3:
return -1
elif num == 3:
_, r1_T = cv2.solve(normalize_3D.T, normalize_2D.T[0], _)
_, r2_T = cv2.solve(normalize_3D.T, normalize_2D.T[1], _)
else:
_, r1_T = cv2.solve(normalize_3D, normalize_2D.T[0], _, cv2.DECOMP_SVD) # 奇异值分解
_, r2_T = cv2.solve(normalize_3D, normalize_2D.T[1], _, cv2.DECOMP_SVD)
r1 = r1_T.T[0]
r2 = r2_T.T[0]
r3 = np.cross(r1, r2)
P = np.array([r1, r2])
return P
def compress(self, basis, Q):
self.compressed = True
self.Q = Q
# compressed_bins = []
n_features = (1 << len(self.features))
self.compressed_bins = np.zeros((n_features, Q), dtype=int)
self.compressed_coeffs = np.zeros((n_features, Q))
for b, current_bin in enumerate(self.bins):
# compressed_bin = np.zeros((Q, 2))
residual = current_bin.copy()
for k in xrange(Q):
max_i = np.argmax(basis.dot(residual))
self.compressed_bins[b][k] = max_i
compressed_matrix = np.zeros((self.n_landmarks*2, k+1))
for j in xrange(k+1):
compressed_matrix[:, j] = basis[self.compressed_bins[b][j]]
compressed_matrix_t = np.transpose(compressed_matrix)
retval, dst = cv2.solve(compressed_matrix_t.dot(compressed_matrix),
compressed_matrix_t.dot(current_bin), flags=cv2.DECOMP_SVD)
for j in xrange(k+1):
self.compressed_coeffs[b][j] = dst[j]
residual -= compressed_matrix.dot(dst).reshape(2*self.n_landmarks,)
self.bins = None
def LinearTriangulation(P1, p1, P2, p2):
# points u are normalised (x, y)
A = np.zeros((4, 3), dtype='float32')
B = np.zeros((4, 1), dtype='float32')
X = np.zeros((3, 1), dtype='float32')
# Top two rows of A for P1 and p1
A[0][0] = p1.x * P1[2, 0] - P1[0, 0]
A[0][1] = p1.x * P1[2, 1] - P1[0, 1]
A[0][2] = p1.x * P1[2, 2] - P1[0, 2]
A[1][0] = p1.y * P1[2, 0] - P1[1, 0]
A[1][1] = p1.y * P1[2, 1] - P1[1, 1]
A[1][2] = p1.y * P1[2, 2] - P1[1, 2]
# Bottom two rows for P2 and p2 (repeat of above)
A[2][0] = p2.x * P2[2, 0] - P2[0, 0]
A[2][1] = p2.x * P2[2, 1] - P2[0, 1]
A[2][2] = p2.x * P2[2, 2] - P2[0, 2]
A[3][0] = p2.y * P2[2, 0] - P2[1, 0]
A[3][1] = p2.y * P2[2, 1] - P2[1, 1]
A[3][2] = p2.y * P2[2, 2] - P2[1, 2]
# AX = B. B:
B[0][0] = -(p1.x * P1[2, 3] - P1[0, 3])
B[1][0] = -(p1.y * P1[2, 3] - P1[1, 3])
B[2][0] = -(p2.x * P2[2, 3] - P2[0, 3])
B[3][0] = -(p2.y * P2[2, 3] - P2[1, 3])
# Solve AX = B for X by SVD
X = cv2.solve(A, B, flags=cv2.DECOMP_SVD)
return X
def triangulate(camCalFile, sterCalFile, pointsL, pointsR):
# read in camera calibration parameters
foundL, rvecsL, tvecsL, foundR, rvecsR, tvecsR, imageList = loadCamCal(
camCalFile)
# read in stereo calibration parameters
retval, cameraMatrixL, distCoeffsL, cameraMatrixR, distCoeffsR, R, T, E, F = loadSterCal(
sterCalFile)
#----------------------------------------------------------
# *********************************************************
# From Austin's Code calibration.cpp :
# Form projection matrices
# R from stereo calibration gives rotation from R to L camera
R_right_left = R
# left extrinsics are just identity
extL = np.eye(3, 4)
# Transform from right to left cam coordinates
extR = np.zeros((3, 4))
# Add rotation matrix
extR[0:3, 0:3] = R_right_left
# T: offset from right to left camera coordinates, ensure a column vector
T.shape = (3, 1)
# save T in extR
extR[:, 3:4] = T
#---------------------------------------------------------------
# intrinsics in projL(0:3,0:3)
intrinL = cameraMatrixL
# intrinsics in projR(0:3, 0:3)
intrinR = cameraMatrixR
# form projection matrices
projL = np.dot(intrinL, extL)
projR = np.dot(intrinR, extR)
# make array worldPoints where triangulated points will go
worldPoints = np.zeros((3, len(pointsL)))
pt = 0
# pdb.set_trace()
for ptL, ptR in zip(pointsL, pointsR):
# form matrix
A = formA(ptL, projL, ptR, projR)
# form vector
b = formb(ptL, projL, ptR, projR)
# result will be stored here
x = np.zeros((3, 1))
x.shape = (3, 1)
# solve for point
found, x = cv2.solve(A, b, x, cv2.DECOMP_SVD)
x.shape = (3, 1)
# save in array worldPoints
worldPoints[:, pt:pt + 1] = x
# increment counter
pt = pt + 1
return worldPoints
def compute_affine(src, dest):
import numpy as np
if src.shape[0] < 3 or src.shape[0] != dest.shape[0]:
return False
A = []
for i in src:
A.append(np.hstack((i, 1)))
A = np.array(A, dtype='f')
xc = np.array(dest[:, 0], dtype='f')
yc = np.array(dest[:, 1], dtype='f')
resx = 0
resy = 0
resx = cv2.solve(A.T.dot(A), A.T.dot(xc), flags=1)
resy = cv2.solve(A.T.dot(A), A.T.dot(yc), flags=1)
res = np.vstack((np.hstack(resx[1]), np.hstack(resy[1])))
#print "result is... ", res
return res
def affine_copy(image, matrix, dim_out):
image_original = np.float32(image)
image_blank = np.zeros([dim_out[1], dim_out[0], 3], dtype=np.float32)
rows1, columns1 = (dim_out[0], dim_out[1])
matrix_a = matrix[:2, :2]
matrix_b = matrix[:, 2:]
answer = np.array([[0], [0]], dtype=np.float32)
for u in range(rows1):
for v in range(columns1):
value_y = np.array([[u], [v]], dtype=np.float32) - matrix_b
cv.solve(matrix_a, value_y, answer)
valor_x = int(round(answer[0, 0]))
valor_y = int(round(answer[1, 0]))
# image_blank[v, u] = image[valor_y, valor_x]
if 0 <= valor_x < image_original.shape[0] and 0 <= valor_y < image_original.shape[1]:
image_blank[v, u] = image_original[valor_y, valor_x]
return np.uint8(image_blank)
def warp_my(filename, M, rsize):
img = filename
rows, cols = rsize
rows1, cols1 = img.shape[:2]
img_output = np.zeros((rsize[0], rsize[1], 3), np.uint8)
X1 = np.float32([0, 0])
A = np.float32([[M[0, 0], M[0, 1]], [M[1, 0], M[1, 1]]])
for x in range(rows - 1):
for y in range(cols - 1):
Y1 = np.float32([[x - M[0, 2]], [y - M[1, 2]]])
cv2.solve(A, Y1, X1)
newx = int(X1[0])
newy = int(X1[1])
if (newx > 0 and newx < rows1 and newy < cols1 and newy > 0):
img_output[x, y, 0] = img[newx, newy, 0]
img_output[x, y, 1] = img[newx, newy, 1]
img_output[x, y, 2] = img[newx, newy, 2]
return img_output
def lstsq(a, b, out=None):
if out is None:
shape = (a.shape[1], b.shape[1])
out = cp.Mat(shape, dtype=a.dtype, UMat=a.UMat)
if a.shape[0] == a.shape[1]:
decompType = cv.DECOMP_LU
else:
decompType = cv.DECOMP_SVD
isNonSingular, _ = cv.solve(a._, b._, dst=out._, flags=decompType)
return out
def intersect(l1, l2):
delta = np.array([l1[1] - l1[0], l2[1] - l2[0]]).astype(np.float32)
delta = 1 / delta
delta[:, 0] *= -1
b = np.matmul(delta, np.array([l1[0], l2[0]]).transpose())
b = np.diagonal(b).astype(np.float32)
res = cv2.solve(delta, b)
return res[0], tuple(res[1].astype(np.int32).reshape((2)))
def find_ladder_hight(self):
n = symbols('n')
for i in self.head_360:
# print(i)
sin_theta = sin(math.radians(i))
tri_a = self.init_lad_dis + n
tri_b = n - self.robot_high
tri_c = sqrt((tri_a**2) + (tri_b**2))
eq = (sin_theta) - (tri_b / tri_c)
# print(eq)
print(solve(eq, n))
def getPerspectiveTransform(src,dst): M = np.zeros((3, 3), dtype=np.float64) A = np.zeros((8, 8), dtype=np.float64) B = np.zeros((8, 1), dtype=np.float64) X = np.zeros((8, 1), dtype=np.float64) for i in range(4): A[i][0] = A[i+4][3] = src[i][0] A[i][1] = A[i+4][4] = src[i][1] A[i][2] = A[i+4][5] = 1 A[i][3] = A[i][4] = A[i][5] = 0 A[i+4][0] = A[i+4][1] = A[i+4][2] = 0 A[i][6] = (-src[i][0]) * (dst[i][0]) A[i][7] = (-src[i][1]) * (dst[i][0]) A[i+4][6] = (-src[i][0]) * (dst[i][1]) A[i+4][7] = (-src[i][1]) * (dst[i][1]) B[i] =(dst[i][0]) B[i+4] = dst[i][1] cv2.solve(A,B,X) M = Algoritmos.get_M(X,M) return M
def LinearLSTriangulation(u, P, u1, P1):
A = np.array([[u[0]*P[2,0]-P[0,0],u[0]*P[2,1]-P[0,1],u[0]*P[2,2]-P[0,2]],
[u[1]*P[2,0]-P[1,0],u[1]*P[2,1]-P[1,1],u[1]*P[2,2]-P[1,2]],
[u1[0]*P1[2,0]-P1[0,0],u1[0]*P1[2,1]-P1[0,1],u1[0]*P1[2,2]-P1[0,2]],
[u1[1]*P1[2,0]-P1[1,0],u1[1]*P1[2,1]-P1[1,1],u1[1]*P1[2,2]-P1[1,2]]])
B = np.array([[-(u[0]*P[2,3]-P[0,3])],
[-(u[1]*P[2,3]-P[1,3])],
[-(u1[0]*P1[2,3]-P1[0,3])],
[-(u1[1]*P1[2,3]-P1[1,3])]])
(retval, X) = cv2.solve(A,B,flags=cv2.DECOMP_SVD)
return X
def alignTransformation(src_pos, dst_pos):
a = 0.0
b = 0.0
tx = 0.0
ty = 0.0
X1 = 0.0
Y1 = 0.0
X2 = 0.0
Y2 = 0.0
Z = 0.0
C1 = 0.0
C2 = 0.0
W = 2.0
for i in range(0, 2):
x1 = src_pos[i * 2]
y1 = src_pos[i * 2 + 1]
x2 = dst_pos[i * 2]
y2 = dst_pos[i * 2 + 1]
Z = Z + x2 * x2 + y2 * y2
X1 = X1 + x1
Y1 = Y1 + y1
X2 = X2 + x2
Y2 = Y2 + y2
C1 = C1 + x1 * x2 + y1 * y2
C2 = C2 + y1 * x2 - x1 * y2
SolnA = [X2, -Y2, W, 0, Y2, X2, 0, W, Z, 0, X2, Y2, 0, Z, -Y2, X2]
A = np.array(SolnA, dtype=np.float64).reshape(4, 4)
SolnB = [X1, Y1, C1, C2]
B = np.array(SolnB, dtype=np.float64).reshape(4, 1)
Soln = np.zeros((4, 1), dtype=np.float64)
cv2.solve(A, B, Soln, cv2.DECOMP_SVD)
a = Soln[0, 0]
b = Soln[1, 0]
tx = Soln[2, 0]
ty = Soln[3, 0]
norm = a * a + b * b
a_ = a / norm
b_ = -b / norm
tx_ = (-a * tx - b * ty) / norm
ty_ = (b * tx - a * ty) / norm
return a_, b_, tx_, ty_
def linear_LS_triangulation(u, P, u1, P1):
"""
Linear Least Squares based triangulation.
TODO: flip rows and columns to increase performance (improve for cache)
(u, P) is the reference pair containing homogenous image coordinates (x, y) and the corresponding camera matrix.
(u1, P1) is the second pair.
u and u1 are matrices: amount of points equals #columns and should be equal for u and u1.
"""
global linear_LS_triangulation_A, linear_LS_triangulation_b
# Create array of triangulated points
x = np.zeros((3, u.shape[1]))
# Initialize C matrices
C = np.array(linear_LS_triangulation_c)
C1 = np.array(linear_LS_triangulation_c)
for i in range(u.shape[1]):
# Build C matrices, to visualize calculation structure
C[:, 2] = u[:, i]
C1[:, 2] = u1[:, i]
# Build A matrix
linear_LS_triangulation_A[0:2, :] = C.dot(P[0:3, 0:3]) # C * R
linear_LS_triangulation_A[2:4, :] = C1.dot(P1[0:3, 0:3]) # C1 * R1
# Build b vector
linear_LS_triangulation_b[0:2, :] = C.dot(P[0:3, 3:4]) # C * t
linear_LS_triangulation_b[2:4, :] = C1.dot(P1[0:3, 3:4]) # C1 * t1
linear_LS_triangulation_b *= -1
# Solve for x vector
cv2.solve(linear_LS_triangulation_A, linear_LS_triangulation_b,
x[:, i:i + 1], cv2.DECOMP_SVD)
return np.array(
x, dtype=np.float32) # solvePnPRansac() seems to dislike float64...
def get_PerspectiveTransform(origen, destino):
M = np.ndarray((3, 3), dtype=float)
R = np.ndarray((8, 8), dtype=float)
T = np.ndarray((8, 1), dtype=float)
X = np.ndarray((8, 1), dtype=float)
for i in range(len(origen)):
R[i, 0] = R[i + 4, 3] = origen[i, 0]
R[i, 1] = R[i + 4, 4] = origen[i, 1]
R[i, 2] = R[i + 4, 5] = 1
R[i, 3] = R[i, 4] = R[i, 5] = R[i + 4, 0] = R[i + 4, 1] = R[i + 4,
2] = 0
R[i, 6] = -origen[i, 0] * destino[i, 0]
R[i, 7] = -origen[i, 1] * destino[i, 0]
R[i + 4, 6] = -origen[i, 0] * destino[i, 1]
R[i + 4, 7] = -origen[i, 1] * destino[i, 1]
T[i] = destino[i, 0]
T[i + 4] = destino[i, 1]
cv2.solve(R, T, X, cv2.DECOMP_LU)
for i in range(len(X)):
M[i // 3, i % 3] = X[i]
M[2, 2] = 1.
return M
def copy_get_perspective_transform(src, dst):
M = np.ndarray((3, 3), dtype=float)
A = np.ndarray((8, 8), dtype=float)
B = np.ndarray((8, 1), dtype=float)
X = np.ndarray((8, 1), dtype=float)
for i in range(len(src)):
A[i, 0] = A[i+4, 3] = src[i, 0]
A[i, 1] = A[i + 4, 4] = src[i, 1]
A[i, 2] = A[i + 4, 5] = 1
A[i, 3] = A[i, 4] = A[i, 5] = A[i + 4, 0] = A[i + 4, 1] = A[i + 4, 2] = 0
A[i, 6] = -src[i, 0] * dst[i, 0]
A[i, 7] = -src[i, 1] * dst[i, 0]
A[i + 4, 6] = -src[i, 0] * dst[i, 1]
A[i + 4, 7] = -src[i, 1] * dst[i, 1]
B[i] = dst[i, 0]
B[i + 4] = dst[i, 1]
cv.solve(A, B, X, cv.DECOMP_LU)
for i in range(len(X)):
M[i//3, i % 3] = X[i]
M[2, 2] = 1.
return M
def affine_copy(image, matrix, dim_out):
image_blank = np.zeros([dim_out[1], dim_out[0], 3], dtype=np.uint32)
rows1, columns1 = (dim_out[0], dim_out[1])
matrix_a = matrix[:2, :2]
matrix_b = matrix[:, 2:]
for u in range(rows1):
for v in range(columns1):
value_y = np.array([[u], [v]], dtype=np.float32) - matrix_b
answer = cv.solve(matrix_a, value_y)[1]
valor_x = int(answer[0, 0])
valor_y = int(answer[1, 0])
image_blank[v, u] = image[valor_y, valor_x]
return np.uint8(image_blank)
def solve_M(pair_1, pair_2):
# solving Ax = B
A = np.zeros((len(pair_2)*2, 3))
B = np.zeros((len(pair_1)*2, 2))
for i in range(len(pair_1)):
B[2*i, 0] = pair_1[i, 0]
B[2*i, 1] = pair_1[i, 1]
A[2*i, 0] = pair_2[i, 0]
A[2*i, 1] = pair_2[i, 1]
A[2*i, 2] = 1.0
x = np.zeros((3,2))
err, x = cv2.solve(A, B, x, cv2.DECOMP_SVD)
return np.transpose(x)
def linear_LS_triangulation(u, P, u1, P1):
"""
Linear Least Squares based triangulation.
TODO: flip rows and columns to increase performance (improve for cache)
(u, P) is the reference pair containing homogenous image coordinates (x, y) and the corresponding camera matrix.
(u1, P1) is the second pair.
u and u1 are matrices: amount of points equals #columns and should be equal for u and u1.
"""
global linear_LS_triangulation_A, linear_LS_triangulation_b
# Create array of triangulated points
x = np.zeros((3, u.shape[1]))
# Initialize C matrices
C = np.array(linear_LS_triangulation_c)
C1 = np.array(linear_LS_triangulation_c)
for i in range(u.shape[1]):
# Build C matrices, to visualize calculation structure
C[:, 2] = u[:, i]
C1[:, 2] = u1[:, i]
# Build A matrix
linear_LS_triangulation_A[0:2, :] = C.dot(P[0:3, 0:3]) # C * R
linear_LS_triangulation_A[2:4, :] = C1.dot(P1[0:3, 0:3]) # C1 * R1
# Build b vector
linear_LS_triangulation_b[0:2, :] = C.dot(P[0:3, 3:4]) # C * t
linear_LS_triangulation_b[2:4, :] = C1.dot(P1[0:3, 3:4]) # C1 * t1
linear_LS_triangulation_b *= -1
# Solve for x vector
cv2.solve(linear_LS_triangulation_A, linear_LS_triangulation_b, x[:, i:i+1], cv2.DECOMP_SVD)
return np.array(x, dtype=np.float32) # solvePnPRansac() seems to dislike float64...
def triangulacion_lineal(self,u1, P1, u2, P2):
A = np.array([u1[0]*P1[2, 0] - P1[0, 0], u1[0]*P1[2, 1] - P1[0, 1],
u1[0]*P1[2, 2] - P1[0, 2], u1[1]*P1[2, 0] - P1[1, 0],
u1[1]*P1[2, 1] - P1[1, 1], u1[1]*P1[2, 2] - P1[1, 2],
u2[0]*P2[2, 0] - P2[0, 0], u2[0]*P2[2, 1] - P2[0, 1],
u2[0]*P2[2, 2] - P2[0, 2], u2[1]*P2[2, 0] - P2[1, 0],
u2[1]*P2[2, 1] - P2[1, 1],
u2[1]*P2[2, 2] - P2[1, 2]]).reshape(4, 3)
B = np.array([-(u1[0]*P1[2, 3] - P1[0, 3]),
-(u1[1]*P1[2, 3] - P1[1, 3]),
-(u2[0]*P2[2, 3] - P2[0, 3]),
-(u2[1]*P2[2, 3] - P2[1, 3])]).reshape(4, 1)
ret, X = cv2.solve(A, B, flags=cv2.DECOMP_SVD)
return X.reshape(1, 3)
def _compute_affine_transform_ocvlsq(refpoints, points, w = None):
if w == None:
w = [1] * (len(points) * 2)
assert(len(w) == 2*len(points))
y = []
for n, p in enumerate(refpoints):
y += [p[0]/w[n*2], p[1]/w[n*2+1]]
A = []
for n, p in enumerate(points):
A.extend([ [p[0]/w[n*2], p[1]/w[n*2], 0, 0, 1/w[n*2], 0], [0, 0, p[0]/w[n*2+1], p[1]/w[n*2+1], 0, 1/w[n*2+1]] ])
lstsq = cv2.solve(np.array(A), np.array(y), flags=cv2.DECOMP_SVD)
h11, h12, h21, h22, dx, dy = lstsq[1]
err = 0#lstsq[1]
#R = np.array([[h11, h12, dx], [h21, h22, dy]])
# The row above works too - but creates a redundant dimension
R = np.array([[h11[0], h12[0], dx[0]], [h21[0], h22[0], dy[0]]])
return R, err
def _linear_ls_triangulation(self, u1, P1, u2, P2):
"""Triangulation via Linear-LS method"""
# build A matrix for homogeneous equation system Ax=0
# assume X = (x,y,z,1) for Linear-LS method
# which turns it into AX=B system, where A is 4x3, X is 3x1 & B is 4x1
A = np.array([u1[0]*P1[2, 0] - P1[0, 0], u1[0]*P1[2, 1] - P1[0, 1],
u1[0]*P1[2, 2] - P1[0, 2], u1[1]*P1[2, 0] - P1[1, 0],
u1[1]*P1[2, 1] - P1[1, 1], u1[1]*P1[2, 2] - P1[1, 2],
u2[0]*P2[2, 0] - P2[0, 0], u2[0]*P2[2, 1] - P2[0, 1],
u2[0]*P2[2, 2] - P2[0, 2], u2[1]*P2[2, 0] - P2[1, 0],
u2[1]*P2[2, 1] - P2[1, 1],
u2[1]*P2[2, 2] - P2[1, 2]]).reshape(4, 3)
B = np.array([-(u1[0]*P1[2, 3] - P1[0, 3]),
-(u1[1]*P1[2, 3] - P1[1, 3]),
-(u2[0]*P2[2, 3] - P2[0, 3]),
-(u2[1]*P2[2, 3] - P2[1, 3])]).reshape(4, 1)
ret, X = cv2.solve(A, B, flags=cv2.DECOMP_SVD)
return X.reshape(1, 3)
def IterativeLinearLSTriangulation(u, P, u1, P1):
wi = 1
wi1 = 1
X = np.empty([4,1]);
for i in xrange(0, 10):
X_ = LinearLSTriangulation(u,P,u1,P1)
X[0] = X_[0]
X[1] = X_[1]
X[2] = X_[2]
X[3] = 1.0
p2x = (P[2,].dot(X))[0]
p2x1 = (P1[2,].dot(X))[0]
if (abs(wi - p2x) <= EPSILON and abs(wi1 - p2x1) <= EPSILON):
break
wi = p2x
wi1 = p2x1
A = np.array([[(u[0]*P[2,0]-P[0,0])/wi, (u[0]*P[2,1]-P[0,1])/wi, (u[0]*P[2,2]-P[0,2])/wi],
[(u[1]*P[2,0]-P[1,0])/wi, (u[1]*P[2,1]-P[1,1])/wi, (u[1]*P[2,2]-P[1,2])/wi],
[(u1[0]*P1[2,0]-P1[0,0])/wi1, (u1[0]*P1[2,1]-P1[0,1])/wi1, (u1[0]*P1[2,2]-P1[0,2])/wi1],
[(u1[1]*P1[2,0]-P1[1,0])/wi1, (u1[1]*P1[2,1]-P1[1,1])/wi1, (u1[1]*P1[2,2]-P1[1,2])/wi1]])
B = np.array([[-(u[0]*P[2,3]-P[0,3])/wi],
[-(u[1]*P[2,3]-P[1,3])/wi],
[-(u1[0]*P1[2,3]-P1[0,3])/wi1],
[-(u1[1]*P1[2,3]-P1[1,3])/wi1]])
(retval, X_) = cv2.solve(A,B,flags=cv2.DECOMP_SVD)
X[0] = X_[0]
X[1] = X_[1]
X[2] = X_[2]
X[3] = 1.0
return X
def iterative_point_triangulation(P0, P1, pt0, pt1): w0 = 1 w1 = 1 x0 = pt0[0] y0 = pt0[1] x1 = pt1[0] y1 = pt1[1] # DLT triangulation X = triangulate_point(P0, P1, pt0, pt1) for i in range(10): # Measure weights based on reprojection error w0 = np.dot(P0[2,:], X) w1 = np.dot(P1[2,:], X) # Solve with weights A = np.array([x0*P0[2,:] - P0[0,:], y0*P0[2,:] - P0[1,:], x1*P1[2,:] - P1[0,:], y1*P1[2,:] - P1[1,:]]) A[0,:] = A[0,:] * (1/w0) A[1,:] = A[1,:] * (1/w0) A[2,:] = A[2,:] * (1/w1) A[3,:] = A[3,:] * (1/w1) B = -A[:,-1] A = A[:,0:3] tmp, X = cv2.solve(A,B, flags=cv2.DECOMP_SVD) X.reshape(3) X = np.append(X,1) return X
def main():
'''Read point correspondences from kinects and computer transformation
between the two spaces
'''
ros.init_node('Calibrate_Receive', anonymous=True)
receiver = ReceiveKinectImageInfo()
# print ros.get_param_names()
# print ros.get_param("~depth_topic")
ros.Subscriber(ros.get_param('~depth_topic'), Image, receiver.DepthImage)
ros.Subscriber(ros.get_param('~color_topic'), Image, receiver.ColorImage)
# Wait until images are received
while not (receiver.color_image_received and receiver.depth_image_received):
time.sleep(.05)
if ros.is_shutdown():
break
ros.loginfo("Received Messages: %s", ros.get_param('~depth_topic'))
# find Checkboard points
bool, corners = receiver.corners
rgb_camera_info = ros.wait_for_message(ros.get_param('~color_camera_params'), CameraInfo, 3)
# Project Points Into 3D
K = rgb_camera_info.K
depth_points = []
for corner in corners:
Z_c = receiver.depth_image[int(corner[0][0]+0.5)][int(corner[0][1]+0.5)] * 0.001
X_c = (corner[0][0] - K[2]) * (1.0/K[0]) * Z_c
Y_c = (corner[0][1] - K[5]) * (1.0/K[4]) * Z_c
depth_points.append((X_c, Y_c, Z_c))
# print depth_points
# Get message from Kinect 2.0
context = zmq.Context()
socket = context.socket(zmq.REP)
socket.bind("tcp://*:5555")
message = socket.recv()
ros.loginfo("Received Checker Points %d", len(message))
points = Points()
points.ParseFromString(message)
# print points
# Solve for Transformation Matrices
X = np.zeros((4,1))
A = np.zeros((len(corners), 4))
B = np.zeros((len(corners), 1))
T = np.zeros((4,4))
for i, point in enumerate(depth_points):
A[i,:] = [x for x in point]+ [1]
# Initialize B for X
for i, point in enumerate(points.points):
B[i] = point.x
cv.solve(A,B,X, cv.DECOMP_SVD)
T[0,:] = np.transpose(X)
for i, point in enumerate(points.points):
B[i] = point.y
cv.solve(A,B,X, cv.DECOMP_SVD)
T[1,:] = np.transpose(X)
for i, point in enumerate(points.points):
B[i] = point.z
cv.solve(A,B,X, cv.DECOMP_SVD)
T[2,:] = np.transpose(X)
T[3,:] = np.array([0,0,0,1])
print T
coord = np.mat(T) * np.mat(A[0,:]).transpose()
print coord
print points.points[0]
def iterative_LS_triangulation(u1, P1, u2, P2, tolerance=3.e-5):
"""
Iterative (Linear) Least Squares based triangulation.
From "Triangulation", Hartley, R.I. and Sturm, P., Computer vision and image understanding, 1997.
Relative speed: 0.025
(u1, P1) is the reference pair containing normalized image coordinates (x, y) and the corresponding camera matrix.
(u2, P2) is the second pair.
"tolerance" is the depth convergence tolerance.
Additionally returns a status-vector to indicate outliers:
1: inlier, and in front of both cameras
0: outlier, but in front of both cameras
-1: only in front of second camera
-2: only in front of first camera
-3: not in front of any camera
Outliers are selected based on non-convergence of depth, and on negativity of depths (=> behind camera(s)).
u1 and u2 are matrices: amount of points equals #rows and should be equal for u1 and u2.
"""
A = np.zeros((4, 3))
b = np.zeros((4, 1))
# Create array of triangulated points
x = np.empty((4, len(u1))); x[3, :].fill(1) # create empty array of homogenous 3D coordinates
x_status = np.empty(len(u1), dtype=int)
# Initialize C matrices
C1 = np.array(iterative_LS_triangulation_C)
C2 = np.array(iterative_LS_triangulation_C)
for xi in range(len(u1)):
# Build C matrices, to construct A and b in a concise way
C1[:, 2] = u1[xi, :]
C2[:, 2] = u2[xi, :]
# Build A matrix
A[0:2, :] = C1.dot(P1[0:3, 0:3]) # C1 * R1
A[2:4, :] = C2.dot(P2[0:3, 0:3]) # C2 * R2
# Build b vector
b[0:2, :] = C1.dot(P1[0:3, 3:4]) # C1 * t1
b[2:4, :] = C2.dot(P2[0:3, 3:4]) # C2 * t2
b *= -1
# Init depths
d1 = d2 = 1.
for i in range(10): # Hartley suggests 10 iterations at most
# Solve for x vector
#x_old = np.array(x[0:3, xi]) # TODO: remove
cv2.solve(A, b, x[0:3, xi:xi+1], cv2.DECOMP_SVD)
# Calculate new depths
d1_new = P1[2, :].dot(x[:, xi])
d2_new = P2[2, :].dot(x[:, xi])
# Convergence criterium
#print i, d1_new - d1, d2_new - d2, (d1_new > 0 and d2_new > 0) # TODO: remove
#print i, (d1_new - d1) / d1, (d2_new - d2) / d2, (d1_new > 0 and d2_new > 0) # TODO: remove
#print i, np.sqrt(np.sum((x[0:3, xi] - x_old)**2)), (d1_new > 0 and d2_new > 0) # TODO: remove
##print i, u1[xi, :] - P1[0:2, :].dot(x[:, xi]) / d1_new, u2[xi, :] - P2[0:2, :].dot(x[:, xi]) / d2_new # TODO: remove
#print bool(i) and ((d1_new - d1) / (d1 - d_old), (d2_new - d2) / (d2 - d1_old), (d1_new > 0 and d2_new > 0)) # TODO: remove
##if abs(d1_new - d1) <= tolerance and abs(d2_new - d2) <= tolerance: print "Orig cond met" # TODO: remove
if abs(d1_new - d1) <= tolerance and \
abs(d2_new - d2) <= tolerance:
#if i and np.sum((x[0:3, xi] - x_old)**2) <= 0.0001**2:
#if abs((d1_new - d1) / d1) <= 3.e-6 and \
#abs((d2_new - d2) / d2) <= 3.e-6: #and \
#abs(d1_new - d1) <= tolerance and \
#abs(d2_new - d2) <= tolerance:
#if i and 1 - abs((d1_new - d1) / (d1 - d_old)) <= 1.e-2 and \ # TODO: remove
#1 - abs((d2_new - d2) / (d2 - d1_old)) <= 1.e-2 and \ # TODO: remove
#abs(d1_new - d1) <= tolerance and \ # TODO: remove
#abs(d2_new - d2) <= tolerance: # TODO: remove
break
# Re-weight A matrix and b vector with the new depths
A[0:2, :] *= 1 / d1_new
A[2:4, :] *= 1 / d2_new
b[0:2, :] *= 1 / d1_new
b[2:4, :] *= 1 / d2_new
# Update depths
#d_old = d1 # TODO: remove
#d1_old = d2 # TODO: remove
d1 = d1_new
d2 = d2_new
# Set status
x_status[xi] = ( i < 10 and # points should have converged by now
(d1_new > 0 and d2_new > 0) ) # points should be in front of both cameras
if d1_new <= 0: x_status[xi] -= 1
if d2_new <= 0: x_status[xi] -= 2
return x[0:3, :].T.astype(output_dtype), x_status
def linear_LS_triangulation(u1, P1, u2, P2):
"""
Linear Least Squares based triangulation.
Relative speed: 0.1
(u1, P1) is the reference pair containing normalized image coordinates (x, y) and the corresponding camera matrix.
(u2, P2) is the second pair.
u1 and u2 are matrices: amount of points equals #rows and should be equal for u1 and u2.
The status-vector will be True for all points.
"""
A = np.zeros((4, 3))
b = np.zeros((4, 1))
# Create array of triangulated points
x = np.zeros((3, len(u1)))
# Initialize C matrices
C1 = np.array(linear_LS_triangulation_C)
C2 = np.array(linear_LS_triangulation_C)
for i in range(len(u1)):
# Derivation of matrices A and b:
# for each camera following equations hold in case of perfect point matches:
# u.x * (P[2,:] * x) = P[0,:] * x
# u.y * (P[2,:] * x) = P[1,:] * x
# and imposing the constraint:
# x = [x.x, x.y, x.z, 1]^T
# yields:
# (u.x * P[2, 0:3] - P[0, 0:3]) * [x.x, x.y, x.z]^T + (u.x * P[2, 3] - P[0, 3]) * 1 = 0
# (u.y * P[2, 0:3] - P[1, 0:3]) * [x.x, x.y, x.z]^T + (u.y * P[2, 3] - P[1, 3]) * 1 = 0
# and since we have to do this for 2 cameras, and since we imposed the constraint,
# we have to solve 4 equations in 3 unknowns (in LS sense).
# Build C matrices, to construct A and b in a concise way
C1[:, 2] = u1[i, :]
C2[:, 2] = u2[i, :]
# Build A matrix:
# [
# [ u1.x * P1[2,0] - P1[0,0], u1.x * P1[2,1] - P1[0,1], u1.x * P1[2,2] - P1[0,2] ],
# [ u1.y * P1[2,0] - P1[1,0], u1.y * P1[2,1] - P1[1,1], u1.y * P1[2,2] - P1[1,2] ],
# [ u2.x * P2[2,0] - P2[0,0], u2.x * P2[2,1] - P2[0,1], u2.x * P2[2,2] - P2[0,2] ],
# [ u2.y * P2[2,0] - P2[1,0], u2.y * P2[2,1] - P2[1,1], u2.y * P2[2,2] - P2[1,2] ]
# ]
A[0:2, :] = C1.dot(P1[0:3, 0:3]) # C1 * R1
A[2:4, :] = C2.dot(P2[0:3, 0:3]) # C2 * R2
# Build b vector:
# [
# [ -(u1.x * P1[2,3] - P1[0,3]) ],
# [ -(u1.y * P1[2,3] - P1[1,3]) ],
# [ -(u2.x * P2[2,3] - P2[0,3]) ],
# [ -(u2.y * P2[2,3] - P2[1,3]) ]
# ]
b[0:2, :] = C1.dot(P1[0:3, 3:4]) # C1 * t1
b[2:4, :] = C2.dot(P2[0:3, 3:4]) # C2 * t2
b *= -1
# Solve for x vector
cv2.solve(A, b, x[:, i:i+1], cv2.DECOMP_SVD)
return x.T.astype(output_dtype), np.ones(len(u1), dtype=bool)
