【玩转VLA具身智能机械臂】（四）：点云分割与 OBB 夹取——从 PCA 失效到主动扫描

前言

前阵子休假两周小小停更了一下，现在开始干活了，请谅解，接下来恢复更新
最近 VLA（Vision-Language-Action，视觉-语言-动作）具身智能 发展迅速，从谷歌的 RT-2、OpenVLA 到 π0，大模型开始直接输出机器人动作，而这一切的物理载体正是机械臂
因此本系列将逐步上手 VLA 具身智能机械臂。在正式开始 VLA 部署之前，我们用两期分别讲解传统方法与深度学习方法在夹取上的应用，最后进入 OpenVLA 的部署
往期内容：
回到本期：上一期我们把腕部深度相机的点云接入 MoveIt 的 octomap，机械臂已能绕开障碍物到达目标点
但上一期结束时，机械臂只会到达目标点，并不知道桌上物体的形状，也不知道该从哪里下爪
本期从传统机械臂方案入手，讲解点云分割、抓取中的 6D 姿态、PCA 与 OBB，最终实现传统方案的夹取，案例如下图

文章目录

- 前言
- [0 修复](#0 修复)
- - - [0-1 末端夹爪修复](#0-1 末端夹爪修复)
    - [0-2 过滤节点](#0-2 过滤节点)
    - [0-3 Grasp Fix 插件](#0-3 Grasp Fix 插件)
    - - 插件安装
- [1 点云分割](#1 点云分割)
- - - [1-1 回顾](#1-1 回顾)
    - [1-2 常用方法介绍](#1-2 常用方法介绍)
    - [1-3 RANSAC 平面拟合](#1-3 RANSAC 平面拟合)
    - - [1-3-1 介绍](#1-3-1 介绍)
      - [1-3-2 平面 Π 的紧凑参数化](#1-3-2 平面 Π 的紧凑参数化)
      - [1-3-3 内点判据](#1-3-3 内点判据)
      - [1-3-4 样本抽取](#1-3-4 样本抽取)
      - [1-3-5 代码实现](#1-3-5 代码实现)
    - [1-4 顺序 RANSAC 与"主导度"](#1-4 顺序 RANSAC 与"主导度")
    - - [1-4-1 顺序 RANSAC](#1-4-1 顺序 RANSAC)
      - [1-4-2 内点主导度](#1-4-2 内点主导度)
    - [1-5 欧式聚类](#1-5 欧式聚类)
    - - [1-5-1 介绍](#1-5-1 介绍)
      - [1-5-2 数学定义](#1-5-2 数学定义)
      - [1-5-3 代码实现](#1-5-3 代码实现)
      - [1-5-4 筛选](#1-5-4 筛选)
      - [1-5-5 DBSCAN结果发布](#1-5-5 DBSCAN结果发布)
    - [1-6 平面持久化](#1-6 平面持久化)
    - - [1-6-1 背景](#1-6-1 背景)
      - [1-6-2 共面判定](#1-6-2 共面判定)
      - [1-6-3 平面硬化](#1-6-3 平面硬化)
    - [1-7 参数配置](#1-7 参数配置)
    - [1-8 运行测试](#1-8 运行测试)
- [2 Object Pose与PCA](#2 Object Pose与PCA)
- - - [2-1 介绍](#2-1 介绍)
    - [2-2 常用方法](#2-2 常用方法)
    - [2-3 PCA 数学原理](#2-3 PCA 数学原理)
    - - [2-3-1 介绍](#2-3-1 介绍)
      - [2-3-2 去中心化](#2-3-2 去中心化)
      - [2-3-3 协方差矩阵](#2-3-3 协方差矩阵)
      - [2-3-4 最大方差与特征分解](#2-3-4 最大方差与特征分解)
      - [2-3-5 代码实现：pcaObb](#2-3-5 代码实现：pcaObb)
    - [2-4 PCA的局限](#2-4 PCA的局限)
    - - [2-4-1 正方体PCA估算问题](#2-4-1 正方体PCA估算问题)
      - [2-4-2 斜视估计问题](#2-4-2 斜视估计问题)
      - [2-4-3 高度估计问题](#2-4-3 高度估计问题)
      - [2-4-4 最好的PCA方法](#2-4-4 最好的PCA方法)
- [3 OBB 数学原理](#3 OBB 数学原理)
- - - [3-1 介绍](#3-1 介绍)
    - [3-2 标准OBB](#3-2 标准OBB)
    - [3-3 扫描 OBB](#3-3 扫描 OBB)
    - [3-4 两者OBB合并](#3-4 两者OBB合并)
    - [3-5 粗估计与细估计](#3-5 粗估计与细估计)
    - [3-6 测试](#3-6 测试)
- [4 夹取位姿计算与状态机串连](#4 夹取位姿计算与状态机串连)
- - - [4-1 夹取位姿计算](#4-1 夹取位姿计算)
    - [4-2 跨帧跟踪](#4-2 跨帧跟踪)
    - - [4-2-1 关联](#4-2-1 关联)
      - [4-2-2 平滑](#4-2-2 平滑)
      - [4-2-3 质量只升不降](#4-2-3 质量只升不降)
    - [4-3 物体选择](#4-3 物体选择)
    - [4-4 状态机](#4-4 状态机)
    - [4-5 完整链路](#4-5 完整链路)
    - [4-6 完整测试](#4-6 完整测试)
- [附录工程外壳](#附录工程外壳)
- - - [附-1 接口定义](#附-1 接口定义)
    - [附-2 参数文件](#附-2 参数文件)
    - [附-3 构建文件](#附-3 构建文件)
- 总结

0 修复

进入正题之前，先修复上一期遗留的三个问题

0-1 末端夹爪修复

我们在上一期把 mimic 注释掉了，原因在于 gazebo_ros2_control 会把带 mimic 的关节以假名 panda_finger_joint2_mimic 上报，模型中没有这个名字，MoveIt 状态监视器因此每 10 ms 刷一条 Joint not found
- 根本原因在于真实的 Franka Emika Panda 手爪是单电机 + 平行连杆结构：手掌内只有一台电机，经丝杠驱动一组连杆，两指在机械上被耦合为一体
- 而仿真无法复现这种机械耦合
因此我们需要显式设置，给主动指补上 effort 状态接口（从动部分不需要），同时按 0-2 增加过滤节点消噪
完整源码（对应文件 config/panda_hand.ros2_control.xacro）：

xml 复制代码

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

    <xacro:macro name="panda_hand_ros2_control" params="name ros2_control_hardware_type">
        <ros2_control name="${name}" type="system">
            <hardware>
                <xacro:if value="${ros2_control_hardware_type == 'mock_components'}">
                    <plugin>mock_components/GenericSystem</plugin>
                </xacro:if>
                <xacro:if value="${ros2_control_hardware_type == 'isaac'}">
                    <plugin>topic_based_ros2_control/TopicBasedSystem</plugin>
                    <param name="joint_commands_topic">/isaac_joint_commands</param>
                    <param name="joint_states_topic">/isaac_joint_states</param>
                </xacro:if>
                <xacro:if value="${ros2_control_hardware_type == 'gazebo'}">
                    <plugin>gazebo_ros2_control/GazeboSystem</plugin>
                </xacro:if>
            </hardware>
            <joint name="panda_finger_joint1">
                <command_interface name="position" />
                <state_interface name="position">
                  <param name="initial_value">0.0</param>
                </state_interface>
                <state_interface name="velocity">
                    <param name="initial_value">0.0</param>
                </state_interface>
                <state_interface name="effort">
                    <param name="initial_value">0.0</param>
                </state_interface>
            </joint>
            <joint name="panda_finger_joint2">
                <param name="mimic">panda_finger_joint1</param>
                <param name="multiplier">1</param>
                <command_interface name="position" />
                <state_interface name="position">
                  <param name="initial_value">0.0</param>
                </state_interface>
                <state_interface name="velocity">
                    <param name="initial_value">0.0</param>
                </state_interface>
            </joint>
        </ros2_control>
    </xacro:macro>

</robot>

同时，夹爪的 position_controllers/GripperActionController 配置需一同修改（此处为保守设置，真正起作用的夹取是 0-3 的夹取插件）

yaml 复制代码

panda_hand_controller:
  ros__parameters:
    joint: panda_finger_joint1
    goal_tolerance: 0.001
    # 收紧之后**必须**开这个：40mm 的开口被 50mm 的方块挡着，物理上不可达，
    # 光收紧容差会让 CLOSE 永远不返回、每次超时。靠"停住"来判定"夹住了"。
    allow_stalling: true
    # 默认 0.001 太紧：软接触下手指会以 mm/s 级蠕变，阈值太低压根测不到停顿。
    stall_velocity_threshold: 0.005
    # 默认 1.0s；闭合本身还要时间，别把 CLOSE 拖过 grasp_execute 的
    # gripper_action_timeout（8s）。
    stall_timeout: 0.5

改完重启，执行以下命令打开夹爪：

bash 复制代码

ros2 action send_goal /panda_hand_controller/gripper_cmd control_msgs/action/GripperCommand "{command: {position: 0.04, max_effort: 20.0}}" 2>&1 | tail -6

此时 rviz2 与 gazebo 中的末端夹爪均可打开，且两指联动

0-2 过滤节点

除了 0-1 的接口修改，我们还需要一个过滤节点消除无效报错。这里采用一个简单的思路：不动模型，只在数据通路上加一级改名 ------ 新写 joint_states_filter 节点，订阅 /joint_states，去掉 _mimic 后缀后转发为 /joint_states_clean，由 move_group / robot_state_publisher 订阅
这样不影响 RViz 中的对称性：MoveIt 会自行计算 mimic，robot_state_publisher 拿到真名后也能算出右手指 TF，被丢弃的只是一个模型中不存在的假名
完整源码（对应文件 panda_gazebo_bringup/scripts/joint_states_filter.py）：

python 复制代码

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""joint_states_filter ------ 把 /joint_states 里 ros2_control 的假关节名
panda_finger_joint2_mimic 改回真实关节名 panda_finger_joint2，转发到
/joint_states_clean，给 move_group / robot_state_publisher 消费：
  1) 消除 MoveIt "Joint '..._mimic' not found in model" 每 10ms 刷屏
  2) 让 robot_state_publisher 能算出右手指 TF（rviz 对称）

原因：ros2_control 的 mimic 参数让 gazebo 端以 '<关节名>_mimic' 上报状态，
但 URDF 模型里该关节真名是 panda_finger_joint2 ------ 名字对不上才出噪音。
这里只改名字、不改数值，其余原样转发。"""
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState

class JointStateFilter(Node):
    def __init__(self):
        super().__init__('joint_states_filter')
        self.pub = self.create_publisher(JointState, 'joint_states_clean', 10)
        self.sub = self.create_subscription(JointState, 'joint_states', self._cb, 10)

    def _cb(self, msg):
        out = JointState()
        out.header = msg.header
        out.name = [n[:-6] if n.endswith('_mimic') else n for n in msg.name]
        out.position = list(msg.position)
        out.velocity = list(msg.velocity)
        out.effort = list(msg.effort)
        self.pub.publish(out)

def main(args=None):
    rclpy.init(args=args)
    node = JointStateFilter()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

随后把它加进 launch，并让 robot_state_publisher、move_group、grasp_obb、grasp_execute 统一订阅 /joint_states_clean：

python 复制代码

    # ---- 3) 发布 /robot_description + 关节 TF（spawn_entity 从这里取 URDF）----
    #   ros2_control 的 mimic 参数让 gazebo 端把右手指状态上报成假名
    #   panda_finger_joint2_mimic（模型里该关节真名是 panda_finger_joint2）。
    #   这里订阅过滤后的 /joint_states_clean：真名齐全 -> 能算右手指 TF（rviz 对称）。
    robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        name='robot_state_publisher',
        parameters=[moveit_config.robot_description, {'use_sim_time': True}],
        remappings=[('/joint_states', '/joint_states_clean')],
        output='screen',
    )

    joint_states_filter = Node(
        package='panda_gazebo_bringup',
        executable='joint_states_filter.py',
        name='joint_states_filter',
        parameters=[{'use_sim_time': True}],
        output='screen',
    )

需注意 CMakeLists.txt 中要将其 install 出去，否则 ros2 launch 找不到该可执行文件：

cmake 复制代码

# joint_states 过滤节点（去掉 ros2_control mimic 假关节名 _mimic 后缀）
install(PROGRAMS scripts/joint_states_filter.py
  DESTINATION lib/${PROJECT_NAME}
)

0-3 Grasp Fix 插件

除了 0-1 和 0-2 的修复，这里需要提一个 Gazebo 11 Classic 机械臂的已知 bug，根据官方 issue，有几种修复的解决方法：
- 调整接触参数 ：给夹爪 link 补上 <minDepth> 与 <maxVel>，把物体的摩擦系数调高、质量调小，必要时加阻尼
- 把位置接口换成 EffortJointInterface ：官方问答区的结论是，位置驱动会强行推进物理仿真，接触行为因此异常；改用 set force 才能得到正常的接触交互
- 碰撞体改用简单几何：把网格碰撞体换成包围盒等简单形状，避免持续的异常接触
- 用插件把物体固定到手上 ：gazebo_grasp_fix、IFRA_LinkAttacher、ARIAC 真空吸盘插件等
相关讨论：
- Friction insufficient in grasping simulation（原 answers.gazebosim.org/question/19287；Gazebo 官方问答区已于 2023 年停止服务并迁移至 Robotics Stack Exchange，旧链接会自动跳转）
- Panda Grasp fail in Gazebo 9.0 via ROS and Moveit（同样是 Franka Panda，与本项目最接近）
- The Gazebo grasp fix plugin（本文所用插件的官方 wiki，说明了这个问题本身与插件的判定逻辑）
本文采用插件的形式修复该 bug
- 理由：本文的主线是视觉与几何链路，夹取环节只需让物体稳定跟随；且此处手指按位置驱动，命令的"穿透量"不是力，靠调参无法根治
由于这里我们只学习传统机械臂夹取的基本操作，因此放弃原本力学驱动的方案

Grasp Fix插件检测到两指对物体施加方向相反、量级相当 的接触力并持续若干帧后，直接在手掌与物体之间建立一条关节，把物体刚性固定到手上，绕过摩擦求解；手张开时解除

说人话：它不再依赖摩擦系数，而是确认"两指确实同时从两侧顶住了物体"之后，把物体固定到手上

xml 复制代码

    <gazebo>
        <plugin name="gazebo_grasp_fix" filename="libgazebo_grasp_fix.so">
            <arm>
                <arm_name>panda</arm_name>
                <palm_link>panda_link7</palm_link>
                <gripper_link>panda_leftfinger</gripper_link>
                <gripper_link>panda_rightfinger</gripper_link>
            </arm>
            <forces_angle_tolerance>120</forces_angle_tolerance>
            <update_rate>100</update_rate>
            <max_grip_count>10</max_grip_count>
            <grip_count_threshold>3</grip_count_threshold>
            <release_tolerance>0.005</release_tolerance>
        </plugin>
    </gazebo>

需要注意的是 arm_name / palm_link / gripper_link 必须填 Gazebo 中的 link 名，而非 TF 名

插件安装

插件来自第三方仓库 gazebo-pkgs

bash 复制代码

#!/usr/bin/env bash

set -euo pipefail

SRC="${SRC:-你自己的安装目录}"
REPO="https://github.com/JenniferBuehler/gazebo-pkgs.git"
HERE="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"

if [ ! -d "$SRC/gazebo_grasp_plugin" ]; then
    echo ">>> 没有找到 gazebo-pkgs，clone 到 $SRC"
    git clone --depth 1 "$REPO" "$SRC"
fi

BUILD="$SRC/build"
mkdir -p "$BUILD/msgs"

echo ">>> protoc：把 grasp_event.proto 生成 C++"
protoc -I"$SRC/gazebo_grasp_plugin/msgs" --cpp_out="$BUILD/msgs" \
       "$SRC/gazebo_grasp_plugin/msgs/grasp_event.proto"

echo ">>> g++：编 libgazebo_grasp_fix.so"
g++ -std=c++11 -O2 -fPIC -shared -w \
    -I"$SRC/gazebo_grasp_plugin/include" \
    -I"$SRC/gazebo_version_helpers/include" \
    -I"$BUILD" \
    $(pkg-config --cflags gazebo) \
    "$SRC/gazebo_version_helpers/src/GazeboVersionHelpers.cpp" \
    "$SRC/gazebo_grasp_plugin/src/GazeboGraspFix.cpp" \
    "$SRC/gazebo_grasp_plugin/src/GazeboGraspGripper.cpp" \
    "$BUILD/msgs/grasp_event.pb.cc" \
    $(pkg-config --libs gazebo) -lprotobuf \
    -o "$BUILD/libgazebo_grasp_fix.so"

echo ">>> 拷回本包"
cp "$BUILD/libgazebo_grasp_fix.so" "$HERE/libgazebo_grasp_fix.so"
ls -l "$HERE/libgazebo_grasp_fix.so"

echo ">>> 完成。改了源码或换了 Gazebo 版本后重新跑本脚本即可。"

编译后还需让 gzserver 找到该 .so：Gazebo 的插件查找走 GAZEBO_PLUGIN_PATH，因此在 launch 中把它指向本包 install 后的 plugins/ 目录
- 不往系统目录拷贝：拷贝只在本机生效，换机器即静默失效；而插件加载失败时 Gazebo 仅打一行 ERROR 便继续运行，表现为偶发夹不起来，极难排查
- 用 SetEnvironmentVariable 而非在 shell 中 export：该 launch 要能被 ./1_depth_gazebo.sh 或 ros2 launch 直接起，不应依赖调用者环境

python 复制代码

    # ---- 抓取固定插件（gazebo_grasp_fix）----
    _plugin_dir = os.path.join(panda_bringup_dir, 'plugins')
    _prev_plugin_path = os.environ.get('GAZEBO_PLUGIN_PATH', '')
    gazebo_plugin_path = SetEnvironmentVariable(
        'GAZEBO_PLUGIN_PATH',
        _plugin_dir if not _prev_plugin_path else _plugin_dir + os.pathsep + _prev_plugin_path)

这一行 gazebo_plugin_path 在 LaunchDescription 列表中必须排在 gazebo 前面，环境变量按列表顺序生效：

python 复制代码

    return LaunchDescription([
        DeclareLaunchArgument(
            'world', default_value=default_world,
            description='Gazebo world 文件；默认 pick_place.world（抓取场景），'
                        '可用 world:= 覆盖'),
        DeclareLaunchArgument(
            'octomap_cloud_topic', default_value='/grasp_scene/environment_points',
            description='move_group octomap 输入点云话题；默认环境点云（scene_filter 随本 '
                        'launch 一同启动，物体簇不进 octomap）。回归老行为：'
                        'octomap_cloud_topic:=/camera/depth/points'),
        gazebo_plugin_path,      # 必须在 gazebo 之前 ------ 环境变量按列表顺序生效
        gazebo,
        robot_state_publisher,
        joint_states_filter,
        static_tf,

需要说明的是，后续进行 VLA 训练和学习时不再使用 Gazebo，因此该问题仅在本文中临时修复

1 点云分割

1-1 回顾

回顾一下我们上一期：把深度相机的整幅点云（/camera/depth/points）直接送入了 MoveIt 的 octomap 更新器，于是 octomap 不仅把桌子当成障碍物，还把桌上待夹取的物料一并当成了障碍物
- 此时若直接发布一个物料的夹取位姿，机械臂会直接表示无法进行有效规划，原因就是我们没有对点云进行分割
因此传统机械臂方案下有一个非常核心的前处理：点云分割

说人话：我们要把桌子和物料分开，桌子和地板交给 MoveIt 的 octomap 让它规划时躲开，物料则不要送进 octomap，交给 Object Pose 计算去求夹取位姿

1-2 常用方法介绍

点云分割是指按某种判据把一整幅点云划分为若干互不相交的子集，使每个子集在物理上对应一个独立的实体
- 它一般用于从原始深度数据里剥离出我们真正关心的部分，例如地面、桌面、货架与待操作的物体
- 在机械臂上，它的作用是把"环境"与"目标"分开：前者送入规划器当障碍物，后者送入位姿估计算法求夹取位姿
点云分割的做法很多，这里先把主流几类的依据与代价并列，再说明我们为何选择 RANSAC 平面拟合与欧式聚类的组合：

方法	判据	优点	局限
直通滤波 / 高度阈值	world 系某一个轴上的坐标区间	一行代码、零算力	需要"物体与环境在该轴上可分"，不可泛化
RANSAC 平面拟合	点到一个平面模型的距离	对噪声与离群点极稳，能处理平面占少数的场景	一次只能提一个平面，需顺序调用
区域生长	相邻点的法向夹角与曲率差	能分出曲面、不假设平面	对深度噪声敏感，需先估法向（噪声下法向最不可靠）
欧式聚类 / DBSCAN	点之间的三维距离与密度	不需要预设簇的个数，能自动判噪声	`eps` 对点云稀疏度敏感；簇之间离得近会粘连
法向 / 曲率分割	点的法向量场	能分平面与曲面	依赖法向估计质量，深度噪声下法向抖动大
深度学习分割	学习到的点云特征	泛化好、可端到端	要训练、要 GPU、要标注，且对没见过的物体不一定更稳

本文采用的方案是 RANSAC 平面拟合 + 欧式聚类 ：
- RANSAC 平面拟合用于分离桌子
- 欧式聚类用于聚合不同的物料然后进行编号

1-3 RANSAC 平面拟合

1-3-1 介绍

RANSAC 平面拟合是一种基于随机采样与一致性投票 的模型估计方法：从点云中反复随机抽取最小样本，由该样本确定一个候选平面，再用全体点到该平面的距离投票，内点数最多的候选平面即为结果
- 相对整体最小二乘，它的关键性质是对离群点不敏感：随机抽取的最小样本几乎不会同时命中三个离群点，少数正确的样本就足以定出平面；而本帧中属于桌面的点恰恰只占少数
- 名字是 RANdom SAmple Consensus 的缩写，直译是"随机采样一致"："随机采样"指每次只抽取一个最小样本，"一致"指用全体点对该样本投票，票数最高的候选即为结果 ------ 缩写的前三个词与最后一个词，正对应上面定义里的两件事

1-3-2 平面 Π 的紧凑参数化

通常来说，一个平面 Π \Pi Π 的紧凑参数化是"法向 + 截距"：
Π : n ⋅ p + c = 0 , ∥ n ∥ = 1 \Pi:\ \mathbf{n}\cdot\mathbf{p} + c = 0,\qquad \|\mathbf{n}\| = 1 Π: n⋅p+c=0,∥n∥=1
式中 p = ( x , y , z ) T \mathbf{p} = (x, y, z)^{\mathsf T} p=(x,y,z)T 是空间中任意一点的坐标， n \mathbf{n} n 是平面的单位法向量（垂直于平面、指向平面的一侧）， c c c 是截距。该方程的含义是：空间中所有满足它的点 p \mathbf{p} p 恰好构成平面 Π \Pi Π
- 它为什么能表示"平"： n ⋅ p \mathbf{n}\cdot\mathbf{p} n⋅p 是 p \mathbf{p} p 在法向上的投影长度，方程要求所有点的这个投影都相等，即所有点落在同一个"等投影"的位置上 ------ 这正是"共面"的代数形式
- 取 p = 0 \mathbf{p} = \mathbf{0} p=0 代入，得 d ( 0 ) = c d(\mathbf{0}) = c d(0)=c。配合 ∥ n ∥ = 1 \|\mathbf{n}\| = 1 ∥n∥=1，可知原点到平面的距离恰为 ∣ c ∣ |c| ∣c∣，截距因此有直接的长度量纲
单位约束把 n \mathbf{n} n 的独立分量从 3 个降到 2 个，因此平面共有 3 个自由度：法向的 2 个方向角与 1 个截距。这个数字在后面确定 RANSAC 的最小样本数时会直接用到

说人话：一个平面就说清两件事 ------ 朝哪个方向、离原点多远。方向由法向量 n \mathbf{n} n 定，位置由截距 c c c 定。方向是球面上的一个朝向、要 2 个数，位置 1 个数，合起来正好 3 个自由度

1-3-3 内点判据

由该参数化，任意点 p \mathbf{p} p 到平面的有符号距离就是一次内积，取绝对值即几何距离，于是内点判据为

d ( p ) = n ⋅ p + c , ∣ d ( p ) ∣ < τ d(\mathbf{p}) = \mathbf{n}\cdot\mathbf{p} + c,\qquad |d(\mathbf{p})| < \tau d(p)=n⋅p+c,∣d(p)∣<τ
- τ \tau τ 由深度噪声决定。深度相机在 0.5 0.5 0.5 m 量程上的测量噪声近似零均值高斯，标准差 σ \sigma σ 在毫米量级，取 τ ≈ 3 σ \tau \approx 3\sigma τ≈3σ 可保留约 99.7 % 99.7\% 99.7% 的真实平面点
- 本工作区取 plane_dist_thresh = 0.008 m，与实测深度噪声同量级
通常来说，这类问题的求解可能会使用最小二乘，但问题随之而来：最小二乘把平面估计写成残差平方和的最小化

n ^ , c ^ = arg ⁡ min ⁡ n , c ∑ i = 1 N ( n ⋅ p i + c ) 2 \hat{\mathbf{n}}, \hat{c} = \arg\min_{\mathbf{n}, c} \sum_{i=1}^{N} \left(\mathbf{n}\cdot\mathbf{p}_i + c\right)^2 n^,c^=argn,cmini=1∑N(n⋅pi+c)2
- 平方和里每个点的权重随残差二次增长且没有上界，于是一个离得远的点就能压过成百上千个贴面的点
- 本帧的实际情况恰好最坏：目标平面只占少数点，物体、地面、墙面与远处噪点才是多数，而且它们离目标平面都很远

1-3-4 样本抽取

RANSAC 的出发点相反：不要求全部点参与拟合，而是反复抽取一个最小样本、再由全体点投票
- 平面的自由度为 3，故最小样本为 3 个点；三点定平面的叉积公式与水平约束为
  n = ( p 2 − p 1 ) × ( p 3 − p 1 ) ∥ ( p 2 − p 1 ) × ( p 3 − p 1 ) ∥ , c = − n ⋅ p 1 , n z ≥ cos ⁡ ( θ max ⁡ ) \mathbf{n} = \frac{(\mathbf{p}_2-\mathbf{p}_1)\times(\mathbf{p}_3-\mathbf{p}_1)}{\|(\mathbf{p}_2-\mathbf{p}_1)\times(\mathbf{p}_3-\mathbf{p}_1)\|},\qquad c = -\mathbf{n}\cdot\mathbf{p}1,\qquad n_z \ge \cos(\theta{\max}) n=∥(p2−p1)×(p3−p1)∥(p2−p1)×(p3−p1),c=−n⋅p1,nz≥cos(θmax)
- 本工作区取 plane_max_tilt_deg = 10.0，对应 cos ⁡ 10 ° ≈ 0.985 \cos 10° \approx 0.985 cos10°≈0.985。缺少该约束时，墙面、斜靠箱体的侧面以及物体的侧面都会拟合出方向任意的伪平面
采样次数由内点率决定。记内点率为 w w w，则 3 个抽样点同时为内点的概率为 w 3 w^3 w3，单次抽样失败的概率为 1 − w 3 1-w^3 1−w3
- N N N 次抽样全部失败 的概率为 ( 1 − w 3 ) N (1-w^3)^N (1−w3)N。要求"至少成功一次"的概率不低于 P P P，即 ( 1 − w 3 ) N ≤ 1 − P (1-w^3)^N \le 1-P (1−w3)N≤1−P，两边取对数得
  N ≥ log ⁡ ( 1 − P ) log ⁡ ( 1 − w 3 ) N \ge \frac{\log(1-P)}{\log(1-w^3)} N≥log(1−w3)log(1−P)
- 代入 w = 0.5 w = 0.5 w=0.5、 P = 0.99 P = 0.99 P=0.99，得 N ≥ 35 N \ge 35 N≥35；本工作区取 plane_ransac_iters = 200，对应可容忍的内点率下限约 w ≈ 0.28 w \approx 0.28 w≈0.28
- 该下界对 w w w 极其敏感： w w w 降到 0.2 0.2 0.2 时需 N ≈ 574 N \approx 574 N≈574 次。这解释了下面为何要顺序提取 ------ 每提取一个平面就移除其内点，剩余点集的内点率随之被抬高

说人话：每次随机抓三个点定出一个面，再数一数整片点云里有多少点贴在这个面上，贴得越多说明这个面越可能是真的。重复 200 次，留下票数最高的那个。

1-3-5 代码实现

对应上面的公式，主估计的代码如下。三个 continue 分别对应"三点共线""法向反向""倾角超限"这三种作废情形：

python 复制代码

def ransac_plane(pts, rng, dist_thresh, iters, cos_max_tilt):
    """三点 RANSAC 拟合水平面，返回 (n, c) 或 None。法向统一朝 +Z。"""
    if len(pts) < 3:
        return None
    best_cnt, best_plane = 0, None
    for _ in range(iters):
        i = rng.choice(len(pts), 3, replace=False)             # 最小样本 m = 3
        p0, p1, p2 = pts[i]
        n = np.cross(p1 - p0, p2 - p0)                         # 叉积定法向
        norm = np.linalg.norm(n)
        if norm < 1e-9:                                        # 三点近似共线，无法定面
            continue
        n = n / norm
        if n[2] < 0:
            n = -n                                             # 统一朝 +Z
        if n[2] < cos_max_tilt:                                # 不满足 n_z >= cos(theta_max)
            continue
        c = -float(n @ p0)
        cnt = int((np.abs(pts @ n + c) < dist_thresh).sum())   # 内点计数：|d| < tau
        if cnt > best_cnt:
            best_cnt, best_plane = cnt, (n, c)
    if best_plane is None:
        return None
    n, c = best_plane
    refined = refine_plane(pts[np.abs(pts @ n + c) < dist_thresh])
    return refined if refined[0][2] >= cos_max_tilt else (n, c)

RANSAC 给出的解仅由 3 个点确定，属于"粗略正确"，因此最后要用全部内点 做一次最小二乘精修
- 记内点集为 { p i } \{\mathbf{p}_i\} {pi}、质心为 μ \boldsymbol{\mu} μ，去质心后构造散布矩阵。该矩阵与去质心数据矩阵 X \mathbf{X} X 的关系是 M = X T X \mathbf{M} = \mathbf{X}^{\mathsf T}\mathbf{X} M=XTX
  X = $p 1 - μ , ... , p N - μ$ T , M = ∑ i ( p i − μ ) ( p i − μ ) T = X T X \mathbf{X} = \left $\\mathbf{p}_1 - \\boldsymbol{\\mu},\\ \\dots,\\ \\mathbf{p}_N - \\boldsymbol{\\mu}\\right$ ^{\mathsf T},\qquad \mathbf{M} = \sum_i (\mathbf{p}_i - \boldsymbol{\mu})(\mathbf{p}_i - \boldsymbol{\mu})^{\mathsf T} = \mathbf{X}^{\mathsf T}\mathbf{X} X= $p1-μ, ..., pN-μ$ T,M=i∑(pi−μ)(pi−μ)T=XTX
- 对 X \mathbf{X} X 做奇异值分解 X = U Σ V T \mathbf{X} = \mathbf{U}\boldsymbol{\Sigma}\mathbf{V}^{\mathsf T} X=UΣVT，则 M = V Σ 2 V T \mathbf{M} = \mathbf{V}\boldsymbol{\Sigma}^2\mathbf{V}^{\mathsf T} M=VΣ2VT，即 V \mathbf{V} V 的列就是 M \mathbf{M} M 的特征向量。最小奇异值对应的右奇异向量即法向 （对应最小散布方向）， μ \boldsymbol{\mu} μ 在法向上的投影即截距
- 这与 PCA 取最小主轴是同一个数学结构（见 2-3）。代码因此不需要显式构造 M \mathbf{M} M，直接对 X \mathbf{X} X 做 SVD 再取最后一行的右奇异向量：

python 复制代码

def refine_plane(pts_in):
    """对一组内点做 SVD 最小二乘拟合，返回 (n, c)，法向朝 +Z。"""
    centroid = pts_in.mean(axis=0)                                     # mu
    n = np.linalg.svd(pts_in - centroid, full_matrices=False)[2][2]    # V 的最后一列
    if n[2] < 0:
        n = -n
    return n, -float(n @ centroid)                                     # c = -n . mu

精修后必须重新校验水平约束 ：refine_plane 是无约束的 SVD 拟合，当内点集退化时（只有一小片桌角、点近似共线）法向可以转到任意方向，会把刚被倾角阈值过滤掉的斜面重新放回来。ransac_plane 末尾的 refined if refined[0][2] >= cos_max_tilt else (n, c) 就是这一道兜底
精修同样不能省。切分位置直接等于平面高度，三点粗解的误差会整段平移分割面；实测精修后桌面高度误差约 0.17 0.17 0.17 mm

1-4 顺序 RANSAC 与"主导度"

1-4-1 顺序 RANSAC

一帧场景点云中的水平面不止一个 ：桌面、地面，以及物体顶面与侧面在倾角阈值内偶然构成的伪平面。因此单次 RANSAC 不够，需要顺序提取
- 提取内点最多的平面，从点云中移除其内点，在剩余点上重复该过程，直至内点数低于下限或达到 plane_max_count = 3
- 每轮的内点判定都直接复用上面的 ∣ d ( p ) ∣ < τ |d(\mathbf{p})| < \tau ∣d(p)∣<τ，只是把点集换成上一轮剔除后的 remaining
- 移除内点同时抬高了剩余点的内点率 w w w，正对应上面 N N N 随 w w w 下降而迅速增大的结论

说人话：桌子和地板都需要提取

对应上面的流程，实现如下。注意 count 一律在本帧全云 pts_w 上统计，而不是在逐级缩小的 remaining 上：

python 复制代码

    def _extract_planes(self, pts_w):
        """顺序 RANSAC 提取最多 plane_max_count 个水平面，返回 [(n, c, 全云内点数)]。"""
        rng = self._rng
        max_pts = int(self.get_parameter('plane_fit_max_points').value)
        iters = int(self.get_parameter('plane_ransac_iters').value)
        dist_thresh = float(self.get_parameter('plane_dist_thresh').value)
        min_ratio = float(self.get_parameter('plane_min_inlier_ratio').value)
        cos_max_tilt = math.cos(
            math.radians(float(self.get_parameter('plane_max_tilt_deg').value)))

        remaining = pts_w
        planes = []
        for _ in range(int(self.get_parameter('plane_max_count').value)):
            if len(remaining) < 3:
                break
            if len(remaining) > max_pts:
                sub = remaining[rng.choice(len(remaining), max_pts, replace=False)]
            else:
                sub = remaining

            fit = ransac_plane(sub, rng, dist_thresh, iters, cos_max_tilt)
            if fit is None:
                break
            n, c = fit
            if int((np.abs(sub @ n + c) < dist_thresh).sum()) < min_ratio * len(sub):
                break                       # 剩下的点已凑不出像样的平面
            count = int((np.abs(pts_w @ n + c) < dist_thresh).sum())
            planes.append((n, c, count))
            remaining = remaining[np.abs(remaining @ n + c) >= dist_thresh]
        return planes

搜索在至多 plane_fit_max_points = 4000 点的随机子样本上做以省时，但内点数一律在本帧全云 上统计
- 本场景中相邻两个水平面的高度差远大于 τ \tau τ，内点几乎不重叠；口径又完全一致（都数本帧全云），因此绝对点数可以直接横向比较
- 若在逐级缩小的 remaining 上统计，后面的平面会因分母变小而虚高，主导度判据随之失真

1-4-2 内点主导度

提取完成后需要在若干平面中选出桌面。最容易误用的判据是按高度取最高平面
- 物体顶面构成的伪平面位于桌面之上，按高度会被误选为桌面
- 实测这些伪平面的倾角可达 11.3 ° 11.3° 11.3°，确实落在水平约束内，上面的倾角阈值无法排除
本文采用的判据是内点主导度 ：合格平面须达到本帧最大内点数的给定比例，再在合格面中取高度最高者
∣ I k ∣ max ⁡ j ∣ I j ∣ ≥ ρ \frac{|I_k|}{\max_j |I_j|} \ge \rho maxj∣Ij∣∣Ik∣≥ρ
- 本工作区取 plane_dominance = 0.70，等价于要求内点数达到最大值的 70 % 70\% 70%，即两者比值不超过 1.43 1.43 1.43
- 实测一帧的内点数：桌面 20072、地面 7275、最大伪平面 2110
- 桌面与最大伪平面的比值为 20072 / 2110 ≈ 9.5 20072/2110 \approx 9.5 20072/2110≈9.5，与地面的比值为 20072 / 7275 ≈ 2.76 20072/7275 \approx 2.76 20072/7275≈2.76；阈值两侧的余量分别为 6.7 6.7 6.7 倍与 1.9 1.9 1.9 倍

说人话：这里相对比例的计算保证了不会因为点云多少影响决策，最后取高度最高者保证了同时看到桌子和地板的时候会选桌子

判据的实现分两步：先用 ρ \rho ρ 筛出合格面，再在其中取高度最大者。平面高度取其与 world z z z 轴的交点：

python 复制代码

def plane_height(plane):
    """平面与 world z 轴的交点高度。"""
    n, c = plane
    return -c / n[2]

# _update_plane 中筛选桌面的部分
planes = self._extract_planes(pts_w)
if planes:
    cmax = max(p[2] for p in planes)
    dom = float(self.get_parameter('plane_dominance').value)
    qualified = [p for p in planes if p[2] >= dom * cmax]       # 主导度：|I_k| >= rho * max
    n, c, _ = max(qualified, key=lambda p: plane_height((p[0], p[1])))
    fit = (n, c)

该判据利用的是平面的面积差异而非高度差异。在本场景中桌面是视野内面积最大的平面，其内点数与伪平面相差近一个数量级，因此比较内点数比比较高度更稳定

1-5 欧式聚类

平面确定后，把离平面距离大于 band 的点 作为候选物体点（本工作区 band = 0.01 m），再把这些点划分为若干物体

1-5-1 介绍

DBSCAN 是一种基于密度的聚类 方法：不预设簇的个数，而是按点邻域的稠密程度把点云划分成若干簇，并把落在稀疏区域的点显式判为噪声 ，而不是强行塞进某个簇
- 这一点是选它的直接原因：平面上方既有物料，也有桌面边缘的碎片与噪声粘连成的细长点带，后者必须能被丢掉；而 K-means 要求事先给定簇数 k k k，且每个点都必须归属于某个簇，两头都做不到
- 名字是 Density-Based Spatial Clustering of Applications with Noise 的缩写，直译是"带噪声的基于密度的空间聚类"，说的正是上面这两件事
它的全部输入只有两个参数：邻域半径 ε \varepsilon ε 与最小点数 min_points，两者共同定义了"稠密"这一概念的尺度。下一节先把这两个参数展开成严格的数学定义

1-5-2 数学定义

DBSCAN 的输入是两个参数：邻域半径 ε \varepsilon ε（本工作区 cluster_eps = 0.015 m）与最小点数 min_points（本工作区 cluster_min_points = 20）
点 p \mathbf{p} p 的 ε \varepsilon ε 邻域定义为
N ε ( p ) = { q ∈ D ∣ dist ⁡ ( p , q ) ≤ ε } N_\varepsilon(\mathbf{p}) = \{\mathbf{q} \in D \mid \operatorname{dist}(\mathbf{p}, \mathbf{q}) \le \varepsilon\} Nε(p)={q∈D∣dist(p,q)≤ε}
由邻域基数把点分为三类：
- 核心点 ： ∣ N ε ( p ) ∣ ≥ min_points |N_\varepsilon(\mathbf{p})| \ge \text{min\_points} ∣Nε(p)∣≥min_points，自身邻域足够稠密
- 边界点 ： ∣ N ε ( p ) ∣ < min_points |N_\varepsilon(\mathbf{p})| < \text{min\points} ∣Nε(p)∣<min_points，但存在核心点 o \mathbf{o} o 使 p ∈ N ε ( o ) \mathbf{p} \in N\varepsilon(\mathbf{o}) p∈Nε(o)
- 噪声点：既非核心点也非边界点
在三类点之上定义可达性：
- 密度直达 ： p \mathbf{p} p 为核心点，且 q ∈ N ε ( p ) \mathbf{q} \in N_\varepsilon(\mathbf{p}) q∈Nε(p)
- 密度可达 ：存在点链 p 1 , ... , p n \mathbf{p}_1,\dots,\mathbf{p}_n p1,...,pn，其中 p 1 = p \mathbf{p}_1 = \mathbf{p} p1=p、 p n = q \mathbf{p}_n = \mathbf{q} pn=q，且每一对相邻点密度直达
- 密度相连 ：存在点 o \mathbf{o} o，使 p \mathbf{p} p 与 q \mathbf{q} q 都从 o \mathbf{o} o 密度可达
簇定义为密度相连关系的最大集合
- 密度直达与密度可达都不对称：核心点可达其邻域内的边界点，反之不成立
- 密度相连是对称的，因此"属于同一簇"构成等价关系，簇的划分良定义
- 簇的个数不是输入参数，而是密度连通分量的个数，是算出来的 ------ 这是相对 K-means 的主要差别

说人话：一堆点被判成同一个物体簇，依据是连通性 ------ 从某个足够密的点出发，能一路踩着距离不超过 ε \varepsilon ε 的邻居走到的所有点，都算同一个物体。所以两点不必直接挨着，中间只要有一串点把它们连起来就成立；怎么走都连不上的点就是噪声，不算物体

1-5-3 代码实现

上述定义全部由 open3d 的 cluster_dbscan 实现，输入 ε \varepsilon ε 与 min_points，输出与点一一对应的标签数组（-1 表示噪声）。簇的编号就是标签值，无需再做一次映射：

python 复制代码

    def _cluster_objects(self, pts_above):
        """DBSCAN 欧式聚类，返回与输入等长的整数标签数组：>=0 = 物体簇，-1 = 非物体。"""
        eps = float(self.get_parameter('cluster_eps').value)
        min_points = int(self.get_parameter('cluster_min_points').value)
        min_extent = float(self.get_parameter('cluster_min_extent').value)
        max_extent = float(self.get_parameter('cluster_max_extent').value)

        ids = np.full(len(pts_above), -1, dtype=np.int64)
        pcd = o3d.geometry.PointCloud()
        pcd.points = o3d.utility.Vector3dVector(
            np.asarray(pts_above, dtype=np.float64))
        labels = np.asarray(pcd.cluster_dbscan(
            eps=eps, min_points=min_points, print_progress=False))
        if labels.size == 0 or labels.max() < 0:   # 全是噪声
            return ids

        for lab in range(int(labels.max()) + 1):
            m = labels == lab
            if int(m.sum()) < min_points:
                continue
            cl = pts_above[m]
            extent = float(np.linalg.norm(cl.max(axis=0) - cl.min(axis=0)))
            # 用物理尺度而不是点数做门槛：点数随距离漂移，尺度不漂
            if extent < min_extent:
                continue
            if max_extent > 0 and extent > max_extent:
                continue
            ids[m] = lab
        return ids

实现上，邻域查询由 open3d 内部用 KD-tree 加速，均匀分布点云下的单次查询复杂度为 O ( log ⁡ n ) O(\log n) O(logn)，整体聚类复杂度约为 O ( n log ⁡ n ) O(n \log n) O(nlogn)
标签直接沿用 open3d 的 DBSCAN label，被门槛否掉的簇与噪声都留 -1。因此发出去的 id 存在空洞 （例如 {0, 2, 5}）
- 这是刻意的：id 是帧内分组标签，不是物体身份 。open3d 的 label 编号随点序变化，跨帧不稳定，下游做跨帧关联必须按几何位置匹配，不能直接用 id
直接在原始点云上聚类而不下采样，是为了让 labels 与索引一一对应，不需要最近邻回投，也不会因为降采样把贴桌的小物体抹掉

1-5-4 筛选

仅靠 DBSCAN 仍不够，还需叠加物理尺度门槛 ，否则噪声粘连成的细长簇与桌面边缘碎片都会被判为物体：
- cluster_min_extent = 0.01 m：小于可抓尺度的簇直接丢弃
- cluster_max_extent = 0.30 m：上限兜底。万一把平面锁到了地面，整张桌子会成为"平面上方的一个大簇"，不拦住会被当成物体从环境点云里抠掉，机械臂就会直接规划穿桌
- 判据用物理尺度 而非点数：点数随物体远近漂移，尺度不漂。尺度取簇的轴对齐包围盒对角线 ∥ max ⁡ − min ⁡ ∥ \|\max - \min\| ∥max−min∥

说人话：DBSCAN 的规则是"距离够近就归为一类，一类里的点够多才算数"。簇的个数由数据本身决定，孤立的噪点会被单独剔出来。

1-5-5 DBSCAN结果发布

经过筛选后，剩下的点就是确认属于物体的点。但光有一片点还不够：下游需要知道哪个点属于哪个物体 ，因此发布时还要附带一个帧内分组标签 id

话题	类型	QoS	内容
`/grasp_scene/object_points`	`PointCloud2`（`x`、`y`、`z`、`id`）	`best_effort`	确认的物体点，带帧内分组标签 `id`

打标签并发布的代码如下：

python 复制代码

def make_cloud_xyz_id(frame_id, stamp, pts, ids):
    """(N,3) 点 + (N,) 整数标签 -> PointCloud2（x,y,z,id 四个 float32 字段）。"""
    header = Header()
    header.frame_id = frame_id
    header.stamp = stamp
    # ROS 2 的 PointField 没有 ROS 1 那种便利构造，只能逐个字段赋值
    fields = []
    for name, offset in (('x', 0), ('y', 4), ('z', 8), ('id', 12)):
        f = PointField()
        f.name = name
        f.offset = offset
        f.datatype = PointField.FLOAT32
        f.count = 1
        fields.append(f)
    buf = np.column_stack([pts, ids]).astype(np.float32)
    cloud = point_cloud2.create_cloud(header, fields, buf)
    cloud.is_dense = True
    return cloud

多带这一列 id 的理由：
- 让下游不必重跑一遍 DBSCAN ：若下游自行聚类，eps、min_points 就要在两个 yaml 中各写一份，而重复的参数迟早会不同步
- id 放在最后一个字段 （offset = 12），x、y、z 的 offset 仍是 0、4、8，于是 read_points(field_names=('x','y','z')) 会自然忽略它，现有消费者不受影响
- 下游只需按 id 分点即可逐簇计算，不需要任何聚类代码
- 发布端用的是 best_effort（深度相机点云的惯例）：下游若用默认的 RELIABLE 订阅，两边 QoS 不兼容会一条都收不到，且不报错 ，订阅时必须显式用 qos_profile_sensor_data

1-6 平面持久化

1-6-1 背景

眼在手（eye-in-hand） 的相机安装方式带来一个固有问题：机械臂靠近物体时，桌面会移出视野
此时若逐帧重新拟合，平面会消失， ∣ d ∣ > band |d| > \text{band} ∣d∣>band 的判据随之失效，整片点云都会被判为物体
因此平面必须持久化 ：连续 plane_lock_after = 5 帧拟合结果一致后把平面硬化，此后不再接受不一致的拟合

1-6-2 共面判定

前后两帧的平面是否"同一个面"，由法向夹角与高度差两个标量判定
Δ θ = arccos ⁡ ⁣ ( n a ⋅ n b ) ≤ Δ θ max ⁡ , ∣ h a − h b ∣ ≤ Δ h max ⁡ \Delta\theta = \arccos\!\left(\mathbf{n}a \cdot \mathbf{n}b\right) \le \Delta\theta{\max},\qquad |h_a - h_b| \le \Delta h{\max} Δθ=arccos(na⋅nb)≤Δθmax,∣ha−hb∣≤Δhmax
- 本工作区取 plane_update_tol_deg = 5.0 与 plane_update_tol_m = 0.03
- 实现上就是一次内积加一次高度相减：

python 复制代码

    def _plane_close(self, a, b):
        """两个平面是否"同一个面"（法向夹角 + 高度差都在容差内）。"""
        cosang = float(np.clip(a[0] @ b[0], -1.0, 1.0))
        ang = math.degrees(math.acos(cosang))
        dh = abs(plane_height(a) - plane_height(b))
        return (ang <= float(self.get_parameter('plane_update_tol_deg').value)
                and dh <= float(self.get_parameter('plane_update_tol_m').value))

1-6-3 平面硬化

硬化分两级：硬化前允许改主意 （防止第一帧视野不好时锁错后永久锁死），硬化后只接受与锁定平面一致的微调。两级由 _lock_hits（连续一致的帧数）与 _lock_hardened（是否已硬化）两个状态量区分
完整状态机如下，1-8 运行日志里的两行 INFO 分别打在第一级与第二级的出口上：

python 复制代码

    def _update_plane(self, pts_w):
        # 前半段见 1-4：顺序 RANSAC 提取水平面、按主导度选出桌面，得到本帧的 fit；
        # 另有 plane_lock 开关（默认开），关掉时直接返回本帧 fit，不走下面的状态机
        ...
        if fit is None:
            return self._lock

        if self._lock is None:                                  # 第一级：还没锁定过
            self._lock, self._lock_anchor = fit, fit
            self._lock_hits = 1
            self.get_logger().info(
                f'锁定桌面平面: z={plane_height(fit):.4f} m')                        # <- 1-8 的第一行
            return self._lock

        if not self._lock_hardened:                             # 第二级：已锁定、未硬化
            if self._plane_close(fit, self._lock):              # 比的是上一帧，允许改主意
                self._lock, self._lock_hits = fit, self._lock_hits + 1
                if self._lock_hits >= int(self.get_parameter('plane_lock_after').value):
                    self._lock_hardened = True
                    self._lock_anchor = fit                     # 锚点钉在硬化那一帧
                    self.get_logger().info(
                        f'平面已硬化: z={plane_height(fit):.4f} m（此后不再接受不一致的拟合）')    # <- 1-8 的第二行
            else:
                self._lock, self._lock_anchor, self._lock_hits = fit, fit, 1
            return self._lock

        if self._plane_close(fit, self._lock_anchor):           # 第三级：已硬化，只跟锚点比
            self._lock = fit
        else:
            self.get_logger().debug(
                f'新拟合 z={plane_height(fit):.4f} 与锚定平面 '
                f'z={plane_height(self._lock_anchor):.4f} 不一致，沿用锁定平面')
        return self._lock

硬化的关键在于比较基准 而非容差本身：硬化后的容差比较基准必须是硬化那一刻的平面 _lock_anchor，而不是上一帧的平面 _lock
- 硬化前比上一帧是刻意的：那正是"允许改主意"的实现方式
- 硬化后换成比锚点，"微调"（对噪声做平均）照收，但累积漂移被总容差挡住
- 锚点钉的是硬化那一刻的 fit，不是第一帧 ------ 前 5 帧若在容差内缓慢漂移，硬化下来的就是漂移后的值

说人话：平面锁定不是"每帧微调一下"，而是"和刚锁定时比一下"。差得不多就采纳，差得多就沿用旧的。否则每一帧都只差一点点，几百帧后平面就飘走了。

1-7 参数配置

为了方便，本文将参数分离出来，全部集中在 grasp_scene/config/scene_filter.yaml

yaml 复制代码

# scene_filter ------ 场景/物体点云分割参数
scene_filter:
  ros__parameters:
    input_topic: /camera/depth/points
    env_topic: /grasp_scene/environment_points
    obj_topic: /grasp_scene/object_points   # 多带一个 float32 `id` 字段（帧内分组标签，非身份）
    plane_topic: /grasp_scene/table_plane   # PoseStamped，TRANSIENT_LOCAL latched
    plane_band_topic: /grasp_scene/plane_band  # Float64，物体点云的截断高度，latched
    source_frame: camera_optical_frame
    target_frame: world
    publish_object: true

    # ---- 平面提取（RANSAC，法向约束 ≈ world +Z）----
    plane_dist_thresh: 0.008      # 内点距离阈值 = 深度噪声尺度（不是桌面高度！）
    plane_max_tilt_deg: 10.0      # 允许法向偏离 +Z 的最大角
                                  #   实测物体顶/侧面会拟合出 11.3° 的伪平面，取 10° 顺便挡掉
    plane_ransac_iters: 200
    plane_fit_max_points: 4000    # 搜索用随机子样本上限（内点数仍在本帧全云上统计）
    plane_min_inlier_ratio: 0.15  # 单个平面的接受门槛（比例，与点云规模无关）
    plane_max_count: 3            # 顺序提取的平面数上限（够拿到地面 + 桌面）
    plane_dominance: 0.70         # 合格面须达到"最大内点数"的这个比例，再在其中取最高
                                  #   实测：桌面 20072 点 / 地面 7275 / 伪平面 2110，
                                  #   主导度 0.7 恰好留下桌面与地面，再取最高 = 桌面
    # ---- 平面持久化（眼在手：夹爪贴近后桌面会移出视野）----
    plane_lock: true
    plane_lock_after: 5           # 连续一致多少帧后"硬化"（之前允许改主意）
    plane_update_tol_deg: 5.0     # 硬化后：法向差超过这个角度就不接受新拟合
    plane_update_tol_m: 0.03      # 硬化后：高度差超过这个距离就不接受新拟合
    plane_init_frames: 30         # 超过这么多帧仍未锁定 -> 退回整幅云当环境并报错
                                  #   （故意在锁定前不发 env：octomap 无占位衰减，
                                  #     宁可为空也别把物体插成永久障碍）

    # ---- DBSCAN 欧式聚类 ----
    cluster_eps: 0.015            # 邻域半径；工作距离 0.39m 处点间距约 1mm，1m 处约 2.6mm
                                  #   场景中物体间距 ≥12cm，不会误合并
    cluster_min_points: 20        # 滤稀疏噪声
    cluster_min_extent: 0.01      # 簇的包围盒对角线下限（物理尺度，比任何可抓物体都小）
    cluster_max_extent: 0.30      # 上限兜底：万一把平面锁到地面，整张桌子会成一个大簇，
                                  #   拦住它才不会被当"物体"抠出障碍图（那会规划穿桌）

    # ---- 平面上下划分 ----
    band: 0.01                    # 平面上方这么多米以内仍算"贴面"。物体坐在桌面上，
                                  #   底座这 1cm 与桌面顶占同一批 octomap 体素
                                  #   （res 0.01 -> [0.20,0.21)，res 0.02 -> [0.20,0.22)），
                                  #   不新增被占体素，所以无害

    use_sim_time: true

同时我们需要注意的是：分离点云后，MoveIt 的 octomap 输入不能再是相机原始点云，需要换成 scene_filter 分离出来的环境点云，具体要改的就是 move_group 里 octomap 更新器的订阅话题

python 复制代码

    # ---- move_group_node 的 parameters 里，octomap 更新器的输入话题 ----
    #      （话题本身的声明见 0-3）
    'camera': {
        'sensor_plugin': 'occupancy_map_monitor/PointCloudOctomapUpdater',
        'point_cloud_topic': LaunchConfiguration('octomap_cloud_topic'),   # 原本写的是 /camera/depth/points
        'max_range': 3.0,
        'point_subsample': 1,
        'padding_offset': 0.005,
        'padding_scale': 1.0,
        'max_update_rate': 5.0,
        'filtered_cloud_topic': 'filtered_cloud',
    },

该话题做成了 launch 参数而不是写死的常量，随时能退回第三期的行为做对比调试：octomap_cloud_topic:=/camera/depth/points

1-8 运行测试

运行：

bash 复制代码

./1_depth_gazebo.sh

该脚本启动的是 gazebo_camera.launch.py，scene_filter 节点随其一同启动（并非单独启动），因此无需额外开终端
输出如下：

bash 复制代码

[scene_filter-6] [INFO] [1789274143.472090207] [scene_filter]: 锁定桌面平面: z=0.2000 m
[scene_filter-6] [INFO] [1789274145.193866594] [scene_filter]: 平面已硬化: z=0.2000 m（此后不再接受不一致的拟合）

这两行的含义对应 1-6：第一行是首次拟合出平面，第二行是连续 5 帧一致、平面被硬化
再确认三路话题是否都在发布：

bash 复制代码

ros2 topic hz /grasp_scene/object_points
ros2 topic hz /grasp_scene/environment_points
ros2 topic echo /grasp_scene/plane_band --once

打开 rviz2，关闭 Planning Scene 的 Show Scene Geometry 即可关闭 octomap，此时红色为物体，蓝色为其他部分
启动以后，该节点除上述点云外还发布两路常量话题：

话题	类型	QoS	内容
`/grasp_scene/table_plane`	`PoseStamped`	`reliable` + `transient_local`	锁定的桌面平面：位置是面上一点，姿态把 `+Z` 转到法向
`/grasp_scene/plane_band`	`Float64`	`reliable` + `transient_local`	物体点云的截断高度 `band`

发布 /grasp_scene/table_plane ，是因为桌面平面是"不写死抓取高度"的唯一来源
- 顶视相机永远看不到物体的底面，所以物体高度只能由"顶面高度减去平面高度"得到
- 平面还给出了"竖直"的定义：下游要按它的法向把物体摆正
发布 /grasp_scene/plane_band ，是因为 band 改变了下游能观测到的东西
- band 以上的点才算物体点，所以物体点云的最低可见高度恒 ≥ \ge ≥ band
- 换言之，"物体是否坐在平面上"在 band 这一尺度以下不可判
- 因此下游判断"物体是否坐在平面上"时，参照系必须是 band 而非平面；band 也就必须发布，不能在各包的 yaml 中重复写入 0.01
两路都用 transient_local（latch），是为了让后加入的订阅者也能立即取到值 ------ 它们不是某一帧的数据，而是整个会话期都成立的常量

2 Object Pose与PCA

上一章解决了"哪些点属于物体 "，本章解决"物体在哪、朝向如何"

2-1 介绍

Object Pose（物体位姿） 指物体自身坐标系到世界坐标系的刚体变换 S E ( 3 ) SE(3) SE(3)，共 6 个自由度：
- 位置 t ∈ R 3 \mathbf{t} \in \mathbb{R}^3 t∈R3 ------ 物体中心在世界系中的坐标
- 姿态 R ∈ S O ( 3 ) \mathbf{R} \in SO(3) R∈SO(3) ------ 物体自身的三条轴在世界系里各自指向何方

2-2 常用方法

获取 Object Pose 的路线很多，按"要不要物体模型"可把主流几类并列如下：

方法	依据	优点	局限
模板匹配	多视角渲染的模板与当前图像比相似度	简单，对纹理物体有效	需模型；视角离散；对遮挡敏感
点云配准（ICP）	迭代最小化模型点云与观测点云的距离	精度高，可得到完整 S E ( 3 ) SE(3) SE(3)	需模型与好的初值，易陷入局部最优
全局配准（PPF 等）	点对特征投票	不需初值，能处理杂乱堆叠	需模型；对噪声与遮挡敏感；算力大
基元拟合	平面、圆柱等基元的参数直接给出朝向	不需模型，快	只对规则几何体有效，且只看得到的面可用
PCA / OBB（本方案）	点云主轴 + 桌面法向 + 最窄方向扫描	不需模型、不需训练、毫秒级	姿态只能确定到对称性为止；竖直轴由外部给定
深度学习位姿回归	网络从 RGB-D 直接回归或配准	泛化好，能处理遮挡与无纹理	要训练、要标注、要 GPU
学习式抓取位姿估计	网络直接在点云上回归 6-DoF 夹取位姿	不依赖物体位姿，可以斜抓	不给出物体位姿，可解释性弱

本章因此分两步走：先用主成分分析（PCA） 找出物体自身的轴（2-3），再据此构造有向包围盒（OBB，Oriented Bounding Box）（3-2 至 3-4），并从盒子里读出夹取位姿（4-1）

2-3 PCA 数学原理

2-3-1 介绍

问题的起点很基础：给定一堆点，如何找出这堆点自身的三条主轴
PCA（Principal Component Analysis，主成分分析）给出的回答是：找一组正交基，使点云在这组基上的投影方差依次最大
- 这组新基称为主成分 ，按方差从大到小依次是长轴、中轴、短轴，三者互相正交
- 三维里主轴必然是三根：包围盒是立体的，只找长、短两根凑不出一个盒子，中轴同样是盒子的一条边

说人话：输入一个物体的点云簇，选出三条互相垂直的轴（可以理解为物体自身的 x , y , z x, y, z x,y,z 坐标系），使这些点在这三个轴上的投影方差依次最大 ------ 这三个轴就是物体自身坐标系的朝向，也就直接估计出了物体的姿态

2-3-2 去中心化

第一步，把数据去中心化。设点集 { p 1 , ... , p N } \{\mathbf{p}_1, \dots, \mathbf{p}N\} {p1,...,pN}，先求质心：
μ = 1 N ∑ i = 1 N p i , q i = p i − μ \boldsymbol{\mu} = \frac{1}{N}\sum{i=1}^{N}\mathbf{p}_i,\qquad \mathbf{q}_i = \mathbf{p}_i - \boldsymbol{\mu} μ=N1i=1∑Npi,qi=pi−μ

2-3-3 协方差矩阵

第二步，计算协方差矩阵 。它把"任意两个方向的耦合"一次性装进一个 3 × 3 3\times3 3×3 对称矩阵：
C = 1 N − 1 ∑ i = 1 N q i q i T \mathbf{C} = \frac{1}{N-1}\sum_{i=1}^{N}\mathbf{q}_i\mathbf{q}_i^{\mathsf T} C=N−11i=1∑NqiqiT
- 此处除以 N − 1 N-1 N−1 而非 N N N，是统计上对样本方差 的无偏约定（obb.cpp 用的就是 N − 1 N-1 N−1）

2-3-4 最大方差与特征分解

第三步是理解 PCA 的关键。取任意单位向量 u \mathbf{u} u，把点投影到它上面得到标量 s i = u T q i s_i = \mathbf{u}^{\mathsf T}\mathbf{q}_i si=uTqi，那么这组投影的方差是
Var ⁡ ( u ) = 1 N − 1 ∑ i s i 2 = u T ( 1 N − 1 ∑ i q i q i T ) u = u T C u \operatorname{Var}(\mathbf{u}) = \frac{1}{N-1}\sum_i s_i^2 = \mathbf{u}^{\mathsf T}\left(\frac{1}{N-1}\sum_i \mathbf{q}_i\mathbf{q}_i^{\mathsf T}\right)\mathbf{u} = \mathbf{u}^{\mathsf T}\mathbf{C}\,\mathbf{u} Var(u)=N−11i∑si2=uT(N−11i∑qiqiT)u=uTCu
于是"哪个方向散得最开"转化为一个带约束的优化问题 ：
max ⁡ ∥ u ∥ = 1 u T C u \max_{\|\mathbf{u}\|=1}\ \mathbf{u}^{\mathsf T}\mathbf{C}\,\mathbf{u} ∥u∥=1max uTCu
用拉格朗日乘子法求解。构造 L = u T C u − λ ( u T u − 1 ) \mathcal{L} = \mathbf{u}^{\mathsf T}\mathbf{C}\mathbf{u} - \lambda(\mathbf{u}^{\mathsf T}\mathbf{u} - 1) L=uTCu−λ(uTu−1)，令对 u \mathbf{u} u 的偏导为零：
2 C u − 2 λ u = 0 ⟹ C u = λ u 2\mathbf{C}\mathbf{u} - 2\lambda\mathbf{u} = 0 \quad\Longrightarrow\quad \mathbf{C}\mathbf{u} = \lambda\mathbf{u} 2Cu−2λu=0⟹Cu=λu
结果是：驻点只可能是协方差矩阵的特征向量 ，而对应的方差恰好就是特征值
- 因为 C \mathbf{C} C 是对称半正定矩阵，由谱定理，它的特征向量可以选成一组标准正交基 ，特征值全部非负
- 所以只要把特征值从大到小排好 λ 1 ≥ λ 2 ≥ λ 3 \lambda_1 \ge \lambda_2 \ge \lambda_3 λ1≥λ2≥λ3，对应的 v 1 , v 2 , v 3 \mathbf{v}_1, \mathbf{v}_2, \mathbf{v}_3 v1,v2,v3 就是我们要的三条主轴
至此 PCA 的语义已经明确：
- v 1 \mathbf{v}_1 v1 是方差最大的方向，即点云最"细长"的方向
- v 3 \mathbf{v}_3 v3 是方差最小的方向，即点云最"扁"的方向

工程上一般走 SVD 而不是直接对 C \mathbf{C} C 求特征分解（obb.cpp 用的是 Eigen 的 SelfAdjointEigenSolver，因为 C \mathbf{C} C 只有 3 × 3 3\times3 3×3，直接特征分解既精确又便宜）
有三处细节必须处理，否则代码在真实数据上会出错：
- 特征值降序排列 。一般特征分解器不保证特征值的顺序（Eigen 的 SelfAdjointEigenSolver 保证升序），必须自己按 λ \lambda λ 重排，并把三个特征向量同步换位
- 行列式强制为 + 1 +1 +1 。数学上 ± v \pm\mathbf{v} ±v 都是特征向量，若直接拼成矩阵，有 1 / 2 1/2 1/2 的概率得到 d e t = − 1 det = -1 det=−1 的镜像 ------ 那时 R \mathbf{R} R 不是旋转矩阵，转成四元数会得到一个无意义的姿态（盒子本身反而不受影响：轴取反会让投影的上下限同时换号，盒心与跨度不变）。因此算出 R \mathbf{R} R 后需检查行列式，为 − 1 -1 −1 时将某一列取反
- 退化判据 。点数少于 3、点全部重合、点共线（第二特征值近似 0 0 0，面内无方向可言）这三种情况返回失败，避免 NaN 流入下游
  - 需要特别注意：共面不算退化。正上方观测到的物体点云就是顶面这一张平面，属于最正常的输入；判为退化会使主动感知失去意义（共面时第三轴即面法向，这正是 PCA 在正上方能给出精确竖直轴的来源）

说人话：PCA 就是"转一下坐标系，让点在新的三个轴上尽量散开"。转完之后，最长的那根轴是物体的长边方向，最短的那根轴是物体最扁的方向。

2-3-5 代码实现：pcaObb

步骤与 2-3 完全一一对应：
- 求质心 μ \boldsymbol{\mu} μ，去质心得到 q i \mathbf{q}_i qi
- 累加协方差 C = 1 N − 1 ∑ q i q i T \mathbf{C} = \frac{1}{N-1}\sum \mathbf{q}_i\mathbf{q}_i^{\mathsf T} C=N−11∑qiqiT，这里用的就是 N − 1 N-1 N−1
- SelfAdjointEigenSolver 做特征分解，因为 C \mathbf{C} C 是实对称矩阵
- 按特征值降序重排三列（Eigen 给出的是升序，需自行反转）
- 检查行列式，为 − 1 -1 −1 就把第三列取反，强制 d e t ( R ) = + 1 det(\mathbf{R}) = +1 det(R)=+1
- 按 3-2 的极差公式算 e \mathbf{e} e，按轴取中点算盒心 c \mathbf{c} c
- 退化判据：点数 < 3 < 3 <3、点全部重合、第二特征值近似 0 0 0（共线）。共面不判退化

cpp 复制代码

// std::optional<Obb> 定义见下面
std::optional<Obb> pcaObb(const std::vector<Vec3> & pts)
{
  const size_t n = pts.size();
  if (n < 3) {
    return std::nullopt;
  }

  Vec3 center0 = Vec3::Zero();
  for (const auto & p : pts) {
    center0 += p;
  }
  center0 /= static_cast<double>(n);

  Mat3 cov = Mat3::Zero();
  for (const auto & p : pts) {
    const Vec3 d = p - center0;
    cov += d * d.transpose();
  }
  cov /= static_cast<double>(n - 1);   // 与 numpy np.cov 的默认 (N-1) 归一一致
  if (!cov.allFinite()) {
    return std::nullopt;
  }

  Eigen::SelfAdjointEigenSolver<Mat3> es(cov);   // 特征值升序
  if (es.info() != Eigen::Success) {
    return std::nullopt;
  }
  // 降序重排
  std::array<int, 3> order = {0, 1, 2};
  std::sort(order.begin(), order.end(), [&](int a, int b) {
    return es.eigenvalues()[a] > es.eigenvalues()[b];
  });
  Vec3 ev;
  Mat3 evec;
  for (int i = 0; i < 3; ++i) {
    ev[i] = es.eigenvalues()[order[i]];
    evec.col(i) = es.eigenvectors().col(order[i]);
  }

  if (ev[0] <= 0.0) {
    return std::nullopt;            // 点全部重合
  }
  if (ev[1] <= 1e-6 * ev[0]) {
    return std::nullopt;            // 共线：面内没有可定义的方向
  }

  Mat3 R = evec;
  if (R.determinant() < 0.0) {      // 反射 -> 翻掉最小轴，保证右手
    R.col(2) = -R.col(2);
  }

  // 投影到三个轴上取包围盒。轴翻转会让 (lo+hi)/2 同时翻号，故 center 与符号无关。
  Vec3 lo = Vec3::Constant(std::numeric_limits<double>::infinity());
  Vec3 hi = Vec3::Constant(-std::numeric_limits<double>::infinity());
  for (const auto & p : pts) {
    const Vec3 proj = R.transpose() * (p - center0);
    lo = lo.cwiseMin(proj);
    hi = hi.cwiseMax(proj);
  }

  Obb out;
  out.R = R;
  out.center = center0 + R * ((lo + hi) / 2.0);
  out.extents = hi - lo;
  return out;
}

自此，通过上述函数，输入一族点集合 std::vector<Vec3> & pts，我们就能得到：

cpp 复制代码

// 返回值：三根轴 + 盒心 + 三边跨度。退化输入（点数 < 3 / 全部重合 / 共线）返回 nullopt。
struct Obb
{
  Vec3 center;
  Mat3 R;         // 列 = 三个轴（特征值降序），det(R) 强制 +1
  Vec3 extents;   // 各轴上的 max - min
};

std::optional<Obb> pcaObb(const std::vector<Vec3> & pts);

2-4 PCA的局限

2-4-1 正方体PCA估算问题

对正方形顶面这类高度对称的物体，PCA 的两根水平轴没有唯一解
- 面内协方差各向同性使两个水平特征值相等，此时任意一组面内正交基都是合法的特征向量，实际取到哪一组由浮点误差决定
- 表现是水平轴退化为一个随机转角：盒心不受影响，而 yaw 不可确定
此外，物体自身的四重对称使 θ \theta θ 与 θ + 90 ° \theta + 90° θ+90° 给出两个完全等价 的解
- 该歧义可由上一帧方向作先验消解，但平滑必须在消歧之后，否则会把两个等价解混为一个介于其间的错误角度
- 只有足迹接近正方形时，两个极小才会同时落进容差带，消歧才成为必需；长方形的宽度曲线只有一个极小，不存在该歧义

说人话：正方形怎么转都长得一样，PCA 因此挑不出一组"正确"的横轴，只能随便给一组 ------ 盒子的中心还对，但盒子是歪的，歪多少全看运气

2-4-2 斜视估计问题

视角倾斜时，问题由"解不唯一"转为"解被污染"
- 斜视观测中的侧面点把协方差由面内分布拉向竖直分布，最小方差轴因此偏离物体竖直轴，指向点云缺失的背面方向
- 该偏离是系统偏差而非随机噪声，时序平滑无法消除；点云本身也不提供该帧可信度的判据

说人话：从斜上方看，PCA 会把侧面的点也算进来，于是它以为物体是斜的 ------ 而且它不会告诉你自己算错了

2-4-3 高度估计问题

前两节讨论轴，本节讨论边：即使轴正确，跨度也不应由 max ⁡ − min ⁡ \max - \min max−min 给出
- max ⁡ \max max 与 min ⁡ \min min 是极值统计量，其期望高于真实边界，且偏高量随噪声与点数同时增大
- 该偏差方向恒定，无法通过平均消除；它直接决定盒顶位置，进而影响下压深度
- 本方案只对竖直跨度 做替换。两个水平跨度仍取 max ⁡ − min ⁡ \max - \min max−min，因为它们由最窄方向的扫描直接给出，实测偏差中位数约 0.6 % 0.6\% 0.6%，量级远小于竖直方向
替代的统计量是在锁定平面附近的切片内取中值（见 3-3）

说人话： max ⁡ \max max 取的是"最外面那个点"，而最外面的点往往是噪声 ------ 盒子因此被撑高，所以高度得用中值而不是极值来定

2-4-4 最好的PCA方法

2-4-1 与 2-4-2 同源于观测视角
- 正上方观测时点云只含顶面，协方差落在面内，最小方差轴等于平面法向
- 视角倾斜后侧面点进入点云，竖直轴被带偏；而水平轴的歧义源于物体自身对称性，改变视角无益
因此 PCA 可用的条件有两条：从正上方观测 ，或取得完整点云
- 前者使竖直轴可靠，但水平轴仍需由扫描与消歧绕开（见 3-3）
- 后者使三个方向同时有约束，代价是需要额外的传感器或扫描动作
这也是本方案引入主动感知的动机：不是等待合适视角，而是将相机主动移至正上方，使 PCA 的适用条件被制造出来

说人话：PCA 只在正上方看的时候才靠谱 ------ 与其指望运气好，不如让机械臂自己挪到正上方再看一眼

3 OBB 数学原理

3-1 介绍

有向包围盒（OBB，Oriented Bounding Box） 是指三条边可以沿任意一组正交基、不必平行于世界坐标轴的包围盒
- 与之相对的是轴对齐包围盒（AABB） ：三条边固定平行于世界坐标轴，形状与物体朝向无关，因此描述不了 y a w yaw yaw ------ 无论方块怎么转，AABB 都是同一个盒子
- OBB 的三条边沿物体自身主轴，物体旋转时盒子随之旋转，这正是抓取需要的

说人话：AABB 是"永远摆正"的盒子，方块转它也不转，看不出物体朝哪；OBB 是"跟着物体一起转"的盒子，盒子最窄的那条边就是两指该合拢的方向。

一个 OBB 由三个量完全确定 OBB = { c , R , e } \text{OBB} = \{\mathbf{c},\ \mathbf{R},\ \mathbf{e}\} OBB={c, R, e}：盒心 c \mathbf{c} c、主轴拼成的旋转矩阵 R = $v 1 , v 2 , v 3$ \mathbf{R} = $\\mathbf{v}_1, \\mathbf{v}_2, \\mathbf{v}_3$ R= $v1,v2,v3$ （列向量即主轴）、三边跨度 e \mathbf{e} e

3-2 标准OBB

标准 OBB 的构造很直接：三条轴全部取 PCA 的主成分，每条边的跨度取点在该轴上投影的极差
e k = max ⁡ i ( q i ⋅ v k ) − min ⁡ i ( q i ⋅ v k ) e_k = \max_i \left(\mathbf{q}_i \cdot \mathbf{v}_k\right) - \min_i \left(\mathbf{q}_i \cdot \mathbf{v}_k\right) ek=imax(qi⋅vk)−imin(qi⋅vk)
结合我们 PCA 的结果，代码有

cpp 复制代码

  // 投影到三个轴上取包围盒。轴翻转会让 (lo+hi)/2 同时翻号，故 center 与符号无关。
  Vec3 lo = Vec3::Constant(std::numeric_limits<double>::infinity());
  Vec3 hi = Vec3::Constant(-std::numeric_limits<double>::infinity());
  for (const auto & p : pts) {
    const Vec3 proj = R.transpose() * (p - center0);
    lo = lo.cwiseMin(proj);
    hi = hi.cwiseMax(proj);
  }

  Obb out;
  out.R = R;
  out.center = center0 + R * ((lo + hi) / 2.0);
  out.extents = hi - lo;
  return out;

这里的 R 就是 2-3-5 特征分解得到的三个主轴（列向量），lo 与 hi 是点在三根轴上的投影极小值与极大值
- out.extents = hi - lo 即上面的极差公式
- out.center 是"沿三根轴各取一次中点"再平移回世界系
盒心不是质心 ，要沿三根轴各取一次中点；只有点云关于质心对称时两者才相等

c = μ + ∑ k = 1 3 max ⁡ i ( q i ⋅ v k ) + min ⁡ i ( q i ⋅ v k ) 2 v k \mathbf{c} = \boldsymbol{\mu} + \sum_{k=1}^{3} \frac{\max_i(\mathbf{q}_i \cdot \mathbf{v}_k) + \min_i(\mathbf{q}_i \cdot \mathbf{v}_k)}{2}\,\mathbf{v}_k c=μ+k=1∑32maxi(qi⋅vk)+mini(qi⋅vk)vk
- 真实点云并不对称（斜视时侧面点更密），盒心与质心可差数毫米；而顶视夹取要对准的正是盒心，算错就等于夹在偏心位置
把这段接回 2-3-5 的 pcaObb（把特征分解得到的 R 传进来），标准 OBB 就完整了 ------ 它把全部可靠性都押在 PCA 上

3-3 扫描 OBB

这里对应了 2-4-1 到 2-4-3 的三个问题 ------ 每一根失效的轴、每一个偏掉的跨度，各有各的替换来源：
- 对 2-4-1（水平轴无唯一解） ：不做特征分解，改为在水平面内扫一圈找最窄方向，用极值取代特征向量
- 对 2-4-2（竖直轴被侧面点带偏） ：竖直轴不从点云估计，直接取桌面平面法向 n \mathbf{n} n ------ 它在第 1 章已由 RANSAC 拟合给出，且与视角无关
- 对 2-4-3（跨度被极值撑大） ：高度不取 max ⁡ − min ⁡ \max - \min max−min，改在锁定平面附近的切片内取中值
本文的妥协 OBB 因此把三条边的来源拆开，让每一根都来自它最可靠的那个观测量：

量	来源	为什么不用 PCA
竖直轴 v 3 \mathbf{v}_3 v3	桌面平面法向 n \mathbf{n} n	底面永远看不见，法向是唯一可靠来源；PCA 的最小轴只在正上方时才与法向重合
水平轴 v 1 \mathbf{v}_1 v1	最窄方向扫描	PCA 在方形足迹上可证不定
水平轴 v 2 \mathbf{v}_2 v2	由 v 3 × v 1 \mathbf{v}_3 \times \mathbf{v}_1 v3×v1 定出	前两者定死后由右手系约束补上，保证 det ⁡ R = + 1 \det \mathbf{R} = +1 detR=+1
竖直跨度 e 3 \mathbf{e}_3 e3	锁定桌面平面 + 切片中值	见 2-4-3，PCA 的 max ⁡ − min ⁡ \max - \min max−min 是被噪声撑大的有偏极值统计量

汇总成矩阵即 R = $v 1 , v 3 \times v 1 , v 3$ \mathbf{R} = $\\mathbf{v}_1,\\ \\mathbf{v}_3 \\times \\mathbf{v}_1,\\ \\mathbf{v}_3$ R= $v1, v3\timesv1, v3$ ------ 第一列由扫描定、第三列由桌面定、第二列由正交性补上
代价要说清楚：这套构造换来了鲁棒，但放弃了对 r o l l , p i t c h roll, pitch roll,pitch 的估计 ------ 竖直轴不再来自物体，而来自桌面

说人话：标准 OBB 三根轴全听 PCA 的；本文的 OBB 是分工的 ------ 竖着那根听桌面的，横着那根听扫描

还有一件事必须能判：这个物体的 y a w yaw yaw 到底有没有定义 。足迹接近圆或正方形时各个方向的跨度几乎一样， arg ⁡ min ⁡ θ e x t ( θ ) \arg\min_\theta \mathrm{ext}(\theta) argminθext(θ) 就退化成一个由噪声决定的位置，此时不存在"正确的横轴"
- 判据是各向异性比 ρ aniso = max ⁡ θ e x t ( θ ) / min ⁡ θ e x t ( θ ) \rho_{\text{aniso}} = \max_\theta \mathrm{ext}(\theta) / \min_\theta \mathrm{ext}(\theta) ρaniso=maxθext(θ)/minθext(θ)（代码里记作 rho，与 1-4-2 的平面主导度 ρ \rho ρ 不是一回事）
- 比值接近 1 1 1 说明足迹各向同性：圆柱的 y a w yaw yaw 根本无定义，正方形的 y a w yaw yaw 只在 90 ° 90° 90° 对称的意义下有解
- 实测方形足迹 1.28 ∼ 1.38 1.28 \sim 1.38 1.28∼1.38、圆形足迹 1.004 ∼ 1.04 1.004 \sim 1.04 1.004∼1.04，阈值取 isotropy_ratio_max = 1.10，两侧分离度足够
- 结果落在 est.yaw_defined 上，作为诊断量说明这个 y a w yaw yaw 值该不该被当真
实现顺序与上表一致：先扫出宽度曲线，再消歧定出唯一方向，最后由切片中值定高度

cpp 复制代码

// ------------------------------------------------- 角度与中值（下面三处都要用）
double modPi(double x)              // 结果恒落在 [0, pi)
{
  double r = std::fmod(x, M_PI);
  if (r < 0.0) {
    r += M_PI;
  }
  return r;
}

double medianOf(std::vector<double> v)
{
  if (v.empty()) {
    return 0.0;
  }
  const size_t n = v.size();
  const size_t mid = n / 2;
  std::nth_element(v.begin(), v.begin() + mid, v.end());
  const double hi = v[mid];
  if (n % 2 == 1) {
    return hi;
  }
  const double lo = *std::max_element(v.begin(), v.begin() + mid);
  return 0.5 * (lo + hi);
}

// ------------------------------------------------------------ 最小宽度扫描
WidthScan widthScan(const std::vector<double> & a, const std::vector<double> & b,
                    int n_theta)
{
  WidthScan out;
  const int T = std::max(1, n_theta);
  out.thetas.resize(static_cast<size_t>(T));
  out.ext.resize(static_cast<size_t>(T));
  for (int t = 0; t < T; ++t) {
    // endpoint=False，与 np.linspace(0, pi, n, endpoint=False) 一致
    const double th = M_PI * static_cast<double>(t) / static_cast<double>(T);
    const double ct = std::cos(th), st = std::sin(th);
    double lo = std::numeric_limits<double>::infinity();
    double hi = -std::numeric_limits<double>::infinity();
    for (size_t i = 0; i < a.size(); ++i) {
      const double proj = a[i] * ct + b[i] * st;
      lo = std::min(lo, proj);
      hi = std::max(hi, proj);
    }
    out.thetas[static_cast<size_t>(t)] = th;
    out.ext[static_cast<size_t>(t)] = (a.empty() ? 0.0 : hi - lo);
  }
  return out;
}

// 各向异性比 rho = max ext / min ext ------ "yaw 到底有没有定义"的判据
double anisotropy(const std::vector<double> & ext)
{
  if (ext.empty()) {
    return std::numeric_limits<double>::infinity();
  }
  const auto mm = std::minmax_element(ext.begin(), ext.end());
  const double lo = *mm.first;
  const double hi = *mm.second;
  if (lo <= 1e-12) {
    return std::numeric_limits<double>::infinity();
  }
  return hi / lo;
}

// 模 pi 的角度距离：theta 与 theta+90 在线意义上只差 90
double angDistModPi(double a, double b)
{
  return std::abs(modPi(a - b + M_PI / 2.0) - M_PI / 2.0);
}

// ------------------------------------------------------------------ 消歧
Direction pickDirection(const WidthScan & scan, std::optional<double> theta_prev,
                        double tol_m)
{
  const auto mm = std::minmax_element(scan.ext.begin(), scan.ext.end());
  const double emin = *mm.first;
  const size_t imin = static_cast<size_t>(std::distance(scan.ext.begin(), mm.first));

  std::vector<size_t> cand;
  for (size_t i = 0; i < scan.ext.size(); ++i) {
    if (scan.ext[i] <= emin + tol_m) {
      cand.push_back(i);
    }
  }
  if (cand.empty()) {
    return {scan.thetas[imin], emin};
  }

  const size_t T = scan.thetas.size();

  // 1. 按下标连续性切"弧"。ext(theta) 以 pi 为周期，一个周期内有两个极小（theta 与
  //    theta+90），每个极小周围的容差带在下标上连续，两个极小因此给出两段互不相邻的弧。
  //    theta=0 与 theta=pi 是同一条直线，下标 0 与 T-1 在圆上相邻，首尾两段要合回去。
  std::vector<std::vector<size_t>> runs;
  for (size_t i : cand) {
    if (!runs.empty() && i == runs.back().back() + 1) {
      runs.back().push_back(i);
    } else {
      runs.emplace_back(1, i);
    }
  }
  if (runs.size() >= 2 && runs.front().front() == 0 && runs.back().back() == T - 1) {
    runs.front().insert(runs.front().begin(), runs.back().begin(), runs.back().end());
    runs.pop_back();
  }

  // 2. 每段弧出一个代表：代表角取弧内 theta 的圆中值，宽度取弧内最小。
  //    代表角必须取"弧心"而不是"弧内格点最低处" ------ 两个等价极小的差别只由亚格点相位
  //    决定，按格点 argmin 挑等于让浮点噪声掷骰子。
  std::vector<double> arc_theta, arc_ext;
  arc_theta.reserve(runs.size());
  arc_ext.reserve(runs.size());
  for (const auto & r : runs) {
    const double ref = scan.thetas[r.front()];
    std::vector<double> dev;
    dev.reserve(r.size());
    double e = std::numeric_limits<double>::infinity();
    for (size_t i : r) {
      dev.push_back(modPi(scan.thetas[i] - ref + M_PI / 2.0) - M_PI / 2.0);
      e = std::min(e, scan.ext[i]);
    }
    arc_theta.push_back(modPi(ref + medianOf(dev)));
    arc_ext.push_back(e);
  }

  // 3. 选弧
  size_t pick = 0;
  if (theta_prev.has_value()) {
    // 有先验：挑离上一帧最近的弧。各弧几何上等价，选哪个都对，关键是不跨帧改主意。
    double best = std::numeric_limits<double>::infinity();
    for (size_t a = 0; a < arc_theta.size(); ++a) {
      const double d = angDistModPi(arc_theta[a], *theta_prev);
      if (d < best) {
        best = d;
        pick = a;
      }
    }
  } else {
    // 无先验：宽度显著更小的弧优先；落在同一 tol_m 内就是等价的，取编号小的一端，
    // 保证同一场景跨运行结果一致（等价弧之间本来就不存在谁更"对"）。
    const double e_best = *std::min_element(arc_ext.begin(), arc_ext.end());
    double best_theta = std::numeric_limits<double>::infinity();
    for (size_t a = 0; a < arc_theta.size(); ++a) {
      if (arc_ext[a] <= e_best + tol_m && arc_theta[a] < best_theta) {
        best_theta = arc_theta[a];
        pick = a;
      }
    }
  }
  return {arc_theta[pick], arc_ext[pick]};
}

// ------------------------------------------------------------------ 高度
double sliceMedian(const std::vector<double> & d, double sigma,
                   double slice_sigmas, int iters)
{
  if (d.empty()) {
    return 0.0;
  }
  double top = *std::max_element(d.begin(), d.end());
  for (int it = 0; it < iters; ++it) {
    std::vector<double> sel;
    sel.reserve(d.size());
    for (double v : d) {
      if (v >= top - slice_sigmas * sigma) {   // 只留顶面层
        sel.push_back(v);
      }
    }
    if (sel.size() < 3) {
      break;
    }
    const double new_top = medianOf(sel);
    if (std::abs(new_top - top) < 1e-9) {
      break;
    }
    top = new_top;
  }
  return top;
}

3-4 两者OBB合并

两条路线采用分工：扫描 OBB 是夹取实际使用的那个 ，PCA OBB 退为辅助
- 夹取用的盒子：水平轴由扫描定、竖直轴取平面法向、竖直跨度取切片中值、盒心落在半高处 ------ 即 est.R_box 与 est.center
- PCA OBB 的数值不进夹取链路，只保留一个诊断量 pca_tilt_deg（PCA 最小轴与平面法向的夹角），用来量化"这一帧的视角配不配得上 PCA"，也就是 2-4-2 那个偏离量
- 但它的退化判据会否决整次估计 ：estimateObject 里 pcaObb 一旦返回 nullopt，整个函数就返回 nullopt ------ 点少于 3、全部重合、共线这三种情况下，连混合 OBB 都拿不到
estimateObject 是这次合并的实现，末尾三道守卫决定物体是否可夹：

守卫	判据	不通过的含义
坐在平面上	只在"看得到低于顶面的部分"时才判；只见顶面时不判，按坐着处理	疑为悬空或叠放
过矮	高度 < < < `min_height`	两指合不拢
宽度可夹	`min_graspable_width` ≤ \le ≤ 宽度 ≤ \le ≤ `gripper_open`	夹爪开不到或夹不住

cpp 复制代码

std::optional<ObjectEstimate> estimateObject(const std::vector<Vec3> & pts,
                                             const Vec3 & plane_n_in, double plane_c,
                                             const EstimateParams & prm)
{
  if (pts.size() < 3) {
    return std::nullopt;
  }
  const Vec3 n = plane_n_in.normalized();
  Vec3 u, v;
  inplaneBasis(n, u, v);

  // 平面上的锚点（垂足）+ 点分解到 (u, v, n)
  const Vec3 p0 = -plane_c * n;
  std::vector<double> a(pts.size()), b(pts.size()), d(pts.size());
  for (size_t i = 0; i < pts.size(); ++i) {
    const Vec3 rel = pts[i] - p0;
    a[i] = rel.dot(u);
    b[i] = rel.dot(v);
    d[i] = rel.dot(n);
  }

  // 水平两根轴：扫描取最窄，再由 pickDirection 消歧
  const WidthScan scan = widthScan(a, b, prm.n_theta);
  const Direction dir = pickDirection(scan, prm.theta_prev, prm.width_tol_m);
  const double theta = dir.theta;
  const double aniso = anisotropy(scan.ext);

  // 与 theta 正交的那个方向上的跨度：把坐标转到 (theta, theta+90°)
  const double ct = std::cos(theta), st = std::sin(theta);
  double a_lo = std::numeric_limits<double>::infinity(), a_hi = -a_lo;
  double b_lo = std::numeric_limits<double>::infinity(), b_hi = -b_lo;
  for (size_t i = 0; i < pts.size(); ++i) {
    const double a2 = a[i] * ct + b[i] * st;
    const double b2 = -a[i] * st + b[i] * ct;
    a_lo = std::min(a_lo, a2);
    a_hi = std::max(a_hi, a2);
    b_lo = std::min(b_lo, b2);
    b_hi = std::max(b_hi, b2);
  }
  const double perp_width = b_hi - b_lo;
  const double width = a_hi - a_lo;   // 用扫描出的方向重算，与 perp 同一基准

  // 高度：从平面起算，用切片中值
  const double height = sliceMedian(d, prm.depth_sigma, prm.slice_sigmas, prm.slice_iters);

  // 混合 OBB 中心：水平取 (theta, theta+90°) 双向包围盒中点，竖直取半高
  const double ca = (a_lo + a_hi) / 2.0, cb = (b_lo + b_hi) / 2.0;
  const double cu = ca * ct - cb * st;    // 转回 (u, v) 坐标
  const double cv = ca * st + cb * ct;

  ObjectEstimate est;
  est.n_points = static_cast<int>(pts.size());
  est.center = p0 + cu * u + cv * v + (height / 2.0) * n;
  est.extents = Vec3(width, perp_width, height);
  est.theta = theta;
  est.width = width;
  est.perp_width = perp_width;
  est.aniso = aniso;
  est.yaw_defined = aniso >= prm.aniso_yaw_min;
  est.height = height;

  const Vec3 e_theta = ct * u + st * v;
  const Vec3 e_perp = -st * u + ct * v;
  est.R_box.col(0) = e_theta;
  est.R_box.col(1) = e_perp;
  est.R_box.col(2) = n;                   // det = +1（e_theta × e_perp = n）

  // PCA OBB：不参与夹取，只留一个视角质量的诊断量
  const auto pca = pcaObb(pts);
  if (!pca.has_value()) {
    return std::nullopt;
  }
  est.pca_center = pca->center;
  est.pca_R = pca->R;
  est.pca_extents = pca->extents;
  const double cos_tilt = std::clamp(std::abs(pca->R.col(2).dot(n)), -1.0, 1.0);
  est.pca_tilt_deg = std::acos(cos_tilt) * 180.0 / M_PI;

  // 守卫一：坐在平面上。只在看得到低于顶面的部分时才判得了，只见顶面时按坐着处理
  const double d_min = *std::min_element(d.begin(), d.end());
  const double d_max = *std::max_element(d.begin(), d.end());
  if (!prm.plane_band_known) {
    est.resting = true;                   // 没依据就不冤枉物体
    est.notes_undetermined.push_back("未收到上游 band，是否坐在平面上无法判定（按坐在平面上处理）");
  } else if ((d_max - d_min) > kSideSeenSigmas * prm.depth_sigma) {
    const double tol = prm.plane_band + prm.resting_margin;
    est.resting = d_min <= tol;
    if (!est.resting) {
      est.notes.push_back(fmt("悬空/叠放: 最低点离平面 %.1fmm（band %.1f + 裕度 %.1f = %.1fmm）",
                              d_min * 1000.0, prm.plane_band * 1000.0,
                              prm.resting_margin * 1000.0, tol * 1000.0));
    }
  } else {
    est.resting = true;
    est.notes_undetermined.push_back("只见顶面，是否坐在平面上无法判定（按坐在平面上处理）");
  }

  // 守卫二：过矮
  est.height_ok = height >= prm.min_height;
  if (!est.height_ok) {
    est.notes.push_back(fmt("过矮: 高度 %.1fmm", height * 1000.0));
  }

  // 守卫三：宽度合适
  est.width_ok = (width >= prm.min_graspable_width) && (width <= prm.gripper_open);
  if (!est.width_ok) {
    est.notes.push_back(fmt("宽度不合适: %.1fmm（可夹 %.0f~%.0fmm）",
                            width * 1000.0, prm.min_graspable_width * 1000.0,
                            prm.gripper_open * 1000.0));
  }
  return est;
}

自此，对每一组点云，我们都得到了一个 OBB

3-5 粗估计与细估计

3-3 与 3-4 讲的是"怎么估"，本节讲"在哪估"。同一套公式在不同视角下给出的结果并不等价，因此本方案把估计分成两档：
- 粗估计：节点常驻运行，相机停在启动位姿，对每一帧点云直接出估计并发布
- 细估计：把相机主动移到物体正上方后再估一次 ------ 公式完全相同，只是观测条件换了
两者的分界不在算法，而在视角，判据由 qualityFor 给出：
Δ = arccos ⁡ ( ( p − c ) ⋅ ( − n ) ∥ p − c ∥ ) \Delta = \arccos\left( \frac{(\mathbf{p} - \mathbf{c}) \cdot (-\mathbf{n})}{\|\mathbf{p} - \mathbf{c}\|} \right) Δ=arccos(∥p−c∥(p−c)⋅(−n))
- p \mathbf{p} p 为物体中心， c \mathbf{c} c 为相机位置， n \mathbf{n} n 为桌面平面法向
- Δ \Delta Δ 即"相机到物体"的连线与竖直方向 − n -\mathbf{n} −n 的夹角； Δ \Delta Δ 不超过 nadir_angle_max_deg（默认 5 ° 5° 5°）判为 NADIR，否则判为 OBLIQUE
- 三个量全部来自 TF 与平面拟合，没有一个是写死的场景常量

cpp 复制代码

  Quality qualityFor(const Vec3 & obj, const Vec3 & cam_pos, const Vec3 & n) const
  {
    const Vec3 v = obj - cam_pos;
    if (v.norm() < 1e-9) {
      return Quality::OBLIQUE;
    }
    const double cos_t = std::clamp(v.normalized().dot(-n), -1.0, 1.0);
    const double deg = std::acos(cos_t) * 180.0 / M_PI;
    return deg <= nadir_angle_max_deg_ ? Quality::NADIR : Quality::OBLIQUE;
  }

相机位置必须取该帧点云自己的时间戳 ，不能用"最新位姿"
- 机械臂运动时两者相差整整一段轨迹；用最新位姿会把"臂还在路上时拍的那一帧"评成 NADIR
- 因此 waitForNadir 在"等臂停稳"之外还要再等一个条件：该物体确实拿到了正上方的估计
质量升到 NADIR 后只升不降 ：
- 物体是静止的，扫描一轮就够；若允许降级，同一帧点云里其他物体的斜视估计会把刚精估完的物体冲掉
- "刷新存活时间"与"更新估计"是两件事 ------ 斜视帧仍然刷新存活时间，只是不改数值
pca_tilt_deg 是这一节效果的量化：斜视下实测偏离可达 47.6 ° 47.6° 47.6°，正上方为 0.0 ° 0.0° 0.0°
日志里每个物体都带视角标签（#0 nadir / #0 oblique），"这个盒子此刻可不可信"一眼可见

说人话：粗估计是站在原地估，细估计是走过去从正上方再看一眼。判断有没有站对位置，看的是"相机到物体"这条线与竖直方向的夹角，而不是靠猜

3-6 测试

启动仿真和 OBB 节点：

bash 复制代码

./1_depth_gazebo.sh     # 终端一：Gazebo + 相机 + scene_filter + move_group
./2_grasp_obb.sh        # 终端二：grasp_obb

刚启动时，rviz2 中每个物体都有一个向上的箭头，这是粗略估计
- 之所以称为粗略：此刻相机处于斜视，PCA 的竖直轴被侧面点云拉偏（实测 14.9 ° / 21.6 ° / 47.6 ° 14.9° / 21.6° / 47.6° 14.9°/21.6°/47.6°）
- 但夹取点本身够用，箭头指向也正确，有偏差的只是 PCA 那个盒子
然后调一次扫描服务，机械臂会依次前往每个物体的正上方对准，进行详细的 OBB 计算：

bash 复制代码

ros2 service call /grasp_obb/scan std_srvs/srv/Trigger

扫描完成后，机械臂回到原位，日志中每个物体的标签由 oblique 变为 nadir

rviz2 中可以看到：红色和蓝色是分离的点云，橙色箭头是 approach 轴（从物体中心沿平面法向朝外画出，与 /best_grasp 里指向物体内部的列 0 恰好相反），绿色方块是 OBB 盒子

4 夹取位姿计算与状态机串连

4-1 夹取位姿计算

通过上述计算，我们已经有了 OBB，据此就能估算夹取位姿
核心就是：

cpp 复制代码

std::optional<Grasp> topdownGrasp(const ObjectEstimate & est)
{
  const Vec3 n = est.R_box.col(2);
  const Vec3 closing = est.R_box.col(0);
  const Vec3 col1 = closing.cross(n);
  const double norm = col1.norm();
  if (norm < 1e-9) {                      // 退化：closing 与法向平行，构不出右手系
    return std::nullopt;
  }
  Mat3 R;
  R.col(0) = n;
  R.col(1) = col1 / norm;
  R.col(2) = closing;
  if (R.determinant() < 0.0) {
    return std::nullopt;
  }
  Grasp g;
  g.position = est.center;
  g.R = R;
  g.width = est.width;
  return g;
}

但这份 R \mathbf{R} R 还不能直接发出去 ：/best_grasp 用的约定是"列 0 = approach 轴指向物体内部 "，而这里列 0 取的是平面法向 n \mathbf{n} n，指向物体外部
- 发布前要把列 0 与列 2 同时取反 ------ 只反一列会让 d e t = − 1 det = -1 det=−1，两列一起反才保住 d e t = + 1 det = +1 det=+1，拿到的仍是合法姿态
- 列 2（闭合轴）跟着反号不影响夹取：两指沿同一条直线合拢，方向正反等价
扫描阶段还需要另一个位姿 ------ 让相机位于物体正上方的那个位姿。它同样不能直接执行：
- topdownGrasp 给出的是"夹取点该在哪"，而机械臂能执行的是法兰位姿
- 相机并不装在法兰原点上，它与法兰之间隔着一个固定偏置 t l c \mathbf{t}_{lc} tlc；要让相机到某处，法兰必须去另一个地方
设相机在法兰系下的姿态与位置为 R l c , t l c \mathbf{R}{lc}, \mathbf{t}{lc} Rlc,tlc（由 TF 现查，不写死），下标 f f f 记法兰、 c c c 记相机目标位置，则

R f = R w c R l c ⊤ , p f = p c − R f t l c \mathbf{R}{f} = \mathbf{R}{wc}\,\mathbf{R}{lc}^{\top}, \qquad \mathbf{p}{f} = \mathbf{p}{c} - \mathbf{R}{f}\,\mathbf{t}_{lc} Rf=RwcRlc⊤,pf=pc−Rftlc
- 第一式把"相机在世界系的姿态"换成"法兰在世界系的姿态"
- 第二式把偏置从法兰系旋到世界系后减去 ------ 相机要在哪，法兰就退到它后面 t l c \mathbf{t}_{lc} tlc 处
相机目标位姿的朝向同样不写死：光轴（光学系 + Z +Z +Z）取 − n -\mathbf{n} −n 正对下方； x x x 轴取启动位姿的相机 x x x 轴投影到面内，好让探测姿态离启动位姿不远，利于规划

cpp 复制代码

  bool flangeForCameraAbove(const Vec3 & obj, geometry_msgs::msg::Pose & out)
  {
    Mat3 R_lc;
    Vec3 t_lc, x_ready;
    double h = 0.0;
    {
      std::lock_guard<std::mutex> lk(tf_mutex_);
      if (!cam_geom_ok_) {
        return false;
      }
      R_lc = R_link8_cam_;
      t_lc = t_link8_cam_;
      x_ready = cam_x_ready_;
      h = cam_height_;
    }
    Vec3 n;
    {
      std::lock_guard<std::mutex> lk(mutex_);
      n = plane_n_;
    }

    const Vec3 p_cam(obj.x(), obj.y(), h);

    const Vec3 z_c = -n;
    Vec3 x_c = x_ready - x_ready.dot(n) * n;
    if (x_c.norm() < 1e-6) {
      Vec3 u, v;
      inplaneBasis(n, u, v);
      x_c = u;
    }
    x_c.normalize();
    const Vec3 y_c = z_c.cross(x_c);
    Mat3 R_world_cam;
    R_world_cam.col(0) = x_c;
    R_world_cam.col(1) = y_c;
    R_world_cam.col(2) = z_c;

    const Mat3 R_target = R_world_cam * R_lc.transpose();
    const Vec3 p_flange = p_cam - R_target * t_lc;

    out.position.x = p_flange.x();
    out.position.y = p_flange.y();
    out.position.z = p_flange.z();
    const Eigen::Quaterniond q(R_target);
    out.orientation.x = q.x();
    out.orientation.y = q.y();
    out.orientation.z = q.z();
    out.orientation.w = q.w();
    return true;
  }

探测高度取启动位姿的相机高度，FOV 与平面锁定行为因此都不变

说人话：想的是"相机到正上方"，发给机械臂的却必须是"法兰到哪"。两者差一个固定偏置，所以得先换算；换算用的偏置从 TF 现查，不把 0.078 写死

4-2 跨帧跟踪

到这里，每一帧都能独立给出一组 OBB 与夹取位姿了。但把帧连起来看有三个麻烦：
- 单帧估计带噪声，盒子每帧都在抖，原样发出去机械臂会跟着抖
- 同一帧里各物体的视角未必一样，一个斜视帧就能把上一帧辛苦拿到的正上方精估冲掉
- 选物是"某一刻调一次服务、之后连续发很多帧"，需要一个稳定的身份把前后接起来
这个角色由 tracker 担任。它只做三件事，但顺序不能换 （tracker.hpp 开头就写着这句）：

顺序	做什么	换顺序会怎样
1	关联：按世界系最近中心认人，门限从数据推	认错人会把两个物体的估计混进同一个滑窗
2	y a w yaw yaw 先消歧、再平滑	先平滑会把 0 ° 0° 0° 与 90 ° 90° 90° 糊成 45 ° 45° 45°，宽度反涨 41%
3	质量只升不降	斜视帧会覆盖掉刚拿到的正上方精估

4-2-1 关联

为什么不能用点云里那个 float32 id：它是 DBSCAN 的标号，随点序变化；而且扫描时机臂在动、视角大变，帧内编号毫无延续性。位置才是稳定的
所以按世界系里的几何距离 认人。门限同样不写死，取当前帧物体两两最小中心间距的 assoc_gate_frac 倍 ------ 物体间距本身就是数据给的：

cpp 复制代码

double Tracker::computeGate(const std::vector<ObjectEstimate> & ests) const
{
  if (ests.size() < 2) {
    return std::numeric_limits<double>::quiet_NaN();
  }
  double best = std::numeric_limits<double>::infinity();
  for (size_t i = 0; i < ests.size(); ++i) {
    for (size_t j = i + 1; j < ests.size(); ++j) {
      best = std::min(best, (ests[i].center - ests[j].center).norm());
    }
  }
  return best * prm_.assoc_gate_frac;
}

只有一个物体时返回 NaN，含义是不设门限 ------ 没有可混淆的对象，挑谁都是它
认领时每个 track 一帧只允许被一个观测认走（used 集合），认不到就新建一条；正上方观测认不到也照样新建，因为那说明物体是新出现的：

cpp 复制代码

  std::unordered_set<int> used;
  for (const auto & o : obs) {
    const ObjectEstimate & e = o.first;
    const Quality q = o.second;

    Track * best = nullptr;
    double best_d = std::numeric_limits<double>::infinity();
    for (auto & t : tracks_) {
      if (used.count(t.tid) != 0u) {
        continue;
      }
      const double d = (e.center - t.est.center).norm();
      if (d < best_d) {
        best = &t;
        best_d = d;
      }
    }

    if (best != nullptr && (!has_gate || best_d <= gate_)) {
      used.insert(best->tid);
      best->observe(e, q, now, prm_.aniso_yaw_min);
    } else {
      // 认不到就是新物体（正上方观测认不到也一样：说明它新出现）
      tracks_.emplace_back(next_tid_, e, q, now, prm_.window);
      used.insert(next_tid_);
      ++next_tid_;
    }
  }

门限还有第二个用处：选物服务的拒绝阈值就是它的 2 倍（4-3），日志里也会把实际用的值印出来

4-2-2 平滑

窗口本身很朴素：若干个量各开一个滑窗取中值。唯独 θ \theta θ 不能直接取中值 ------ 角度有环绕， 179 ° 179° 179° 与 1 ° 1° 1° 的中值不是 90 ° 90° 90° 而是 0 ° 0° 0°。做法是先把样本平移到参考点附近，再取中值：

cpp 复制代码

double medianAngle(const std::deque<double> & angles)
{
  if (angles.empty()) {
    return 0.0;
  }
  const double ref = angles.front();
  std::vector<double> d;
  d.reserve(angles.size());
  for (double a : angles) {
    d.push_back(modPi(a - ref + M_PI / 2.0) - M_PI / 2.0);
  }
  return modPi(ref + medianOf(d));
}

更麻烦的是 90 ° 90° 90° 歧义：正方形足迹的 0 ° 0° 0° 与 90 ° 90° 90° 都是极小值，逐帧 argmin 会在两者之间跳（2-4-1）
- 若先平滑，窗口里同时含 { 0 ° , 90 ° } \{0°, 90°\} {0°,90°}，中值正好是 45 ° 45° 45° ------ 那正是正方形的对角线方向，宽度涨 41%，与"取最窄方向"的意图正好相反
- 所以顺序必须是：先把上一帧的平滑值当先验传给 pickDirection（3-3），让它挑离上一帧最近的等价候选；再进中值窗口
落到节点里就是两遍估计：第一遍不带先验，只为了拿到中心去定位是哪条 track；取出它的平滑值当先验，再估第二遍：

cpp 复制代码

      std::optional<double> theta_prev;
      {
        std::lock_guard<std::mutex> lk(mutex_);
        const Track * t = nearestTrack(first->center);
        if (t != nullptr) {
          theta_prev = t->smoother();
        }
      }
      if (theta_prev.has_value()) {
        EstimateParams p2 = prm;
        p2.theta_prev = theta_prev;     // 让 pickDirection 在等价候选里挑离上一帧最近的
        auto second = estimateObject(kv.second, n, c, p2);
        if (second.has_value()) {
          obs.emplace_back(*second, q);
          continue;
        }
      }
      obs.emplace_back(*first, q);

最后一行是兜底：第二遍估不出来（例如足迹退化）就退回第一遍的结果，而不是把这个物体整个丢掉
这个坑在静止仿真里复现不出来 ：每帧点云完全相同，argmin 每帧落在同一处，根本不会跳。仓库里是靠合成的翻转序列测的（test_obb.cpp 的 yaw 翻转回归）------ 也就是说只跑仿真看结果，这个 bug 会一直潜伏到上真机才发作

4-2-3 质量只升不降

物体是静止的，扫描一轮就够。所以某物体一旦拿到正上方（NADIR）的估计，后续斜视帧不得覆盖它
- 关键区分是"刷新存活时间"与"更新估计"是两件事：斜视帧照样刷新 last_seen，只是不改数值

cpp 复制代码

bool Track::observe(const ObjectEstimate & e, Quality q, double now, double aniso_yaw_min)
{
  last_seen = now;
  if (q < quality) {
    return false;                       // 斜视帧只刷新存活时间，不覆盖 nadir 估计
  }
  quality = std::max(quality, q);
  push(e);
  // ...（下面用滑窗中值覆盖中心、两个水平跨度、高度、theta 与各向异性比）

通过守卫的这一帧，中值只作用在滑窗内的量上，其余字段（pca_*、resting、notes、n_points）沿用本帧的
退役也分两级，与质量一一对应：

cpp 复制代码

  for (auto & t : tracks_) {
    const double age = now - t.last_seen;
    const double limit = (t.quality == Quality::NADIR) ? prm_.nadir_hold_s : prm_.max_age_s;
    if (age <= limit) {
      keep.push_back(std::move(t));
    }
  }

斜视 track 2 秒没见到就丢；正上方 track 留 10 秒 ------ 它是"结果"，扫描途中物体短暂移出视野，不能把刚精估出来的结果一起丢掉。这一条直接支撑 4-4 状态机的等待逻辑
返回前按 tid 排序，保证 RViz 里的编号不会乱跳
还有一处要如实说明：observe 里算了 est.yaw_defined（各向异性比是否超过 isotropy_ratio_max，见 3-3），但下游没有任何地方读它 ------ 它目前只是一个放在估计里、供日志与人工查看的诊断量

说人话：单帧算出来的盒子会抖，所以要拿一个滑窗把同一个物体的历次估计压成一条稳定的值。难点不在窗口，在顺序 ------ 正方形有两个等价的"最窄方向"，必须先让这一帧的方向跟上一帧对齐，再取中值；反过来做，中值会给出一个根本不存在的 45°。

4-3 物体选择

至此，OBB 已经能对场景里每个物体各返回一个结果 ------ 有几个物体，/grasp_obb/poses 里就有几个位姿。但一次只能夹一个，所以还需要一个动作把目标从多个结果里挑出来：

bash 复制代码

./4_select_object.sh 0.56 -0.03     # 蓝方块
./4_select_object.sh 0.46 -0.10     # 红方块
./4_select_object.sh 0.52  0.09     # 绿圆柱

这个脚本做的事就是调一次选物服务，锚点 ( x , y ) (x, y) (x,y) 是目标物体在世界系里的大致水平坐标；三个物体各给各的，换物体就换一组数
需要说明的是，之所以不用 tid 标注物体，是因为 tid 会退役重建 ：正上方的轨迹有保留期，超时就不再跟踪，同一个物体下次出现时会拿到一个全新的编号（实测扫描途中 #3 变成了 #4）------ 编号会变，位置不会
我们这样实现：

cpp 复制代码

  void selectCb(const grasp_interfaces::srv::SelectObject::Request::SharedPtr req,
                grasp_interfaces::srv::SelectObject::Response::SharedPtr res)
  {
    const Vec2 anchor(req->x, req->y);
    std::lock_guard<std::mutex> lk(mutex_);

    const Track * best = nullptr;
    double best_d = std::numeric_limits<double>::infinity();
    for (const auto & t : tracker_->tracks()) {
      const double d = (t.est.center.head<2>() - anchor).norm();
      if (d < best_d) {
        best_d = d;
        best = &t;
      }
    }

    if (best == nullptr) {
      res->success = false;
      res->message = "还没看到任何物体（点云里没有可分组的 id），没得选";
      return;
    }
    const double gate = tracker_->gate();
    if (std::isfinite(gate) && best_d > 2.0 * gate) {
      res->success = false;
      char buf[160];
      std::snprintf(buf, sizeof(buf),
                    "锚点 (%.3f, %.3f) 离最近的物体还有 %.1f mm（门限 %.1f mm）："
                    "附近没有物体，不选", anchor.x(), anchor.y(), best_d * 1000.0,
                    gate * 2000.0);
      res->message = buf;
      return;
    }
    if (!best->est.graspOk()) {
      std::string why;
      for (size_t k = 0; k < best->est.notes.size(); ++k) {
        why += (k ? " | " : "") + best->est.notes[k];
      }
      res->success = false;
      res->message = "最近的物体没过守卫，不接：" + (why.empty() ? "（notes 为空）" : why);
      return;
    }

    has_selection_ = true;
    select_anchor_ = anchor;
    select_refuse_warned_ = 0;

    grasp_interfaces::msg::BestGrasp bg;
    if (!toBestGrasp(best->est, scan_frame_, now(), bg)) {
      has_selection_ = false;
      res->success = false;
      res->message = "顶视夹取位姿构造失败（足迹退化：闭合轴与 approach 共线）";
      return;
    }
    res->pose = bg.pose;
    res->width = bg.width;
    res->success = true;
    char buf[220];
    std::snprintf(buf, sizeof(buf),
                  "选中 tid %d（%s）：中心 (%.4f, %.4f, %.4f)，宽度 %.1f mm，"
                  "水平偏移 %.1f mm；开始每帧发 %s",
                  best->tid, qualityName(best->quality).c_str(), best->est.center.x(),
                  best->est.center.y(), best->est.center.z(), bg.width * 1000.0,
                  best_d * 1000.0, best_grasp_topic_.c_str());
    res->message = buf;
    RCLCPP_INFO(get_logger(), "%s", res->message.c_str());
  }

选中之后，每一帧 都重新解析一次锚点再发 /best_grasp：
- 解析的是位置而非编号，所以 tid 退役重建也不会选丢
- 解析不到或守卫没过就不发 ------ 与其发一个当下已经站不住的位姿，不如让 grasp_execute 手里只剩上一帧那份还算数的
- 失败按理由去重报一次，不按帧刷屏
脚本在服务调用之后还多做了一件事：核对 /best_grasp 是否真的在发
- 点云一旦断流，select 仍会拿记忆里那条冻结的轨迹报 success=True，而发布写在点云回调里，于是永远发不出来
- 若不核对，要等到 ./5_trigger_grasp.sh 报 no /best_grasp cached yet 才发现

说人话：给一个大致坐标，程序挑离它最近的那个物体。用坐标而不是编号，是因为编号会变、位置不会；调完还当场验一下位姿有没有真的发出来，免得"报成功其实没发"

4-4 状态机

上述流程串通以后，需要一个状态机把整个扫描过程管起来。它只有四态，且没有回头边 ：
- IDLE 持续用斜视点云出估计并发布，机械臂不动
- ROUGH 从当前斜视点云取各物体的粗略位置，定下扫描顺序，同时记下此刻的关节角
- PROBING(i) 算出"让相机位于物体 i i i 正上方"的法兰目标位姿（见 4-1），规划过去，停稳后做一次精估
- RETURN 回到扫描开始时的关节位姿
IDLE 到 ROUGH 由服务回调完成 ------ 四道拒绝全部在这里，通过之后立刻返回，扫描跑在独立线程上：

cpp 复制代码

  void scanCb(const std_srvs::srv::Trigger::Request::SharedPtr,
              std_srvs::srv::Trigger::Response::SharedPtr res)
  {
    std::vector<Vec3> targets;
    std::vector<double> start_joints;
    {
      std::lock_guard<std::mutex> lk(tf_mutex_);
      if (!cam_geom_ok_) {
        res->success = false;
        res->message = "还没拿到 TF（" + scan_frame_ + " -> " + camera_frame_ +
                       "），算不出相机偏置，不扫";
        return;
      }
    }
    if (!move_group_) {
      res->success = false;
      res->message = "MoveGroupInterface 未初始化";
      return;
    }
    {
      std::lock_guard<std::mutex> lk(mutex_);
      if (!plane_locked_) {
        res->success = false;
        res->message = "尚未锁定桌面平面（scene_filter 还没发出 " + plane_topic_ +
                       "），无法定竖直和高度";
        return;
      }
      if (scanning_) {
        res->success = false;
        res->message = "已在扫描中（第 " + std::to_string(scan_index_ + 1) + "/" +
                       std::to_string(scan_total_) + " 个，阶段 " + scan_stage_ +
                       "），不接受并发调用";
        return;
      }
      if (tracker_->tracks().empty()) {
        res->success = false;
        res->message = "还没看到任何物体（点云里没有可分组的 id），不扫";
        return;
      }
      for (const auto & t : tracker_->tracks()) {
        targets.push_back(t.est.center);
      }
      if (got_joints_) {
        for (const auto & name : arm_joints_) {
          const auto it = std::find(joint_states_.name.begin(), joint_states_.name.end(),
                                    name);
          if (it != joint_states_.name.end()) {
            start_joints.push_back(
              joint_states_.position[static_cast<size_t>(
                std::distance(joint_states_.name.begin(), it))]);
          }
        }
      }
      scanning_ = true;
      scan_index_ = 0;
      scan_total_ = static_cast<int>(targets.size());
      scan_stage_ = "rough";
    }

    RCLCPP_INFO(get_logger(), "收到扫描触发：%zu 个物体", targets.size());
    auto self = std::static_pointer_cast<ObbNode>(shared_from_this());
    std::thread([self, targets, start_joints]() {
      self->runScan(targets, start_joints);
    }).detach();

    res->success = true;
    res->message = "扫描已开始：依次探测 " + std::to_string(targets.size()) +
                   " 个物体，进度看日志、结果看 RViz";
  }

ROUGH 之后由 runScan 串起 PROBING(i) 与 RETURN：
- 上游是否存活只在开始移动之前判一次 ------ 扫描期间机械臂自己在动，物体整段出画是正常的，逐物体复查会把正常出画当上游死亡
- 每个点位两步走，缺一不可：先等臂停稳（凑够 dwell_frames 帧新点云），再等结果 ------ 该物体真的拿到了正上方的估计
- RETURN 只由 return_to_start 决定，中途失败也要回位，否则机械臂会被丢在探测位姿上

cpp 复制代码

  void runScan(std::vector<Vec3> targets, std::vector<double> start_joints)
  {
    if (isStale()) {
      RCLCPP_ERROR(get_logger(), "上游点云已失效（scene_filter 挂了？），未开始移动即中止扫描");
      std::lock_guard<std::mutex> lk(mutex_);
      scanning_ = false;
      scan_stage_ = "idle";
      scan_index_ = scan_total_ = 0;
      stale_ = false;
      last_cloud_wall_ = nowSeconds();
      return;
    }
    bool ok = true;
    for (size_t i = 0; i < targets.size(); ++i) {
      if (!rclcpp::ok()) {
        ok = false;
        break;
      }
      {
        std::lock_guard<std::mutex> lk(mutex_);
        scan_index_ = static_cast<int>(i);
        scan_stage_ = "probing";
      }

      geometry_msgs::msg::Pose flange;
      if (!flangeForCameraAbove(targets[i], flange)) {
        RCLCPP_ERROR(get_logger(), "物体 %zu：算不出探测位姿", i);
        ok = false;
        break;
      }
      RCLCPP_INFO(get_logger(), "物体 %zu/%zu：相机移到 [%.3f %.3f] 正上方，高度 %.3f",
                  i + 1, targets.size(), targets[i].x(), targets[i].y(), cam_height_);
      if (!moveToPose(flange)) {
        RCLCPP_ERROR(get_logger(), "物体 %zu：规划/执行失败", i);
        ok = false;
        break;
      }
      const uint64_t since = nadir_seq_.load();
      if (!waitForFreshFrames()) {
        RCLCPP_WARN(get_logger(), "物体 %zu：等新点云超时（%.1fs 没凑够 %d 帧），仍继续",
                    i + 1, dwell_timeout_, std::max(1, dwell_frames_));
      }
      if (!waitForNadir(targets[i], since, dwell_timeout_)) {
        RCLCPP_WARN(get_logger(),
                    "物体 %zu：等了 %.1fs 仍未拿到正上方的估计 ------ 这一站按失败记，"
                    "该物体保持原有质量（不升级、也不假装探测成功）",
                    i + 1, dwell_timeout_);
      }
      logViewAngles();
    }

    {
      std::lock_guard<std::mutex> lk(mutex_);
      scan_stage_ = "return";
    }
    if (return_to_start_ && !start_joints.empty()) {
      RCLCPP_INFO(get_logger(), "回到扫描前的关节位姿");
      if (!moveToJoints(start_joints)) {
        RCLCPP_ERROR(get_logger(), "回到起始位姿失败");
      }
    } else if (!start_joints.empty()) {
      RCLCPP_WARN(get_logger(), "return_to_start 为假，机械臂留在最后一个探测位姿");
    }

    {
      std::lock_guard<std::mutex> lk(mutex_);
      scanning_ = false;
      scan_stage_ = "idle";
      scan_index_ = scan_total_ = 0;
      last_cloud_wall_ = nowSeconds();
      stale_ = false;
    }
    RCLCPP_INFO(get_logger(), "扫描结束%s", ok ? "" : "（中途失败，见上面日志）");
  }

说人话：常驻态一直在估，一调用服务就切进"挨个走过去看"的流程，看完回原位。状态只有前进没有后退，失败也只是跳过这一站，绝不半路卡死

4-5 完整链路

至此三个节点各就各位，整条链路是单向的：先分割、再估计、最后执行

节点	包	订阅	发布	服务 / 动作
`scene_filter`	`grasp_scene`（Python）	相机点云	`/grasp_scene/object_points`、`/grasp_scene/environment_points`、`/grasp_scene/table_plane`、`/grasp_scene/plane_band`	无
`grasp_obb`	`grasp_obb`（C++）	`/grasp_scene/object_points`、`/grasp_scene/table_plane`、`/grasp_scene/plane_band`、`/joint_states_clean`	`/grasp_obb/markers`、`/grasp_obb/poses`、`/best_grasp`	`/grasp_obb/scan`、`/grasp_obb/select`
`grasp_execute`	`grasp_execute`（C++）	`/best_grasp`、`/joint_states_clean`	无	`/grasp_execute`（action）

QoS 不是细节，配错会静默失败 ：
- /grasp_scene/object_points 由 scene_filter 以 qos_profile_sensor_data（BEST_EFFORT）发布；订阅端若用默认的 RELIABLE，会一条都收不到且不报错
- /grasp_scene/table_plane 与 /grasp_scene/plane_band 是常量性质的数据，用 TRANSIENT_LOCAL latch，晚加入的订阅者也要能立刻拿到 ------ grasp_obb 因此可以后启动
- /grasp_obb/markers 用 RELIABLE，因为 RViz 的 MarkerArray 显示按可靠订阅
/best_grasp 是这条链路里唯一的耦合点 ：
- 它由 grasp_interfaces/BestGrasp 定义（接触点位姿 + 宽度 + 分数），grasp_obb 发、grasp_execute 收，两者互不知道对方是谁
- 所以第 5 期换 GraspNet 时只需换掉发布者，执行器一行都不用改
- grasp_obb 发的 score 恒为 0 0 0 ------ 这是几何解析出来的，本就没有学习出来的质量分；造一个假分数会让下游分不清"算出来的"和"猜出来的"
三个节点分别在三个终端里起，顺序不能颠倒：

bash 复制代码

./1_depth_gazebo.sh     # Gazebo + 相机 + scene_filter + move_group
./2_grasp_obb.sh        # grasp_obb
./3_grasp_execute.sh    # grasp_execute

scene_filter 随第一个脚本的 launch 一同启动；move_group 的 octomap 吃 /grasp_scene/environment_points，所以待抓物体不会被当成障碍
grasp_obb 与 grasp_execute 的 launch 都把 /joint_states 重映射为 /joint_states_clean，绕开 _mimic 假关节（见 0-2）------ 上表两行写的都是重映射之后的名字

说人话：分割负责把物体从桌面里挑出来，估计负责算出盒子，执行负责伸手去夹。三者只通过话题互相认识，所以换掉中间那个，两头都不用动

4-6 完整测试

按顺序打开五个终端。第一步，启动仿真：

bash 复制代码

./1_depth_gazebo.sh

第二步，启动 OBB 估计节点：

bash 复制代码

./2_grasp_obb.sh

第三步，启动抓取执行节点：

bash 复制代码

./3_grasp_execute.sh

此时可在 rviz2 中看到三个物体的 OBB 与夹取姿态。接下来选定一个物体，锚点取其中心坐标（本场景中蓝方块位于 ( 0.56 , − 0.03 ) (0.56, -0.03) (0.56,−0.03)）：

bash 复制代码

./4_select_object.sh 0.56 -0.03

最后触发抓取：

bash 复制代码

./5_trigger_grasp.sh

可以看到蓝方块被原地抓起

绿色圆柱也是同理

附录工程外壳

由于篇幅限制和整理项目的综合考量，本文不再嵌入完整的运行代码，但是考虑到如今AI时代发达，想必各位还是能够根据核心文件进行复现
此外这一节给的是最小集合，对照正文的算法函数补齐即可复现整个包

附-1 接口定义

三个接口都放在 grasp_interfaces 包里，它是 grasp_obb 与 grasp_execute 共用的中间层：
- msg/BestGrasp 是 /best_grasp 的类型，pose 的列 0 指向物体内部（约定见 4-1）
- srv/SelectObject 的请求是两个浮点数而不是物体编号，因为编号会变、位置不会（理由见 4-3）
- action/GraspExecute 把一次抓取分成预抓取、下探、闭合、抬升四段，反馈里带回当前处在哪一段
BestGrasp 只有三个字段，其中 score 在传统方案里恒为 0 0 0：

text 复制代码

# msg/BestGrasp.msg
geometry_msgs/PoseStamped pose   # 接触点位姿：position=接触点 t，orientation=抓取 R（approach 轴指向物体内部）
float32 width                    # 抓取宽度（5cm 方块≈0.05），闭合位置≈width/2
float32 score

选物服务只回一个位姿加一个宽度，宽度是从点云实时量出来的，调用方不必再猜：

text 复制代码

# srv/SelectObject.srv
float64 x
float64 y
---
bool success
string message
geometry_msgs/PoseStamped pose
float32 width

use_best_grasp 为真时执行器走 /best_grasp，为假时才读请求里显式给的位姿，因此这个接口对传统方案与学习式方案都成立：

text 复制代码

# action/GraspExecute.action
bool use_best_grasp        # true(默认)=用 graspnet_node 发布的最新 /best_grasp；false=用下面显式 target
geometry_msgs/Pose target  # world 系 approach 系接触点位姿（use_best_grasp=false 时使用）
---
bool success
string message
---
string stage               # PRE_GRASP / APPROACH / CLOSE / LIFT / DONE

接口包自己不写一行算法，构建文件只把这三个文件交给 rosidl 登记，并在 package.xml 里声明它属于 rosidl_interface_packages 成员组 ------ 漏了这条，消息不会被生成

附-2 参数文件

下面是 grasp_obb/config/obb.yaml。参数名与数值与原文件逐字一致，注释只留了行尾那句话 ------ 原文件里成段的设计说明已经删掉（那些理由第 3、4 章都讲过）：

yaml 复制代码

grasp_obb:
  ros__parameters:
    # ---- 接口名 ----
    object_cloud_topic: /grasp_scene/object_points   # 带 float32 `id` 字段
    plane_topic: /grasp_scene/table_plane            # PoseStamped，TRANSIENT_LOCAL
    markers_topic: /grasp_obb/markers
    poses_topic: /grasp_obb/poses
    scan_service: /grasp_obb/scan                    # std_srvs/Trigger
    select_service: /grasp_obb/select                # grasp_interfaces/SelectObject
    best_grasp_topic: /best_grasp                    # grasp_execute 吃的抓取位姿
    scan_frame: world
    camera_frame: camera_optical_frame
    tip_link: panda_link8
    planning_group: panda_arm

    # ---- 传感器属性：深度噪声尺度 ----
    depth_noise_sigma: 0.002      # [m]
    top_slice_sigmas: 4.0         # 顶面切片带宽 = 该倍数 × sigma
    top_slice_iters: 3            # 迭代收紧次数

    # ---- 估计器：最小宽度扫描 ----
    n_theta: 361                  # 面内扫描角数（步长 0.5°）
    width_tol_m: 0.002            # "宽度接近最小值"的候选带宽
    isotropy_ratio_max: 1.10

    # ---- 任务属性：夹爪 ----
    gripper_open: 0.08            # 夹爪最大开口 [m]。来自机器人硬件，非场景。
    min_graspable_width: 0.005    # 比这还窄就不认为还能夹（只是细/退化簇）
    min_height: 0.005             # 低于此高度视为贴面噪点而非物体

    # ---- 任务属性：物体是否坐在平面上 ----
    plane_band_topic: /grasp_scene/plane_band  # Float64，latched（scene_filter 发）
    resting_margin: 0.008         # 最低可见点高出 band 多少以内算"坐住" [m]

    # ---- 任务属性：时序 ----
    window: 7                     # 中值滑窗长度（帧）
    assoc_gate_frac: 0.5          # 关联门限 = 该比例 × 当前帧最小中心间距（从数据推）
    max_age_s: 2.0                # 斜视 track 多久没见到就退役
    nadir_hold_s: 10.0            # 正上方 track 是"结果"，留得久一点

    # ---- 质量判据：这一帧算不算"正上方" ----
    nadir_angle_max_deg: 5.0

    # ---- 主动扫描 ----
    dwell_frames: 5               # 到达探测位后要凑够几帧新点云才算"臂停稳了"
    dwell_timeout: 5.0            # 等臂停稳、以及等"这个物体真的拿到正上方估计"的上限
    stale_timeout: 2.0            # 多久没收到点云就认为上游断了，发 DELETEALL 清标记
    return_to_start: true         # 扫描完回到扫描前的关节位姿

    # ---- 规划（硬件/任务属性）----
    planning_time: 10.0           # MoveIt allowed_planning_time [s]
    velocity_scale: 0.3
    acceleration_scale: 0.3
    num_planning_attempts: 10

    use_sim_time: true

这些量的出处都在正文里：isotropy_ratio_max 见 3-3，resting_margin 与夹爪两个门限见 3-4，nadir_angle_max_deg 见 3-5，扫描时序见 4-4
打开 BUILD_TESTING 时，test/test_obb.cpp 把这些实测数字钉成了断言，只链接几何核心、不需要仿真，可以随时复跑

附-3 构建文件

grasp_obb 的 CMakeLists.txt 里只有三段是漏了就编不过的：几何核心单独成静态库（这样它能脱离 ROS 被 gtest 链接）、节点链上接口包、launch 与 config 要 install 出去

cmake 复制代码

cmake_minimum_required(VERSION 3.8)
project(grasp_obb)

set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)

find_package(ament_cmake REQUIRED)
find_package(eigen3_cmake_module REQUIRED)
find_package(Eigen3 REQUIRED)

add_library(grasp_obb_core STATIC
  src/obb.cpp
  src/tracker.cpp
)
target_include_directories(grasp_obb_core PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>
)
target_link_libraries(grasp_obb_core PUBLIC Eigen3::Eigen)

find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(visualization_msgs REQUIRED)
find_package(std_srvs REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2_eigen REQUIRED)
find_package(moveit_msgs REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
find_package(grasp_interfaces REQUIRED)

add_executable(grasp_obb_node
  src/obb_node.cpp
  src/markers.cpp
)
target_link_libraries(grasp_obb_node grasp_obb_core)
ament_target_dependencies(grasp_obb_node
  rclcpp
  sensor_msgs
  geometry_msgs
  visualization_msgs
  std_srvs
  tf2_ros
  tf2_eigen
  moveit_msgs
  moveit_ros_planning_interface
  grasp_interfaces
)

install(TARGETS grasp_obb_node
  DESTINATION lib/${PROJECT_NAME})

install(DIRECTORY launch config
  DESTINATION share/${PROJECT_NAME})

package.xml 的依赖与上面的 find_package 一一对应，另外三个 exec_depend 是运行期才需要的：

xml 复制代码

  <buildtool_depend>ament_cmake</buildtool_depend>
  <buildtool_depend>eigen3_cmake_module</buildtool_depend>

  <!-- 其余 depend 项与上面的 find_package 一一对应（rclcpp / sensor_msgs / tf2_ros / moveit_msgs 等）-->
  <depend>grasp_interfaces</depend>
  <depend>eigen</depend>

  <exec_depend>grasp_scene</exec_depend>
  <exec_depend>moveit_configs_utils</exec_depend>
  <exec_depend>panda_gazebo_bringup</exec_depend>

到这里，正文的算法函数加这一节的三个文件，就是一个能 colcon build 出可执行节点的完整包

总结

本期用纯几何 的办法让一台传统机械臂把方块夹了起来：第 0 章修好夹爪与 joint_states，第 1 章用顺序 RANSAC 加 DBSCAN 把场景拆成桌面与物体两路，第 2、3 章从 PCA 的失效走到妥协 OBB（竖直轴取桌面法向、水平轴取最窄方向扫描、竖直跨度取切片中值），第 4 章完成位姿换算、跨帧跟踪、四态扫描状态机与选物守卫，最终把位姿发到 /best_grasp。代价是 approach 固定在平面法向上，只能从正上方下压，姿态也只可观测到物体的对称群为止 ------ 这是手写几何本身的上限，而非 bug。因此下一期换学习式方法逐点预测 approach，grasp_execute 原样复用，再往后才把语言指令接进来。
如有错误，欢迎指出！感谢观看！

【玩转VLA具身智能机械臂】（四）：点云分割与 OBB 夹取——从 PCA 失效到主动扫描

前言

文章目录

0 修复

0-1 末端夹爪修复

0-2 过滤节点

0-3 Grasp Fix 插件

插件安装

1 点云分割

1-1 回顾

1-2 常用方法介绍

1-3 RANSAC 平面拟合

1-3-1 介绍

1-3-2 平面 Π 的紧凑参数化

1-3-3 内点判据

1-3-4 样本抽取

1-3-5 代码实现

1-4 顺序 RANSAC 与"主导度"

1-4-1 顺序 RANSAC

1-4-2 内点主导度

1-5 欧式聚类

1-5-1 介绍

1-5-2 数学定义

1-5-3 代码实现

1-5-4 筛选

1-5-5 DBSCAN结果发布

1-6 平面持久化

1-6-1 背景

1-6-2 共面判定

1-6-3 平面硬化

1-7 参数配置

1-8 运行测试

2 Object Pose与PCA

2-1 介绍

2-2 常用方法

2-3 PCA 数学原理

2-3-1 介绍

2-3-2 去中心化

2-3-3 协方差矩阵

2-3-4 最大方差与特征分解

2-3-5 代码实现：pcaObb

2-4 PCA的局限

2-4-1 正方体PCA估算问题

2-4-2 斜视估计问题

2-4-3 高度估计问题

2-4-4 最好的PCA方法

3 OBB 数学原理

3-1 介绍

3-2 标准OBB

3-3 扫描 OBB

3-4 两者OBB合并

3-5 粗估计与细估计

3-6 测试

4 夹取位姿计算与状态机串连

4-1 夹取位姿计算

4-2 跨帧跟踪

4-2-1 关联

4-2-2 平滑

4-2-3 质量只升不降

4-3 物体选择

4-4 状态机

4-5 完整链路

4-6 完整测试

附录 工程外壳

附-1 接口定义

附-2 参数文件

附-3 构建文件

总结

附录工程外壳