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)