第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)

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

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

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

    # 标定
    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
    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):
    """视差转深度"""
    depth = (baseline * focal_length) / disparity
    return depth

import torch
import torch.nn as nn

class DepthEstimationNet(nn.Module):  # 继承nn.Module定义网络层
    def __init__(self):
        super(DepthEstimationNet, self).__init__()

        # 编码器
        self.encoder = nn.Sequential(
            nn.Conv2d(3, 64, 7, stride=2, padding=3),
            nn.ReLU(inplace=True),
            nn.Conv2d(64, 64, 3, padding=1),
            nn.ReLU(inplace=True),
            nn.MaxPool2d(2, 2),
            # ... 更多层
        )

        # 解码器
        self.decoder = nn.Sequential(
            nn.Conv2d(512, 256, 3, padding=1),
            nn.ReLU(inplace=True),
            nn.ConvTranspose2d(256, 128, 3, stride=2, padding=1, output_padding=1),
            nn.ReLU(inplace=True),
            # ... 更多层
            nn.Conv2d(64, 1, 3, padding=1)
        )

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

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

    def process_frame(self, frame):
        """处理帧"""
        # 特征提取
        kp, des = self.orb.detectAndCompute(frame, None)

        if self.prev_frame is not None:
            # 特征匹配
            bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
            matches = bf.match(self.prev_des, des)
            matches = sorted(matches, key=lambda x: x.distance)  # lambda匿名函数

            # 提取匹配点
            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)

            # 恢复位姿
            _, R, t, mask = 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])

        # 更新前一帧
        self.prev_frame = frame
        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