sfm三维重建python代码

SFM（Structure from Motion）是一种从图像序列中恢复三维结构的技术，常用于三维重建。以下是一个使用Python实现SFM三维重建的代码示例：

import cv2
import numpy as np
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt

# 读取图像序列
img1 = cv2.imread('image1.jpg')
img2 = cv2.imread('image2.jpg')
img3 = cv2.imread('image3.jpg')

# 提取特征点
sift = cv2.xfeatures2d.SIFT_create()
kp1, des1 = sift.detectAndCompute(img1, None)
kp2, des2 = sift.detectAndCompute(img2, None)
kp3, des3 = sift.detectAndCompute(img3, None)

# 匹配特征点
bf = cv2.BFMatcher()
matches1 = bf.knnMatch(des1, des2, k=2)
matches2 = bf.knnMatch(des2, des3, k=2)

# 筛选匹配点
good_matches1 = []
for m, n in matches1:
    if m.distance < 0.75 * n.distance:
        good_matches1.append(m)

good_matches2 = []
for m, n in matches2:
    if m.distance < 0.75 * n.distance:
        good_matches2.append(m)

# 提取匹配点对应的特征点坐标
src_pts1 = np.float32([kp1[m.queryIdx].pt for m in good_matches1]).reshape(-1, 1, 2)
dst_pts1 = np.float32([kp2[m.trainIdx].pt for m in good_matches1]).reshape(-1, 1, 2)

src_pts2 = np.float32([kp2[m.queryIdx].pt for m in good_matches2]).reshape(-1, 1, 2)
dst_pts2 = np.float32([kp3[m.trainIdx].pt for m in good_matches2]).reshape(-1, 1, 2)

# 计算相机位姿
E1, mask1 = cv2.findEssentialMat(src_pts1, dst_pts1)
R1, t1, mask1 = cv2.recoverPose(E1, src_pts1, dst_pts1)

E2, mask2 = cv2.findEssentialMat(src_pts2, dst_pts2)
R2, t2, mask2 = cv2.recoverPose(E2, src_pts2, dst_pts2)

# 三角化重建
P1 = np.hstack((np.eye(3), np.zeros((3, 1))))
P2 = np.hstack((R1, t1))
P3 = np.hstack((R2, t2))

points4D = cv2.triangulatePoints(P1, P2, src_pts1, dst_pts1)
points4D /= points4D[3]
points3D = points4D[:3].T

# 可视化
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.scatter(points3D[:, 0], points3D[:, 1], points3D[:, 2])
plt.show()