基于Python的Structure from Motion(SfM)三维重建实战教程

本文还有配套的精品资源，点击获取

简介：结构从运动（SfM）是一种计算机视觉技术，用于从多个二维图像恢复三维场景结构。本教程将指导你使用Python实现SfM算法，涵盖从特征检测到点云构建的各个步骤。通过实践任务，你将掌握SfM算法的原理和实现方法，并学会使用OpenCV、scikit-image等库进行三维重建。本教程适合计算机视觉和图像处理领域的初学者和爱好者。

1. 基于 Python 的三维重建算法 Structure from Motion (SfM) 实现代码

第一章：SfM 算法原理

Structure from Motion (SfM) 是一种三维重建算法，它从图像序列中估计相机姿态和场景结构。SfM 算法的基本原理如下：

• 图像采集： 使用相机或其他成像设备从不同角度拍摄场景的图像序列。
• 特征检测与匹配： 在图像中检测特征点并匹配它们以找到图像之间的对应关系。
• 相机姿态估计： 根据匹配的特征点估计每个图像的相机姿态，包括位置和方向。
• 三角化： 利用相机姿态和匹配的特征点三角化场景中点的三维坐标。
• 点云构建： 将三角化的点连接起来形成场景的点云模型。

2. 特征检测与匹配

2.1 特征检测算法

特征检测算法旨在从图像中识别具有显著性的区域，这些区域在图像变换或噪声影响下保持稳定。在 SfM 中，特征检测对于建立图像之间的对应关系至关重要。以下介绍三种常用的特征检测算法：

2.1.1 Harris 角点检测

Harris 角点检测是一种基于局部图像梯度计算的算法。它通过计算图像每个像素周围的梯度变化量来识别角点。角点具有较大的梯度变化，表明图像在该区域有明显的边缘或纹理。

Harris 角点检测算法的步骤如下：

1. 计算图像每个像素的梯度：

import cv2
import numpy as np

# 计算图像梯度
Ix = cv2.Sobel(image, cv2.CV_64F, 1, 0, ksize=3)
Iy = cv2.Sobel(image, cv2.CV_64F, 0, 1, ksize=3)

1. 计算结构张量：

# 计算结构张量
M = np.array([[np.sum(Ix**2), np.sum(Ix*Iy)],
              [np.sum(Ix*Iy), np.sum(Iy**2)]])

1. 计算角点响应函数：

# 计算角点响应函数
R = np.trace(M)**2 - np.linalg.det(M)

1. 阈值化和非极大值抑制：

# 阈值化
R[R < threshold] = 0

# 非极大值抑制
R = cv2.dilate(R, np.ones((3,3)))
R = cv2.erode(R, np.ones((3,3)))

2.1.2 SIFT 特征检测

尺度不变特征变换 (SIFT) 是一种基于图像局部梯度分布的特征检测算法。它通过在不同尺度空间中搜索极值点来识别特征点。SIFT 特征具有尺度不变性、旋转不变性和局部不变性。

SIFT 特征检测算法的步骤如下：

1. 构建尺度空间：

import cv2

# 构建尺度空间
sift = cv2.SIFT_create()
keypoints, descriptors = sift.detectAndCompute(image, None)

1. 检测极值点：

# 检测极值点
for keypoint in keypoints:
    if keypoint.response > threshold:
        # 标记为极值点
        keypoint.flag = True

1. 精确定位极值点：

# 精确定位极值点
for keypoint in keypoints:
    if keypoint.flag:
        # 精确定位极值点
        keypoint.pt = cv2.KeyPoint_convert(keypoint)

1. 方向赋值：

# 方向赋值
for keypoint in keypoints:
    if keypoint.flag:
        # 计算梯度直方图
        hist = cv2.calcHist([image], [0], None, [36], [0, 360])
        # 找到最大值
        max_idx = np.argmax(hist)
        # 赋值方向
        keypoint.angle = max_idx * 10

2.1.3 ORB 特征检测

定向快速二进制鲁棒特征 (ORB) 是一种快速且鲁棒的特征检测算法。它基于 FAST 角点检测算法和 BRIEF 二进制描述符。ORB 特征具有良好的旋转不变性和计算效率。

ORB 特征检测算法的步骤如下：

1. FAST 角点检测：

import cv2

# FAST 角点检测
orb = cv2.ORB_create()
keypoints, descriptors = orb.detectAndCompute(image, None)

1. BRIEF 二进制描述符：

# BRIEF 二进制描述符
for keypoint in keypoints:
    # 计算 BRIEF 二进制描述符
    descriptor = cv2.calcHist([image], [0], None, [256], [0, 256])
    # 二值化
    descriptor = np.where(descriptor > 128, 1, 0)
    # 赋值描述符
    keypoint.descriptor = descriptor

3. 相机姿态估计

相机姿态估计是 SfM 算法中的关键步骤，它确定了相机在不同时刻的位置和方向。相机姿态估计算法可以分为单目相机姿态估计和双目相机姿态估计。

3.1 单目相机姿态估计

单目相机姿态估计使用单张图像来估计相机姿态。常用的单目相机姿态估计算法有八点法和五点法。

3.1.1 八点法

八点法是一种经典的单目相机姿态估计算法，它使用 8 个匹配的特征点来估计本质矩阵。本质矩阵是一个 3x3 矩阵，它描述了相机的旋转和平移。

步骤：

1. 检测并匹配两张图像中的特征点。
2. 构建一个 8x9 的矩阵 A，其中每一行对应一个特征点对。
3. 使用奇异值分解（SVD）分解矩阵 A，得到矩阵 U、S 和 V。
4. 从矩阵 U 和 V 中提取本质矩阵 E。

代码块：

import numpy as np
from cv2 import findEssentialMat

def eight_point_algorithm(points1, points2):
    """
    使用八点法估计本质矩阵。

    参数：
        points1：第一张图像中的特征点。
        points2：第二张图像中的特征点。

    返回：
        本质矩阵。
    """

    # 构建矩阵 A
    A = np.zeros((8, 9))
    for i in range(8):
        x1, y1 = points1[i]
        x2, y2 = points2[i]
        A[i, :] = [x1 * x2, x1 * y2, x1, y1 * x2, y1 * y2, y1, x2, y2, 1]

    # 使用 SVD 分解矩阵 A
    U, S, Vh = np.linalg.svd(A)

    # 从 U 和 Vh 中提取本质矩阵
    E = U[:, :3] @ np.diag([1, 1, 0]) @ Vh[:3, :]

    # 返回本质矩阵
    return E

逻辑分析：

该代码块实现了八点法算法。它首先构建矩阵 A，然后使用 SVD 分解矩阵 A，最后从 U 和 Vh 中提取本质矩阵 E。

3.1.2 五点法

五点法是一种更鲁棒的单目相机姿态估计算法，它使用 5 个匹配的特征点来估计本质矩阵。五点法比八点法更鲁棒，因为它对噪声和外点更不敏感。

步骤：

1. 检测并匹配两张图像中的特征点。
2. 构建一个 5x12 的矩阵 A，其中每一行对应一个特征点对。
3. 使用奇异值分解（SVD）分解矩阵 A，得到矩阵 U、S 和 V。
4. 从矩阵 U 和 V 中提取本质矩阵 E。

代码块：

import numpy as np
from cv2 import findEssentialMat

def five_point_algorithm(points1, points2):
    """
    使用五点法估计本质矩阵。

    参数：
        points1：第一张图像中的特征点。
        points2：第二张图像中的特征点。

    返回：
        本质矩阵。
    """

    # 构建矩阵 A
    A = np.zeros((5, 12))
    for i in range(5):
        x1, y1 = points1[i]
        x2, y2 = points2[i]
        A[i, :] = [x1 * x2, x1 * y2, x1, y1 * x2, y1 * y2, y1, x2, y2, 1, 0, 0, 0]

    # 使用 SVD 分解矩阵 A
    U, S, Vh = np.linalg.svd(A)

    # 从 U 和 Vh 中提取本质矩阵
    E = U[:, :3] @ np.diag([1, 1, 0]) @ Vh[:3, :]

    # 返回本质矩阵
    return E

逻辑分析：

该代码块实现了五点法算法。它首先构建矩阵 A，然后使用 SVD 分解矩阵 A，最后从 U 和 Vh 中提取本质矩阵 E。

3.2 双目相机姿态估计

双目相机姿态估计使用两张图像来估计相机姿态。常用的双目相机姿态估计算法有本质矩阵估计和 PnP 算法。

3.2.1 本质矩阵估计

本质矩阵估计算法使用两张图像中的匹配特征点来估计本质矩阵。本质矩阵描述了相机的旋转和平移。

步骤：

1. 检测并匹配两张图像中的特征点。
2. 使用八点法或五点法估计本质矩阵。
3. 使用奇异值分解（SVD）分解本质矩阵，得到矩阵 U、S 和 V。
4. 从矩阵 U 和 V 中提取旋转和平移矩阵。

代码块：

import numpy as np
from cv2 import findEssentialMat, recoverPose

def essential_matrix_estimation(points1, points2):
    """
    使用本质矩阵估计算法估计相机姿态。

    参数：
        points1：第一张图像中的特征点。
        points2：第二张图像中的特征点。

    返回：
        旋转和平移矩阵。
    """

    # 估计本质矩阵
    E = eight_point_algorithm(points1, points2)

    # 使用 SVD 分解本质矩阵
    U, S, Vh = np.linalg.svd(E)

    # 从 U 和 Vh 中提取旋转和平移矩阵
    W = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]])
    R = U @ W @ Vh
    t = U[:, 2]

    # 返回旋转和平移矩阵
    return R, t

逻辑分析：

该代码块实现了本质矩阵估计算法。它首先估计本质矩阵，然后使用 SVD 分解本质矩阵，最后从 U 和 Vh 中提取旋转和平移矩阵。

3.2.2 PnP 算法

PnP 算法（Perspective-n-Point）是一种常用的双目相机姿态估计算法。PnP 算法使用 3D 点和 2D 点之间的对应关系来估计相机姿态。

步骤：

1. 检测并匹配两张图像中的特征点。
2. 使用 3D 点和 2D 点之间的对应关系构建一个齐次方程组。
3. 使用奇异值分解（SVD）分解齐次方程组，得到矩阵 U、S 和 V。
4. 从矩阵 U 和 V 中提取旋转和平移矩阵。

代码块：

import numpy as np
from cv2 import solvePnP

def pnp_algorithm(points3d, points2d, camera_matrix, distortion_coefficients):
    """
    使用 PnP 算法估计相机姿态。

    参数：
        points3d：3D 点。
        points2d：2D 点。
        camera_matrix：相机矩阵。
        distortion_coefficients：畸变系数。

    返回：
        旋转和平移矩阵。
    """

    # 构建齐次方程组
    A = np.zeros((3 * len(points3d), 12))
    for i in range(len(points3d)):
        x, y, z = points3d[i]
        u, v = points2d[i]
        A[3 * i, :] = [x, y, z, 1, 0, 0, 0, 0, -u * x, -u * y, -u * z, -u]
        A[3 * i + 1, :] = [0, 0, 0, 0, x, y, z, 1, -v * x, -v * y, -v * z, -v]

    # 使用 SVD 分解齐次方程组
    U, S, Vh = np.linalg.svd(A)

    # 从 U 和 Vh 中提取旋转和平移矩阵
    R, t = Vh[-1, :3], Vh[-1, 3:]

    # 返回旋转和平移矩阵
    return R, t

逻辑分析：

该代码块实现了 PnP 算法。它首先构建齐次方程组，然后使用 SVD 分解齐次方程组，最后从 U 和 Vh 中提取旋转和平移矩阵。

4. 本质矩阵与双目立体匹配

4.1 本质矩阵

4.1.1 本质矩阵的性质

本质矩阵是一个 3x3 的矩阵，描述了两个相机之间的相对位姿。它具有以下性质：

• 秩为 2： 本质矩阵的秩为 2，这意味着它只有两个独立的行或列。
• 行列式为 0： 本质矩阵的行列式为 0，这表明它是一个奇异矩阵。
• 旋转矩阵： 本质矩阵的前 3 行构成一个旋转矩阵，表示两个相机之间的旋转关系。
• 平移向量： 本质矩阵的最后一行是一个平移向量，表示两个相机之间的平移关系。

4.1.2 本质矩阵的分解

本质矩阵可以分解为旋转矩阵和平移向量：

import numpy as np

def decompose_essential_matrix(E):
  """
  分解本质矩阵为旋转矩阵和平移向量。

  参数：
    E (3x3 np.ndarray): 本质矩阵。

  返回：
    R (3x3 np.ndarray): 旋转矩阵。
    t (3x1 np.ndarray): 平移向量。
  """

  # 奇异值分解
  U, S, Vh = np.linalg.svd(E)

  # 旋转矩阵
  R = U @ Vh.T

  # 平移向量
  t = U[:, 2]

  return R, t

4.2 双目立体匹配

双目立体匹配是使用两个相机同时拍摄的图像来计算场景中点的 3D 坐标的过程。本质矩阵在双目立体匹配中起着至关重要的作用，因为它提供了两个相机之间的相对位姿信息。

4.2.1 稠密匹配算法

稠密匹配算法使用本质矩阵来计算图像中每个像素的 3D 坐标。以下是一个简单的稠密匹配算法：

1. 对于图像中的每个像素：
2. 计算像素在两幅图像中的对应点。
3. 使用本质矩阵计算对应点的 3D 坐标。

4.2.2 半稠密匹配算法

半稠密匹配算法只计算图像中感兴趣区域的 3D 坐标。这可以提高计算效率，同时仍然提供准确的结果。以下是一个半稠密匹配算法：

1. 识别图像中的感兴趣区域。
2. 对于感兴趣区域中的每个像素：
3. 计算像素在两幅图像中的对应点。
4. 使用本质矩阵计算对应点的 3D 坐标。

graph LR
subgraph 左相机
    A[像素1] --> B[像素2]
    B[像素2] --> C[像素3]
end
subgraph 右相机
    D[像素1] --> E[像素2]
    E[像素2] --> F[像素3]
end
A --> D
B --> E
C --> F

5. 三角化

5.1 三角化原理

三角化是 SfM 算法中将匹配的特征点投影到三维空间中的过程。通过已知的相机参数和特征点的对应关系，可以计算出特征点在三维空间中的位置。

5.2 三角化算法

5.2.1 线性三角化

线性三角化是一种简单的三角化方法，它假设相机投影矩阵是已知的，并且特征点在图像平面上是线性的。

步骤：

1. 将匹配的特征点投影到相机坐标系中，得到点 P1 和 P2 。
2. 构建齐次方程组：

[P1_x - P2_x, P1_y - P2_y, P1_z - P2_z] * [X, Y, Z, 1] = 0

其中 (X, Y, Z) 为三维空间中的点坐标。 3. 求解方程组，得到三维点坐标。

5.2.2 非线性三角化

非线性三角化是一种更精确的三角化方法，它考虑了相机投影矩阵的非线性失真。

步骤：

1. 初始化三维点坐标 (X, Y, Z) 。
2. 计算投影误差：

error = (P1 - P1_proj)^2 + (P2 - P2_proj)^2

其中 P1_proj 和 P2_proj 是三维点在两个相机坐标系中的投影。 3. 使用最小二乘法优化三维点坐标，以最小化投影误差。

代码示例：

import numpy as np
from scipy.linalg import lstsq

def linear_triangulation(P1, P2, K1, K2, R1, R2, t1, t2):
    """
    线性三角化

    Args:
        P1 (np.ndarray): 第一个相机中的特征点坐标
        P2 (np.ndarray): 第二个相机中的特征点坐标
        K1 (np.ndarray): 第一个相机的内参矩阵
        K2 (np.ndarray): 第二个相机的内参矩阵
        R1 (np.ndarray): 第一个相机的旋转矩阵
        R2 (np.ndarray): 第二个相机的旋转矩阵
        t1 (np.ndarray): 第一个相机的平移向量
        t2 (np.ndarray): 第二个相机的平移向量

    Returns:
        np.ndarray: 三维空间中的点坐标
    """

    # 将特征点投影到相机坐标系
    P1_cam = np.dot(np.linalg.inv(K1), P1.T).T
    P2_cam = np.dot(np.linalg.inv(K2), P2.T).T

    # 构建齐次方程组
    A = np.array([P1_cam[0] - P2_cam[0], P1_cam[1] - P2_cam[1], P1_cam[2] - P2_cam[2]])

    # 求解三维点坐标
    _, _, V = np.linalg.svd(A)
    X = V[:, -1] / V[-1, -1]

    return X

def nonlinear_triangulation(P1, P2, K1, K2, R1, R2, t1, t2, max_iter=10):
    """
    非线性三角化

    Args:
        P1 (np.ndarray): 第一个相机中的特征点坐标
        P2 (np.ndarray): 第二个相机中的特征点坐标
        K1 (np.ndarray): 第一个相机的内参矩阵
        K2 (np.ndarray): 第二个相机的内参矩阵
        R1 (np.ndarray): 第一个相机的旋转矩阵
        R2 (np.ndarray): 第二个相机的旋转矩阵
        t1 (np.ndarray): 第一个相机的平移向量
        t2 (np.ndarray): 第二个相机的平移向量
        max_iter (int): 最大迭代次数

    Returns:
        np.ndarray: 三维空间中的点坐标
    """

    # 初始化三维点坐标
    X = np.zeros(3)

    for _ in range(max_iter):
        # 计算投影误差
        P1_proj = np.dot(K1, np.dot(R1, X.T) + t1.T)
        P2_proj = np.dot(K2, np.dot(R2, X.T) + t2.T)
        error = np.sum((P1 - P1_proj)**2 + (P2 - P2_proj)**2)

        # 计算雅可比矩阵
        J = np.array([
            [K1[0, 0] * R1[0, 0] + K1[0, 1] * R1[1, 0] + K1[0, 2] * R1[2, 0],
             K1[0, 0] * R1[0, 1] + K1[0, 1] * R1[1, 1] + K1[0, 2] * R1[2, 1],
             K1[0, 0] * R1[0, 2] + K1[0, 1] * R1[1, 2] + K1[0, 2] * R1[2, 2]],
            [K2[0, 0] * R2[0, 0] + K2[0, 1] * R2[1, 0] + K2[0, 2] * R2[2, 0],
             K2[0, 0] * R2[0, 1] + K2[0, 1] * R2[1, 1] + K2[0, 2] * R2[2, 1],
             K2[0, 0] * R2[0, 2] + K2[0, 1] * R2[1, 2] + K2[0, 2] * R2[2, 2]]
        ])

        # 更新三维点坐标
        delta_X = np.dot(np.linalg.inv(J), np.array([P1 - P1_proj, P2 - P2_proj]).T)
        X += delta_X.T

    return X

6.1 点云表示

点云是一种三维数据结构，它由一组三维点组成，每个点都有一个 x、y 和 z 坐标。点云通常用于表示三维场景，例如物体、房间或环境。

点云可以以不同的格式表示，最常见的格式是 PLY（Polygon File Format）和 LAS（LASer）。PLY 格式是一个文本文件格式，它存储每个点的坐标、法线和颜色等信息。LAS 格式是一个二进制文件格式，它专门用于存储激光扫描仪数据。

6.2 点云构建算法

点云构建算法将从 SfM 算法中估计的相机姿态和三角化点组合成一个完整的点云。有两种主要的点云构建算法：增量式点云构建和全局点云构建。

6.2.1 增量式点云构建

增量式点云构建算法逐帧处理图像，并根据当前帧的相机姿态和三角化点更新点云。该算法的优点是它不需要存储所有图像和点，并且可以实时构建点云。

6.2.2 全局点云构建

全局点云构建算法一次性处理所有图像，并使用所有相机姿态和三角化点来构建点云。该算法的优点是它可以生成更准确和完整的点云，但它需要存储所有图像和点，并且计算成本更高。

本文还有配套的精品资源，点击获取

简介：结构从运动（SfM）是一种计算机视觉技术，用于从多个二维图像恢复三维场景结构。本教程将指导你使用Python实现SfM算法，涵盖从特征检测到点云构建的各个步骤。通过实践任务，你将掌握SfM算法的原理和实现方法，并学会使用OpenCV、scikit-image等库进行三维重建。本教程适合计算机视觉和图像处理领域的初学者和爱好者。

本文还有配套的精品资源，点击获取