├── README.md ├── config.py └── revise_v2.py /README.md: -------------------------------------------------------------------------------- 1 | # Sfm-python 2 | 三维重建算法Structure from Motion(Sfm)的python实现 3 | 4 | 需要的包： 5 | opencv-python 6 | opencv-python-contrib 7 | numpy 8 | scipy 9 | matplotlib 10 | 可选包： 11 | mayavi 12 | 13 | 根据需要选择绘图工具，mayavi的绘图效果相对更好 14 | 15 | 运行方法： 16 | 配置config.py 中的图片路径后运行revise_v2.py即可。 17 | 18 | 原理参考博客： 19 | https://blog.csdn.net/aichipmunk/article/details/48132109 20 | 21 | 22 | -------------------------------------------------------------------------------- /config.py: -------------------------------------------------------------------------------- 1 | import os 2 | import numpy as np 3 | 4 | image_dir = '/Users/wuyingtianhua/Desktop/电信项目/3维重建/images/' 5 | MRT = 0.7 6 | #相机内参矩阵,其中，K[0][0]和K[1][1]代表相机焦距，而K[0][2]和K[1][2] 7 | #代表图像的中心像素。 8 | K = np.array([ 9 | [2362.12, 0, 720], 10 | [0, 2362.12, 578], 11 | [0, 0, 1]]) 12 | 13 | #选择性删除所选点的范围。 14 | x = 0.5 15 | y = 1 -------------------------------------------------------------------------------- /revise_v2.py: -------------------------------------------------------------------------------- 1 | ''' 2 | 原理可参考https://zhuanlan.zhihu.com/p/30033898 3 | ''' 4 | import os 5 | import cv2 6 | import sys 7 | import math 8 | import config 9 | import collections 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | from mayavi import mlab 13 | from scipy.linalg import lstsq 14 | from mpl_toolkits.mplot3d import Axes3D 15 | from scipy.optimize import least_squares 16 | 17 | ########################## 18 | #两张图之间的特征提取及匹配 19 | ########################## 20 | def extract_features(image_names): 21 | 22 | sift = cv2.xfeatures2d.SIFT_create(0, 3, 0.04, 10) 23 | key_points_for_all = [] 24 | descriptor_for_all = [] 25 | colors_for_all = [] 26 | for image_name in image_names: 27 | image = cv2.imread(image_name) 28 | 29 | if image is None: 30 | continue 31 | key_points, descriptor = sift.detectAndCompute(cv2.cvtColor(image, cv2.COLOR_BGR2GRAY), None) 32 | 33 | if len(key_points) <= 10: 34 | continue 35 | 36 | key_points_for_all.append(key_points) 37 | descriptor_for_all.append(descriptor) 38 | colors = np.zeros((len(key_points), 3)) 39 | for i, key_point in enumerate(key_points): 40 | p = key_point.pt 41 | colors[i] = image[int(p[1])][int(p[0])] 42 | colors_for_all.append(colors) 43 | return np.array(key_points_for_all), np.array(descriptor_for_all), np.array(colors_for_all) 44 | 45 | def match_features(query, train): 46 | bf = cv2.BFMatcher(cv2.NORM_L2) 47 | knn_matches = bf.knnMatch(query, train, k=2) 48 | matches = [] 49 | #Apply Lowe's SIFT matching ratio test(MRT)，值得一提的是，这里的匹配没有 50 | #标准形式，可以根据需求进行改动。 51 | for m, n in knn_matches: 52 | if m.distance < config.MRT * n.distance: 53 | matches.append(m) 54 | 55 | return np.array(matches) 56 | 57 | def match_all_features(descriptor_for_all): 58 | matches_for_all = [] 59 | for i in range(len(descriptor_for_all) - 1): 60 | matches = match_features(descriptor_for_all[i], descriptor_for_all[i + 1]) 61 | matches_for_all.append(matches) 62 | return np.array(matches_for_all) 63 | 64 | ###################### 65 | #寻找图与图之间的对应相机旋转角度以及相机平移 66 | ###################### 67 | def find_transform(K, p1, p2): 68 | 69 | focal_length = 0.5 * (K[0, 0] + K[1, 1]) 70 | principle_point = (K[0, 2], K[1, 2]) 71 | E,mask = cv2.findEssentialMat(p1, p2, focal_length, principle_point, cv2.RANSAC, 0.999, 1.0) 72 | cameraMatrix = np.array([[focal_length, 0, principle_point[0]], [0, focal_length, principle_point[1]], [0, 0, 1]]) 73 | pass_count, R, T, mask = cv2.recoverPose(E, p1, p2, cameraMatrix, mask) 74 | 75 | return R, T, mask 76 | 77 | def get_matched_points(p1, p2, matches): 78 | 79 | src_pts = np.asarray([p1[m.queryIdx].pt for m in matches]) 80 | dst_pts = np.asarray([p2[m.trainIdx].pt for m in matches]) 81 | 82 | return src_pts, dst_pts 83 | 84 | def get_matched_colors(c1, c2, matches): 85 | 86 | color_src_pts = np.asarray([c1[m.queryIdx] for m in matches]) 87 | color_dst_pts = np.asarray([c2[m.trainIdx] for m in matches]) 88 | 89 | return color_src_pts, color_dst_pts 90 | 91 | #选择重合的点 92 | def maskout_points(p1, mask): 93 | 94 | p1_copy = [] 95 | for i in range(len(mask)): 96 | if mask[i] > 0: 97 | p1_copy.append(p1[i]) 98 | 99 | return np.array(p1_copy) 100 | 101 | def init_structure(K, key_points_for_all, colors_for_all, matches_for_all): 102 | p1, p2 = get_matched_points(key_points_for_all[0], key_points_for_all[1], matches_for_all[0]) 103 | c1, c2 = get_matched_colors(colors_for_all[0], colors_for_all[1], matches_for_all[0]) 104 | 105 | if find_transform(K, p1, p2): 106 | R,T,mask = find_transform(K, p1, p2) 107 | else: 108 | R,T,mask = np.array([]), np.array([]), np.array([]) 109 | 110 | p1 = maskout_points(p1, mask) 111 | p2 = maskout_points(p2, mask) 112 | colors = maskout_points(c1, mask) 113 | #设置第一个相机的变换矩阵，即作为剩下摄像机矩阵变换的基准。 114 | R0 = np.eye(3, 3) 115 | T0 = np.zeros((3, 1)) 116 | structure = reconstruct(K, R0, T0, R, T, p1, p2) 117 | rotations = [R0, R] 118 | motions = [T0, T] 119 | correspond_struct_idx = [] 120 | for key_p in key_points_for_all: 121 | correspond_struct_idx.append(np.ones(len(key_p)) *- 1) 122 | correspond_struct_idx = np.array(correspond_struct_idx) 123 | idx = 0 124 | matches = matches_for_all[0] 125 | for i, match in enumerate(matches): 126 | if mask[i] == 0: 127 | continue 128 | correspond_struct_idx[0][int(match.queryIdx)] = idx 129 | correspond_struct_idx[1][int(match.trainIdx)] = idx 130 | idx += 1 131 | return structure, correspond_struct_idx, colors, rotations, motions 132 | 133 | ############# 134 | #三维重建 135 | ############# 136 | def reconstruct(K, R1, T1, R2, T2, p1, p2): 137 | 138 | proj1 = np.zeros((3, 4)) 139 | proj2 = np.zeros((3, 4)) 140 | proj1[0:3, 0:3] = np.float32(R1) 141 | proj1[:, 3] = np.float32(T1.T) 142 | proj2[0:3, 0:3] = np.float32(R2) 143 | proj2[:, 3] = np.float32(T2.T) 144 | fk = np.float32(K) 145 | proj1 = np.dot(fk, proj1) 146 | proj2 = np.dot(fk, proj2) 147 | s = cv2.triangulatePoints(proj1, proj2, p1.T, p2.T) 148 | structure = [] 149 | 150 | for i in range(len(s[0])): 151 | col = s[:, i] 152 | col /= col[3] 153 | structure.append([col[0], col[1], col[2]]) 154 | 155 | return np.array(structure) 156 | 157 | ########################### 158 | #将已作出的点云进行融合 159 | ########################### 160 | def fusion_structure(matches, struct_indices, next_struct_indices, structure, next_structure, colors, next_colors): 161 | 162 | for i,match in enumerate(matches): 163 | query_idx = match.queryIdx 164 | train_idx = match.trainIdx 165 | struct_idx = struct_indices[query_idx] 166 | if struct_idx >= 0: 167 | next_struct_indices[train_idx] = struct_idx 168 | continue 169 | structure = np.append(structure, [next_structure[i]], axis = 0) 170 | colors = np.append(colors, [next_colors[i]], axis = 0) 171 | struct_indices[query_idx] = next_struct_indices[train_idx] = len(structure) - 1 172 | return struct_indices, next_struct_indices, structure, colors 173 | 174 | #制作图像点以及空间点 175 | def get_objpoints_and_imgpoints(matches, struct_indices, structure, key_points): 176 | 177 | object_points = [] 178 | image_points = [] 179 | for match in matches: 180 | query_idx = match.queryIdx 181 | train_idx = match.trainIdx 182 | struct_idx = struct_indices[query_idx] 183 | if struct_idx < 0: 184 | continue 185 | object_points.append(structure[int(struct_idx)]) 186 | image_points.append(key_points[train_idx].pt) 187 | 188 | return np.array(object_points), np.array(image_points) 189 | 190 | ######################## 191 | #bundle adjustment 192 | ######################## 193 | 194 | # 这部分中，函数get_3dpos是原方法中对某些点的调整，而get_3dpos2是根据笔者的需求进行的修正，即将原本需要修正的点全部删除。 195 | # bundle adjustment请参见https://www.cnblogs.com/zealousness/archive/2018/12/21/10156733.html 196 | 197 | def get_3dpos(pos, ob, r, t, K): 198 | dtype = np.float32 199 | def F(x): 200 | p,J = cv2.projectPoints(x.reshape(1, 1, 3), r, t, K, np.array([])) 201 | p = p.reshape(2) 202 | e = ob - p 203 | err = e 204 | 205 | return err 206 | res = least_squares(F, pos) 207 | return res.x 208 | 209 | def get_3dpos_v1(pos,ob,r,t,K): 210 | p,J = cv2.projectPoints(pos.reshape(1, 1, 3), r, t, K, np.array([])) 211 | p = p.reshape(2) 212 | e = ob - p 213 | if abs(e[0]) > config.x or abs(e[1]) > config.y: 214 | return None 215 | return pos 216 | 217 | def bundle_adjustment(rotations, motions, K, correspond_struct_idx, key_points_for_all, structure): 218 | 219 | for i in range(len(rotations)): 220 | r, _ = cv2.Rodrigues(rotations[i]) 221 | rotations[i] = r 222 | for i in range(len(correspond_struct_idx)): 223 | point3d_ids = correspond_struct_idx[i] 224 | key_points = key_points_for_all[i] 225 | r = rotations[i] 226 | t = motions[i] 227 | for j in range(len(point3d_ids)): 228 | point3d_id = int(point3d_ids[j]) 229 | if point3d_id < 0: 230 | continue 231 | new_point = get_3dpos_v1(structure[point3d_id], key_points[j].pt, r, t, K) 232 | structure[point3d_id] = new_point 233 | 234 | return structure 235 | 236 | ####################### 237 | #作图 238 | ####################### 239 | 240 | # 这里有两种方式作图，其中一个是matplotlib做的，但是第二个是基于mayavi做的，效果上看，fig_v1效果更好。fig_v2是mayavi加颜色的效果。 241 | 242 | def fig(structure, colors): 243 | colors /= 255 244 | for i in range(len(colors)): 245 | colors[i, :] = colors[i, :][[2, 1, 0]] 246 | fig = plt.figure() 247 | fig.suptitle('3d') 248 | ax = fig.gca(projection = '3d') 249 | for i in range(len(structure)): 250 | ax.scatter(structure[i, 0], structure[i, 1], structure[i, 2], color = colors[i, :], s = 5) 251 | ax.set_xlabel('x axis') 252 | ax.set_ylabel('y axis') 253 | ax.set_zlabel('z axis') 254 | ax.view_init(elev = 135, azim = 90) 255 | plt.show() 256 | 257 | def fig_v1(structure): 258 | 259 | mlab.points3d(structure[:, 0], structure[:, 1], structure[:, 2], mode = 'point', name = 'dinosaur') 260 | mlab.show() 261 | 262 | def fig_v2(structure, colors): 263 | 264 | for i in range(len(structure)): 265 | 266 | mlab.points3d(structure[i][0], structure[i][1], structure[i][2], 267 | mode = 'point', name = 'dinosaur', color = colors[i]) 268 | 269 | mlab.show() 270 | 271 | def main(): 272 | imgdir = config.image_dir 273 | img_names = os.listdir(imgdir) 274 | img_names = sorted(img_names) 275 | 276 | for i in range(len(img_names)): 277 | img_names[i] = imgdir + img_names[i] 278 | # img_names = img_names[0:10] 279 | 280 | # K是摄像头的参数矩阵 281 | K = config.K 282 | 283 | key_points_for_all, descriptor_for_all, colors_for_all = extract_features(img_names) 284 | matches_for_all = match_all_features(descriptor_for_all) 285 | structure, correspond_struct_idx, colors, rotations, motions = init_structure(K, key_points_for_all, colors_for_all, matches_for_all) 286 | 287 | for i in range(1, len(matches_for_all)): 288 | object_points, image_points = get_objpoints_and_imgpoints(matches_for_all[i], correspond_struct_idx[i], structure, key_points_for_all[i + 1]) 289 | #在python的opencv中solvePnPRansac函数的第一个参数长度需要大于7，否则会报错 290 | #这里对小于7的点集做一个重复填充操作，即用点集中的第一个点补满7个 291 | if len(image_points) < 7: 292 | while len(image_points) < 7: 293 | object_points = np.append(object_points, [object_points[0]], axis = 0) 294 | image_points = np.append(image_points, [image_points[0]], axis = 0) 295 | 296 | _, r, T, _ = cv2.solvePnPRansac(object_points, image_points, K, np.array([])) 297 | R, _ = cv2.Rodrigues(r) 298 | rotations.append(R) 299 | motions.append(T) 300 | p1, p2 = get_matched_points(key_points_for_all[i], key_points_for_all[i + 1], matches_for_all[i]) 301 | c1, c2 = get_matched_colors(colors_for_all[i], colors_for_all[i + 1], matches_for_all[i]) 302 | next_structure = reconstruct(K, rotations[i], motions[i], R, T, p1, p2) 303 | 304 | correspond_struct_idx[i], correspond_struct_idx[i + 1], structure, colors = fusion_structure(matches_for_all[i],correspond_struct_idx[i],correspond_struct_idx[i+1],structure,next_structure,colors,c1) 305 | structure = bundle_adjustment(rotations, motions, K, correspond_struct_idx, key_points_for_all, structure) 306 | i = 0 307 | # 由于经过bundle_adjustment的structure，会产生一些空的点（实际代表的意思是已被删除） 308 | # 这里删除那些为空的点 309 | while i < len(structure): 310 | if math.isnan(structure[i][0]): 311 | structure = np.delete(structure, i, 0) 312 | colors = np.delete(colors, i, 0) 313 | i -= 1 314 | i += 1 315 | 316 | print(len(structure)) 317 | print(len(motions)) 318 | # np.save('structure.npy', structure) 319 | # np.save('colors.npy', colors) 320 | 321 | # fig(structure,colors) 322 | fig_v1(structure) 323 | # fig_v2(structure, colors) 324 | 325 | if __name__ == '__main__': 326 | main() --------------------------------------------------------------------------------