第 10 章 三维视觉¶

import cv2
import numpy as np
import glob

def camera_calibration(images_path, chessboard_size=(9, 6)):
    """相机标定"""
    # 准备标定板角点
    objp = np.zeros((chessboard_size[0] * chessboard_size[1], 3), np.float32)
    objp[:, :2] = np.mgrid[0:chessboard_size[0], 0:chessboard_size[1]].T.reshape(-1, 2)  # 重塑张量形状

    objpoints = []  # 3D点
    imgpoints = []  # 2D点

    # 读取图像
    images = glob.glob(images_path)
    if not images:
        raise FileNotFoundError(f"未找到标定图像: {images_path}")

    for fname in images:
        img = cv2.imread(fname)
        if img is None:
            continue
        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

        # 寻找角点
        ret, corners = cv2.findChessboardCorners(gray, chessboard_size, None)

        if ret:
            objpoints.append(objp)
            imgpoints.append(corners)

    if not imgpoints:
        raise RuntimeError("未检测到可用棋盘格角点，无法完成标定")

    # 标定
    ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)

    return ret, mtx, dist, rvecs, tvecs

# 使用
ret, mtx, dist, rvecs, tvecs = camera_calibration('calibration_images/*.jpg')
print(f"内参矩阵:\n{mtx}")
print(f"畸变系数:\n{dist}")

def undistort_image(image, mtx, dist):
    """畸变校正"""
    h, w = image.shape[:2]  # 切片操作，取前n个元素

    # 计算新的相机矩阵
    newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 1, (w, h))

    # 畸变校正
    dst = cv2.undistort(image, mtx, dist, None, newcameramtx)

    # 裁剪图像
    x, y, w, h = roi
    if w > 0 and h > 0:
        dst = dst[y:y+h, x:x+w]

    return dst

def stereo_matching(left_img, right_img):
    """立体匹配"""
    # 转换为灰度图
    left_gray = cv2.cvtColor(left_img, cv2.COLOR_BGR2GRAY)
    right_gray = cv2.cvtColor(right_img, cv2.COLOR_BGR2GRAY)

    # 创建立体匹配器
    stereo = cv2.StereoSGBM_create(
        minDisparity=0,
        numDisparities=16*5,  # 必须是16的倍数
        blockSize=5,
        P1=8 * 3 * 5**2,
        P2=32 * 3 * 5**2,
        disp12MaxDiff=1,
        uniquenessRatio=10,
        speckleWindowSize=100,
        speckleRange=32,
        preFilterCap=63,
        mode=cv2.STEREO_SGBM_MODE_SGBM_3WAY
    )

    # 计算视差图
    disparity = stereo.compute(left_gray, right_gray)
    disparity = disparity.astype(np.float32) / 16.0

    return disparity

# 计算深度图
def disparity_to_depth(disparity, baseline, focal_length):
    """视差转深度"""
    disparity = np.where(disparity <= 0, np.nan, disparity)
    depth = (baseline * focal_length) / disparity
    return depth

import torch
import torch.nn as nn
import torch.nn.functional as F

class DepthEstimationNet(nn.Module):
    def __init__(self):
        super().__init__()
        self.encoder = nn.Sequential(
            nn.Conv2d(3, 32, 3, stride=2, padding=1), nn.ReLU(inplace=True),
            nn.Conv2d(32, 64, 3, stride=2, padding=1), nn.ReLU(inplace=True),
            nn.Conv2d(64, 128, 3, stride=2, padding=1), nn.ReLU(inplace=True),
        )
        self.decoder = nn.Sequential(
            nn.ConvTranspose2d(128, 64, 4, stride=2, padding=1), nn.ReLU(inplace=True),
            nn.ConvTranspose2d(64, 32, 4, stride=2, padding=1), nn.ReLU(inplace=True),
            nn.ConvTranspose2d(32, 16, 4, stride=2, padding=1), nn.ReLU(inplace=True),
            nn.Conv2d(16, 1, 3, padding=1),
        )

    def forward(self, x):
        depth = self.decoder(self.encoder(x))
        return F.relu(depth)

class VisualOdometry:
    def __init__(self, camera_matrix):
        self.camera_matrix = camera_matrix
        self.orb = cv2.ORB_create(2000)
        self.prev_kp = None
        self.prev_des = None
        self.trajectory = []
        self.pose = np.eye(4)

    def process_frame(self, frame):
        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        kp, des = self.orb.detectAndCompute(gray, None)

        if self.prev_des is not None and des is not None:
            bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
            matches = sorted(bf.match(self.prev_des, des), key=lambda x: x.distance)
            matches = matches[:200]
            if len(matches) >= 8:
                prev_pts = np.float32([self.prev_kp[m.queryIdx].pt for m in matches])
                curr_pts = np.float32([kp[m.trainIdx].pt for m in matches])
                E, mask = cv2.findEssentialMat(prev_pts, curr_pts, self.camera_matrix,
                                               method=cv2.RANSAC, prob=0.999, threshold=1.0)
                if E is not None:
                    _, R, t, _ = cv2.recoverPose(E, prev_pts, curr_pts, self.camera_matrix)
                    T = np.eye(4)
                    T[:3, :3] = R
                    T[:3, 3] = t.ravel()
                    self.pose = self.pose @ T
                    self.trajectory.append(self.pose[:3, 3].copy())

        self.prev_kp = kp
        self.prev_des = des
        return self.trajectory

import open3d as o3d

def visualize_point_cloud(points, colors=None):
    """可视化点云"""
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(points)

    if colors is not None:
        pcd.colors = o3d.utility.Vector3dVector(colors)

    o3d.visualization.draw_geometries([pcd])

def point_cloud_filtering(pcd, voxel_size=0.02):
    """点云滤波"""
    # 体素下采样
    downpcd = pcd.voxel_down_sample(voxel_size)

    # 统计滤波
    cl, ind = downpcd.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0)
    filtered = downpcd.select_by_index(ind)

    return filtered

def point_cloud_registration(source, target):
    """点云配准"""
    # ICP配准
    threshold = 0.02
    trans_init = np.identity(4)

    reg_p2p = o3d.pipelines.registration.registration_icp(
        source, target, threshold, trans_init,
        o3d.pipelines.registration.TransformationEstimationPointToPoint(),
        o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=2000)
    )

    return reg_p2p.transformation