├── README.md ├── flann_based_matcher.py ├── pose_estimation_2d2d.py ├── pose_estimation_3d2d.py ├── ransac_test.py ├── simStereoCamera.py ├── testRefine.py └── utils.py /README.md: -------------------------------------------------------------------------------- 1 | Python implements of some of the projects/files in [slambook](https://github.com/gaoxiang12/slambook). 2 | 3 | 4 | ## SOME NOTES: 5 | 6 | * image color and corresponding difference, `im1 = cv2.imread(im1_file)` and `im1 = cv2.imread(im1_file, 0)`. Or the color space (RGB, BGR) and the consequences. 7 | 8 | * inner points in findFundamentalMat, when it is not the same as the input num? 9 | 10 | * when we calc Fundamental Matrix 11 | 12 | `pts1 = np.int32(pts1)` and `pts1 = np.array(pts1)` will lead to very different answers, so, which is better?? 13 | 14 | Use `np.array(pts1)` or `np.float32(pts1)` 15 | 16 | 17 | * Matrix manipulation in C++ and python 18 | 19 | [Opencv中Mat矩阵相乘——点乘、dot、mul运算详解](http://blog.csdn.net/dcrmg/article/details/52404580 20 | 21 | ``` 22 | arr = [[1,2,3],[4,5,6]] 23 | a = np.array(arr) 24 | b = np.array(arr) 25 | 26 | C++ python 27 | * dot 28 | a*b.t() a.dot(b.T) or np.dot(a,b.T) 29 | 30 | dot * -> sum 31 | a.dot(b) (a*b).sum() 32 | 33 | mul * 34 | a.mul(b) a*b 35 | 36 | So, C++: E = K2.t () * F * K1; 37 | python: E = K2.T.dot(F).dot(K1) 38 | ``` 39 | 40 | * The corresponding relationship between F,E and H 41 | 42 | * [recoverPose](http://docs.opencv.org/3.2.0/d9/d0c/group__calib3d.html) 43 | 44 | ``` vi 45 | This function decomposes an essential matrix using decomposeEssentialMat and then verifies possible pose hypotheses by doing cheirality check. The cheirality check basically means that the triangulated 3D points should have positive depth. Some details can be found in [119] . 46 | ``` 47 | 48 | * check_solutions(fp, sp, K, R1, R2, t): 49 | should have same result as recoverPose, but without good luck 50 | 51 | * some notes online 52 | 53 | - [import cv2 #Notes](https://pythonpath.wordpress.com/import-cv2/) 54 | 55 | * The usage of 3xN and Nx3 56 | 57 | * findFundamentalMat, findEssentialMat all have `mask` return value and means `inner points`, we should take care of them 58 | 59 | * [相机位姿求解问题？](https://www.zhihu.com/question/51510464) 60 | 61 | -------------------------------------------------------------------------------- /flann_based_matcher.py: -------------------------------------------------------------------------------- 1 | """ 2 | http://opencv-python-tutroals.readthedocs.io/en/latest/py_tutorials/py_feature2d/py_matcher/py_matcher.html 3 | """ 4 | 5 | if __name__ == "__main__": 6 | import numpy as np 7 | import cv2 8 | from matplotlib import pyplot as plt 9 | 10 | 11 | base_dir = "H:/projects/SLAM/python_code/dataset/our/trajs2/" 12 | 13 | im1_file = base_dir + '1.jpg' 14 | im2_file = base_dir + '4.jpg' 15 | 16 | img1 = cv2.imread(im1_file, 0) # queryImage 17 | img2 = cv2.imread(im2_file, 0) # trainImage 18 | 19 | # Initiate SIFT detector 20 | # sift = cv2.SIFT() 21 | # fd_de = cv2.xfeatures2d.SIFT_create() 22 | fd_de = cv2.xfeatures2d.SURF_create() 23 | # fd_de = cv2.ORB_create(nfeatures=5000) 24 | 25 | # find the keypoints and descriptors with SIFT 26 | kp1, des1 = fd_de.detectAndCompute(img1, None) 27 | kp2, des2 = fd_de.detectAndCompute(img2, None) 28 | 29 | # FLANN parameters 30 | FLANN_INDEX_KDTREE = 0 31 | index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=5) 32 | 33 | index_params_orb = dict(algorithm=6, # FLANN_INDEX_LSH <-> 6, not so sure why I can not use it 34 | table_number=6, # 12 35 | key_size=12, # 20 36 | multi_probe_level=1) # 2 37 | 38 | index_params_orb2 = dict(algorithm=6, # FLANN_INDEX_LSH <-> 6, not so sure why I can not use it 39 | table_number=6, # 12 40 | key_size=12, # 20 41 | multi_probe_level=1) # 2 42 | 43 | """ 44 | # FAILED 45 | index_params_auto = dict(algorithm=255, 46 | target_precision=0.9, 47 | build_weight=0.01, 48 | memory_weight=0, 49 | sample_fraction=0.1) # FLANN_INDEX_AUTOTUNED <-> 255 50 | # flann = cv2.FlannBasedMatcher(index_params_auto, search_params) 51 | """ 52 | 53 | search_params = dict(checks=50) # or pass empty dictionary 54 | 55 | flann = cv2.FlannBasedMatcher(index_params, search_params) 56 | #flann = cv2.FlannBasedMatcher(index_params_orb, search_params) 57 | #flann = cv2.FlannBasedMatcher(index_params_orb2, search_params) 58 | #matches_HAMMING = flann.match(des1, des2) # for ORB like 59 | 60 | matches = flann.knnMatch(des1, des2, k=2) 61 | 62 | # Need to draw only good matches, so create a mask 63 | matchesMask = [[0, 0] for i in range(len(matches))] 64 | 65 | 66 | 67 | matches_good = [] 68 | # ratio test as per Lowe's paper 69 | for i, (m, n) in enumerate(matches): 70 | if m.distance < 0.7 * n.distance: 71 | matchesMask[i] = [1, 0] 72 | matches_good.append(m) 73 | 74 | draw_params = dict(matchColor=(0, 255, 0), 75 | singlePointColor=(255, 0, 0), 76 | matchesMask=matchesMask, 77 | flags=0) 78 | 79 | print("Matches with ratio test:{}->{}".format(len(matches), len(matches_good))) 80 | 81 | img3 = cv2.drawMatchesKnn(img1, kp1, img2, kp2, matches, None, **draw_params) 82 | #img3 = cv2.drawMatchesKnn(img1, kp1, img2, kp2, matches_good, None, flags=2) 83 | 84 | 85 | plt.imshow(img3, ), plt.show() -------------------------------------------------------------------------------- /pose_estimation_2d2d.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | Something interesting, but not so pleasing. 4 | IN our recoverPose, seems the output mask in None, a.k.a inliners are none. As said in [recoverPose](http://docs.opencv.org/3.2.0/d9/d0c/group__calib3d.html), 5 | In the output mask only inliers which pass the cheirality check, so, we did a wrong calculation? 6 | Update: after try print out the values, we find that, the mask is make up by 0 and 255, not like others with 0 and 1. 7 | 8 | """ 9 | 10 | """ 11 | # Transport of [triangulation_2d3d2d](https://github.com/MiaoDX-fork-and-pruning/slambook/blob/windows/ch7/triangulation_2d3d2d.cpp) 12 | 13 | 1.find feature matches, [ref](https://opencv-python-tutroals.readthedocs.io/en/latest/py_tutorials/py_feature2d/py_matcher/py_matcher.html) 14 | 2.estimate the relative R,t (eight-points, five-points). In real projects, we get the `exact` relative motion ahead 15 | 3.triangulation to find the 3d points 16 | 4.prune out some points 17 | 5.use solvePnP to re-calc the R,t 18 | 19 | """ 20 | import cv2 21 | from mpl_toolkits.mplot3d import Axes3D #projection='3d' 22 | from matplotlib import pyplot as plt 23 | import numpy as np 24 | 25 | from utils import * 26 | import imutils 27 | 28 | 29 | DEBUG = False 30 | 31 | def find_keypoints_and_description(img): 32 | # Initiate STAR detector 33 | orb = cv2.ORB_create(1000) 34 | 35 | """ 36 | # find the keypoints with ORB 37 | kp = orb.detect(img, None) 38 | 39 | # compute the descriptors with ORB 40 | # NOTE: it will output another key-points, but the are presenting the same thing 41 | kpp, des = orb.compute(img, kp) 42 | """ 43 | 44 | kpp, des = orb.detectAndCompute(img, None) 45 | if DEBUG: 46 | print("KP points:{}".format(len(kpp))) 47 | 48 | 49 | 50 | """ 51 | # Make sure the key-points are the same 52 | 53 | kp1, des1 = orb.detectAndCompute(img, None) 54 | for kpi, kppi, kp1i in zip(kp, kpp, kp1): 55 | # print("{},{},{}\n".format(kpi, kppi,cv2.KeyPoint_overlap(kpi, kppi))) 56 | print("{},{},{}".format(cv2.KeyPoint_convert([kpi]), cv2.KeyPoint_convert([kppi]), cv2.KeyPoint_convert([kp1i]))) 57 | assert cv2.KeyPoint_overlap(kpi, kppi) == 1.0 58 | assert cv2.KeyPoint_overlap(kpi, kp1i) == 1.0 59 | assert cv2.KeyPoint_overlap(kppi, kp1i) == 1.0 60 | 61 | for d1, d2 in zip(des, des1): 62 | print("des1:{},des2:{},equal{}\n".format(d1, d2, d1==d2)) 63 | """ 64 | 65 | if DEBUG: 66 | # draw only keypoints location,not size and orientation 67 | img2 = cv2.drawKeypoints(img, kpp, None, color=(0, 255, 0), flags=0) 68 | plt.imshow(img2),plt.show() 69 | return kpp, des 70 | 71 | 72 | 73 | def DEBUG_Matches(img1, kp1, img2, kp2, matches, name): 74 | """ drawMatches(img1, keypoints1, img2, keypoints2, matches1to2, outImg[, matchColor[, singlePointColor[, matchesMask[, flags]]]]) -> outImg """ 75 | im = cv2.drawMatches(img1, kp1, img2, kp2, matches, None, flags=2) 76 | plt.imshow(cv2plt(im)), plt.title(name), plt.show() 77 | 78 | def find_matches_from_descriptors(des1, des2): 79 | 80 | # create BFMatcher object 81 | bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True) 82 | 83 | # Match descriptors. 84 | matches = bf.match(des1, des2) 85 | 86 | # Sort them in the order of their distance. 87 | matches_sorted = sorted(matches, key=lambda x: x.distance) 88 | 89 | if DEBUG: 90 | print("Max distance:{}, min distance:{}".format(matches_sorted[-1].distance, matches_sorted[0].distance)) 91 | # 当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限. 92 | prune_dis = max(matches_sorted[0].distance, 30.0) 93 | 94 | matches = list(filter(lambda x: x.distance <= prune_dis, matches)) 95 | 96 | if DEBUG: 97 | print("We found {} pairs of points in total".format(len(matches))) 98 | 99 | return matches 100 | 101 | 102 | def kps2pts(kps1, kps2, matches): 103 | pts1 = [] 104 | pts2 = [] 105 | dis = [] 106 | for m in matches: 107 | pts2.append(kps2[m.trainIdx].pt) 108 | pts1.append(kps1[m.queryIdx].pt) 109 | dis.append(m.distance) 110 | return np.float32(pts1), np.float32(pts2), np.float32(dis) 111 | 112 | 113 | def print_kps(kps1, kps2, matches, start, end): 114 | assert len(kps1) == len(kps2) 115 | assert start>=0 and end > start and len(matches) >= end 116 | 117 | print("All matches:{}, Key points {}-{}:".format(len(matches), start, end)) 118 | 119 | pts1, pts2, dis = kps2pts(kps1, kps2, matches) 120 | print_pts(pts1, pts2, dis, start, end) 121 | 122 | 123 | 124 | def print_pts(pts1, pts2, dis, start, end): 125 | assert len(pts1) == len(pts2) 126 | for i in range(start, end): 127 | print("i:{},p1:{},p2:{},d:{}".format(i, pts1[i], pts2[i], dis[i])) 128 | 129 | def find_H_and_refineMatches(kps1, kps2, matches): 130 | 131 | pts1, pts2, _ = kps2pts(kps1, kps2, matches) 132 | H, mask_H = cv2.findHomography(pts1, pts2, method=cv2.RANSAC, ransacReprojThreshold=3) 133 | 134 | # We select only inlier points 135 | pts1_H = pts1[mask_H.ravel() == 1] 136 | pts2_H = pts2[mask_H.ravel() == 1] 137 | #new_matches = matches[mask_H.ravel() == 1] 138 | matches_H = [] 139 | matches_H_bad = [] 140 | for i in range(len(matches)): 141 | if mask_H[i] == 1: 142 | matches_H.append(matches[i]) 143 | else: 144 | matches_H_bad.append(matches[i]) 145 | 146 | print("In find_H_and_refineMatches, matches:{} -> {}".format(len(matches), len(matches_H))) 147 | print("H:\n{}".format(H)) 148 | 149 | return H, matches_H, matches_H_bad, pts1_H, pts2_H 150 | 151 | 152 | def find_F_and_refineMatches(kps1, kps2, matches): 153 | 154 | pts1, pts2, _ = kps2pts(kps1, kps2, matches) 155 | #F, mask_F = cv2.findFundamentalMat(pts1, pts2, cv2.FM_RANSAC, 0.1, 0.99) # F, mask_F = cv2.findFundamentalMat(pts1, pts2, cv2.FM_LMEDS) 156 | F, mask_F = cv2.findFundamentalMat(pts1, pts2, cv2.FM_RANSAC) 157 | 158 | # We select only inlier points 159 | pts1_F = pts1[mask_F.ravel() == 1] 160 | pts2_F = pts2[mask_F.ravel() == 1] 161 | #new_matches = matches[mask_H.ravel() == 1] 162 | matches_F = [] 163 | matches_F_bad = [] 164 | for i in range(len(matches)): 165 | if mask_F[i] == 1: 166 | matches_F.append(matches[i]) 167 | else: 168 | #print("bad in F refine:{}".format(i)) 169 | matches_F_bad.append(matches[i]) 170 | 171 | print("In find_F_and_refineMatches, matches:{} -> {}".format(len(matches), len(matches_F))) 172 | print("F:\n{}".format(F)) 173 | 174 | random_check_F(pts1_F, pts2_F, F) 175 | 176 | return F, matches_F, matches_F_bad, pts1_F, pts2_F 177 | 178 | 179 | 180 | def random_check_F(pts1, pts2, F): 181 | # [ref](https://github.com/kevin-george/cv_tools/blob/master/calculate_fmatrix.py#L195) 182 | # Pick a random match, homogenize it and check if Fundamental 183 | # matrix calculated is good enough x' * F * x = 0 184 | import random 185 | import sys 186 | index = random.randint(0, len(pts1)-1) 187 | pt1 = np.array([[pts1[index][0], pts1[index][1], 1]]) 188 | pt2 = np.array([[pts2[index][0], pts2[index][1], 1]]) 189 | error = np.dot(pt1, np.dot(F, pt2.T)) 190 | if error >= 1: 191 | print("Fundamental matrix calculation Failed! " + str(error)) 192 | # sys.exit() 193 | input("Press Enter to continue ...") 194 | 195 | 196 | def DEBUG_Rt(R, t, name): 197 | print("DEBUG RT:{}".format(name)) 198 | 199 | r, _ = cv2.Rodrigues(R) 200 | print("R:\n{}".format(R)) 201 | print("r:{}".format(r.T)) 202 | rotate_angle(R) 203 | print("t:{}".format(t.T)) 204 | 205 | def DEBUG_Rt_simple(R, t, name): 206 | print("DEBUG RT:{}".format(name)) 207 | rotate_angle(R) 208 | print("t:{}".format(t.T)) 209 | 210 | 211 | def scaled_E(E): 212 | return E/E[-1,-1] 213 | 214 | 215 | def DEBUG_E(E): 216 | print("E:\n{}".format(E)) 217 | print("Scaled E:\n{}".format(scaled_E(E))) 218 | 219 | 220 | def recoverPoseFromE_cv3_with_kps(E, kps1, kps2, matches, K): 221 | pts1, pts2, _ = kps2pts(kps1, kps2, matches) 222 | 223 | _, R, t, mask_rp = cv2.recoverPose(E, pts1, pts2, 224 | K) # this can have the determiate results, so we choose it 225 | 226 | 227 | # We select only inlier points 228 | pts1_rp = pts1[mask_rp.ravel() != 0] 229 | pts2_rp = pts2[mask_rp.ravel() != 0] 230 | matches_rp = [] 231 | matches_rp_bad = [] 232 | for i in range(len(matches)): 233 | if mask_rp[i] != 0: 234 | matches_rp.append(matches[i]) 235 | else: 236 | #print("bad in F refine:{}".format(i)) 237 | matches_rp_bad.append(matches[i]) 238 | 239 | 240 | 241 | #print("In recoverPoseFromE_cv3, points:{} -> inliner:{}".format(len(pts1), len(pts1_rp))) 242 | 243 | return R, t, matches_rp, matches_rp_bad, pts1_rp, pts2_rp 244 | 245 | 246 | def recoverPoseFromE_cv3(E, pts1, pts2, K): 247 | 248 | assert len(pts1) == len(pts2) 249 | 250 | """ 251 | RE1, RE2, tE = cv2.decomposeEssentialMat( 252 | E) # there will be two answers for R, and four answers in total: R1 t, R1 -t, R2 t, R2 -t 253 | print("R t from decomposeEssentialMat, note that there are four potential answers") 254 | DEBUG_Rt(RE1, tE, "RE1, tE") 255 | DEBUG_Rt(RE2, tE, "RE2, tE") 256 | """ 257 | assert imutils.is_cv3() == True 258 | 259 | _, R, t, mask_rc = cv2.recoverPose(E, pts1, pts2, 260 | K) # this can have the determiate results, so we choose it 261 | 262 | # for m in mask_rc: 263 | # print(m.ravel()) 264 | 265 | # We select only inlier points 266 | pts1_rc = pts1[mask_rc.ravel() != 0] 267 | #pts2_rc = pts2[mask_rc.ravel() != 0] 268 | print("In recoverPoseFromE_cv3, points:{} -> inliner:{}".format(len(pts1), len(pts1_rc))) 269 | 270 | return R, t 271 | 272 | def find_E_from_F(F, K1, K2): 273 | """ 274 | Normally, we only need to set K1 and K2 to the same one 275 | """ 276 | return K2.T.dot(F).dot(K1) 277 | 278 | 279 | def find_E_cv3(kp1, kp2, matches, K): 280 | 281 | pts1, pts2, _ = kps2pts(kp1, kp2, matches) 282 | 283 | """ findEssentialMat(points1, points2, cameraMatrix[, method[, prob[, threshold[, mask]]]]) -> retval, mask """ 284 | #E, mask_E = cv2.findEssentialMat(pts1, pts2, cameraMatrix=K, method=cv2.RANSAC, prob=0.999, threshold=0.2) 285 | E, mask_E = cv2.findEssentialMat(pts1, pts2, cameraMatrix=K) 286 | 287 | # We select only inlier points 288 | pts1_E = pts1[mask_E.ravel() == 1] 289 | pts2_E = pts2[mask_E.ravel() == 1] 290 | 291 | #new_matches = matches[mask_H.ravel() == 1] 292 | matches_E = [] 293 | for i in range(len(matches)): 294 | if mask_E[i] == 1: 295 | matches_E.append(matches[i]) 296 | 297 | 298 | # print("In find_E_cv3, matches:{} -> {}".format(len(matches), len(matches_E))) 299 | 300 | return E, matches_E, pts1_E, pts2_E 301 | 302 | 303 | def find_F_E_R_t(kp1, kp2, matches, K): 304 | """ 305 | [ref](https://opencv-python-tutroals.readthedocs.io/en/latest/py_tutorials/py_calib3d/py_epipolar_geometry/py_epipolar_geometry.html) 306 | :param kp1: 307 | :param kp2: 308 | :param matches: 309 | :return: 310 | """ 311 | 312 | E_cv3, _,pts1_E_cv3, pts2_E_cv3 = find_E_cv3(kp1, kp2, matches, K) 313 | DEBUG_E(E_cv3) 314 | print("Try to use E from findEssentialMat ot recover pose") 315 | R_cv3, t_cv3 = recoverPoseFromE_cv3(E_cv3, pts1_E_cv3, pts2_E_cv3, K) 316 | DEBUG_Rt(R_cv3, t_cv3, "E from findEssential") 317 | 318 | F, _, _, pts1_F, pts2_F = find_F_and_refineMatches(kp1, kp2, matches) 319 | 320 | return F, E_cv3, R_cv3, t_cv3, pts1_F, pts2_F, pts1_E_cv3, pts2_E_cv3 321 | 322 | 323 | def drawlines(img1,img2,lines,pts1,pts2): 324 | ''' img1 - image on which we draw the epilines for the points in img2 325 | lines - corresponding epilines ''' 326 | print("img1.shape:{}".format(img1.shape)) 327 | r,c = img1.shape[:2] # it can be gray or RGB 328 | #img1 = cv2.cvtColor(img1,cv2.COLOR_GRAY2BGR) 329 | #img2 = cv2.cvtColor(img2,cv2.COLOR_GRAY2BGR) 330 | for r,pt1,pt2 in zip(lines,pts1,pts2): 331 | color = tuple(np.random.randint(0,255,3).tolist()) 332 | x0,y0 = map(int, [0, -r[2]/r[1] ]) 333 | x1,y1 = map(int, [c, -(r[2]+r[0]*c)/r[1] ]) 334 | img1 = cv2.line(img1, (x0,y0), (x1,y1), color,1) 335 | img1 = cv2.circle(img1,tuple(pt1),5,color,-1) 336 | img2 = cv2.circle(img2,tuple(pt2),5,color,-1) 337 | return img1,img2 338 | 339 | def draw_epilines_from_F(img1, img2, pts1, pts2, F): 340 | # Find epilines corresponding to points in right image (second image) and 341 | # drawing its lines on left image 342 | 343 | # make them to int so that we can draw on them 344 | pts1 = np.int32(pts1) 345 | pts2 = np.int32(pts2) 346 | 347 | lines1 = cv2.computeCorrespondEpilines(pts2.reshape(-1,1,2), 2,F) 348 | lines1 = lines1.reshape(-1,3) 349 | img5,img6 = drawlines(img1,img2,lines1,pts1,pts2) 350 | 351 | # Find epilines corresponding to points in left image (first image) and 352 | # drawing its lines on right image 353 | lines2 = cv2.computeCorrespondEpilines(pts1.reshape(-1,1,2), 1,F) 354 | lines2 = lines2.reshape(-1,3) 355 | img3,img4 = drawlines(img2,img1,lines2,pts2,pts1) 356 | 357 | plt.subplot(121),plt.imshow(img5) 358 | plt.subplot(122),plt.imshow(img3) 359 | plt.show() 360 | 361 | if __name__ == '__main__': 362 | 363 | base_dir = "H:/projects/SLAM/python_code/dataset/our/trajs2/" 364 | base_dir2 = "H:/projects/SLAM/python_code/dataset/our/trajs_bright/" 365 | base_dir3 = "H:/projects/SLAM/python_code/dataset/our/trajs_r/" 366 | 367 | im1_file = base_dir + '1.jpg' 368 | im2_file = base_dir + '4.jpg' 369 | 370 | # DEBUG = False 371 | 372 | if DEBUG: 373 | print("HHHHH") 374 | 375 | 376 | im1 = cv2.imread(im1_file, 0) 377 | im2 = cv2.imread(im2_file, 0) 378 | 379 | # im1 = cv2.imread(im1_file) 380 | # im2 = cv2.imread(im2_file) 381 | 382 | kp1, des1 = find_keypoints_and_description(im1) 383 | kp2, des2 = find_keypoints_and_description(im2) 384 | 385 | matches = find_matches_from_descriptors(des1, des2) 386 | 387 | DEBUG_Matches(im1, kp1, im2, kp2, matches, "Matches first") 388 | 389 | 390 | K = np.array([[8607.8639, 0, 2880.72115], [0, 8605.4303, 1913.87935], [0, 0, 1]]) # Canon5DMarkIII-EF50mm 391 | 392 | F, E, R, t, pts1_F, pts2_F, pts1_E, pts2_E = find_F_E_R_t(kp1, kp2, matches, K) 393 | 394 | #draw_epilines_from_F(im1, im2, pts1_F, pts2_F, F) 395 | -------------------------------------------------------------------------------- /pose_estimation_3d2d.py: -------------------------------------------------------------------------------- 1 | from pose_estimation_2d2d import * 2 | 3 | def pixel2cam(pt, K): 4 | """ 5 | // 像素坐标转相机归一化坐标 6 | [1、像素坐标与像平面坐标系之间的关系 ](http://blog.csdn.net/waeceo/article/details/50580607) 7 | :param pt: point position in pixel coordinate 8 | :param K: 9 | :return: point position in camera coordinate 10 | """ 11 | """ 12 | return Point2f 13 | ( 14 | (p.x - K.at < double > (0, 2)) / K.at < double > (0, 0), 15 | (p.y - K.at < double > (1, 2)) / K.at < double > (1, 1) 16 | ); 17 | """ 18 | return np.array([ (pt[0]-K[0,2])/K[0,0], (pt[1]-K[1,2])/K[1,1] ]) 19 | 20 | 21 | 22 | def triangulation(R, t, pts1, pts2, K): 23 | """ 24 | https://pythonpath.wordpress.com/2012/08/29/cv2-triangulatepoints/ 25 | :param R: 26 | :param t: 27 | :param pts1: 28 | :param pts2: 29 | :param K: 30 | :return: pts3xN 31 | """ 32 | 33 | projM1 = np.eye(4) 34 | 35 | projM2 = np.eye(4) 36 | 37 | print("R.type:{}, R.shape:{}".format(type(R), R.shape)) 38 | print("t.type:{}, t.shape:{}".format(type(t), t.shape)) 39 | projM2[:3, :3] = R 40 | projM2[:3, -1] = t.T 41 | 42 | assert len(pts1) == len(pts2) 43 | pts1_cam_Nx2 = np.array([pixel2cam(x, K) for x in pts1]) 44 | pts2_cam_Nx2 = np.array([pixel2cam(x, K) for x in pts2]) 45 | 46 | """ triangulatePoints(projMatr1, projMatr2, projPoints1, projPoints2[, points4D]) -> points4D """ 47 | pts4d = cv2.triangulatePoints(projM1[:3], projM2[:3], pts1_cam_Nx2.T, pts2_cam_Nx2.T) 48 | 49 | 50 | # convert from homogeneous coordinates to 3D 51 | pts4D = pts4d.T 52 | pts3D = pts4D[:, :3] / np.repeat(pts4D[:, 3], 3).reshape(-1, 3) 53 | 54 | # plot with matplotlib 55 | # Ys = pts3D[:, 0] 56 | # Zs = pts3D[:, 1] 57 | # Xs = pts3D[:, 2] 58 | Xs = pts3D[:, 0] 59 | Ys = pts3D[:, 1] 60 | Zs = pts3D[:, 2] 61 | 62 | 63 | fig = plt.figure() 64 | ax = fig.add_subplot(111, projection='3d') 65 | ax.scatter(Xs, Ys, Zs, c='r', marker='o') 66 | ax.set_xlabel('X') 67 | ax.set_ylabel('Y') 68 | ax.set_zlabel('Z') 69 | plt.title('3D point cloud: Use pan axes button below to inspect') 70 | plt.show() 71 | 72 | # 转换成非齐次坐标 73 | pts1_cam_3xN = pts4d[:3] / pts4d[-1] 74 | 75 | if DEBUG: 76 | 77 | # 验证三角化点与特征点的重投影关系 78 | pts1_cam_3xN_norm = pts1_cam_3xN / pts1_cam_3xN[-1] # normalization 79 | 80 | print("Points in first camera frame:\n{}".format(pts1_cam_Nx2)) 81 | print("Point projected from 3D:\n{}".format(pts1_cam_3xN_norm.T)) 82 | 83 | # -second 84 | #pts2_trans_3xN = np.array([R.dot(x)+t for x in pts3xN.T]) 85 | pts2_trans_3xN = R.dot(pts1_cam_3xN) + t 86 | pts2_trans_3xN_norm = pts2_trans_3xN/pts2_trans_3xN[-1] 87 | 88 | print("Points in second camera frame:\n{}".format(pts2_cam_Nx2)) 89 | print("Point reprojected from second frame:\n{}".format(pts2_trans_3xN_norm.T)) 90 | 91 | pts1_cam_Nx3 = pts1_cam_3xN.T 92 | return pts1_cam_Nx3 93 | 94 | 95 | def PNPSolver_img2_points_and_3DPoints(pts1_cam_Nx3, pts2_pixel_Nx2, K): 96 | 97 | assert len(pts1_cam_Nx3) == len(pts2_pixel_Nx2) 98 | 99 | print("In PNPSolver points num:{}".format(len(pts1_cam_Nx3))) 100 | 101 | """solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs[, rvec[, tvec[, useExtrinsicGuess[, flags]]]]) -> retval, rvec, tvec""" 102 | _, r, t = cv2.solvePnP(pts1_cam_Nx3, pts2_pixel_Nx2, K, None, useExtrinsicGuess=False) 103 | 104 | print("R t from solvePnP") 105 | R, _ = cv2.Rodrigues(r) 106 | print("R:\n{}".format(R)) 107 | print("r:\n{}".format(r)) 108 | rotate_angle(R) 109 | print("t:\n{}".format(t)) 110 | 111 | return R, t 112 | 113 | 114 | if __name__ == '__main__': 115 | base_dir = "H:/projects/SLAM/python_code/dataset/our/trajs2/" 116 | base_dir2 = "H:/projects/SLAM/python_code/dataset/our/trajs_bright/" 117 | base_dir3 = "H:/projects/SLAM/python_code/dataset/our/trajs_r/" 118 | 119 | im1_file = base_dir + '1.jpg' 120 | im2_file = base_dir + '4.jpg' 121 | 122 | # DEBUG = False 123 | 124 | if DEBUG: 125 | print("HHHHH") 126 | 127 | 128 | im1 = cv2.imread(im1_file, 0) 129 | im2 = cv2.imread(im2_file, 0) 130 | 131 | # im1 = cv2.imread(im1_file) 132 | # im2 = cv2.imread(im2_file) 133 | 134 | kp1, des1 = find_keypoints_and_description(im1) 135 | kp2, des2 = find_keypoints_and_description(im2) 136 | 137 | matches = find_matches_from_descriptors(des1, des2) 138 | 139 | DEBUG_Matches(im1, kp1, im2, kp2, matches, "Matches first") 140 | 141 | 142 | K = np.array([[8607.8639, 0, 2880.72115], [0, 8605.4303, 1913.87935], [0, 0, 1]]) # Canon5DMarkIII-EF50mm 143 | 144 | F, E, R, t, pts1_F, pts2_F, pts1_E, pts2_E = find_F_E_R_t(kp1, kp2, matches, K) 145 | 146 | draw_epilines_from_F(im1, im2, pts1_F, pts2_F, F) 147 | 148 | t = t/(t[0]/-40.0) #1-4 149 | #t = t / (t[-1] / -15.0) #1-2 150 | #t = t/(t[0]/40.0) #1-7 # it will be totally wrong if we are using 7a 151 | print("Scaled t:{}".format(t)) 152 | pts1_cam_Nx3 = triangulation(R, t, pts1_E, pts2_E, K) 153 | for i in range(10): 154 | print(pts1_cam_Nx3[i]) 155 | 156 | import pylab 157 | pylab.hist(pts1_cam_Nx3[:,-1], bins=10) 158 | pylab.show() 159 | 160 | """ 161 | # just do it, without all funcy and forward and backward calculation 162 | R = np.eye(3) 163 | 164 | #t = np.array([0.0, 0.0, -15.0]) # 1-2 165 | t = np.array([-40.0, 0, 0]) # 1-4 166 | # t = np.array([40.0, 0, 0]) # 1-7a # we can still use 7a without any problem 167 | 168 | pts1 = [] 169 | pts2 = [] 170 | for m in matches: 171 | pts2.append(kp2[m.trainIdx].pt) 172 | pts1.append(kp1[m.queryIdx].pt) 173 | pts1_cam_Nx3_another = triangulation(R, t, pts1, pts2, K) 174 | #pts1_cam_Nx3_another = triangulation(R, t, pts1_E, pts2_E, K) # it is a liitle worse than all points, why? 175 | for i in range(10): 176 | print(pts1_cam_Nx3_another[i]) 177 | 178 | #pylab.hist(data, normed=1) 179 | pylab.hist(pts1_cam_Nx3_another[:,-1], bins=10) 180 | pylab.show() 181 | """ 182 | 183 | 184 | """ 185 | # prune out some points and re-calc the R, t from the 3d points 186 | pts1_cam_Nx3_half = pts1_cam_Nx3[:len(pts1_cam_Nx3)//2] 187 | pts2_E_half = pts2_E[:len(pts2_E)//2] 188 | 189 | PNPSolver_img2_points_and_3DPoints(pts1_cam_Nx3, pts2_E, K) 190 | PNPSolver_img2_points_and_3DPoints(pts1_cam_Nx3_half, pts2_E_half, K) 191 | """ 192 | -------------------------------------------------------------------------------- /ransac_test.py: -------------------------------------------------------------------------------- 1 | """ 2 | When we have many many points pairs, the findEssentialMat and recoverPose will give us only one answer anyway, 3 | which will definitely loose the benefit of numbers and we can do some `RANSAC` or `sliding-window` approaches. 4 | 5 | Let's say, we have 1000 pairs of matched points, there can be ways to do it: 6 | 7 | =========== 8 | First: 9 | 1. Calculate every 100 points and get 10 answers 10 | 2. Remove the answers with less confidence, aka, the less percentage of points passing the cheirality check (in recoverPose) 11 | 3. Get the main centroid of the remaining answers 12 | =========== 13 | 14 | =========== 15 | Second: 16 | 1. Calculate every 100 points and get 10 answers 17 | 2. For each answer, calc the reprojection error (or the epipolar equation) on all points and filter out some answers 18 | 3. Get the main centroid of the remaining answers 19 | =========== 20 | 21 | """ 22 | from pose_estimation_2d2d import * 23 | 24 | 25 | DEBUG = True 26 | 27 | 28 | def plot_the_Rt_pairs(): 29 | pass 30 | 31 | 32 | def split_the_matches(matches, step): 33 | for i in range(len(matches)//step + 1): 34 | yield matches[i*step:(i+1)*step] 35 | 36 | def remove_less_confidence(kps1, kps2, matches, K, splitnum, threshold): 37 | 38 | Rs = [] 39 | ts = [] 40 | confidences = [] 41 | 42 | splited_matches = split_the_matches(matches, splitnum) 43 | 44 | for m in splited_matches: 45 | if len(m) < 5: 46 | print("Less than 5 points, no need") 47 | continue 48 | 49 | #print(len(m)) 50 | E_cv3, matches_E, _, _ = find_E_cv3(kps1, kps2, m, K) 51 | R, t, matches_rp, matches_rp_bad, _, _ = recoverPoseFromE_cv3_with_kps(E_cv3, kps1, kps2, m, K) 52 | 53 | conf = len(matches_rp)/len(m) 54 | if conf >= threshold: 55 | Rs.append(R) 56 | ts.append(t) 57 | confidences.append(conf) 58 | 59 | return Rs, ts, confidences 60 | 61 | def mean_Rt(Rs, ts): 62 | rs = [] 63 | for R in Rs: 64 | rs.append(rotate_angle_no_output(R)) 65 | 66 | rs = np.array(rs) 67 | ts = np.array(ts) 68 | 69 | assert rs[0].shape == (3, 1) and ts[0].shape == (3, 1) 70 | 71 | #print("Rs:{}, ts:{}".format(rs, ts)) 72 | #print("rs.shape:{}, ts.shape:{}".format(rs.shape, ts.shape)) 73 | return rs.mean(axis=0), ts.mean(axis=0) 74 | 75 | 76 | 77 | def test_remove_less_confidence(im1_file, im2_file): 78 | 79 | 80 | K = np.array([[8607.8639, 0, 2880.72115], [0, 8605.4303, 1913.87935], [0, 0, 1]]) # Canon5DMarkIII-EF50mm 81 | 82 | #im1_file = '1.jpg' 83 | #im2_file = '3.jpg' 84 | 85 | #print(im1_file, im2_file) 86 | 87 | im1 = cv2.imread(im1_file, 0) 88 | im2 = cv2.imread(im2_file, 0) 89 | 90 | kps1, des1 = find_keypoints_and_description(im1) 91 | kps2, des2 = find_keypoints_and_description(im2) 92 | 93 | matches = find_matches_from_descriptors(des1, des2) 94 | 95 | 96 | splitnum = 42 97 | 98 | Rs, ts, confidences = remove_less_confidence(kps1, kps2, matches, K, splitnum, 0.9) 99 | #test_remove_less_confidence_only_time(kps1, kps2, matches, K, splitnum, 0.6) 100 | 101 | if DEBUG: 102 | print("The answers num:{} of total:{}".format(len(Rs), len(matches)//splitnum)) 103 | for R, t, con in zip(Rs, ts, confidences): 104 | print("=============") 105 | print("Confidence:{}".format(con)) 106 | DEBUG_Rt_simple(R, t, str(con)) 107 | print("=============") 108 | 109 | 110 | rs_mean, ts_mean = mean_Rt(Rs, ts) 111 | print("rs_mean:{}\nts_mean:{}".format(rs_mean.T, ts_mean.T)) 112 | 113 | 114 | def test_remove_less_confidence_only_time(kps1, kps2, matches, K, splitnum, thres): 115 | start_lsd = cv2.getTickCount() 116 | freq = cv2.getTickFrequency() 117 | 118 | test_num = 10 119 | for i in range(test_num): 120 | Rs, ts, confidences = remove_less_confidence(kps1, kps2, matches, K, splitnum, thres) 121 | 122 | duration_ms_lsd = (cv2.getTickCount() - start_lsd) * 1000 / freq / test_num 123 | print("Elapsed time for remove less confidence: {} ms".format(duration_ms_lsd )) 124 | 125 | def test_remove_less_confidence_time(im1_file, im2_file): 126 | start_lsd = cv2.getTickCount() 127 | freq = cv2.getTickFrequency() 128 | 129 | test_num = 10 130 | for i in range(test_num): 131 | test_remove_less_confidence(im1_file, im2_file) 132 | 133 | duration_ms_lsd = (cv2.getTickCount() - start_lsd) * 1000 / freq / test_num 134 | print("Elapsed time for remove less confidence: {} ms".format(duration_ms_lsd )) 135 | 136 | if __name__ == "__main__": 137 | import argparse 138 | # construct the argument parse and parse the arguments 139 | ap = argparse.ArgumentParser() 140 | ap.add_argument("-l", "--image1", required=True, 141 | help="path to input image1") 142 | ap.add_argument("-r", "--image2", required=True, 143 | help="path to input image2") 144 | args = vars(ap.parse_args()) 145 | 146 | base_dir = "H:/projects/SLAM/python_code/dataset/our/trajs2/" 147 | 148 | im1_file = base_dir + args["image1"] 149 | im2_file = base_dir + args["image2"] 150 | 151 | #print("{}\n{}".format(im1_file, im2_file)) 152 | print("{}\n{}".format(args["image1"], args["image2"])) 153 | 154 | # test_remove_less_confidence_time() 155 | test_remove_less_confidence(im1_file, im2_file) 156 | 157 | 158 | -------------------------------------------------------------------------------- /simStereoCamera.py: -------------------------------------------------------------------------------- 1 | """ 2 | The results are not so ideal. Potential errors are here!!! 3 | 4 | We are going to simulate a stereo system: 5 | The first image can be treat as the left camera, the second (move x=40mm) can be treated as the second image 6 | 7 | [ref:使用OpenCV/python进行双目测距](http://www.cnblogs.com/zhiyishou/p/5767592.html) 8 | [ref:机器视觉学习笔记（8）——基于OpenCV的Bouguet立体校正](http://blog.csdn.net/xuelabizp/article/details/50476639) 9 | """ 10 | 11 | from pose_estimation_2d2d import * 12 | 13 | 14 | 15 | def stereoRectifyCalc(K, size, R, t): 16 | """ 17 | :param K: 18 | :param size: 19 | :param R: 20 | :param t: 21 | :return: 22 | """ 23 | 24 | """ stereoRectify(cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, imageSize, R, T[, R1[, R2[, P1[, P2[, Q[, flags[, alpha[, newImageSize]]]]]]]]) -> R1, R2, P1, P2, Q, validPixROI1, validPixROI2 """ 25 | 26 | dist = np.zeros(5) # since we are using the same camera, this dose not matter at all!! 27 | #R1, R2, P1, P2, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(K, dist, K, dist, size, R, t) 28 | R1, R2, P1, P2, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(K, dist, K, dist, size, R, t, flags=cv2.CALIB_ZERO_DISPARITY, alpha=0) 29 | 30 | 31 | print("R1:{}\n,R2:{}\n,P1:{}\n,P2:{}\n,Q:{}".format(R1, R2, P1, P2, Q)) 32 | left_map1, left_map2 = cv2.initUndistortRectifyMap(K, dist, R1, P1, size, cv2.CV_16SC2) 33 | right_map1, right_map2 = cv2.initUndistortRectifyMap(K, dist, R2, P2, size, cv2.CV_16SC2) 34 | 35 | return left_map1, left_map2, right_map1, right_map2, Q 36 | 37 | 38 | 39 | def testDis2(left_map1, left_map2, right_map1, right_map2, Q, frame1 , frame2): 40 | import utils 41 | import imutils 42 | 43 | """ 44 | [4.3检验效果](http://blog.csdn.net/xuelabizp/article/details/50476639) 45 | 46 | Mat img1 = imread("left01.jpg"), img1r; 47 | Mat img2 = imread("right01.jpg"), img2r; 48 | 49 | Mat img(imageSize.height, imageSize.width * 2, CV_8UC3);//高度一样，宽度双倍 50 | imshow("rectified", img); 51 | remap(img1, img1r, rmap[0][0], rmap[0][1], CV_INTER_AREA);//左校正 52 | remap(img2, img2r, rmap[1][0], rmap[1][1], CV_INTER_AREA);//右校正 53 | 54 | Mat imgPart1 = img( Rect(0, 0, imageSize.width, imageSize.height) );//浅拷贝 55 | Mat imgPart2 = img( Rect(imageSize.width, 0, imageSize.width, imageSize.height) );//浅拷贝 56 | resize(img1r, imgPart1, imgPart1.size(), 0, 0, CV_INTER_AREA); 57 | resize(img2r, imgPart2, imgPart2.size(), 0, 0, CV_INTER_AREA); 58 | 59 | //画横线 60 | for( int i = 0; i < img.rows; i += 32 ) 61 | line(img, Point(0, i), Point(img.cols, i), Scalar(0, 255, 0), 1, 8); 62 | 63 | //显示行对准的图形 64 | Mat smallImg;//由于我的分辨率1:1显示太大，所以缩小显示 65 | resize(img, smallImg, Size(), 0.5, 0.5, CV_INTER_AREA); 66 | imshow("rectified", smallImg); 67 | :return: 68 | """ 69 | 70 | # 根据更正map对图片进行重构 71 | img1_rectified = cv2.remap(frame1, left_map1, left_map2, cv2.INTER_LINEAR) 72 | img2_rectified = cv2.remap(frame2, right_map1, right_map2, cv2.INTER_LINEAR) 73 | 74 | # 将图片置为灰度图，为StereoBM作准备 75 | imgL = cv2.cvtColor(img1_rectified, cv2.COLOR_BGR2GRAY) 76 | imgR = cv2.cvtColor(img2_rectified, cv2.COLOR_BGR2GRAY) 77 | 78 | 79 | height, width = img1_rectified.shape[:2] 80 | cv2.imshow("imgL", imutils.resize(imgL, width//10)) 81 | cv2.waitKey(0) 82 | cv2.destroyAllWindows() 83 | 84 | print("img1_rectified.shape:{}".format(img1_rectified.shape)) 85 | print("imgL.shape:{}".format(imgL.shape)) 86 | 87 | vis = np.concatenate((img1_rectified, img2_rectified), axis=1) 88 | 89 | # draw horizontal lines every 25 px accross the side by side image 90 | for i in range(20, vis.shape[0], 25): 91 | cv2.line(vis, (0, i), (vis.shape[1], i), (255, 0, 0)) 92 | 93 | plt.imshow(utils.cv2plt(vis)), plt.show() 94 | 95 | 96 | def testDis(left_map1, left_map2, right_map1, right_map2, Q, frame1 , frame2): 97 | import numpy as np 98 | import cv2 99 | import imutils 100 | import pylab 101 | 102 | height, width = frame1.shape[:2] 103 | RESIZE = 10 104 | 105 | cv2.namedWindow("left") 106 | cv2.namedWindow("right") 107 | cv2.namedWindow("depth") 108 | cv2.moveWindow("left", 0, 0) 109 | cv2.moveWindow("right", width//RESIZE, 0) 110 | cv2.createTrackbar("num", "depth", 0, 10, lambda x: None) 111 | cv2.createTrackbar("blockSize", "depth", 5, 255, lambda x: None) 112 | # camera1 = cv2.VideoCapture(0) 113 | # camera2 = cv2.VideoCapture(1) 114 | 115 | # 添加点击事件，打印当前点的距离 116 | def callbackFunc(e, x, y, f, p): 117 | if e == cv2.EVENT_LBUTTONDOWN: 118 | print(threeD[y][x]) # the coordinate seems a little wrong 119 | 120 | cv2.setMouseCallback("depth", callbackFunc, None) 121 | 122 | while True: 123 | # ret1, frame1 = camera1.read() 124 | # ret2, frame2 = camera2.read() 125 | # 126 | # if not ret1 or not ret2: 127 | # break 128 | 129 | 130 | # 根据更正map对图片进行重构 131 | img1_rectified = cv2.remap(frame1, left_map1, left_map2, cv2.INTER_LINEAR) 132 | img2_rectified = cv2.remap(frame2, right_map1, right_map2, cv2.INTER_LINEAR) 133 | 134 | # 将图片置为灰度图，为StereoBM作准备 135 | imgL = cv2.cvtColor(img1_rectified, cv2.COLOR_BGR2GRAY) 136 | imgR = cv2.cvtColor(img2_rectified, cv2.COLOR_BGR2GRAY) 137 | 138 | # 两个trackbar用来调节不同的参数查看效果 139 | num = cv2.getTrackbarPos("num", "depth") 140 | blockSize = cv2.getTrackbarPos("blockSize", "depth") 141 | if blockSize % 2 == 0: 142 | blockSize += 1 143 | if blockSize < 5: 144 | blockSize = 5 145 | 146 | # 根据Block Maching方法生成差异图（opencv里也提供了SGBM/Semi-Global Block Matching算法，有兴趣可以试试） 147 | stereo = cv2.StereoBM_create(numDisparities=16 * num, blockSize=blockSize) 148 | disparity = stereo.compute(imgL, imgR) 149 | 150 | disp = cv2.normalize(disparity, disparity, alpha=0, beta=255, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_8U) 151 | # 将图片扩展至3d空间中，其z方向的值则为当前的距离 152 | threeD = cv2.reprojectImageTo3D(disparity.astype(np.float32) / 16., Q) 153 | 154 | # for i in range(10): 155 | # print(threeD[i]) 156 | 157 | # cv2.imshow("left", img1_rectified) 158 | # cv2.imshow("right", img2_rectified) 159 | # cv2.imshow("depth", disp) 160 | 161 | cv2.imshow("left", imutils.resize(img1_rectified, width//RESIZE)) 162 | cv2.imshow("right", imutils.resize(img2_rectified, width//RESIZE)) 163 | cv2.imshow("depth", imutils.resize(disp, width//RESIZE)) 164 | 165 | key = cv2.waitKey(1) 166 | if key == ord("q"): 167 | break 168 | elif key == ord("s"): 169 | print("Going to write to disk") 170 | cv2.imwrite("./snapshot/BM_left.jpg", imgL) 171 | cv2.imwrite("./snapshot/BM_right.jpg", imgR) 172 | cv2.imwrite("./snapshot/BM_depth.jpg", disp) 173 | print("Wirte done") 174 | elif key == ord("c"): 175 | print("show the hist, block operation") 176 | pylab.hist(threeD[:, -1], bins=100) 177 | pylab.show() 178 | 179 | # camera1.release() 180 | # camera2.release() 181 | cv2.destroyAllWindows() 182 | 183 | if __name__ == '__main__': 184 | 185 | base_dir = "H:/projects/SLAM/python_code/dataset/our/trajs2/" 186 | 187 | 188 | im1_file = base_dir + '1.jpg' 189 | im2_file = base_dir + '4.jpg' 190 | 191 | K = np.array([[8607.8639, 0, 2880.72115], [0, 8605.4303, 1913.87935], [0, 0, 1]]) # Canon5DMarkIII-EF50mm 192 | 193 | #DEBUG = False 194 | 195 | if DEBUG: 196 | print("HHHHH") 197 | 198 | 199 | # im1 = cv2.imread(im1_file, 0) 200 | # im2 = cv2.imread(im2_file, 0) 201 | 202 | im1 = cv2.imread(im1_file) 203 | im2 = cv2.imread(im2_file) 204 | 205 | """ 206 | # find_feature_matches( im1, im2 ) 207 | kp1, des1 = find_keypoints_and_description(im1) 208 | kp2, des2 = find_keypoints_and_description(im2) 209 | 210 | matches = find_feature_matches_from_keypoints_and_descriptors(im1, kp1, des1, im2, kp2, des2) 211 | F, E, R, t, pts1_F, pts2_F, pts1_E, pts2_E = find_F_E_R_t(kp1, kp2, matches, K) 212 | """ 213 | 214 | """ 215 | R = np.array([[ 0.99995568, 0.00584152, 0.00738401], 216 | [-0.00577213, 0.99993931, -0.00938406], 217 | [-0.00743838, 0.00934102, 0.99992871]] 218 | ) 219 | 220 | t = np.array([[-0.99457732], [ 0.06874528], [ 0.07803867]]) 221 | t = t/(t[0]/-40.0) #1-4 222 | 223 | height, width = im1.shape[:2] 224 | size = (width, height) 225 | left_map1, left_map2, right_map1, right_map2, Q = stereoRectifyCalc(K, size, R, t) 226 | 227 | testDis(left_map1, left_map2, right_map1, right_map2, Q, im1, im2) 228 | #testDis2(left_map1, left_map2, right_map1, right_map2, Q, im1, im2) 229 | """ 230 | 231 | 232 | # just do it, without all funcy and forward and backward calculation 233 | R = np.eye(3) 234 | t = np.array([-40.0, 0, 0]) # 1-4 235 | height, width = im1.shape[:2] 236 | size = (width, height) 237 | left_map1, left_map2, right_map1, right_map2, Q = stereoRectifyCalc(K, size, R, t) 238 | #testDis(left_map1, left_map2, right_map1, right_map2, Q, im1, im2) 239 | testDis2(left_map1, left_map2, right_map1, right_map2, Q, im1, im2) 240 | 241 | -------------------------------------------------------------------------------- /testRefine.py: -------------------------------------------------------------------------------- 1 | """ 2 | It seems that, with Homograhpy refine is no big harm (:<). 3 | While with Fundamental is not so ideal. 4 | However, when draw out the BAD matched points, I have not see why they are bad -.- 5 | """ 6 | 7 | 8 | from pose_estimation_2d2d import * 9 | 10 | #@profile 11 | def testRefine(): 12 | base_dir = "H:/projects/SLAM/python_code/dataset/our/trajs2/" 13 | 14 | im1_file = base_dir + '1.jpg' 15 | im2_file = base_dir + '7b.jpg' 16 | 17 | im1 = cv2.imread(im1_file, 0) 18 | im2 = cv2.imread(im2_file, 0) 19 | 20 | # im1 = cv2.imread(im1_file) 21 | # im2 = cv2.imread(im2_file) 22 | 23 | kp1, des1 = find_keypoints_and_description(im1) 24 | kp2, des2 = find_keypoints_and_description(im2) 25 | 26 | matches = find_matches_from_descriptors(des1, des2) 27 | 28 | matches = sorted(matches, key=lambda x: x.distance) # we do the sorting 29 | # matches = matches[:len(matches)//2] # we crop it 30 | # np.random.shuffle(matches) # Shuffle it 31 | 32 | 33 | K = np.array([[8607.8639, 0, 2880.72115], [0, 8605.4303, 1913.87935], [0, 0, 1]]) # Canon5DMarkIII-EF50mm 34 | 35 | print_kps(kp1, kp2, matches, 0, 10) 36 | 37 | # DEBUG_Matches(im1, kp1, im2, kp2, matches, "Matches first") 38 | E_cv3, matches_E, pts1_E_cv3, pts2_E_cv3 = find_E_cv3(kp1, kp2, matches, K) 39 | 40 | # for i in range(10): 41 | # E_cv3, matches_E, pts1_E_cv3, pts2_E_cv3 = find_E_cv3(kp1, kp2, matches_E, K) # this should have no changes 42 | 43 | 44 | 45 | # DEBUG_E(E_cv3) 46 | print("Try to use E from findEssentialMat to recover pose") 47 | R_cv3, t_cv3 = recoverPoseFromE_cv3(E_cv3, pts1_E_cv3, pts2_E_cv3, K) 48 | DEBUG_Rt(R_cv3, t_cv3, "E from findEssential") 49 | 50 | """ 51 | F, matches_F, matches_F_bad, pts1_F, pts2_F = find_F_and_refineMatches(kp1, kp2, matches) 52 | 53 | print_kps(kp1, kp2, matches_F, 0, 10) 54 | print("==========") 55 | print_kps(kp1, kp2, matches_F_bad, 0, 10) 56 | 57 | 58 | #DEBUG_Matches(im1, kp1, im2, kp2, matches_F, "Matches first") 59 | #DEBUG_Matches(im1, kp1, im2, kp2, matches_F_bad, "Matches find_H_and_refineMatches BAD") 60 | 61 | E_F_refine, _, pts1_E_F_refine, pts2_E_F_refine = find_E_cv3(kp1, kp2, matches_F, K) 62 | DEBUG_E(E_F_refine) 63 | print("Try to use E from findEssentialMat then refined with F to recover pose") 64 | R_F_refine, t_F_refine = recoverPoseFromE_cv3(E_F_refine, pts1_E_F_refine, pts2_E_F_refine, K) 65 | DEBUG_Rt(R_F_refine, t_F_refine, "E from findEssentialMat then refined with F") 66 | """ 67 | 68 | """ 69 | H, matches_H, matches_H_bad, pts1_H, pts2_H = find_H_and_refineMatches(kp1, kp2, matches) 70 | print_kps(kp1, kp2, matches_H, 0, 10) 71 | print("==========") 72 | print_kps(kp1, kp2, matches_H_bad, 0, 10) 73 | #DEBUG_Matches(im1, kp1, im2, kp2, matches_H, "Matches find_H_and_refineMatches") 74 | #DEBUG_Matches(im1, kp1, im2, kp2, matches_H_bad, "Matches find_H_and_refineMatches BAD") 75 | 76 | E_H_refine, _, pts1_H_refine, pts2_H_refine = find_E_cv3(kp1, kp2, matches_H, K) 77 | DEBUG_E(E_H_refine) 78 | print("Try to use E from findEssentialMat then refined with F to recover pose") 79 | R_H_refine, t_H_refine = recoverPoseFromE_cv3(E_H_refine, pts1_H_refine, pts2_H_refine, K) 80 | DEBUG_Rt(R_H_refine, t_H_refine, "E from findEssentialMat then refined with H") 81 | """ 82 | 83 | if __name__ == '__main__': 84 | testRefine() -------------------------------------------------------------------------------- /utils.py: -------------------------------------------------------------------------------- 1 | """ 2 | Check_solutions is copied from [calculate_homography](https://github.com/kevin-george/cv_tools/blob/master/calculate_homography.py) 3 | 4 | The reference can be found at [](http://igt.ip.uca.fr/~ab/Classes/DIKU-3DCV2/Handouts/Lecture16.pdf) 5 | """ 6 | 7 | import numpy as np 8 | import cv2 9 | 10 | def check_solutions(fp, sp, K, R1, R2, t): 11 | """Using cv2.triangulatePoints to calculate position 12 | in real-world co-ordinate system. If z value for both 13 | points are positive, then that is our solution. 14 | More details can be found in the paper, 15 | David Nister, An efficient solution to the five-point relative pose problem. 16 | TODO: Need to take care of points that are far away 17 | Args: 18 | fp: Set of matched points from first image 19 | sp: Set of matched points from second image 20 | K: Calibration matrix 21 | R1: First rotation matrix 22 | R2: Second rotation matrix 23 | t: Translation vector 24 | Returns: 25 | R: Correct rotation matrix 26 | """ 27 | dist = 50.0 28 | P0 = np.eye(3,4) 29 | P1 = np.dot(K, np.concatenate((R1, t.reshape(3,1)), axis=1)) 30 | Q = cv2.triangulatePoints(P0, P1, fp.T, sp.T) 31 | mask1 = (Q[2, :] * Q[3, :]) > 0 32 | Q[0, :] /= Q[3, :] 33 | Q[1, :] /= Q[3, :] 34 | Q[2, :] /= Q[3, :] 35 | Q[3, :] /= Q[3, :] 36 | mask1 &= (Q[2,:] < dist) 37 | Q = np.dot(P1, Q) 38 | mask1 &= (Q[2,:] > 0) 39 | mask1 &= (Q[2,:] < dist) 40 | 41 | P2 = np.dot(K, np.concatenate((R2, t.reshape(3, 1)), axis=1)) 42 | Q = cv2.triangulatePoints(P0, P2, fp.T, sp.T) 43 | mask2 = (Q[2, :] * Q[3, :]) > 0 44 | Q[0, :] /= Q[3, :] 45 | Q[1, :] /= Q[3, :] 46 | Q[2, :] /= Q[3, :] 47 | Q[3, :] /= Q[3, :] 48 | mask2 &= (Q[2, :] < dist) 49 | Q = np.dot(P1, Q) 50 | mask2 &= (Q[2, :] > 0) 51 | mask2 &= (Q[2, :] < dist) 52 | 53 | P3 = np.dot(K, np.concatenate((R1, -t.reshape(3, 1)), axis=1)) 54 | Q = cv2.triangulatePoints(P0, P3, fp.T, sp.T) 55 | mask3 = (Q[2, :] * Q[3, :]) > 0 56 | Q[0, :] /= Q[3, :] 57 | Q[1, :] /= Q[3, :] 58 | Q[2, :] /= Q[3, :] 59 | Q[3, :] /= Q[3, :] 60 | mask3 &= (Q[2, :] < dist) 61 | Q = np.dot(P1, Q) 62 | mask3 &= (Q[2, :] > 0) 63 | mask3 &= (Q[2, :] < dist) 64 | 65 | P4 = np.dot(K, np.concatenate((R2, -t.reshape(3, 1)), axis=1)) 66 | Q = cv2.triangulatePoints(P0, P4, fp.T, sp.T) 67 | mask4 = (Q[2, :] * Q[3, :]) > 0 68 | Q[0, :] /= Q[3, :] 69 | Q[1, :] /= Q[3, :] 70 | Q[2, :] /= Q[3, :] 71 | Q[3, :] /= Q[3, :] 72 | mask4 &= (Q[2, :] < dist) 73 | Q = np.dot(P1, Q) 74 | mask4 &= (Q[2, :] > 0) 75 | mask4 &= (Q[2, :] < dist) 76 | 77 | good1 = np.count_nonzero(mask1) 78 | good2 = np.count_nonzero(mask2) 79 | good3 = np.count_nonzero(mask3) 80 | good4 = np.count_nonzero(mask4) 81 | 82 | print("The four good:{},{},{},{}".format(good1, good2, good3, good4)) 83 | 84 | max_count = max(good1, good2, good3, good4) 85 | if max_count == good1: 86 | return R1, t 87 | elif max_count == good2: 88 | return R2, t 89 | elif max_count == good3: 90 | return R1, -t 91 | elif max_count == good4: 92 | return R2, -t 93 | 94 | return None 95 | 96 | 97 | 98 | 99 | def rotate_angle_no_output(R): 100 | """ 101 | http://www.cnblogs.com/singlex/p/RotateMatrix2Euler.html 102 | :param R: 103 | :return: thetaz, thetay, thetax 104 | """ 105 | r11 = R[0][0] 106 | r21 = R[1][0] 107 | r31 = R[2][0] 108 | r32 = R[2][1] 109 | r33 = R[2][2] 110 | 111 | from math import pi, atan2, sqrt 112 | z = atan2(r21, r11)/pi*180 113 | y = atan2(-r31, sqrt(r32*r32 + r33*r33))/pi*180 114 | x = atan2(r32, r33)/pi*180 115 | 116 | return np.array([z, y, x]).reshape(3,1) # to make it the same as t 117 | 118 | 119 | def rotate_angle(R): 120 | z, y, x = rotate_angle_no_output(R) 121 | print("rotate_angle:z:{},y:{},x:{}".format(z, y, x)) 122 | 123 | def cv2plt(im): 124 | dim = len(im.shape) 125 | assert dim >= 2 126 | 127 | if dim == 2: # Gray 128 | return cv2.cvtColor(im, cv2.COLOR_GRAY2RGB) 129 | elif dim == 3: # BGR 130 | return cv2.cvtColor(im, cv2.COLOR_BGR2RGB) 131 | elif dim == 4: # BGRA 132 | return cv2.cvtColor(im, cv2.COLOR_BGRA2RGBA) --------------------------------------------------------------------------------