ORB-SLAM3视觉SLAM原理与ROS2实战

一、引言

SLAM(同时定位与建图)是机器人和AR的核心。ORB-SLAM3是视觉SLAM的SOTA方案，支持单目/双目/RGB-D+IMU融合，能在动态环境中鲁棒运行。本文将解析其三大线程并给出ROS2集成。

二、三线程架构

┌──────────────┐  帧  ┌──────────────┐  关键帧  ┌──────────────┐
│  Tracking    │─────→│ Local Mapping│────────→│ Loop Closing │
│  (实时跟踪)  │      │ (局部建图)   │         │ (回环检测)    │
│              │←─────│              │         │              │
│ ORB提取+匹配 │      │ BA优化       │         │ 词袋检测      │
│ 位姿估计     │      │ 新地图点生成 │         │ 全局BA        │
│ 重定位       │      │ 冗余帧剔除   │         │ 位姿图优化    │
└──────────────┘      └──────────────┘         └──────────────┘

三、ORB特征提取

import cv2
import numpy as np

class ORBExtractor:
    def __init__(self, n_features=1000, scale_factor=1.2, n_levels=8):
        self.orb = cv2.ORB_create(
            nfeatures=n_features,
            scaleFactor=scale_factor,
            nlevels=n_levels,
            edgeThreshold=31,
            patchSize=31,
            fastThreshold=20
        )
    
    def extract(self, image):
        """提取ORB特征点+描述子"""
        keypoints, descriptors = self.orb.detectAndCompute(image, None)
        
        # 四叉树均匀分布（ORB-SLAM独有，避免特征点聚集）
        keypoints = self._distribute_quadtree(keypoints, image.shape)
        
        return keypoints, descriptors
    
    def _distribute_quadtree(self, kps, shape, min_nodes=4):
        """四叉树均匀分布特征点"""
        h, w = shape[:2]
        nodes = [Node(0, 0, w, h, kps)]
        
        while len(nodes) < min_nodes:
            new_nodes = []
            for node in nodes:
                if len(node.kps) <= 1:
                    new_nodes.append(node)
                else:
                    n1, n2, n3, n4 = node.split()
                    new_nodes.extend([n1, n2, n3, n4])
            nodes = new_nodes
        
        # 每个节点保留最强响应点
        result = []
        for node in nodes:
            if node.kps:
                best = max(node.kps, key=lambda k: k.response)
                result.append(best)
        return result

四、PnP位姿估计

def estimate_pose_ransac(pts_3d, pts_2d, K):
    """EPnP + RANSAC: 3D-2D匹配估计相机位姿"""
    # 降级方案1: 用3D-2D PnP
    success, rvec, tvec, inliers = cv2.solvePnPRansac(
        pts_3d, pts_2d, K, None,
        iterationsCount=100,
        reprojectionError=4.0,
        confidence=0.99,
        flags=cv2.SOLVEPNP_EPNP
    )
    
    if success:
        R, _ = cv2.Rodrigues(rvec)
        T = np.eye(4); T[:3, :3] = R; T[:3, 3] = tvec.flatten()
        return T, len(inliers)
    
    # 降级方案2: 单应矩阵(平面场景)
    if len(pts_2d) >= 4:
        H, mask = cv2.findHomography(pts_3d[:, :2], pts_2d, cv2.RANSAC, 4.0)
        if H is not None:
            # 从H分解位姿
            solutions = cv2.decomposeHomographyMat(H, K)
            return solutions[0], mask.sum()
    
    return None, 0

五、局部BA优化

import g2o

class BundleAdjustment:
    def optimize(self, keyframes, mappoints, fixed_kf_id):
        optimizer = g2o.SparseOptimizer()
        solver = g2o.BlockSolverSE3(g2o.LinearSolverEigenSE3())
        optimizer.set_algorithm(g2o.OptimizationAlgorithmLevenberg(solver))
        
        # 添加相机顶点
        kf_vertices = {}
        for kf in keyframes:
            v = g2o.VertexSE3Expmap()
            v.set_id(kf.id)
            v.set_estimate(g2o.SE3Quat(kf.R, kf.t))
            v.set_fixed(kf.id == fixed_kf_id)
            optimizer.add_vertex(v)
            kf_vertices[kf.id] = v
        
        # 添加地图点顶点
        for mp in mappoints:
            v = g2o.VertexPointXYZ()
            v.set_id(mp.id + 10000)
            v.set_estimate(mp.position)
            v.set_marginalized(True)
            optimizer.add_vertex(v)
        
        # 添加重投影边
        for kf in keyframes:
            for mp_id, (kp, _) in kf.observations.items():
                if mp_id in mappoints:
                    edge = g2o.EdgeSE3ProjectXYZ()
                    edge.set_vertex(0, kf_vertices[kf.id])
                    edge.set_vertex(1, optimizer.vertex(mp_id + 10000))
                    edge.set_measurement(kp.pt)
                    edge.set_information(np.eye(2))
                    edge.set_parameter_id(0, K)  # 内参
                    optimizer.add_edge(edge)
        
        optimizer.initialize_optimization()
        optimizer.optimize(10)
        
        # 更新位姿和地图点
        for kf in keyframes:
            kf.set_pose(kf_vertices[kf.id].estimate())

六、回环检测(DBoW2词袋)

# 词袋模型：ORB描述子→视觉词汇→BoW向量
# 相似度>阈值 → 回环候选 → Sim3验证 → 位姿图优化

class LoopDetector:
    def __init__(self, vocabulary_path):
        self.vocab = cv2.BOWKMeansTrainer.load(vocabulary_path)
    
    def detect(self, current_frame, all_keyframes, min_score=0.3):
        # 编码当前帧
        current_bow = self._compute_bow(current_frame.descriptors)
        
        # 与所有历史关键帧比较
        candidates = []
        for kf in all_keyframes:
            score = self._bow_similarity(current_bow, kf.bow)
            if score > min_score:
                candidates.append((kf, score))
        
        # 组一致性检查(连续3帧检测到同组)
        groups = self._group_consistency(candidates)
        
        # 几何验证(Sim3)
        for group in groups:
            best_kf = max(group, key=lambda x: x[1])[0]
            # 3D-3D RANSAC验证
            if self._geometric_verification(current_frame, best_kf):
                return best_kf
        
        return None

七、ROS2集成

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from geometry_msgs.msg import PoseStamped

class SLAMNode(Node):
    def __init__(self):
        super().__init__('orb_slam3')
        self.slam = ORBSLAM3("/path/to/vocabulary.txt", "/path/to/config.yaml")
        
        self.create_subscription(Image, '/camera/color', self.rgb_callback, 10)
        self.create_subscription(Image, '/camera/depth', self.depth_callback, 10)
        self.pose_pub = self.create_publisher(PoseStamped, '/slam/pose', 10)
    
    def rgb_callback(self, msg):
        cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
        timestamp = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
        
        pose = self.slam.track_rgbd(cv_image, self.last_depth, timestamp)
        
        if pose is not None:
            pose_msg = PoseStamped()
            pose_msg.header = msg.header
            pose_msg.pose.position.x = pose[0, 3]
            pose_msg.pose.position.y = pose[1, 3]
            pose_msg.pose.position.z = pose[2, 3]
            self.pose_pub.publish(pose_msg)

八、总结

ORB-SLAM3三大创新：ORB特征提取+四叉树分布、三线程并行、DBoW2回环+全局BA。ROS2集成后可实现实时视觉定位建图。