计算机视觉：让机器人看见世界

1. 图像基础与OpenCV

基本操作流水线

import cv2; import numpy as np
img = cv2.imread('robot_view.jpg')
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
hsv  = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
blur = cv2.GaussianBlur(gray, (5,5), 0)
edges = cv2.Canny(gray, 50, 150)  # 边缘检测

# 颜色分割：找到红色物体
lower = np.array([0,100,100]); upper = np.array([10,255,255])
mask = cv2.inRange(hsv, lower, upper)
contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
for c in contours:
    x,y,w,h = cv2.boundingRect(c)
    cv2.rectangle(img, (x,y), (x+w,y+h), (0,255,0), 2)

2. 相机模型与标定

内参矩阵 K 是你相机的"身份证"：$f_x,f_y$ 是焦距（像素），$c_x,c_y$ 是主点。标定一次即可。

# 相机标定
pattern_size = (9, 6)  # 棋盘格内角点数
objp = np.zeros((pattern_size[0]*pattern_size[1], 3), np.float32)
objp[:,:2] = np.mgrid[0:pattern_size[0],0:pattern_size[1]].T.reshape(-1,2)

# 从多张棋盘格照片中标定
ret, K, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, img.shape[:2])
print(f"内参矩阵:\n{K}")

# 去畸变
img_undist = cv2.undistort(img, K, dist)

3. 手眼标定

相机检测到"物体在相机前方0.5m"，你怎么告诉机械臂去哪抓？需要 $\mathbf{T}_{base}^{cam}$——相机在机器人基坐标系中的位姿。

# OpenCV 手眼标定
R_cam2base, t_cam2base = cv2.calibrateHandEye(
    R_gripper2base, t_gripper2base,   # 机械臂每次移动的位姿
    R_target2cam, t_target2cam,        # 标定板在相机中的位姿
    method=cv2.CALIB_HAND_EYE_TSAI
)

4. 目标检测：YOLO实战

YOLO 把整张图一次性输入网络，直接输出边界框和类别。不像传统方法分"候选区域→分类"两步，YOLO 是 one-stage detection。

from ultralytics import YOLO
model = YOLO('yolov8n.pt')  # nano版本，适合边缘设备
results = model('workspace.jpg')
for r in results:
    for box in r.boxes:
        cls = model.names[int(box.cls[0])]
        conf = box.conf[0].item()
        x1,y1,x2,y2 = box.xyxy[0].tolist()
        print(f"{cls}: {conf:.2f} @ ({x1:.0f},{y1:.0f})-({x2:.0f},{y2:.0f})")

ArUco标记是快速6D位姿估计的"作弊码"：相机看到标记后能瞬间算出标记的3D位置和朝向。

aruco = cv2.aruco.ArucoDetector(
    cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_6X6_250),
    cv2.aruco.DetectorParameters()
)
corners, ids, _ = aruco.detectMarkers(img)
rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(corners, 0.05, K, dist)
print(f"标记 {ids[0][0]}: 位置={tvecs[0][0]} m")

5. 3D感知：点云与深度

RGB-D 相机（如 Realsense）输出彩色图+深度图。深度图 + 内参 → 3D 点云。点云是机器人抓取和导航的核心数据。

import open3d as o3d
pcd = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd, intrinsics)
pcd = pcd.voxel_down_sample(0.005)  # 降采样
pcd, _ = pcd.remove_statistical_outlier(30, 2.0)  # 去噪
plane, inliers = pcd.segment_plane(0.01, 3, 1000)  # RANSAC平面分割
objects = pcd.select_by_index(inliers, invert=True)  # 物体=非桌面

4. 立体视觉与深度估计

双目立体视觉

两个相机像两只眼睛。同一物点在左右图像中的像素差（视差）与深度成反比。你在装配时用双眼判断零件距离——双目相机完全一样。

$B$是基线，$f$是焦距，$d$是视差。基线越大→深度分辨率越高。

import cv2
stereo = cv2.StereoSGBM_create(
    minDisparity=0, numDisparities=128, blockSize=11
)
disparity = stereo.compute(imgL_gray, imgR_gray)
depth = (focal_length * baseline) / (disparity + 1e-6)

5. YOLO目标检测实战

YOLO为什么适合机器人

一次前向传播同时输出所有物体的类别和边界框。对于需要实时（>30FPS）反馈的机器人抓取任务，YOLO是首选。

from ultralytics import YOLO
model = YOLO("yolov8n.pt")
results = model("workspace.jpg")[0]
for box in results.boxes:
    x1, y1, x2, y2 = box.xyxy[0].tolist()
    cls = int(box.cls[0])
    print(f"{results.names[cls]}: {float(box.conf[0]):.2f}")

6. 相机模型完整数学推导

从世界坐标系到像素坐标的完整变换链

一个3D世界点要经历4步变换才能成为2D像素坐标。你之前只看到了最后一步（内参矩阵K），现在完整走一遍。

步骤分解：

1. 世界→相机：$P_c = R \cdot P_w + t$（你熟悉的坐标变换！）
2. 相机→归一化平面：$[x_n, y_n] = [X_c/Z_c, Y_c/Z_c]$（透视除法）
3. 径向畸变：$x_d = x_n(1+k_1r^2+k_2r^4+k_3r^6)$，$r=\sqrt{x_n^2+y_n^2}$
4. 归一化→像素：$u = f_x \cdot x_d + c_x, v = f_y \cdot y_d + c_y$

畸变模型详解

import cv2, numpy as np

def project_3d_to_2d(points_3d, K, dist, rvec, tvec):
    """完整投影: 3D世界点→2D像素 (含畸变)"""
    points_2d, _ = cv2.projectPoints(points_3d, rvec, tvec, K, dist)
    return points_2d.reshape(-1, 2)

def undistort_points(points_2d_distorted, K, dist):
    """畸变校正: 畸变像素→理想像素"""
    points_undist = cv2.undistortPoints(
        points_2d_distorted.reshape(-1, 1, 2), K, dist, P=K
    )
    return points_undist.reshape(-1, 2)

# 验证: 标定板角点的3D坐标投影到2D，与检测到的角点对比
objp = np.array([[0,0,0],[0.025,0,0],[0.025,0.025,0]], dtype=np.float32)
proj = project_3d_to_2d(objp, K, dist, rvec, tvec)
print(f"投影坐标:\\n{proj}")

7. 点云配准：ICP完整实现

为什么需要ICP

你有一个物体的CAD模型（源点云）和深度相机拍到的场景点云（目标点云）。你想知道物体在场景中的6D位姿。ICP (Iterative Closest Point) 是最经典的解法：反复做"找最近点→计算变换→应用变换"，直到收敛。

ICP算法步骤

def icp(source, target, max_iters=50, tolerance=1e-6):
    """ICP点云配准: 估计 source→target 的刚体变换"""
    T = np.eye(4)
    prev_error = float('inf')

    for iteration in range(max_iters):
        # 1. 找最近点 (用KD-Tree加速, O(N log M))
        from scipy.spatial import KDTree
        tree = KDTree(target)
        distances, indices = tree.query(source[:,:3])
        matched = target[indices]

        # 2. 计算最优变换 (SVD法: 闭式解!)
        centroid_s = source[:,:3].mean(axis=0)
        centroid_t = matched.mean(axis=0)
        H = (source[:,:3] - centroid_s).T @ (matched - centroid_t)
        U, _, Vt = np.linalg.svd(H)
        R = Vt.T @ U.T
        if np.linalg.det(R) < 0:
            Vt[-1, :] *= -1
            R = Vt.T @ U.T
        t = centroid_t - R @ centroid_s

        # 3. 应用变换
        dT = np.eye(4); dT[:3,:3] = R; dT[:3,3] = t
        T = dT @ T
        source = (dT[:3,:3] @ source[:,:3].T + dT[:3,3:]).T

        # 4. 收敛判断
        mean_error = distances.mean()
        if abs(prev_error - mean_error) < tolerance:
            break
        prev_error = mean_error

    return T, mean_error

5. 对极几何——双目视觉的数学基石

为什么需要理解对极几何

你在Phase 5学了单目标定——但那只能告诉你像素在3D空间中的方向（一条射线），不能告诉你距离。双目视觉用两个相机从不同角度看同一场景，利用三角测量恢复深度。对极几何描述了左右图像之间的几何约束——左图像中的一个点，在右图像中的对应点必须位于一条线上。

核心公式：本质矩阵与基础矩阵

$x$和$x'$是左右图像上的一对对应点的齐次坐标。基础矩阵$F$（3×3，秩2）由相机的内参和相对位姿决定：

$E = [t]_\times R$是本质矩阵——只取决于两相机之间的旋转$R$和平移$t$，与内参无关。

八点算法——从对应点求解基础矩阵

import numpy as np

def compute_fundamental_matrix(pts1, pts2):
    """八点算法：从N>=8组对应点求解基础矩阵F"""
    assert pts1.shape == pts2.shape and pts1.shape[0] >= 8
    
    # 1. 坐标归一化（提高数值稳定性）
    pts1_norm, T1 = normalize_points(pts1)
    pts2_norm, T2 = normalize_points(pts2)
    
    # 2. 构建线性系统: A f = 0
    #    对每组对应点 (u,v,1) 和 (u',v',1):
    #    uu'F11 + uv'F21 + u F31 + vu'F12 + vv'F22 + v F32 + u'F13 + v'F23 + F33 = 0
    N = pts1_norm.shape[0]
    A = np.zeros((N, 9))
    for i in range(N):
        u, v = pts1_norm[i, 0], pts1_norm[i, 1]
        up, vp = pts2_norm[i, 0], pts2_norm[i, 1]
        A[i] = [u*up, v*up, up, u*vp, v*vp, vp, u, v, 1]
    
    # 3. SVD求解最小奇异值对应的向量
    U, S, Vt = np.linalg.svd(A)
    F_norm = Vt[-1].reshape(3, 3)
    
    # 4. 强制秩2约束（基础矩阵必须是秩2的）
    U_f, S_f, Vt_f = np.linalg.svd(F_norm)
    S_f[-1] = 0  # 将最小奇异值置零
    F_norm = U_f @ np.diag(S_f) @ Vt_f
    
    # 5. 去归一化
    F = T2.T @ F_norm @ T1
    
    return F

def normalize_points(pts):
    """将点归一化到均值为0、平均距离为sqrt(2)"""
    centroid = pts.mean(axis=0)
    shifted = pts - centroid
    mean_dist = np.mean(np.sqrt(np.sum(shifted**2, axis=1)))
    scale = np.sqrt(2) / mean_dist
    T = np.array([
        [scale, 0, -scale * centroid[0]],
        [0, scale, -scale * centroid[1]],
        [0, 0, 1]
    ])
    pts_h = np.hstack([pts, np.ones((len(pts), 1))])
    pts_norm = (T @ pts_h.T).T[:, :2]
    return pts_norm, T

从本质矩阵恢复位姿

def recover_pose_from_essential(E, pts1, pts2, K):
    """从本质矩阵恢复相对位姿[R|t]"""
    U, S, Vt = np.linalg.svd(E)
    
    if np.linalg.det(U @ Vt) < 0:
        Vt = -Vt
    
    # W矩阵：将SVD结果转换为旋转矩阵
    W = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]])
    
    # 4种可能的解
    R1 = U @ W @ Vt
    R2 = U @ W.T @ Vt
    t = U[:, 2]  # 平移向量（第三列）
    
    candidates = [
        (R1, t),
        (R2, t),
        (R1, -t),
        (R2, -t),
    ]
    
    # 使用三角测量判断正确解：3D点必须在两个相机前方（正深度）
    for R, t in candidates:
        P1 = K @ np.hstack([np.eye(3), np.zeros((3, 1))])
        P2 = K @ np.hstack([R, t.reshape(-1, 1)])
        
        points_3d = cv2.triangulatePoints(P1, P2, pts1.T, pts2.T)
        points_3d = points_3d[:3] / points_3d[3]
        
        # 检查3D点是否在两个相机前方
        depth1 = points_3d[2]
        depth2 = (R[2] @ points_3d + t[2])
        
        if np.all(depth1 > 0) and np.all(depth2 > 0):
            return R, t, points_3d
    
    return None  # 无合理解

6. 光束平差法（Bundle Adjustment）——全局优化所有参数

为什么需要BA

相机标定和位姿估计是链式传播误差的过程：每个步骤的小误差累积到最后变成大偏差。BA的思想是：把所有的3D点、相机位姿、相机内参放在一起，联合最小化所有图像上的重投影误差。这相当于你同时优化整个测量系统的所有参数，而不是一个个单独标定。

BA的数学形式

其中：$C_j$是第$j$个相机的位姿（外参），$X_i$是第$i$个3D路标点，$\pi$是投影函数（包括内参），$x_{ij}$是2D观测点，$\rho$是鲁棒核函数（降低外点影响，如Huber Loss）。

BA的稀疏性——为什么能高效求解

BA问题的雅可比矩阵是高度稀疏的——每个3D点只出现在少数几张图像中，每张图像的相机参数只与出现在该图像中的点相关。利用这种稀疏结构，BA可以在几秒内优化数千张图像和数百万个3D点。

import g2o  # 图优化库

class BundleAdjustment:
    """基于g2o的稀疏BA求解器"""
    
    def __init__(self, K, max_iterations=100):
        self.K = K  # 内参矩阵（标定后固定）
        self.optimizer = g2o.SparseOptimizer()
        solver = g2o.BlockSolverSE3(g2o.LinearSolverEigen())
        self.optimizer.set_algorithm(g2o.OptimizationAlgorithmLevenberg(solver))
    
    def add_camera_pose(self, pose_id, estimate=None):
        vertex = g2o.VertexSE3Expmap()
        vertex.set_id(pose_id)
        if estimate is not None:
            vertex.set_estimate(estimate)
        self.optimizer.add_vertex(vertex)
    
    def add_landmark(self, point_id, estimate=None):
        vertex = g2o.VertexPointXYZ()
        vertex.set_id(point_id + 10000)  # 偏移避免ID冲突
        vertex.set_marginalized(True)
        if estimate is not None:
            vertex.set_estimate(estimate)
        self.optimizer.add_vertex(vertex)
    
    def add_observation(self, pose_id, point_id, uv_measurement, information=None):
        edge = g2o.EdgeProjectXYZ2UV()
        edge.set_vertex(0, self.optimizer.vertex(point_id + 10000))
        edge.set_vertex(1, self.optimizer.vertex(pose_id))
        edge.set_measurement(uv_measurement)
        edge.set_information(information or np.eye(2))
        edge.fx = self.K[0, 0]
        edge.fy = self.K[1, 1]
        edge.cx = self.K[0, 2]
        edge.cy = self.K[1, 2]
        
        # Huber核函数：降低外点影响
        kernel = g2o.RobustKernelHuber()
        edge.set_robust_kernel(kernel)
        
        self.optimizer.add_edge(edge)
    
    def optimize(self, iterations=None):
        self.optimizer.initialize_optimization()
        self.optimizer.optimize(iterations or self.max_iterations)
        
        # 提取优化后的参数
        optimized_poses = {}
        for v in self.optimizer.vertices().values():
            if isinstance(v, g2o.VertexSE3Expmap):
                optimized_poses[v.id()] = v.estimate()
        
        return optimized_poses

7. 稠密三维重建——从稀疏点到完整模型

稀疏重建 vs 稠密重建

你之前学的内容大多停留在稀疏特征点（几百个点）。但机器人抓取需要知道物体的完整3D形状（数十万个点）。稠密重建解决的就是"从几张RGB图重建完整3D模型"。

多视角立体匹配（MVS）

def multi_view_stereo(images, camera_poses, K):
    """多视角稠密重建——逐像素深度融合"""
    H, W = images[0].shape[:2]
    
    # 1. 选择参考图像和源图像
    ref_img_idx = len(images) // 2  # 中心图像作为参考
    ref_img = images[ref_img_idx]
    ref_pose = camera_poses[ref_img_idx]
    
    # 2. 对每个像素，在其他视图中搜索最佳深度
    depth_map = np.zeros((H, W))
    confidence_map = np.zeros((H, W))
    
    for v in range(H):
        for u in range(W):
            # 对当前像素，在指定深度范围内搜索
            # 通过计算不同深度下的光度一致性找到最优深度
            best_depth = None
            best_cost = float('inf')
            
            for depth in np.linspace(0.1, 5.0, 256):
                # 将参考像素反投影到3D
                ray = np.linalg.inv(K) @ np.array([u, v, 1.0])
                point_3d = ray * depth
                
                # 投影到源图像
                total_cost = 0
                valid_views = 0
                for src_idx, src_img in enumerate(images):
                    if src_idx == ref_img_idx:
                        continue
                    
                    # 投影点3D → 源图像uv
                    src_pose = camera_poses[src_idx]
                    point_src = src_pose[:3, :3] @ point_3d + src_pose[:3, 3]
                    uv_src = (K @ point_src)
                    u_src, v_src = int(uv_src[0]/uv_src[2]), int(uv_src[1]/uv_src[2])
                    
                    if 0 <= u_src < W and 0 <= v_src < H:
                        # 光度一致性：参考像素 vs 源像素的差异
                        diff = ref_img[v, u] - src_img[v_src, u_src]
                        # NCC (归一化互相关) 作为代价
                        cost = np.mean(diff ** 2)
                        total_cost += cost
                        valid_views += 1
                
                if valid_views >= 2 and total_cost / valid_views < best_cost:
                    best_cost = total_cost / valid_views
                    best_depth = depth
            
            depth_map[v, u] = best_depth if best_depth else 0
            confidence_map[v, u] = 1.0 / (1.0 + best_cost)
    
    return depth_map, confidence_map

点云后处理——从深度图到干净3D模型

def depth_map_to_pointcloud(depth_map, K, min_confidence=0.5):
    """深度图 + 内参 → 3D点云"""
    H, W = depth_map.shape
    u, v = np.meshgrid(np.arange(W), np.arange(H))
    
    # 反投影到3D
    fx, fy, cx, cy = K[0,0], K[1,1], K[0,2], K[1,2]
    Z = depth_map
    X = (u - cx) * Z / fx
    Y = (v - cy) * Z / fy
    
    # 过滤无效点和低置信度点
    valid = (Z > 0) & (confidence_map > min_confidence)
    points = np.stack([X[valid], Y[valid], Z[valid]], axis=-1)
    
    # 统计离群点移除（SOR滤波）
    import open3d as o3d
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(points)
    pcd, _ = pcd.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0)
    
    return np.asarray(pcd.points)