def find_essential(self, fname1, fname2, save=False, load_intrinsics=True, intrinsics_load_path=' ', stereo_save_path=' ', mtx1=None, dist1=None, mtx2=None, dist2=None): if load_intrinsics: #Try to load intrnsic values if specified, otherwise raise error try: mtx1, dist1 = self._load_intrinsics(intrinsics_load_path + '/camera1') mtx2, dist2 = self._load_intrinsics(intrinsics_load_path + '/camera2') except: raise AttributeError('No Intrinsics found') #Check that the matricies are of proper form else: if not type(mtx1) != type(np.array()): raise ValueError("R is not of type np.array") if not type(dist1) != type(np.array()): raise ValueError("R is not of type np.array") if not type(mtx2) != type(np.array()): raise ValueError("R is not of type np.array") if not type(dist2) != type(np.array()): raise ValueError("R is not of type np.array") img_name = self._image_mouse_click_directory(fname1, fname2) #Return clicked points pnts1, pnts2 = self.get_points() img = cv2.imread(img_name) imshape = (img.shape[0], img.shape[1]) #print(pnts1) E = cv2.findEssentialMat( pnts1, pnts2) #, focal = mtx1[0,0], pp = (mtx1[0,2], mtx1[1,2])) R1 = np.zeros((3, 3)) R2 = np.zeros((3, 3)) t = np.zeros((3, 1)) cv2.decomposeEssentialMat(E[0], R1, R2, t) print(R1, R2) if save: np.savetxt(stereo_save_path + '/rotation_matrix.csv', R2, fmt='%1.3f', delimiter=",") np.savetxt(stereo_save_path + '/translation_matrix.csv', t, fmt='%1.3f', delimiter=",") np.savetxt(stereo_save_path + '/essential_matrix.csv', E[0], fmt='%1.3f', delimiter=",")
def pose_estimate(K0, K1, hp0, hp1, strict_mask, rot, th=0.0001): # epipolar geometry from models.submodule import F_ngransac tmphp0 = hp0[:, strict_mask] tmphp1 = hp1[:, strict_mask] #num_samp = min(300000,tmphp0.shape[1]) #num_samp = min(30000,tmphp0.shape[1]) num_samp = min(3000, tmphp0.shape[1]) submask = np.random.choice(range(tmphp0.shape[1]), num_samp) tmphp0 = tmphp0[:, submask] tmphp1 = tmphp1[:, submask] rotx, transx, Ex = F_ngransac(torch.Tensor(tmphp0.T[np.newaxis]).cuda(), torch.Tensor(tmphp1.T[np.newaxis]).cuda(), torch.Tensor(K0[np.newaxis]).cuda(), False, 0, Kn=torch.Tensor(K1[np.newaxis]).cuda()) R01 = cv2.Rodrigues(np.asarray(rotx[0]))[0] T01 = np.asarray(transx[0]) E = np.asarray(Ex[0]) # _,R01,T01,_ = cv2.recoverPose(E.astype(float), tmphp0[:2].T, tmphp1[:2].T, K0) # RT are 0->1 points transform # T01 = T01[:,0] # R01=R01.T # T01=-R01.dot(T01) # now are 1->0 points transform R1, R2, T = cv2.decomposeEssentialMat(E) for rott in [(R1, T), (R2, T), (R1, -T), (R2, -T)]: if testEss(K0, K1, rott[0], rott[1], tmphp0, tmphp1): R01 = rott[0].T T01 = -R01.dot(rott[1][:, 0]) if not 'T01' in locals(): T01 = np.asarray([0, 0, 1]) R01 = np.eye(3) # E, maskk = cv2.findEssentialMat(np.linalg.inv(K0).dot(hp0[:,strict_mask])[:2].T, # np.linalg.inv(K1).dot(hp1[:,strict_mask])[:2].T, np.eye(3), # cv2.LMEDS,threshold=th) # # valid_points = np.ones((strict_mask.sum())).astype(bool) # valid_points[~maskk[:,0].astype(bool)]=False # fmask = strict_mask.copy() # fmask[strict_mask]=valid_points # # R1, R2, T = cv2.decomposeEssentialMat(E) # for rott in [(R1,T),(R2,T),(R1,-T),(R2,-T)]: # if testEss(K0,K1,rott[0],rott[1],hp0[:,fmask], hp1[:,fmask]): # R01=rott[0].T # T01=-R01.dot(rott[1][:,0]) # if not 'T01' in locals(): # T01 = np.asarray([0,0,1]) # R01 = np.eye(3) # T01t = T01.copy() # compensate R H01 = K0.dot(R01).dot(np.linalg.inv(K1)) # plane at infinity comp_hp1 = H01.dot(hp1) comp_hp1 = comp_hp1 / comp_hp1[-1:] return R01, T01, H01, comp_hp1, E
def processFrame(self, total_vel, positions): self.px_ref_old,self.px_cur = featureTracking(self.last_frame,self.new_frame,self.px_ref_old) E,mask = cv2.findEssentialMat(self.px_cur,self.px_ref_old, focal = self.focal, pp=self.pp,method=cv2.RANSAC,prob=0.999,threshold=.3) R1,R2,_ = cv2.decomposeEssentialMat(E) if np.trace(R1)>2.5: R_des = R1 elif np.trace(R2)>2.5: R_des = R2 else: print ("both are bad") R_des = np.array([[1,0,0],[0,1,0],[0,0,1]]) R = R_des # print (R1) _,_,t,mask = cv2.recoverPose(E,self.px_cur,self.px_ref_old,focal=self.focal,pp = self.pp) # eulers = transforms3d.euler.mat2euler(R,'rxyz') # print (eulers[0]) absolute_scale = self.getAbsoluteScale(total_vel,positions) if(absolute_scale > 0): self.cur_t = self.cur_t + absolute_scale*self.cur_R.dot(t) self.cur_R = R.dot(self.cur_R) if(self.px_ref.shape[0] < kMinNumFeature): mask = np.zeros_like(self.new_frame) mask[:] = 255 ##goodFeaturesToTrack # self.px_cur = cv2.goodFeaturesToTrack(self.new_frame, mask = mask, **feature_params) # self.px_cur = np.squeeze(self.px_cur) ##FAST self.px_cur = self.detector.detect(self.new_frame) self.px_cur = np.array([x.pt for x in self.px_cur], dtype=np.float32) self.px_ref_old = self.px_ref self.px_ref = self.px_cur
def test_frames(intrinsic_mat, fr1, fr2): corr = build_correspondences(fr1, fr2) if corr.points_1.shape[0] < 5: return None, 0 mat, mask = cv2.findEssentialMat(corr.points_1, corr.points_2, intrinsic_mat, method=cv2.RANSAC, prob=0.999, threshold=1.0) if mat is None or mat.shape != (3, 3) or not validate_mat(corr, mask): return None, 0 # https://kite.com/python/docs/cv2.decomposeEssentialMat R1, R2, t = cv2.decomposeEssentialMat(mat) poses = [ Pose(R1.T, R1.T @ t), Pose(R2.T, R2.T @ t), Pose(R1.T, R1.T @ (-t)), Pose(R2.T, R2.T @ (-t)) ] best_size, best_idx = -1, 0 for i, pose in enumerate(poses): points, _, _ = triangulate_correspondences( remove_correspondences_with_ids(corr, np.argwhere(mask == 0)), eye3x4(), pose_to_view_mat3x4(pose), intrinsic_mat, params) if points.shape[0] > best_size: best_size, best_idx = points.shape[0], i return poses[best_idx], best_size
def recover_pose(self, pts1, pts2, new_img_dim=None): """ Find rotation matrices using epipolar geometry Args: pts1 (np.ndarray): Initial points pts2 (np.ndarray): Resulting points new_img_dim (tuple, optional): New image dimension. Defaults to None. Returns: [type]: [description] """ # https://answers.opencv.org/question/31421/opencv-3-essentialmatrix-and-recoverpose/ img_dim = new_img_dim if new_img_dim else self.calib_dimension scaled_K = self.K * img_dim[0] / self.calib_dimension[0] scaled_K[2][2] = 1.0 E, mask = cv2.findEssentialMat(pts1, pts2, scaled_K, cv2.RANSAC, 0.999, 0.1) # cv2.LMEDS or cv2.RANSAC #retval, R, t, mask = cv2.recoverPose(E, pts1, pts2, scaled_K) try: R1, R2, t = cv2.decomposeEssentialMat(E) except: # Can't figure it out, assume no rotation return np.eye(3), np.eye(3), np.array([0, 0, 0]) return R1, R2, t
def extractScaledMotion(self,currentPoints,currentTriangulated,previousPoints,previousTriangulated,dictionary=False): newPts=np.zeros((len(currentPoints),2),dtype=np.float64) oldPts=np.zeros((len(currentPoints),2),dtype=np.float64) for j in range(0,len(currentPoints)): newPts[j,0]=currentPoints[j][0] newPts[j,1]=currentPoints[j][1] oldPts[j,0]=previousPoints[j][0] oldPts[j,1]=previousPoints[j][1] nisterResults=self.extractMotion(currentPoints,previousPoints,True) R1,R2,t=cv2.decomposeEssentialMat(nisterResults["E"]) nInliers,R,T,matchMask=cv2.recoverPose(nisterResults["E"],oldPts,newPts,self.extract["k"]) nisterResults["inlierMask"]=matchMask nisterResults["nInliers"]=nInliers #print(nInliers) nisterResults["T"]=T nisterResults["R"]=R nisterResults["H"]=createHomog(R,T) if(nisterResults["nInliers"]>0): s,t,inl=estimateScale(previousTriangulated,currentTriangulated,R,T,matchMask) nisterResults["T"]=t nisterResults["nInliers"]=inl nisterResults["H"]=createHomog(nisterResults["R"],nisterResults["T"]) if(not dictionary): return nisterResults["H"] else: return nisterResults
def recoverPose2(E, n1, n2, K1, K2, mask): n1 = n1.reshape((-1, 2)) n2 = n2.reshape((-1, 2)) R2a, R2b, t2 = cv2.decomposeEssentialMat(E) R1 = np.eye(3) t1 = np.zeros((3,1)) def z_count(R1, t1, R2, t2, K1, K2, n1, n2): """ Count number of points appeared in front of the cameras """ P1 = K1 @ np.hstack((R1, t1)) P2 = K2 @ np.hstack((R2, t2)) Xh1 = cv2.triangulatePoints(P1, P2, n1, n2) Xh1 /= Xh1[3,:] z1 = np.sum(Xh1[2,:]>0) # num of positive z points in Cam1 coordinate system Xh2 = R2 @ Xh1[:3,:] + t2 z2 = np.sum(Xh2[2,:]>0) # num of positive z points in Cam2 coordinate system return (z1 + z2), Xh1[:3,:] zmax = -1 for R2x, t2x in [[R2a, t2], [R2a, -t2], [R2b, t2], [R2b, -t2]]: z, Xx = z_count(R1, t1, R2x, t2x, K1, K2, n1.T, n2.T) if zmax < z: zmax = z R2_est = R2x t2_est = t2x X_est = Xx return R2_est, t2_est
def get_rotation_translation(self, vid_set): ''' Calculate the rotation and translation vectors given the unique id for a set of videos ''' r_t_pairs = [] attempts = 20 for _ in range(attempts): # Build dataframe with some random set of data in the video set vid_set_dfs = [] for m in self.metadata: if m['video_set'] == vid_set and np.random.rand() < 0.5: vid_set_dfs.append(self.get_skeleton_data(m['video_index'])) full_df = pd.concat(vid_set_dfs, ignore_index=True) rgb = np.array([full_df['color']]).astype('float32') d = np.array([full_df['depth']]).astype('float32') F, mask = cv2.findFundamentalMat(d, rgb) # Fundamental matrix E = rgb_mat.T @ F @ d_mat # Essential matrix R1, R2, T = cv2.decomposeEssentialMat(E) # Decompose essential matrix # Get rotation matrix that looks most similar to the identity matrix R = R1 if abs(np.sum(R1 - np.identity(3))) < abs(np.sum(R2 - np.identity(3))) else R2 r_t_pairs.append((R, T)) return r_t_pairs
def pose_by_frames(frame1: FrameCorners, frame2: FrameCorners, intrinsic_mat: np.ndarray) -> Tuple[Pose, int]: correspondences = build_correspondences(frame1, frame2) mat, mask = cv2.findEssentialMat(correspondences.points_1, correspondences.points_2, intrinsic_mat, cv2.RANSAC, 0.99, 1) if mat is None or mat.shape != (3, 3): return None, 0 if mask is not None: correspondences = _remove_correspondences_with_ids( correspondences, np.argwhere(mask.flatten() == 0)) R1, R2, t = cv2.decomposeEssentialMat(mat) max_pose = None max_npoints = 0 for mat in [R1.T, R2.T]: for vec in [t, -t]: pose = Pose(mat, mat @ vec) points, _, _ = triangulate_correspondences( correspondences, eye3x4(), pose_to_view_mat3x4(pose), intrinsic_mat, INITIAL_TRIANGULATION_PARAMETERS) if len(points) > max_npoints: max_pose = pose max_npoints = len(points) return max_pose, max_npoints
def reconstruction(img_pairs): for pathes in img_pairs: filename_w_ext1 = os.path.basename(pathes[0]) filename_w_ext2 = os.path.basename(pathes[1]) filename1 = os.path.splitext(filename_w_ext1)[0] filename2 = os.path.splitext(filename_w_ext2)[0] F = np.loadtxt(filename1 + "FMatrix.txt", dtype=float, delimiter=',') pts1 = np.loadtxt(filename1 + "Points1.txt", dtype=int, delimiter=',') pts2 = np.loadtxt(filename1 + "Points2.txt", dtype=int, delimiter=',') fx = fy = 721.5 cx = 690.5 cy = 172.8 K = np.array([[fx, 0., cx], [0., fy, cy], [0., 0., 1.]]) Rt = np.hstack((np.eye(3), np.zeros((3, 1)))) P0 = K.dot(Rt) E = K.T * np.mat(F) * K R1, R2, t = cv2.decomposeEssentialMat(E) P1 = verify_camerapose(P0, K, R1, R2, t, pts1.reshape(-1, 1, 2), pts2.reshape(-1, 1, 2)) pointcloud = cv2.triangulatePoints( P0, P1, np.array(pts1.reshape(-1, 1, 2), dtype=np.float), np.array(pts2.reshape(-1, 1, 2), dtype=np.float)) # 4D Punkt in 3D umwandeln pointcloud = cv2.convertPointsFromHomogeneous(pointcloud.T) write_ply(filename1 + 'punktwolke.ply', pointcloud)
def estimate_motion(image1, image2, depth_map, K): sift = cv2.xfeatures2d.SIFT_create() kp1, des1 = sift.detectAndCompute(cv2.cvtColor(image1, cv2.COLOR_BGR2GRAY), None) kp2, des2 = sift.detectAndCompute(cv2.cvtColor(image2, cv2.COLOR_BGR2GRAY), None) bf = cv2.BFMatcher(cv2.NORM_L2, crossCheck=True) matches = bf.match(des1, des2) img1_pt = [] img2_pt = [] for m in matches: img1_pt.append(kp1[m.queryIdx].pt) img2_pt.append(kp2[m.trainIdx].pt) img1_pt = np.asarray(img1_pt) img2_pt = np.asarray(img2_pt) F, mask = cv2.findFundamentalMat(img1_pt, img2_pt, cv2.FM_RANSAC) img1_pt = img1_pt[mask.ravel() == 1] img2_pt = img2_pt[mask.ravel() == 1] E = ((K.T).dot(F)).dot(K) R1, R2, t = cv2.decomposeEssentialMat(E) R, T = disambiguate_transform(R1, R2, t, img1_pt, img2_pt) scale = find_scale(depth_map, img1_pt, img2_pt, R, T) return R, -T * scale
def calc_known_views(corner_storage, intrinsic_mat, indent=5): num_frames = len(corner_storage) known_view_1 = (None, None) known_view_2 = (None, None) num_points = -1 for frame_1 in range(num_frames): for frame_2 in range(frame_1 + indent, num_frames): corrs = build_correspondences(corner_storage[frame_1], corner_storage[frame_2]) if len(corrs.ids) < 6: continue points_1 = corrs.points_1 points_2 = corrs.points_2 H, mask_h = cv2.findHomography(points_1, points_2, method=cv2.RANSAC) if mask_h is None: continue mask_h = mask_h.reshape(-1) E, mask_e = cv2.findEssentialMat(points_1, points_2, method=cv2.RANSAC, cameraMatrix=intrinsic_mat) if mask_e is None: continue mask_e = mask_e.reshape(-1) if mask_h.sum() / mask_e.sum() > 0.5: continue corrs = Correspondences(corrs.ids[(mask_e == 1)], points_1[(mask_e == 1)], points_2[(mask_e == 1)]) R1, R2, t = cv2.decomposeEssentialMat(E) for poss_pose in [ Pose(R1.T, R1.T @ t), Pose(R1.T, R1.T @ (-t)), Pose(R2.T, R2.T @ t), Pose(R2.T, R2.T @ (-t)) ]: points3d, _, _ = triangulate_correspondences( corrs, eye3x4(), pose_to_view_mat3x4(poss_pose), intrinsic_mat, TriangulationParameters(1, 2, .1)) if len(points3d) > num_points: num_points = len(points3d) known_view_1 = (frame_1, view_mat3x4_to_pose(eye3x4())) known_view_2 = (frame_2, poss_pose) return known_view_1, known_view_2
def main(): img_path = '../Data/stereo/centre/' imgs = sorted(os.listdir(img_path)) fx, fy, cx, cy, G_camera_image, LUT = ReadCameraModel('../Data/model') i = 100 img1 = cv2.imread(img_path + imgs[i], -1) img1 = cv2.cvtColor(img1, cv2.COLOR_BAYER_GR2BGR) img1 = UndistortImage(img1, LUT) img2 = cv2.imread(img_path + imgs[i + 1], -1) img2 = cv2.cvtColor(img2, cv2.COLOR_BAYER_GR2BGR) img2 = UndistortImage(img2, LUT) x1, x2 = utils.getMatchingFeaturePoints(img1, img2) x1 = np.hstack([x1, np.ones((x1.shape[0], 1))]) x2 = np.hstack([x2, np.ones((x2.shape[0], 1))]) # print(x1.shape) # print(x2.shape) features = np.hstack([x1, x2]) # getInliersRANSAC(features,threshold=(0.07),size=8,num_inliers=0.6*features.shape[0],num_iters=1000) x1_in, x2_in = RANSAC.getInliersRANSAC(features, threshold=(0.005), size=8, num_inliers=0.6 * features.shape[0], num_iters=200) fund_mtx = fundamental.EstimateFundamentalMatrix(x1_in, x2_in) K = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]]) ess_mtx = essential.EssentialMatrixFromFundamentalMatrix(fund_mtx, K) print("Essential Matrix") print(ess_mtx) C, R = ExtractCameraPose(ess_mtx) print("Pose Orientation :") print(R) E_act = cv2.findEssentialMat(x1_in[:, :2], x2_in[:, :2], K) # _,R,T,_ = cv2.recoverPose(E_act[0],x1_in[:,:2],x2_in[:,:2]) # print("Pose Position :") # print(T.T) # print("Pose Orientation :") # print(R) R1, R2, T = cv2.decomposeEssentialMat(E_act[0]) print("OpenCV R1") print(R1) print("OpenCV R2") print(R2) print("Calculated Pose Position :") print(C) print("Opencv T") print(T.T)
def feat_match(img_train, img_query, num_fr, camMat, crop=1, foc_len=1200, match_pnts=20, thresh=1): # in practice for original video, foc_len can be applied, # in testing for resized video, camMat is applied kp1, des1 = orb.detectAndCompute(img_train, None) kp2, des2 = orb.detectAndCompute(img_query, None) # frm = np.copy(img_query) if len(des1) < 8 or len(des2) < 8: print("not enough feature pnts to be matched") return np.zeros((3, )) matches = bf.match(des2, des1) # pos1 for query if len(matches) < 8: print("not enough matches in %d frame, pitch unchanged" % num_fr) return np.zeros((3, )) # sort matches according to score matches = sorted(matches, key=lambda x: x.distance)[:match_pnts] # extract matched-feature coorinates kpts1 = np.array([kp1[m.trainIdx].pt for m in matches], dtype=np.int) kpts2 = np.array([kp2[m.queryIdx].pt for m in matches], dtype=np.int) # essMat, mask = cv.findEssentialMat(kpts1, kpts2, focal=foc_len, prob=0.9999, threshold=0.1) # focal length to be revised essMat, mask = cv.findEssentialMat(kpts1, kpts2, camMat, prob=0.9999, threshold=0.1) # matches = [matches[i] for i in range(len(mask)) if mask[i] == 1] # img_out = cv.drawMatches(img_query, kp2, img_train, kp1, matches, None, # flags=cv.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS) R1, R2, t = cv.decomposeEssentialMat(essMat) rotAngs1 = rotMat2EulAng(R1) rotAngs2 = rotMat2EulAng(R2) rotAngs, RMat = sel_ang(rotAngs1, rotAngs2, R1, R2, thresh) # for i in range(len(rotAngs)): # if abs(rotAngs[i]) > 3: # print("the %sth param: %.3f, frame %s" %(i, rotAngs[i], num_fr)) # print(rotAngs1) # print(rotAngs2) # t_ang = "pitch:%.3f; yaw:%.3f; row:%.3f" % (rotAngs[0], rotAngs[1], rotAngs[2]) # t_fr = "%s" % num_fr # img_out = cv.putText(img_out, t_ang, (100, 50), cv.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255)) # img_out = cv.putText(img_out, t_fr, (20, 50), cv.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255)) # return pitch & yaw angle # return rotAngs[0], rotAngs[2] # return rotAngs, img_out return rotAngs
def _initialize_cloud_by_two_frames( prev_corners, cur_corners, intrinsic_mat, base_view, tracking_mode: TrackingMode ): correspondences = build_correspondences(prev_corners, cur_corners) failed = correspondences.points_1.shape[0] <= 5 e, mask = [None] * 2 if not failed: e, mask = cv2.findEssentialMat( correspondences.points_1, correspondences.points_2, intrinsic_mat, **tracking_mode.essential_mat_params ) _, mask_h = cv2.findHomography( correspondences.points_1, correspondences.points_2, method=tracking_mode.essential_mat_params['method'], ransacReprojThreshold=tracking_mode.triangulation_params.max_reprojection_error, maxIters=tracking_mode.solve_pnp_ransac_params['iterationsCount'], confidence=tracking_mode.essential_mat_params['prob'], mask=np.copy(mask) ) essential_inliers = np.sum(mask) homography_inliers = np.sum(mask_h) failed |= homography_inliers == 0 \ or essential_inliers < tracking_mode.essential_homography_ratio_threshold * homography_inliers if not failed: mask = np.ma.make_mask(mask).flatten() r1, r2, t12 = cv2.decomposeEssentialMat(e) points, ids, view = np.array([]), np.array([]), None for R in [r1, r2]: for t in [t12, -t12]: view_ = np.hstack((R, t)) points_, ids_ = triangulate_correspondences( correspondences, base_view, view_, intrinsic_mat, tracking_mode.triangulation_params, mask=mask ) if points_.size > points.size: points, ids, view = points_, ids_, view_ return points.shape[0], view, points, ids return -1, None, None, None
def calculateCamera(p_old, p_new): ''' def cleanPoints(pts): mean = np.sum(pts, axis = 0) pts = map(lambda pt: pt - mean, pts) scale = np.sum(np.linalg.norm(pts, axis=2)) / len(pts) pts = pts * np.sqrt(2) /scale return mean, scale, pts ''' #mean, scale, p_old = cleanPoints(p_old) p_old = p_old.copy() p_new = p_new.copy() def normPts(pts): pts[:, :, 0] = pts[:, :, 0] / w - 0.5 pts[:, :, 1] = pts[:, :, 1] / h - 0.5 return pts #print p_old.shape p_old = normPts(p_old) p_new = normPts(p_new) F, mask = cv2.findFundamentalMat(p_old, p_new, cv2.FM_RANSAC) #F p_old = line in p_new p_old = p_old[mask == 1] p_new = p_new[mask == 1] #U, s, V = np.linalg.svd(F) #, full_matrices=True) #Note that V here is often called V.H eslewhere in literature #F = np.dot(np.dot(U, np.diag([1,1,0])),V) #retval, R, t, mask = cv2.recoverPose(F, p_old, p_new) R1, R2, t = cv2.decomposeEssentialMat(F) def findAngle(R): return np.arccos((np.trace(R) - 1) / 2) #angle = np.arccos((np.trace(R)-1)/2) angle1 = findAngle(R1) angle2 = findAngle(R2) #print angle if angle1 < angle2: R = R1 else: R = R2 #print t if np.linalg.det(R) <= 0: print "YOU IDIOT. Det<0" #if retval < 10: # R=np.identity(3) #print mask return R, t[0]
def evaluate_pose(E, P): R_gt = P[:3, :3] t_gt = P[:3, 3] R1, R2, t = cv2.decomposeEssentialMat(E) t = t.squeeze() theta_1 = np.linalg.norm(scipy.linalg.logm(R1.T.dot(R_gt)), 'fro') / np.sqrt(2) theta_2 = np.linalg.norm(scipy.linalg.logm(R2.T.dot(R_gt)), 'fro') / np.sqrt(2) theta = min(theta_1, theta_2) * 180 / np.pi tran_cos = np.inner(t, t_gt) / (np.linalg.norm(t_gt) * np.linalg.norm(t)) tran = np.arccos(tran_cos) * 180 / np.pi return theta, tran
def recover_pose(self, pts1, pts2, new_img_dim = None): # https://answers.opencv.org/question/31421/opencv-3-essentialmatrix-and-recoverpose/ # Find essential matrix from fundamental matrix img_dim = new_img_dim if new_img_dim else self.calib_dimension scaled_K = self.K * img_dim[0] / self.calib_dimension[0] scaled_K[2][2] = 1.0 E, mask = cv2.findEssentialMat(pts1, pts2, scaled_K, cv2.RANSAC, 0.999, 0.1) # cv2.LMEDS or cv2.RANSAC #retval, R, t, mask = cv2.recoverPose(E, pts1, pts2, scaled_K) R1, R2, t = cv2.decomposeEssentialMat(E) return R1, R2, t
def init_first_camera_positions(intrinsic_mat, corner_storage): frame_count = len(corner_storage) best_pair = (-1, -1) zero_view_mat = eye3x4() best_view_mat = None best_triangulated_points = -1 confidence = 0.9 params = TriangulationParameters(max_reprojection_error=2, min_triangulation_angle_deg=1, min_depth=0.5) for i in range(0, frame_count, 10): print("Init first camera. Frame %d/%d" % (i + 1, frame_count)) for j in range(i + 3, min(i + 30, frame_count), 3): correspondences = build_correspondences(corner_storage[i], corner_storage[j]) if len(correspondences.ids) < 5: continue points_1, points_2 = correspondences.points_1, correspondences.points_2 e_matrix, e_mask = findEssentialMat(points_1, points_2, intrinsic_mat, method=RANSAC, threshold=2, prob=confidence) h_matrix, h_mask = findHomography(points_1, points_2, method=RANSAC, ransacReprojThreshold=2, confidence=confidence) e_inliers, h_inliers = sum(e_mask.reshape(-1)), sum( h_mask.reshape(-1)) if e_inliers / h_inliers < 0.1: continue outliers = np.delete(correspondences.ids, correspondences.ids[e_mask]) correspondences = build_correspondences(corner_storage[i], corner_storage[j], outliers) R1, R2, t = decomposeEssentialMat(e_matrix) for rv in [R1, R2]: for tv in [-t, t]: candidate_veiw_mat = np.hstack((rv, tv)) points, ids, _ = triangulate_correspondences( correspondences, zero_view_mat, candidate_veiw_mat, intrinsic_mat, params) if len(points) > best_triangulated_points: best_triangulated_points = len(points) best_pair = (i, j) best_view_mat = candidate_veiw_mat return (best_pair[0], zero_view_mat), (best_pair[1], best_view_mat)
def process(img2): img1 = cv2.imread("sampleImages/img-center.png") src_points = extract_points(img1) dst_points = extract_points(img2) # retval = cv2.estimateRigidTransform(src_points, dst_points, fullAffine=False) # matrix = None # matrix = cv2.getPerspectiveTransform(src_points, dst_points) retval, mask = cv2.findHomography(src_points, dst_points) # print(retval) # print(MATRIX) time.sleep(1) number, rotations, translations, normals = cv2.decomposeHomographyMat( retval, MATRIX) print(number) for possible_rotation in rotations: rot_vector = cv2.Rodrigues(possible_rotation)[0] print(list(map(math.degrees, rot_vector))) # [print(math.degrees(rotation_val)) for sublist in [cv2.Rodrigues(rotation)[0] for rotation in rotations] for rotation_val in sublist] # print(translations)z # print(normals) # print(output) # print(output[0]) # print(output[1]) # print(output[2]) new_src = np.int32(src_points) new_dst = np.int32(dst_points) # print(src_points) # print(dst_points) essentialMatrix, mask = cv2.findFundamentalMat(new_src, new_dst, cv2.FM_LMEDS) # print(essentialMatrix) # print("\n"*20) # pose = cv2.recoverPose(essentialMatrix, src_points, dst_points, MATRIX) R1, R2, t = cv2.decomposeEssentialMat( essentialMatrix) # for thing in pose: vector1 = cv2.Rodrigues(R1)[0] vector2 = cv2.Rodrigues(R2)[0] pos1 = [ math.degrees(item) for sublist in vector1.tolist() for item in sublist ] pos2 = [ math.degrees(item) for sublist in vector2.tolist() for item in sublist ] # print(vector2.tolist()) print(pos1) print(pos2)
def select_model(_): """ Select model and return possible transform permutations """ if (_['model'] == 'H'): # decompose_H() res_h, Hr, Ht, Hn = cv2.decomposeHomographyMat(_['H'], _['K']) Ht = np.float32(Ht) Ht /= np.linalg.norm(Ht, axis=(1, 2), keepdims=True) T_perm = zip(Hr, Ht) else: # decompose_E() R1, R2, t = cv2.decomposeEssentialMat(_['E']) T_perm = [(R1, t), (R2, t), (R1, -t), (R2, -t)] return T_perm
def recover_pose(E, K1, K2, pts1_undist, pts2_undist): Rx1, Rx2, tx = cv2.decomposeEssentialMat(E) candidate_poses = [(Rx1, tx), (Rx1, -tx), (Rx2, tx), (Rx2,-tx)] # positive depth constraint is used to disambiguate the solutions n_in_front = [] for R, t in candidate_poses: n_pos_depth = positive_depth_count(R, t, K1, K2, pts1_undist, pts2_undist) n_in_front.append(n_pos_depth) return candidate_poses[np.argmax(n_in_front)]
def decomp_essential_mat(E, q1, q2, K, P): def sum_z(R, t): T = form_transf(R, t) P2 = np.matmul(np.concatenate((K, np.zeros((3, 1))), axis=1), T) hom_Q1 = cv2.triangulatePoints(P, P2, q1.T, q2.T) hom_Q2 = np.matmul(T, hom_Q1) return sum(hom_Q1[2, :] / hom_Q1[3, :] > 0) + sum( hom_Q2[2, :] / hom_Q2[3, :] > 0) R1, R2, t = cv2.decomposeEssentialMat(E) t = np.squeeze(t) pairs = [[R1, t], [R1, -t], [R2, t], [R2, -t]] sums = [sum_z(R, t) for R, t in pairs] return pairs[np.argmax(sums)]
def _two_frame_initialization(self, frame1: FrameCorners, frame2: FrameCorners): correspondences = build_correspondences(frame1, frame2) if correspondences.points_1.shape[0] < 5: return None, 0 essential_mat, mask_essential = cv2.findEssentialMat( correspondences.points_1, correspondences.points_2, self._intrinsic_mat, method=cv2.RANSAC, prob=0.9999, threshold=1.) _, mask_homography = cv2.findHomography( correspondences.points_1, correspondences.points_2, method=cv2.RANSAC, confidence=0.9999, ransacReprojThreshold=self._triangulation_parameters. max_reprojection_error) # zeros in mask correspond to outliers essential_inliers = np.count_nonzero(mask_essential) homography_inliers = np.count_nonzero(mask_homography) if essential_inliers < 1. * homography_inliers: return None, 0 correspondences_filtered = remove_correspondences_with_ids( correspondences, np.argwhere(mask_essential == 0)) R1, R2, t = cv2.decomposeEssentialMat(essential_mat) possible_poses = [ Pose(R1.T, R1.T @ t), Pose(R2.T, R2.T @ t), Pose(R1.T, R1.T @ (-t)), Pose(R2.T, R2.T @ (-t)) ] poses2points = [0] * 4 for i, pose in enumerate(possible_poses): points, ids = triangulate_correspondences( correspondences_filtered, eye3x4(), pose_to_view_mat3x4(pose), self._intrinsic_mat, self._triangulation_parameters) poses2points[i] = points.shape[0] best_pose = np.argmax(poses2points) return possible_poses[best_pose], poses2points[best_pose]
def Pose_Est(im1, im2, k1, k2): orb = cv2.ORB_create() kp1, descs1 = orb.detectAndCompute(im1, None) kp2, descs2 = orb.detectAndCompute(im2, None) #pts_1 = np.array([x.pt for x in kp1], dtype=np.float32) #pts_2 = np.array([x.pt for x in kp2], dtype=np.float32) matcher = cv2.BFMatcher() matches = matcher.knnMatch(descs1, descs2, k=2) good = [] pt1 = [] pt2 = [] for i, (m, n) in enumerate(matches): if m.distance < 0.8 * n.distance: good.append(m) pt2.append(kp2[m.trainIdx].pt) pt1.append(kp1[m.queryIdx].pt) pts1 = np.float32(pt1) pts2 = np.float32(pt2) F, mask = cv2.findFundamentalMat(pts1, pts2, cv2.FM_LMEDS) pts1 = pts1[mask.ravel() == 1] pts2 = pts2[mask.ravel() == 1] pts11 = pts1.reshape(-1, 1, 2) pts22 = pts2.reshape(-1, 1, 2) pts1_norm = cv2.undistortPoints(pts11, cameraMatrix=k1, distCoeffs=None) pts2_norm = cv2.undistortPoints(pts22, cameraMatrix=k2, distCoeffs=None) E, mask = cv2.findEssentialMat(pts1_norm, pts2_norm, focal=k1[0, 0], pp=(k1[0, 2], k1[1, 2]), method=cv2.RANSAC, prob=0.99, threshold=1.0) r1, r2, t = cv2.decomposeEssentialMat(E) _, R, T, mask = cv2.recoverPose(E, pts1_norm, pts2_norm, focal=k1[0, 0], pp=(k1[0, 2], k1[1, 2])) return R, T
def get_pose_with_score(frame_1, frame_2, corner_storage, intrinsic_mat): corners1, corners2 = corner_storage[frame_1], corner_storage[frame_2] correspondences = build_correspondences(corners1, corners2) essential_mat, mask_essential = cv2.findEssentialMat(correspondences[1], correspondences[2], intrinsic_mat, method=cv2.RANSAC, threshold=1.0) if essential_mat is None or mask_essential is None: return None, 0 _, mask_homography = cv2.findHomography(correspondences[1], correspondences[2], method=cv2.RANSAC) essential_inliers, homography_inliers = mask_essential.flatten().sum( ), mask_homography.flatten().sum() if homography_inliers > essential_inliers * 0.5: return None, 0 correspondences = _remove_correspondences_with_ids( correspondences, np.argwhere(mask_essential == 0)) R1, R2, t = cv2.decomposeEssentialMat(essential_mat) candidates = [ Pose(R1.T, R1.T @ t), Pose(R1.T, R1.T @ (-t)), Pose(R2.T, R2.T @ t), Pose(R2.T, R2.T @ (-t)) ] best_pose_score, best_pose = 0, None triangulation_parameters = TriangulationParameters(1, 2, .1) for pose in candidates: points, _, _ = triangulate_correspondences(correspondences, eye3x4(), pose_to_view_mat3x4(pose), intrinsic_mat, triangulation_parameters) if len(points) > best_pose_score: best_pose_score = len(points) best_pose = pose return best_pose, best_pose_score
def compute_camera_pose(F, K): E = K.T @ F @ K R_1, R_2, t = cv2.decomposeEssentialMat(E) # 4 cases R1, t1 = R_1, t R2, t2 = R_1, -t R3, t3 = R_2, t R4, t4 = R_2, -t Rs = [R1, R2, R3, R4] ts = [t1, t2, t3, t4] Cs = [] for i in range(4): Cs.append(-Rs[i].T @ ts[i]) return Rs, Cs
def calculate_known_views( intrinsic_mat, corner_storage: CornerStorage, min_correspondencies_count=100, max_homography=0.7, ) -> Tuple[Tuple[int, Pose], Tuple[int, Pose]]: best_points_num, best_known_views = -1, ((None, None), (None, None)) for i in range(len(corner_storage)): for j in range(i + 1, min(i + 40, len(corner_storage))): corresp = build_correspondences(corner_storage[i], corner_storage[j]) if len(corresp[0]) < min_correspondencies_count: break E, mask = cv2.findEssentialMat(corresp.points_1, corresp.points_2, cameraMatrix=intrinsic_mat, method=cv2.RANSAC) mask = (mask.squeeze() == 1) corresp = Correspondences(corresp.ids[mask], corresp.points_1[mask], corresp.points_2[mask]) if E is None: continue # Validate E using homography H, mask = cv2.findHomography(corresp.points_1, corresp.points_2, method=cv2.RANSAC) if np.count_nonzero(mask) / len(corresp.ids) > max_homography: continue R1, R2, T = cv2.decomposeEssentialMat(E) for view_mat2 in [ np.hstack((R, t)) for R in [R1, R2] for t in [T, -T] ]: view_mat1 = np.eye(3, 4) points, _, _ = triangulate_correspondences( corresp, view_mat1, view_mat2, intrinsic_mat, triang_params) print('Try frames {}, {}: {} correspondent points'.format( i, j, len(points))) if len(points) > best_points_num: best_known_views = ((i, view_mat3x4_to_pose(view_mat1)), (j, view_mat3x4_to_pose(view_mat2))) best_points_num = len(points) if best_points_num > 1500: return best_known_views return best_known_views
def errorRotation(self, src, dst): ps = src.points(dst) ps = np.array([ i for i in ps]) pd = dst.points(src) E = cv2.findEssentialMat(ps, pd) if E is None: return try: decomposition = cv2.decomposeEssentialMat(E[0]) except: print E r1 = (self.decomposeRotation(inv(self.rotation(src.r)).dot(decomposition[0].dot(self.rotation(dst.r))))+np.pi/2) % np.pi - np.pi/2 r2 = (self.decomposeRotation(inv(self.rotation(src.r)).dot(decomposition[1].dot(self.rotation(dst.r))))+np.pi/2) % np.pi - np.pi/2 if r1.dot(np.transpose(r1)) < r2.dot(np.transpose(r2)): return r1,r2,E[1] else: return r1,r2,E[1]
def vectorizePose(self): E, mask = cv.findEssentialMat(self.pts_ref, self.pts_frame, focal=1.0, pp=(0., 0.), method=cv2.RANSAC, prob=0.99, threshold=1.0) r1, r2, t = cv.decomposeEssentialMat(E) _, R, T, mask = cv.recoverPose(E, self.pts_ref, self.pts_frame, focal=1.0, pp=(0., 0.)) self.Rmat = R self.Tvec = T
def find_view_by_two_frames(ids_1: np.ndarray, ids_2: np.ndarray, corners_1: np.ndarray, corners_2: np.ndarray, intrinsic_mat: np.ndarray) \ -> Tuple[Optional[np.ndarray], Optional[np.ndarray], int]: correspondences = build_correspondences(ids_1, ids_2, corners_1, corners_2) if len(correspondences.ids) < 7: return None, None, 0 mat, mat_mask = cv2.findEssentialMat( correspondences.points_1, correspondences.points_2, intrinsic_mat, method=cv2.RANSAC, prob=RANSAC_P, threshold=MAX_EPIPOL_LINE_DIST ) mat_mask = mat_mask.flatten() correspondences = remove_correspondences_with_ids( correspondences, np.argwhere(mat_mask == 0).astype(np.int32)) view_1 = eye3x4() best_view = None best_count = -1 rotation_1, rotation_2, translation = cv2.decomposeEssentialMat(mat) rotation_1, rotation_2 = rotation_1.T, rotation_2.T for r in (rotation_1, rotation_2): for t in (translation, -translation): view_2 = pose_to_view_mat3x4(Pose(r, r @ t)) _, point_ids, _ = triangulate_correspondences( correspondences, view_1, view_2, intrinsic_mat, TRIANGULATION_PARAMETERS ) if best_count < len(point_ids): best_view = view_2 best_count = len(point_ids) return view_1, best_view, best_count
myC1 = Camera.myCamera(k) myC1.projectiveMatrix(np.mat([0,0,0]).transpose(),[0, 0, 0]) #retorna pontos correspondentes Xp_1, Xp_2 = clsReconstruction.getMathingPoints('b4.jpg','b5.jpg','k_cam_hp.dat') #evaluate the essential Matrix using the camera parameter(using the original points) E, mask0 = cv2.findEssentialMat(Xp_1,Xp_2,k,cv2.FM_RANSAC) #evaluate the fundamental matrix (using the normilized points) #F, mask = cv2.findFundamentalMat(Xp_1,Xp_2,cv2.FM_RANSAC) #ki = np.linalg.inv(k) R1, R2, t = cv2.decomposeEssentialMat(E) retval, R, t, mask2 = cv2.recoverPose(E,Xp_1,Xp_2) myC2 = Camera.myCamera(k) myC2.projectiveMatrix(np.mat(t),R) Xp_4Dt = cv2.triangulatePoints(myC1.P[:3],myC2.P[:3],Xp_1.transpose()[:2],Xp_2.transpose()[:2]) #Xp_4Dt = cv2.triangulatePoints(myC1.P[:3],myC2.P[:3],Xh_1.transpose()[:2],Xh_2.transpose()[:2]) Xp_4D = Xp_4Dt.T