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

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集成后可实现实时视觉定位建图。