第14课：姿态估计

📖 课程概述

本课学习姿态估计的核心原理与实现。我们将从数学基础出发，通过代码实践真正理解每一个算法的运作机制。

1. 姿态表示方法

姿态表示方法

import numpy as np
# 欧拉角 -> 旋转矩阵
from functools import reduce
def euler_to_rot(roll, pitch, yaw):
    Rx = np.array([[1,0,0],[0,np.cos(roll),-np.sin(roll)],[0,np.sin(roll),np.cos(roll)]])
    Ry = np.array([[np.cos(pitch),0,np.sin(pitch)],[0,1,0],[-np.sin(pitch),0,np.cos(pitch)]])
    Rz = np.array([[np.cos(yaw),-np.sin(yaw),0],[np.sin(yaw),np.cos(yaw),0],[0,0,1]])
    return Rz @ Ry @ Rx
R = euler_to_rot(0.1, 0.2, 0.3)
print(f'欧拉角(0.1,0.2,0.3) -> 旋转矩阵:\n{np.round(R,4)}')
# 四元数
from scipy.spatial.transform import Rotation
q = Rotation.from_euler('xyz', [0.1,0.2,0.3]).as_quat()
print(f'四元数: {q}')

2. PnP问题

PnP问题

import cv2, numpy as np
# PnP: 已知3D点和对应2D点，求相机位姿
obj_pts = np.array([[0,0,0],[1,0,0],[0,1,0],[1,1,0],[0.5,0.5,1]], dtype=np.float32)
K = np.array([[500,0,320],[0,500,240],[0,0,1]], dtype=np.float32)
dist = np.zeros(5)
rvec_true = np.array([0.1, 0.2, 0.05], dtype=np.float32)
tvec_true = np.array([0, 0, 5], dtype=np.float32)
img_pts, _ = cv2.projectPoints(obj_pts, rvec_true, tvec_true, K, dist)
success, rvec, tvec = cv2.solvePnP(obj_pts, img_pts, K, dist, flags=cv2.SOLVEPNP_EPNP)
if success:
    print(f'PnP求解: rvec={rvec.ravel()}, tvec={tvec.ravel()}')

3. solvePnP实现

solvePnP实现

import cv2, numpy as np
obj_pts = np.array([[0,0,0],[1,0,0],[0,1,0],[0,0,1],[1,1,0]], dtype=np.float32)
K = np.array([[500,0,320],[0,500,240],[0,0,1]], dtype=np.float32)
dist = np.zeros(5)
rvec_true = np.array([0.1, -0.1, 0.3], dtype=np.float32)
tvec_true = np.array([0.5, 0.3, 5], dtype=np.float32)
img_pts, _ = cv2.projectPoints(obj_pts, rvec_true, tvec_true, K, dist)
# 不同PnP方法
for method, flag in [('EPnP', cv2.SOLVEPNP_EPNP), ('Iterative', cv2.SOLVEPNP_ITERATIVE)]:
    try:
        ok, rv, tv = cv2.solvePnP(obj_pts, img_pts, K, dist, flags=flag)
        if ok: print(f'{method}: 成功')
    except: print(f'{method}: 失败')

4. 姿态优化

姿态优化

import cv2, numpy as np
# Bundle Adjustment简化版
print('姿态优化方法:')
print('1. Levenberg-Marquardt非线性优化')
print('2. 增量式SfM')
print('3. 光束法平差(Bundle Adjustment)')
print('OpenCV: solvePnPRefineLM()')

5. AR应用基础

AR应用基础

import cv2, numpy as np
img = np.zeros((480,640,3), dtype=np.uint8)
img[:] = [40,40,40]
K = np.array([[500,0,320],[0,500,240],[0,0,1]], dtype=np.float32)
dist = np.zeros(5)
obj_pts = np.array([[0,0,0],[1,0,0],[1,1,0],[0,1,0]], dtype=np.float32)
img_pts = np.array([[280,180],[380,180],[380,280],[280,280]], dtype=np.float32)
success, rvec, tvec = cv2.solvePnP(obj_pts, img_pts, K, dist)
if success:
    axis = np.float32([[1,0,0],[0,1,0],[0,0,1]]).reshape(-1,3)
    axis_pts, _ = cv2.projectPoints(axis, rvec, tvec, K, dist)
    origin = tuple(img_pts[0].astype(int))
    colors = [(0,0,255),(0,255,0),(255,0,0)]
    for i, (pt, color) in enumerate(zip(axis_pts, colors)):
        cv2.arrowedLine(img, origin, tuple(pt.ravel().astype(int)), color, 3)
    print('AR坐标轴绘制完成')
cv2.imwrite('/var/www/ttl/cv/l14_ar.png', img)

6. 手势姿态估计

手势姿态估计

print('手势姿态估计流程:')
print('1. 手部检测 (YOLO/MediaPipe)')
print('2. 关键点检测 (21个手部关键点)')
print('3. 3D姿态恢复')
print('4. 手势分类')
print('MediaPipe Hands: 21关键点实时估计')

7. 姿态估计实战

姿态估计实战

import cv2, numpy as np
# 实战：立方体投影
img = np.zeros((480,640,3), dtype=np.uint8)
img[:] = [40,40,40]
K = np.array([[500,0,320],[0,500,240],[0,0,1]], dtype=np.float32)
dist = np.zeros(5)
cube_3d = np.float32([[0,0,0],[1,0,0],[1,1,0],[0,1,0],[0,0,1],[1,0,1],[1,1,1],[0,1,1]])
rvec = np.array([0.3, 0.5, 0.1], dtype=np.float32)
tvec = np.array([0, 0, 4], dtype=np.float32)
pts2d, _ = cv2.projectPoints(cube_3d, rvec, tvec, K, dist)
pts = pts2d.reshape(-1,2).astype(int)
# 画边
edges = [(0,1),(1,2),(2,3),(3,0),(4,5),(5,6),(6,7),(7,4),(0,4),(1,5),(2,6),(3,7)]
for i,j in edges:
    cv2.line(img, tuple(pts[i]), tuple(pts[j]), (0,255,0), 2)
cv2.imwrite('/var/www/ttl/cv/l14_cube.png', img)
print('立方体投影完成')

📊 方法对比总结

🔬 姿态估计进阶内容

本节深入探讨姿态估计的进阶话题和工程实践中的关键考虑。

# 性能基准测试模板
import time, numpy as np

def benchmark(func, *args, n=100, **kwargs):
    times = []
    for _ in range(n):
        t0 = time.time()
        result = func(*args, **kwargs)
        times.append(time.time() - t0)
    return np.mean(times)*1000, np.std(times)*1000

print(f"性能基准测试: 多次运行取均值和标准差")
print(f"注意: 首次运行可能较慢(JIT编译/缓存加载)")

📊 性能优化策略

💻 姿态估计完整Pipeline代码

以下是一个完整的姿态估计处理管道，从数据准备到结果评估，包含所有关键步骤和参数调优建议：

import cv2
import numpy as np
import time

class 姿态估计Pipeline:
    """完整的姿态估计处理管道"""
    
    def __init__(self, params=None):
        self.params = params or {}
        self.results = {}
    
    def preprocess(self, image):
        """预处理步骤"""
        # 1. 去噪
        if self.params.get('denoise', True):
            image = cv2.GaussianBlur(image, (3, 3), 1.0)
        
        # 2. 对比度增强
        if self.params.get('enhance', False):
            if len(image.shape) == 2:
                image = cv2.equalizeHist(image)
            else:
                lab = cv2.cvtColor(image, cv2.COLOR_BGR2LAB)
                lab[:,:,0] = cv2.equalizeHist(lab[:,:,0])
                image = cv2.cvtColor(lab, cv2.COLOR_LAB2BGR)
        
        # 3. 尺寸调整
        max_size = self.params.get('max_size', 1024)
        h, w = image.shape[:2]
        if max(h, w) > max_size:
            scale = max_size / max(h, w)
            image = cv2.resize(image, (int(w*scale), int(h*scale)))
        
        return image
    
    def process(self, image):
        """核心处理步骤"""
        t0 = time.time()
        preprocessed = self.preprocess(image)
        t1 = time.time()
        
        # 主处理逻辑
        result = self._main_process(preprocessed)
        t2 = time.time()
        
        self.results['timing'] = {
            'preprocess': (t1-t0)*1000,
            'process': (t2-t1)*1000,
            'total': (t2-t0)*1000
        }
        return result
    
    def _main_process(self, image):
        """主处理逻辑（子类可重写）"""
        return image
    
    def evaluate(self, result, ground_truth=None):
        """评估处理结果"""
        metrics = {}
        if ground_truth is not None:
            if len(result.shape) == 2:
                mse = np.mean((result.astype(float) - ground_truth.astype(float))**2)
                metrics['mse'] = mse
                metrics['psnr'] = 10 * np.log10(255**2 / (mse + 1e-10))
        metrics['timing'] = self.results.get('timing', {})
        return metrics
    
    def visualize(self, image, result):
        """可视化对比"""
        if len(image.shape) == 2:
            image = cv2.cvtColor(image, cv2.COLOR_GRAY2BGR)
        if len(result.shape) == 2:
            result = cv2.cvtColor(result, cv2.COLOR_GRAY2BGR)
        h1, w1 = image.shape[:2]
        h2, w2 = result.shape[:2]
        h, w = max(h1,h2), w1+w2+10
        vis = np.zeros((h, w, 3), dtype=np.uint8)
        vis[:h1, :w1] = image
        vis[:h2, w1+10:] = result
        cv2.putText(vis, 'Input', (10, 25), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0,255,0), 2)
        cv2.putText(vis, 'Output', (w1+20, 25), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0,255,0), 2)
        return vis


# 使用示例
pipeline = 姿态估计Pipeline({'denoise': True, 'enhance': False, 'max_size': 512})
test_img = np.random.randint(0, 256, (256, 256, 3), dtype=np.uint8)
result = pipeline.process(test_img)
metrics = pipeline.evaluate(result)
print(f"处理时间: {metrics['timing'].get('total', 0):.1f}ms")

📊 完整评估指标

import numpy as np

class Metrics:
    @staticmethod
    def mse(img1, img2):
        return np.mean((img1.astype(float) - img2.astype(float))**2)
    
    @staticmethod
    def psnr(img1, img2, max_val=255):
        mse_val = Metrics.mse(img1, img2)
        if mse_val == 0: return float('inf')
        return 10 * np.log10(max_val**2 / mse_val)
    
    @staticmethod
    def iou(mask1, mask2):
        inter = np.logical_and(mask1 > 0, mask2 > 0).sum()
        union = np.logical_or(mask1 > 0, mask2 > 0).sum()
        return inter / (union + 1e-10)
    
    @staticmethod
    def f1_score(pred, gt, threshold=0.5):
        pred_bin = (pred > threshold)
        gt_bin = (gt > 0)
        tp = np.logical_and(pred_bin, gt_bin).sum()
        precision = tp / (pred_bin.sum() + 1e-10)
        recall = tp / (gt_bin.sum() + 1e-10)
        return 2 * precision * recall / (precision + recall + 1e-10)

# 示例
img1 = np.random.randint(0, 256, (100,100), dtype=np.uint8)
img2 = img1 + np.random.randint(-10, 10, (100,100))
img2 = np.clip(img2, 0, 255).astype(np.uint8)

print(f"MSE: {Metrics.mse(img1,img2):.2f}")
print(f"PSNR: {Metrics.psnr(img1,img2):.2f} dB")
print(f"说明: PSNR>30dB为良好, >40dB为优秀")

🎯 实际应用场景与案例

本课所学技术在实际工程中有广泛的应用。以下是几个典型场景：

# 工业质检示例
import cv2, numpy as np

# 模拟产品表面
product = np.full((200,200), 180, dtype=np.uint8)
# 添加划痕缺陷
cv2.line(product, (30,80), (170,120), 50, 2)

# 缺陷检测
blurred = cv2.GaussianBlur(product, (5,5), 1.0)
diff = cv2.absdiff(product, blurred)
_, defects = cv2.threshold(diff, 15, 255, cv2.THRESH_BINARY)
defect_area = np.count_nonzero(defects)
print(f"缺陷面积: {defect_area}px, 缺陷率: {defect_area/product.size:.4%}")
is_defective = defect_area > 50
print(f"检测结果: {'不合格' if is_defective else '合格'}")

# 车道检测示例
import cv2, numpy as np

road = np.zeros((480,640), dtype=np.uint8)
# 模拟道路
cv2.fillPoly(road, [np.array([[200,480],[440,480],[350,200],[290,200]])], 100)
# 模拟车道线
cv2.line(road, (280,480), (320,200), 255, 3)
cv2.line(road, (360,480), (320,200), 255, 3)

# 车道检测
edges = cv2.Canny(road, 50, 150)
lines = cv2.HoughLinesP(edges, 1, np.pi/180, 50, minLineLength=100, maxLineGap=50)
print(f"检测到车道线: {len(lines) if lines is not None else 0} 条")

# 简单医学分割
import cv2, numpy as np

# 模拟CT扫描
ct = np.zeros((256,256), dtype=np.uint8)
cv2.ellipse(ct, (128,128), (60,50), 0, 0, 360, 150, -1)  # 器官
cv2.circle(ct, (140,120), 15, 200, -1)  # 病灶

# Otsu分割
_, mask = cv2.threshold(ct, 0, 255, cv2.THRESH_BINARY + cv2.THRESH_OTSU)
contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
print(f"检测到 {len(contours)} 个区域")
for c in contours:
    area = cv2.contourArea(c)
    if area > 100:
        print(f"  区域面积: {area}, 疑似病灶: {area < 1000}")