def getEpipoles(self):
'''Returns the epipole/position of camera 1 in image 2 and 3.'''
u = r_[[cv2.SVDecomp(t.T)[2][-1] for t in self.T]]
ei = cv2.SVDecomp(u)[2][-1]
v = r_[[cv2.SVDecomp(t)[2][-1] for t in self.T]]
eii = cv2.SVDecomp(v)[2][-1]
return ei, eii
def mask_pts_near_epipole(pts1, pts2, P=None, cutoff=0.1, F=None):
'''Get array of indices for points that are away from epipole.
Epipole points cannot be triangulated and must be excluded from
array. The method returns an array of acceptable indices.
:type pts1: list
:arg pts1: List of corresponding image points from image one.
:type pts2: list
:arg pts2: List of corresponding image points from image two.
:type P: 3x4 array
:arg P: Projection
:type cutoff: float
:arg cutoff: Cut off distance for unacceptable points.
:type F: 3x3 array
:arg F: Fundamental matrix
:returns: Array of acceptable indices in point list.
'''
if F == None:
F = E(P)
epipole = cv2.SVDecomp(F.T)[2][-1]
epipole /= epipole[2]
d0 = (pts1.T - epipole).T
d0 = sqrt(sum(d0**2,0)) > cutoff
epipole = cv2.SVDecomp(F)[2][-1]
epipole /= epipole[2]
d1 = (pts2.T - epipole).T
d1 = sqrt(sum(d1**2,0)) > cutoff
return d0 & d1
def LinearSVDTriangulation(self, key_point1, P1, key_point2, P2):
'''
@brief Implementation of the opencv triangulation algorithm, originally Harley & Zisserman.
See: https://github.com/opencv/opencv/blob/master/modules/calib3d/src/triangulate.cpp
@param key_point1 (homogenous image point (u,v,1) - cv2 key point)
@param P1 (camera 1 matrix (projection matrix 3x4))
@param key_point2 (homogenous image point in 2nd camera (u,v,1) - cv2 key point)
@param P2 (camera 2 matrix (projection matrix 3x4))
@return point4D (4x1 matrix as [x,y,z,1]^T)
'''
A = np.zeros((4, 4))
point4D = np.zeros((4, 1))
keypoints = [key_point1, key_point2]
proj_matrices = [P1, P2]
for i in range(2):
x = keypoints[i].pt[0]
y = keypoints[i].pt[1]
for j in range(4):
A[i * 2 + 0,
j] = x * proj_matrices[i][2, j] - proj_matrices[i][0, j]
A[i * 2 + 1,
j] = y * proj_matrices[i][2, j] - proj_matrices[i][1, j]
w, u, vt = cv2.SVDecomp(A)
point4D[0, 0] = vt[0, 3] # X
point4D[1, 0] = vt[1, 3] # Y
point4D[2, 0] = vt[2, 3] # Z
point4D[3, 0] = vt[3, 3] # W
return point4D
def manually_calculate_pose(self, f_mat):
# get essential matrix from the fundamental
# I am assuming that only one calibration matrix is fine here, because
# only one type of camera is being used.
e_mat = self.k_mat.T * f_mat * self.k_mat
singular_values, u_mat, vt = cv2.SVDecomp(e_mat)
# reconstruction from SVD:
# np.dot(u_mat, np.dot(np.diag(singular_values.T[0]), vt))
u_mat = np.matrix(u_mat)
vt = np.matrix(vt)
# from Epipolar Geometry and the Fundamental Matrix 9.13
w_mat = np.matrix([[0, -1, 0], [1, 0, 0], [0, 0, 1]], np.float32)
R_mat = u_mat * w_mat * vt # HZ 9.19
t_mat = u_mat[:, 2] # get third column of u
# check rotation matrix for validity
if np.linalg.det(R_mat) - 1.0 > 1e-07:
print('{}\nDoes not appear to be a valid rotation matrix'.format(
R_mat))
camera_matrix = np.column_stack((R_mat, t_mat))
return camera_matrix, R_mat, t_mat
def draw_gaussain(img, mean, cov, color):
x, y = mean
w, u, _vt = cv.SVDecomp(cov)
ang = np.arctan2(u[1, 0], u[0, 0]) * (180 / np.pi)
s1, s2 = np.sqrt(w) * 3.0
cv.ellipse(img, (int(x), int(y)), (int(s1), int(s2)), ang, 0, 360, color,
1, cv.LINE_AA)
def mtx2rvec(R):
w, u, vt = cv2.SVDecomp(R - np.eye(3))
p = vt[0] + u[:, 0] * w[0] # same as np.dot(R, vt[0])
c = np.dot(vt[0], p)
s = np.dot(vt[1], p)
axis = np.cross(vt[0], vt[1])
return axis * np.arctan2(s, c)
def svd(M):
#U, S, V = np.linalg.svd(M)
#return U,S,V
flags = cv2.SVD_FULL_UV
S, U, V = cv2.SVDecomp(M, flags=flags)
S = S.flatten()
return U, S, V
def calcRigidBodyTransform(self, *argv):
if len(argv) == 3:
coV = argv[0]
pointsA = argv[1]
pointsB = argv[2]
u, s, vh = cv2.SVDecomp(coV)
sign = np.linalg.det(np.dot(np.transpose(vh), s))
dm = np.eye(3)
dm[2, 2] = sign
resultRot = np.dot(np.dot(np.transpose(vh), dm), np.transpose(s))
temp = pointsB - np.dot(resultRot, pointsA)
return [resultRot, temp]
else:
points = argv[0]
length = len(points)
cA = np.zeros(3)
cB = np.zeros(3)
pointsA = np.zeros([3, length])
pointsB = np.zeros([3, length])
for i in range(length):
cA += points[i][0]
cB += points[i][1]
cA = cA / length
cB = cB / length
for i in range(length):
pointsA[0, i] = points[i][0][0] - cA[0]
pointsA[1, i] = points[i][0][1] - cA[1]
pointsA[2, i] = points[i][0][2] - cA[2]
pointsB[0, i] = points[i][1][0] - cB[0]
pointsB[1, i] = points[i][1][1] - cB[1]
pointsB[2, i] = points[i][1][2] - cB[2]
a = np.dot(pointsA, np.transpose(pointsB))
return self.calcRigidBodyTransform(a, cA, cB)
def kabsch(P, Q, calcTranslation=False, centP=None, centQ=None):
'''
Perform Kabsch's algorithm and return optimal rotation and translation
'''
# Pt * Q
cov_mat = np.dot(np.transpose(flatten(P)), flatten(Q))
d, u, vt = cv2.SVDecomp(cov_mat)
rot = np.dot(np.transpose(vt), np.transpose(u))
v = np.transpose(vt)
# Check and fix special reflection case
if cv2.determinant(rot) < 0:
# rot[:, 2] = np.multiply(rot[:, 2], -1)
v[:, 2] = np.multiply(v[:, 2], -1)
rot = np.dot(v, np.transpose(u))
t = None
if calcTranslation:
t = np.dot(-rot, np.transpose(centP)) + np.transpose(centQ)
# return rotation ans translation matrixes
return rot, t
def task_two():
"""Find fundamental matrix between
two cams with some params"""
tetta1 = 45 * np.pi / 180
R1 = np.array([[np.cos(tetta1), -np.sin(tetta1), 0],
[np.sin(tetta1), np.cos(tetta1), 0],
[0, 0, 1]])
T1 = np.array([[0, 0, 0]]).T
tetta2 = -tetta1
R2 = np.array([[np.cos(tetta2), 0, np.sin(tetta2)],
[0, 1, 0],
[-np.sin(tetta2), 0, np.cos(tetta2)]])
T2 = np.array([[-10, 0, 0]]).T
# Formula to calculate:
# F = [e2]_x P2 P1_rev
# P1, P2 - first and second projection matrix
P1 = np.concatenate([R1.T, -R1.T.dot(T1)], axis=1)
P2 = np.concatenate([R2.T, -R2.T.dot(T2)], axis=1)
# Find e2 = P2 * O1
O1 = np.array([[0, 0, 0]]).T
e2 = P2.dot(np.append(O1, np.array([1])))
# Find revers P1
w, u, vt = cv.SVDecomp(P1)
# u w vt r = E
# r = vt.T w^-2 u.T
P1_rev = np.dot(vt.T, np.dot(np.diag([1 / x if x > 1e-5 else 0 for x in w.flatten()]), u.T))
# Calc F
P = P2.dot(P1_rev)
F = np.cross(e2, P, axisa=0, axisb=0, axisc=0)
print("F = \n", F)
print("Check F.T * e2 = ", F.T.dot(e2))
def blob_mean_and_tangent(contour):
moments = cv2.moments(contour)
area = moments['m00']
#print('area', area)
#print(contour)
#print(moments['m10'])
#print(moments['m01'])
if area < 0.00001:
mean_x = 0
mean_y = 0
else:
mean_x = moments['m10'] / area
mean_y = moments['m01'] / area
moments_matrix = np.array([[moments['mu20'], moments['mu11']],
[moments['mu11'], moments['mu02']]]) / area
_, svd_u, _ = cv2.SVDecomp(moments_matrix)
center = np.array([mean_x, mean_y])
tangent = svd_u[:, 0].flatten().copy()
return center, tangent
def blob_mean_and_tangent(contour):
moments = cv2.moments(contour)
area = moments['m00'] # Note that this computed by Green theroem, so it will probably differ from the result of drawContours
mean_x = moments['m10'] / area
mean_y = moments['m01'] / area
# A covariance matrix over all datapoints (contour surrounded regions)
moments_matrix = np.array([
[moments['mu20'], moments['mu11']],
[moments['mu11'], moments['mu02']]
]) / area
# Perform PCA using SVD
_, svd_u, _ = cv2.SVDecomp(moments_matrix)
center = np.array([mean_x, mean_y])
# Use the principle axis as the main direction of this contour object
tangent = svd_u[:, 0].flatten().copy()
# After testing, I found that tangent.x will always be > 0 empirically.
# But I think it will be better checking the tangent.x direction to gaurantee point0 is smaller and point1 is bigger in x
tangent = (-1 if tangent[0] < 0 else 1) * tangent # normalize the x coordinate
return center, tangent
def global_homography(self, src_point, dst_point):
"""
get global homography
This engine used this method to get the final size.
:param src_point: source image
:param dst_point: destination image
:return: global homography
"""
sample_n, _ = src_point.shape
# point normalization
N1, nf1 = self.getNormalize2DPts(src_point)
N2, nf2 = self.getNormalize2DPts(dst_point)
C1 = self.getConditionerFromPts(nf1)
C2 = self.getConditionerFromPts(nf2)
cf1 = self.point_normalize(nf1, C1)
cf2 = self.point_normalize(nf2, C2)
# x' = Ax, make transform matrix
A = self.matrix_generate(sample_n, cf1, cf2)
# Singular Value Decomposition
W, U, V = cv2.SVDecomp(A)
# get global-homography
h = V[-1, :]
h = h.reshape((3, 3))
h = np.linalg.inv(C2).dot(h).dot(C1)
h = np.linalg.inv(N2).dot(h).dot(N1)
h = h / h[2, 2]
return h
def getcontourinfo(c):
"""compute moments and derived quantities such as mean, area, and
basis vectors from a contour as returned by cv2.findContours. """
m = cv2.moments(c)
s00 = m['m00']
s10 = m['m10']
s01 = m['m01']
c20 = m['mu20']
c11 = m['mu11']
c02 = m['mu02']
mx = s10 / s00
my = s01 / s00
A = numpy.array([[c20 / s00, c11 / s00], [c11 / s00, c02 / s00]])
W, U, Vt = cv2.SVDecomp(A)
ul = math.sqrt(W[0, 0])
vl = math.sqrt(W[1, 0])
ux = ul * U[0, 0]
uy = ul * U[1, 0]
vx = vl * U[0, 1]
vy = vl * U[1, 1]
mean = numpy.array([mx, my])
uvec = numpy.array([ux, uy])
vvec = numpy.array([vx, vy])
return {'moments': m, 'area': s00, 'mean': mean, 'b1': uvec, 'b2': vvec}
def keyframe_test(points1, points2):
"""Returns True if the two images can be taken as keyframes."""
homography, mask = cv2.findHomography(points1, points2)
w, u, vt = cv2.SVDecomp(homography, flags=cv2.SVD_NO_UV)
w = w.reshape((-1))
print "w[0]/w[2]:", w[0] / w[2]
return w[0] / w[2] > homography_condition_threshold
def blob_mean_and_tangent(contour):
moments = cv2.moments(contour)
area = moments['m00']
if area != 0:
mean_x = moments['m10'] / area
mean_y = moments['m01'] / area
moments_matrix = np.array([
[moments['mu20'], moments['mu11']],
[moments['mu11'], moments['mu02']]
]) / area
else:
mean_x = mean_y = 0
moments_matrix = np.array([
[0.0, 0.0],
[0.0, 0.0]
])
_, svd_u, _ = cv2.SVDecomp(moments_matrix)
center = np.array([mean_x, mean_y])
tangent = svd_u[:, 0].flatten().copy()
return center, tangent
def svd(M):
r"""
Args:
M (ndarray): must be either float32 or float64
Returns:
tuple: (U, s, Vt)
CommandLine:
python -m vtool.linalg --test-svd
Example:
>>> # ENABLE_DOCTEST
>>> from vtool.linalg import * # NOQA
>>> # build test data
>>> M = np.array([1, 2, 3], dtype=np.float32)
>>> M = np.array([[20.5812, 0], [3.615, 17.1295]], dtype=np.float64)
>>> # execute function
>>> (U, s, Vt) = svd(M)
Timeit::
flags = cv2.SVD_FULL_UV
%timeit cv2.SVDecomp(M, flags=flags)
%timeit npl.svd(M)
"""
# V is actually Vt
flags = cv2.SVD_FULL_UV
S, U, Vt = cv2.SVDecomp(M, flags=flags)
s = S.flatten()
#U, s, Vt = npl.svd(M)
return U, s, Vt
def H_from_E(E, RandT=False):
'''Returns a 4x4x4 matrix of possible H translations.
Or returns the two rotations and translation vectors when keyword is True.
'''
S,U,V = cv2.SVDecomp(E)
#TIP: Recover E by dot(U,dot(diag(S.flatten()),V))
W = array([[0,-1,0],[1,0,0],[0,0,1]])
R1 = dot(dot(U,W),V)
R2 = dot(dot(U,W.T),V)
if cv2.determinant(R1) < 0:
R1,R2 = -R1,-R2
t1 = U[:,2]
t2 = -t1
if RandT:
return R1, R2, t1, t2
H = zeros((4,4,4))
H[:2,:3,:3] = R1
H[2:,:3,:3] = R2
H[[0,2],:3,3] = t1
H[[1,3],:3,3] = t2
H[:,3,3] = 1
return H
def projectionMatrix(camera_parameters, homography):
#Matrice des paramètres extrinsèques
M = np.dot(np.linalg.inv(K), homography)
_lambda1 = np.linalg.norm(M[:, 0])
_lambda2 = np.linalg.norm(M[:, 1])
_lambda = (_lambda1 + _lambda2) / np.float32(2)
M[:, 0] = M[:, 0] / _lambda1
M[:, 1] = M[:, 1] / _lambda2
t = M[:, 2] / _lambda
R = np.c_[M[:, 0], M[:, 1], np.cross(M[:, 0], M[:, 1])]
#R = np.c_[np.cross(R[:, 1], R[:, 2]), R[:, 1], R[:, 2]]
if cv2.determinant(R) < 0:
R[:, 2] = R[:, 2] * (-1)
W, U, Vt = cv2.SVDecomp(R)
R = U.dot(Vt)
extr = np.float32([[R[0, 0], R[0, 1], R[0, 2], t[0]],
[R[1, 0], R[1, 1], R[1, 2], t[1]],
[R[2, 0], R[2, 1], R[2, 2], t[2]]])
return np.dot(camera_parameters, extr)
def ComputePoseFromHomography(new_intrinsics, referencePoints, imagePoints):
# compute homography using RANSAC, this allows us to compute
# the homography even when some matches are incorrect
homography, mask = cv2.findHomography(referencePoints, imagePoints,
cv2.RANSAC, 5.0)
# check that enough matches are correct for a reasonable estimate
# correct matches are typically called inliers
MIN_INLIERS = 30
if (sum(mask) > MIN_INLIERS):
# given that we have a good estimate
# decompose the homography into Rotation and translation
# you are not required to know how to do this for this class
# but if you are interested please refer to:
# https://docs.opencv.org/master/d9/dab/tutorial_homography.html
RT = np.matmul(np.linalg.inv(new_intrinsics), homography)
norm = np.sqrt(np.linalg.norm(RT[:, 0]) * np.linalg.norm(RT[:, 1]))
RT = -1 * RT / norm
c1 = RT[:, 0]
c2 = RT[:, 1]
c3 = np.cross(c1, c2)
T = RT[:, 2]
R = np.vstack((c1, c2, c3)).T
W, U, Vt = cv2.SVDecomp(R)
R = np.matmul(U, Vt)
return True, R, T
# return false if we could not compute a good estimate
return False, None, None
def getTransform(src, dst):
"""get transforming matrix"""
src = np.array(src, np.float)
col_mean_src = np.mean(src, axis=0)
src -= col_mean_src
dst = np.array(dst, np.float)
col_mean_dst = np.mean(dst, axis=0)
dst -= col_mean_dst
mean, dev_src = cv.meanStdDev(src)
dev_src = max(dev_src[0], 1.192e-7)
src /= dev_src[0]
mean, dev_dst = cv.meanStdDev(dst)
dev_dst = max(dev_dst[0], 1.192e-7)
dst /= dev_dst[0]
w, u, vt = cv.SVDecomp(np.matmul(src.T, dst))
r = np.matmul(u, vt)
m = np.zeros((2, 3))
m[:, 0:2] = r * (dev_dst[0] / dev_src[0])
m[:, 2] = col_mean_dst.T - np.matmul(m[:, 0:2], col_mean_src.T)
return m
def findPlaneFromHomography(H, K_inv):
"""
Some black magic to find a plane (origin and normal) from an homography and an inverse intrinsics matrix.
"""
# First, we apply the inverse intrinsics to the homography to remove the camera effects
result = np.matmul(K_inv, H)
# We need to normalize our matrix to remove the scale factor
result /= cv2.norm(result[:, 1])
# We split our resulting homography columns to get the two 2D rotation basis vectors and translation
r0, r1, t = np.hsplit(result, 3)
# To get the third rotation basis vector we simply make the cross product between the 2D basis
r2 = np.cross(r0.T, r1.T).T
# Since r0 and r1 may not be orthogonal, we use the Zhang aproximation:
# we keep only the u and vt part of the SVD of the new rotation basis matrix
# to minimize the Frobenius norm of the difference
_, u, vt = cv2.SVDecomp(np.hstack([r0, r1, r2]))
R = np.matmul(u, vt)
# We finally have our origin center and normal vector
origin = t[:, 0]
normal = R[:, 2]
return origin, normal
def projection_matrix(intrinsic, homography):
"""
Write here the compuatation for the projection matrix, namely the matrix
that maps the camera reference frame to the AprilTag reference frame.
"""
P_list = []
for H in homography:
R2D_t = np.linalg.inv(intrinsic).dot(H) # [R2D_t] = [r1, r2, t]
R2D_t = R2D_t / np.linalg.norm(R2D_t[:, 0]) #normalize
r1 = R2D_t[:, 0]
r2 = R2D_t[:, 1]
t = R2D_t[:, 2]
# r3 must be orthogonal to r1 and r2
r3 = np.cross(r1, r2)
R3D = np.column_stack((r1, r2, r3))
# since R3D is not a proper rotation matrix yet, we have to orthonormalize it with a polar decomposition: A = RP =
W, U, Vt = cv2.SVDecomp(R3D)
R3D = U.dot(Vt)
R3D_t = np.column_stack((R3D, t)) # R3D_t = [r1, r2, r3, t]
P = intrinsic.dot(R3D_t)
P_list.append(P)
return P_list
def findRigidTransform(points1, points2):
t1 = -np.average(points1, axis=0)
t2 = -np.average(points2, axis=0)
T1 = np.eye(4)
T2 = np.eye(4)
T1[:3, 3] = t1
T2[:3, 3] = -t2
C = np.zeros((3, 3))
p1 = points1 + t1
p2 = points2 + t2
C = np.sum(
[p2[i].reshape(3, 1) @ p1[i].reshape(3, 1).T for i in range(len(p2))],
axis=0)
s, u, v = cv2.SVDecomp(C)
I = np.eye(3)
if np.linalg.det(v.T @ u.T) < 0:
I[2, 2] = -I[2, 2]
R = u @ I @ v
M = np.eye(4)
M[:3, :3] = R
result = M / M[3, 3]
result = T2 @ result @ T1
result = result[:3]
return result[:3, :3], result[:, 3]
def svd_solve(A, b):
A = np.asarray(A, dtype=np.float32)
b = np.asarray(b, dtype=np.float32)
S, U, V = cv2.SVDecomp(A)
B = U.T.dot(b)
X = [B[i] / S[i] for i in range(len(S))]
res = V.T.dot(X)
return res
def computeTransformation(F, K):
E = K.T.dot(F).dot(K)
#E = F
d, u, v = cv2.SVDecomp(E, flags=cv2.SVD_FULL_UV)
newD = np.diag(np.array([1, 1, 0]))
newE = u.dot(newD).dot(v)
d, u, v = cv2.SVDecomp(newE)
#print(d)
w = np.zeros((3, 3))
w[0, 1] = -1
w[1, 0] = 1
w[2, 2] = np.linalg.det(u) * np.linalg.det(v) #1#
rot1 = u.dot(w).dot(v)
rot2 = u.dot(w.T).dot(v)
trans1 = u[:, 2]
trans2 = -trans1
return rot1, rot2, trans1, trans2
def getWorldAndImageCoordinates(whichFrame):
"""
Read the X and Y points.
Create measurement matrix.
Apply SVD decompositon to get the required matrices.
Calculate the Motion and Structure matrices.
Calculate the projection matrix.
Project the 3d world points to an image plane.
Return both 3d world points and projected points.
"""
xList = readData("xPoints.csv")
yList = readData("yPoints.csv")
xList = np.transpose(xList)
yList = np.transpose(yList)
xList, xTranslationInFrames = centerPoints(xList)
yList, yTranslationInFrames = centerPoints(yList)
measurementMatrix = createMeasurementMatrix(xList, yList)
# apply svd decomposition
w, u, vt = cv2.SVDecomp(measurementMatrix)
# calculate both motion and structure matrices
singularValues = np.array([[w[0][0], 0, 0], [0, w[1][0], 0], [0, 0, w[2][0]]])
# get the world points, i.e. structure, as the first the column of vt matrix
v = np.transpose(vt)
worldPoints = v[:, :3]
# get the camera matrices, i.e. motion matrices
motionMatrix = np.matmul(u[:, :3], singularValues)
# get projection matrix of the required frame
frameIndex = whichFrame
cameraMatrix = getCameraMatrix(motionMatrix, frameIndex)
translationVector = getTanslationVector(xTranslationInFrames, yTranslationInFrames, frameIndex)
P = createProjectionMatrix(cameraMatrix, translationVector)
# apply projectPoints function to project the 3D points into 2D image plane
pointsFromProjectionMatrix = applyProjectPoints(cameraMatrix, worldPoints)
# project world coordinates to image plane using the projection matrix
imageCoordinates = []
for point in worldPoints:
homogenousPoint = np.concatenate((point, [1]), axis = 0)
coordinate = np.matmul(P, homogenousPoint)
x = coordinate[0] / coordinate[2]
y = coordinate[1] / coordinate[2]
coordinate = np.array([x, y])
imageCoordinates.append(coordinate)
return worldPoints, imageCoordinates, pointsFromProjectionMatrix
def decompose_and_filter(img, kernel_name, kernel):
sigmas, u, vt = cv.SVDecomp(kernel)
vec_index = 0
if (sigmas[0][0] != 0):
print("%s could not be decomposed ... approximating" % kernel_name)
vec_index = np.where(sigmas == sigmas.max())[0][0]
kernel1_V = np.sqrt(sigmas[vec_index, 0]) * u[:, vec_index]
kernel1_H = np.sqrt(sigmas[vec_index, 0]) * vt[vec_index, :]
return cv.sepFilter2D(img, -1, kernel1_H, kernel1_V)
def getPrincipleAngle(eigVecs1, eigVecs2):
#e2t = cv2.transpose(eigVecs2)
#e2t = cv2.transpose(eigVecs2)
evm = cv2.gemm(eigVecs1,eigVecs2,1,None,1,flags=cv2.GEMM_2_T)
#evm = np.outer(eigVecs1,e2t)
w,u,vt = cv2.SVDecomp(evm)
#print (repr(w))
#print (repr(float(w[0])))
return float(w[0]);
def get_H_R_t(self, corners):
H = cv2.findHomography(self.inner_rectangle, corners)[0]
result = self.K_inv @ H
result /= cv2.norm(result[:, 1])
r0, r1, t = np.hsplit(result, 3)
r2 = np.cross(r0.T, r1.T).T
_, u, vt = cv2.SVDecomp(np.hstack([r0, r1, r2]))
R = u @ vt
return Plane(origin=t[:, 0], normal=R[:, 2], R=R)
