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

  
根据论文中给出的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）之前。具体是在这段代码里：

1. // Estimator::addStates(...)
2. // （前面省略了 IMU、深度、视觉部分）
4. // ———— Sonar ————
5. if (sonarMeasurements.size() != 0) {
6.   auto last_sonarMeasurement_it = sonarMeasurements.rbegin();
7.   double range   = last_sonarMeasurement_it->measurement.range;
8.   double heading = last_sonarMeasurement_it->measurement.heading;
10.   // 1) 把声呐读数〈range, heading〉变成声呐坐标系下的点
11.   okvis::kinematics::Transformation sonar_point(
12.       Eigen::Vector3d(range * cos(heading),
13.                       range * sin(heading),
14.                       0.0),
15.       Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0));
17.   // 2) 把它从声呐坐标系 T_SSo 換到世界坐标系
18.   okvis::kinematics::Transformation T_WSo       = T_WS * params.sonar.T_SSo;
19.   okvis::kinematics::Transformation T_WSo_point = T_WSo * sonar_point;
20.   Eigen::Vector3d sonar_landmark = T_WSo_point.r();   // ← 这就是“世界系下的声呐点”
22.   // 3) 在所有视觉地标里找“x,y,z 三个方向都离它不到 0.1m” 的那些点
23.   std::vector<Eigen::Vector3d> landmarkSubset;
24.   for (auto rit = landmarksMap_.rbegin(); rit != landmarksMap_.rend(); ++rit) {
25.     // 把每个视觉地标从齐次坐标转成 3D
26.     Eigen::Vector3d visual_landmark =
27.       (rit->second.point / rit->second.point[3]).head<3>();
28.     if (fabs(sonar_landmark[0] - visual_landmark[0]) < 0.1 &&
29.         fabs(sonar_landmark[1] - visual_landmark[1]) < 0.1 &&
30.         fabs(sonar_landmark[2] - visual_landmark[2]) < 0.1) {
31.       landmarkSubset.push_back(visual_landmark);
32.     }
33.   }
35.   // 4) 如果找到了不止一个，就把它们打包，构造一个 SonarError
36.   if (!landmarkSubset.empty()) {
37.     double information_sonar = 1.0;
38.     auto sonarError = std::make_shared<ceres::SonarError>(
39.         params, range, heading, information_sonar, landmarkSubset);
40.     mapPtr_->addResidualBlock(sonarError, NULL, poseParameterBlock);
41.   }
42. }

复制代码

• 构造天下系下的声呐点：
  
  1. okvis::kinematics::Transformation sonar_point(...);
  2. T_WSo_point = T_WS * params.sonar.T_SSo * sonar_point;
  3. 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 读传感器：
    
    
    • 调用 sensor.send_request() 和 sensor.read_data() 获取原始字节。
      
    • 发布 RawRange：填充 header、data。
      
    • 调用 sensor.interpret_data() 解析成 ProcessedRange。
      
    • 发布到 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)。
    
  • 将丈量点转换到天下坐标：
    
    1. T_WSo = T_WS * params.sonar.T_SSo;
    2. point_local = [range*cos(heading), range*sin(heading), 0];
    3. sonar_landmark = (T_WSo * point_local).r();

    复制代码
  • 在视觉地标 landmarksMap_ 中寻找附近（误差 < 0.1m）的关键点集合 landmarkSubset。
    
  • 构建 SonarError 残差块并 mapPtr_->addResidualBlock(sonarError, nullptr, poseBlock)。
    
  
  
• SonarError 类
  
  
  
  • 生存：range_, heading_, 信息矩阵 information_, 以及参考地标 landmarkSubset_。
    
  • Evaluate()：
    
    
    
    • 计算地标几何中央 mean。
      
    • 猜测间隔 range_pred = ‖T_WS.r() - mean‖。
      
    • 残差 e = range_ - range_pred，加权 sqrtInfo * e。
      
    • 计算雅可比：
                                                                             ∂                                              e                                              /                                              ∂                                              t                                              =                                              (                                                               T                                                 W                                                              S                                              .                                              r                                              (                                              )                                              −                                              s                                              o                                              n                                              a                                              r                                              _                                              l                                              a                                              n                                              d                                              m                                              a                                              r                                              k                                              )                                              /                                              r                                              a                                              n                                              g                                              e                                                          \partial e/\partial t = (T_WS.r() - sonar\_landmark)/range                                       ∂e/∂t=(TW​S.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. // 1. 添加相机位姿 ParameterBlock（6DoF Pose）
2. auto posePB = std::make_shared<okvis::ceres::PoseParameterBlock>(
3.     T_WS,    // 初始世界到载体位姿
4.     poseId,  // 唯一 ID
5.     timestamp);
6. mapPtr_->addParameterBlock(posePB, okvis::ceres::Map::Pose6d);
8. // 2. 添加齐次坐标地标 ParameterBlock （4D homogeneous point）
9. Eigen::Vector4d lm4d(lx, ly, lz, 1.0);
10. auto lmPB = std::make_shared<okvis::ceres::HomogeneousPointParameterBlock>(
11.     lm4d,    // [x,y,z,1]
12.     lmId);   // 唯一 ID
13. mapPtr_->addParameterBlock(lmPB, okvis::ceres::Map::HomogeneousPoint);

复制代码

---

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

主要内容

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

方法论

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

目的

• 把丈量值和当前变量估计联系起来，以残差情势告诉优化器“该如何调解参数来减小丈量误差”。
  

对应代码段

1. // 视觉重投影残差
2. auto reprojError = std::make_shared<okvis::ceres::ReprojectionError>(
3.     cameraModel, keypoint, intrinsics, extrinsics);
5. // IMU 预积分残差
6. auto imuError = std::make_shared<okvis::ceres::ImuError>(
7.     imuMeasurements, imuParams, priorState);
9. // Sonar 距离残差
10. double info = 1.0;
11. auto sonarError = std::make_shared<okvis::ceres::SonarError>(
12.     params, range, heading, info, landmarkSubset);

复制代码

---

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

主要内容

• 将全部 ParameterBlock 和 CostFunction（ResidualBlock）绑定到同一个 Ceres Problem 中（OKVIS 用 mapPtr_ 封装）。
  
• 指定每个残差块依赖哪些参数块，Ceres 内部自动构建希罕雅可比布局。
  

方法论

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

目的

• 构建完备的非线性最小二乘问题，让优化器在全部约束下同时调解全部自由变量。
  

对应代码段

1. // 视觉重投影：优化 pose 和 landmark
2. mapPtr_->addResidualBlock(
3.     reprojError,          // 残差模型
4.     lossFunctionPtr,      // 可选的 robust loss
5.     posePB->id(),         // 依赖的参数块 ID
6.     lmPB->id());
8. // IMU 预积分：连接前后两帧的 pose+bias
9. mapPtr_->addResidualBlock(
10.     imuError,
11.     nullptr,              // 不使用 robust loss
12.     prevPosePB->id(), prevBiasPB->id(),
13.     currPosePB->id(), currBiasPB->id());
15. // Sonar 距离：只优化当前帧位姿
16. mapPtr_->addResidualBlock(
17.     sonarError,
18.     nullptr,
19.     posePB->id());

复制代码

---

阶段 4：配置 Solver 选项

主要内容

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

方法论

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

目的

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

对应代码段

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

复制代码

---

阶段 5：调用 Solve 执行优化

主要内容

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

方法论

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

目的

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

对应代码段

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

复制代码

---

阶段 6：提取并更新结果

主要内容

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

方法论

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

目的

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

对应代码段

1. // 1. 更新当前帧位姿
2. okvis::kinematics::Transformation T_WS_opt;
3. estimator_.get_T_WS(frameId, T_WS_opt);
5. // 2. 更新所有地图点
6. for (auto &kv : landmarksMap_) {
7.   uint64_t lmId = kv.first;
8.   auto pb = std::static_pointer_cast<
9.                okvis::ceres::HomogeneousPointParameterBlock>(
10.                  mapPtr_->parameterBlockPtr(lmId));
11.   kv.second.point = pb->estimate();  // 新的 [x,y,z,1]
12. }

复制代码

免责声明：如果侵犯了您的权益，请联系站长，我们会及时删除侵权内容，谢谢合作！更多信息从访问主页：qidao123.com:ToB企服之家，中国第一个企服评测及商务社交产业平台。