ROS合集（七）SVIn2声呐模块分析

大鹅同志

于 2025-05-23 10:23:42 发布

阅读量487

点赞数 18

分类专栏： ROS 文章标签： SLAM Sonar ROV 水下SLAM

本文链接：https://blog.csdn.net/a_student_2020/article/details/148157812

版权

ROS 专栏收录该内容

9 篇文章

订阅专栏

文章目录

根据论文中给出的DPP-sonar误差计算方法及图示（图6），具体分析声呐误差的计算方式如下：

Here, we assume that the visual-feature based patch is small enough and approximately coplanar with the DPP-sonar point.

在这里，我们假设基于视觉功能的贴片足够小，并且与DPP-Sonar点大约是共面的。

一、整体思想

SVIn2方法使用DPP-sonar声呐传感器来辅助视觉特征，从而提高SLAM的精度和鲁棒性。具体而言：

声呐能够提供对障碍物的距离测量，不受视觉条件（如浑浊水体、光照变化）影响。
声呐的测量点与视觉特征的测量点之间存在一定空间差异，系统通过结合声呐测量点附近的视觉特征（patch）进行误差建模和优化。

二、具体误差建模流程

流程图示如论文图6所示，其中：

蓝色星形：声呐测量点
红色星形：视觉特征点
紫色线段：声呐测量点到视觉特征点群中心（patch）的误差距离
三角形绿色：关键帧（相机位置）
黄色方形：IMU测量位置

具体步骤如下：

三、总结明确（预测值与观测值）

四、选点逻辑

这整个「从声呐量测→世界坐标下的点→在视觉地标里找候选子集」的逻辑，其实就在 Estimator::addStates(...) 里，当你往 OKVIS 后端里挂声呐因子（SonarError）之前。具体是在这段代码里：

// Estimator::addStates(...)
// （前面省略了 IMU、深度、视觉部分）

// ———— Sonar ————
if (sonarMeasurements.size() != 0) {
  auto last_sonarMeasurement_it = sonarMeasurements.rbegin();
  double range   = last_sonarMeasurement_it->measurement.range;
  double heading = last_sonarMeasurement_it->measurement.heading;

  // 1) 把声呐读数〈range, heading〉变成声呐坐标系下的点
  okvis::kinematics::Transformation sonar_point(
      Eigen::Vector3d(range * cos(heading),
                      range * sin(heading),
                      0.0),
      Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0));

  // 2) 把它从声呐坐标系 T_SSo 換到世界坐标系
  okvis::kinematics::Transformation T_WSo       = T_WS * params.sonar.T_SSo;
  okvis::kinematics::Transformation T_WSo_point = T_WSo * sonar_point;
  Eigen::Vector3d sonar_landmark = T_WSo_point.r();   // ← 这就是“世界系下的声呐点”

  // 3) 在所有视觉地标里找“x,y,z 三个方向都离它不到 0.1m” 的那些点
  std::vector<Eigen::Vector3d> landmarkSubset;
  for (auto rit = landmarksMap_.rbegin(); rit != landmarksMap_.rend(); ++rit) {
    // 把每个视觉地标从齐次坐标转成 3D
    Eigen::Vector3d visual_landmark =
      (rit->second.point / rit->second.point[3]).head<3>();
    if (fabs(sonar_landmark[0] - visual_landmark[0]) < 0.1 &&
        fabs(sonar_landmark[1] - visual_landmark[1]) < 0.1 &&
        fabs(sonar_landmark[2] - visual_landmark[2]) < 0.1) {
      landmarkSubset.push_back(visual_landmark);
    }
  }

  // 4) 如果找到了不止一个，就把它们打包，构造一个 SonarError
  if (!landmarkSubset.empty()) {
    double information_sonar = 1.0;
    auto sonarError = std::make_shared<ceres::SonarError>(
        params, range, heading, information_sonar, landmarkSubset);
    mapPtr_->addResidualBlock(sonarError, NULL, poseParameterBlock);
  }
}

构造世界系下的声呐点：

okvis::kinematics::Transformation sonar_point(...);
T_WSo_point = T_WS * params.sonar.T_SSo * sonar_point;
Eigen::Vector3d sonar_landmark = T_WSo_point.r();

从一个点变成一组候选点：
就是上面对于 landmarksMap_ 的那一小段 for 循环，所有离 sonar_landmark 足够近的视觉地标都被 push 进了 landmarkSubset。

五、Sonar 数据处理流水线

1. ROS Launch 配置（imagenex831l.launch）

功能：启动 imagenex831l/sonar_node.py 节点。
关键配置：
- respawn="true"：节点意外退出后自动重启。
- respawn_delay="30"：重启前等待 30s。

2. SonarNode 节点（sonar_node.py）

初始化
- rospy.init_node('imagenex831l')
- 发布两个话题：
  - imagenex831l/range（处理后数据，ProcessedRange）
  - imagenex831l/range_raw（原始数据，RawRange）
- 启动 dynamic_reconfigure 服务器，支持运行时修改驱动参数。
参数回调
- 在 parameters_callback() 中，将 config 下发给驱动并返回更新后的配置。
主循环 (spin())
- 按设定频率 poll_frequency 读传感器：
  1. 调用 sensor.send_request() 和 sensor.read_data() 获取原始字节。
  2. 发布 RawRange：填充 header、data。
  3. 调用 sensor.interpret_data() 解析成 ProcessedRange。
  4. 发布到 imagenex831l/range。
- 若多次读取异常且超过 RESET_TIMEOUT，调用 rospy.signal_shutdown()。
清理
- 节点退出前，调用 sensor.close_connection() 关闭串口或网络。

3. Subscriber 订阅与回调（Subscriber.cpp）

在 setNodeHandle() 中：
- 订阅 /imagenex831l/range（ProcessedRange）并绑定 sonarCallback()。
- 仅在配置 vioParameters_.sensorList.isSonarUsed == true 时启用。
回调 sonarCallback(const ProcessedRange::ConstPtr& msg)：
- 量化：rangeResolution = msg->max_range / msg->intensity.size()。
- 峰值筛选：在前 size-100 个 bin 中找最大强度 max 及其索引 maxIndex。
- 距离计算：range = (maxIndex + 1) * rangeResolution，heading = deg2rad(msg->head_position)。
- 过滤条件：range < 4.5m 且 max > 10。
- 注入后端：vioInterface_->addSonarMeasurement(timestamp, range, heading)。
线程同步
- 在 ThreadedKFVio::startThreads() 中根据 sensorList.isSonarUsed 启动 sonarConsumerThread_。
- 在 sonarConsumerLoop() 中，通过队列 sonarMeasurementsReceived_ 异步消费测量，等待同步器 sonarFrameSynchronizer_ 配合多帧时间戳。

4. 后端优化接入（Estimator.cpp）

状态添加 (addStates())
- 取当前 pose T_WS。
- 从 sonarMeasurements 提取最新一条测量 (range, heading)。
- 将测量点转换到世界坐标：
```
T_WSo = T_WS * params.sonar.T_SSo;
point_local = [range*cos(heading), range*sin(heading), 0];
sonar_landmark = (T_WSo * point_local).r();
```
- 在视觉地标 landmarksMap_ 中寻找附近（误差 < 0.1m）的关键点集合 landmarkSubset。
- 构建 SonarError 残差块并 mapPtr_->addResidualBlock(sonarError, nullptr, poseBlock)。
SonarError 类
- 保存：range_, heading_, 信息矩阵 information_, 以及参考地标 landmarkSubset_。
- Evaluate()：
  1. 计算地标几何中心 mean。
  2. 预测距离 range_pred = ‖T_WS.r() - mean‖。
  3. 残差 e = range_ - range_pred，加权 sqrtInfo * e。
  4. 计算雅可比：
    
    $\partial e/\partial t = (T_WS.r() - sonar\_landmark)/range$

5. 总结

发布层面（sonar_node）负责串口/网络 I/O 与格式封装。
订阅层面（Subscriber）完成 ROS 消息转换、滤波与后端注入。
后端层面（Estimator）将声纳测量融入滑动窗口的非线性优化，通过 SonarError 将声纳对 pose 产生的约束与视觉、IMU 共同优化。

六、Ceres求解

阶段 1：定义并初始化参数块

主要内容

把所有待估计量（相机位姿、IMU 速度-偏置、3D 地标……）封装成 Ceres 的 ParameterBlock。
为每种变量选择合适的参数化：四元数形式的位姿、齐次坐标形式的地标等。

方法论

调用对应的 ParameterBlock 构造函数（如 PoseParameterBlock、HomogeneousPointParameterBlock）。
用 mapPtr_->addParameterBlock(...) 将它们注册到统一的 Ceres Problem（OKVIS 封装在 mapPtr_ 中）。

目的

将所有待优化的自由变量集中管理，Ceres 才能在同一次求解中联合调整它们。

对应代码段

// 1. 添加相机位姿 ParameterBlock（6DoF Pose）
auto posePB = std::make_shared<okvis::ceres::PoseParameterBlock>(
    T_WS,    // 初始世界到载体位姿
    poseId,  // 唯一 ID
    timestamp);
mapPtr_->addParameterBlock(posePB, okvis::ceres::Map::Pose6d);

// 2. 添加齐次坐标地标 ParameterBlock （4D homogeneous point）
Eigen::Vector4d lm4d(lx, ly, lz, 1.0);
auto lmPB = std::make_shared<okvis::ceres::HomogeneousPointParameterBlock>(
    lm4d,    // [x,y,z,1]
    lmId);   // 唯一 ID
mapPtr_->addParameterBlock(lmPB, okvis::ceres::Map::HomogeneousPoint);

阶段 2：构造残差函数（CostFunction）

主要内容

针对每种传感器观测（视觉重投影、IMU 预积分、Sonar 距离、Depth 深度…）实现一个残差模型：误差值和雅可比。

方法论

自定义或继承 Ceres 的 SizedCostFunction／AnalyticCostFunction，在 Evaluate()（或 operator()) 中完成残差计算和对依赖参数的导数计算。

目的

把测量值和当前变量估计联系起来，以残差形式告诉优化器“该如何调整参数来减小测量误差”。

对应代码段

// 视觉重投影残差
auto reprojError = std::make_shared<okvis::ceres::ReprojectionError>(
    cameraModel, keypoint, intrinsics, extrinsics);

// IMU 预积分残差
auto imuError = std::make_shared<okvis::ceres::ImuError>(
    imuMeasurements, imuParams, priorState);

// Sonar 距离残差
double info = 1.0;
auto sonarError = std::make_shared<okvis::ceres::SonarError>(
    params, range, heading, info, landmarkSubset);

阶段 3：添加到 Problem（联合注册）

主要内容

将所有 ParameterBlock 和 CostFunction（ResidualBlock）绑定到同一个 Ceres Problem 中（OKVIS 用 mapPtr_ 封装）。
指定每个残差块依赖哪些参数块，Ceres 内部自动构建稀疏雅可比结构。

方法论

调用 mapPtr_->addResidualBlock(residual, lossFunction, parameterBlockIds...)，将观测残差和对应变量挂钩。

目的

构建完整的非线性最小二乘问题，让优化器在所有约束下同时调整所有自由变量。

对应代码段

// 视觉重投影：优化 pose 和 landmark
mapPtr_->addResidualBlock(
    reprojError,          // 残差模型
    lossFunctionPtr,      // 可选的 robust loss
    posePB->id(),         // 依赖的参数块 ID
    lmPB->id());

// IMU 预积分：连接前后两帧的 pose+bias
mapPtr_->addResidualBlock(
    imuError,
    nullptr,              // 不使用 robust loss
    prevPosePB->id(), prevBiasPB->id(),
    currPosePB->id(), currBiasPB->id());

// Sonar 距离：只优化当前帧位姿
mapPtr_->addResidualBlock(
    sonarError,
    nullptr,
    posePB->id());

阶段 4：配置 Solver 选项

主要内容

设定线性求解器类型（如 SPARSE_SCHUR）、信赖域策略（DOGLEG / Levenberg–Marquardt）、最大迭代次数、并行线程数、日志输出等。

方法论

通过 mapPtr_->options 修改 ceres::Solver::Options。

目的

根据问题规模和实时性要求，选择最合适的求解策略以保证性能和收敛性。

对应代码段

mapPtr_->options.linear_solver_type           = ceres::SPARSE_SCHUR;
mapPtr_->options.trust_region_strategy_type   = ceres::DOGLEG;
mapPtr_->options.max_num_iterations           = 20;
mapPtr_->options.num_threads                  = 4;
mapPtr_->options.minimizer_progress_to_stdout = false;

阶段 5：调用 Solve 执行优化

主要内容

触发底层 Problem::Solve(options, &summary)，Ceres 会迭代地最小化所有残差平方和。

方法论

OKVIS 封装了一个 Estimator::optimize(...) 接口，内部调用 mapPtr_->solve()。

目的

联合优化所有 ParameterBlock 中的变量，实现多源信息融合的非线性最小二乘解。

对应代码段

void Estimator::optimize(size_t numIter, size_t numThreads, bool verbose) {
  // 设置并行线程数和迭代次数
  mapPtr_->options.num_threads           = numThreads;
  mapPtr_->options.max_num_iterations    = numIter;
  mapPtr_->options.minimizer_progress_to_stdout = verbose;
  // 真正调用 Ceres solver
  mapPtr_->solve();
}

阶段 6：提取并更新结果

主要内容

优化完成后，从每个 ParameterBlock 中读取 .estimate()，并将结果回写到 SLAM / VIO 的状态中（位姿、速度-偏置、地图点等）。

方法论

对 PoseParameterBlock，调用 OKVIS 的 estimator_.get_T_WS(...)；
对地标，通过 HomogeneousPointParameterBlock->estimate() 拿到最新坐标。

目的

将优化后的值推送到系统其他模块（里程计输出、地图维护、回环检测、可视化等），完成一次优化闭环。

对应代码段

// 1. 更新当前帧位姿
okvis::kinematics::Transformation T_WS_opt;
estimator_.get_T_WS(frameId, T_WS_opt);

// 2. 更新所有地图点
for (auto &kv : landmarksMap_) {
  uint64_t lmId = kv.first;
  auto pb = std::static_pointer_cast<
               okvis::ceres::HomogeneousPointParameterBlock>(
                 mapPtr_->parameterBlockPtr(lmId));
  kv.second.point = pb->estimate();  // 新的 [x,y,z,1]
}