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）之前。具体是在这段代码里：

// 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) {// 把每个视觉地标从齐次坐标转成 3DEigen::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) 如果找到了不止一个，就把它们打包，构造一个 SonarErrorif (!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）

1. 初始化
   • rospy.init_node('imagenex831l')
   • 发布两个话题：
     • imagenex831l/range（处理后数据，ProcessedRange）
     • imagenex831l/range_raw（原始数据，RawRange）
   • 启动 dynamic_reconfigure 服务器，支持运行时修改驱动参数。
2. 参数回调
   • 在 parameters_callback() 中，将 config 下发给驱动并返回更新后的配置。
3. 主循环 (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()。
4. 清理
   • 节点退出前，调用 sensor.close_connection() 关闭串口或网络。

---

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

1. 在 setNodeHandle() 中：
   • 订阅 /imagenex831l/range（ProcessedRange）并绑定 sonarCallback()。
   • 仅在配置 vioParameters_.sensorList.isSonarUsed == true 时启用。
2. 回调 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)。
3. 线程同步
   • 在 ThreadedKFVio::startThreads() 中根据 sensorList.isSonarUsed 启动 sonarConsumerThread_。
   • 在 sonarConsumerLoop() 中，通过队列 sonarMeasurementsReceived_ 异步消费测量，等待同步器 sonarFrameSynchronizer_ 配合多帧时间戳。

---

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

1. 状态添加 (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)。

2. 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_{W}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. 添加相机位姿 ParameterBlock（6DoF Pose）
auto posePB = std::make_shared<okvis::ceres::PoseParameterBlock>(T_WS,    // 初始世界到载体位姿poseId,  // 唯一 IDtimestamp);
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 lossposePB->id(),         // 依赖的参数块 IDlmPB->id());// IMU 预积分：连接前后两帧的 pose+bias
mapPtr_->addResidualBlock(imuError,nullptr,              // 不使用 robust lossprevPosePB->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 solvermapPtr_->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]
}