ROS2下ORB-SLAM3在Gazebo Classic仿真中无特征点检测问题求助

作为项目环节之一，需在Gazebo Classic仿真中运行SLAM识别兴趣点。已完成以下配置：

• 创建贴近真实模型的URDF：双目相机间距合规，采用与真实相机一致的HFOV，内参及矩阵参数匹配SLAM的EuRoC.yaml文件，通过Gazebo相机插件实现
• 将模型部署在纹理丰富的bayland环境中
• 使用ORBSLAM3-STEREO-FIXED库，该代码在真实双目相机图像上运行正常

核心问题：在仿真环境中无法检测到任何特征点，程序运行无报错。

相关配置文件 #

URDF相机配置示例 #

<sensor name="left_camera" type="camera">
  <update_rate>20.0</update_rate>
  <camera name="left">
    <horizontal_fov>1.047</horizontal_fov>
    <image>
      <width>752</width>
      <height>480</height>
      <format>R8G8B8</format>
    </image>
    <clip>
      <near>0.1</near>
      <far>100.0</far>
    </clip>
    <distortion>
      <k1>0.0</k1>
      <k2>0.0</k2>
      <k3>0.0</k3>
      <p1>0.0</p1>
      <p2>0.0</p2>
    </distortion>
  </camera>
  <plugin name="left_camera_controller" filename="libgazebo_ros_camera.so">
    <cameraName>/stereo/left</cameraName>
    <imageTopicName>image_raw</imageTopicName>
    <cameraInfoTopicName>camera_info</cameraInfoTopicName>
    <frameName>left_camera_link</frameName>
    <hackBaseline>0.11</hackBaseline>
  </plugin>
</sensor>
<!-- 右相机配置逻辑与左相机一致 -->

SLAM节点代码片段 #

int main(int argc, char **argv)
{
    ros::init(argc, argv, "ORB_SLAM3");
    ros::NodeHandle nodeHandler;
    bool bEqual = false;
    ORB_SLAM3::System SLAM(argv[1], argv[2], ORB_SLAM3::System::STEREO, true);
    ImageGrabber igb(&SLAM, bEqual);

    ros::Subscriber sub_left = nodeHandler.subscribe("/stereo/left/image_raw", 1, &ImageGrabber::GrabImageLeft, &igb);
    ros::Subscriber sub_right = nodeHandler.subscribe("/stereo/right/image_raw", 1, &ImageGrabber::GrabImageRight, &igb);

    ros::spin();

    SLAM.Shutdown();
    SLAM.SaveTrajectoryTUM("CameraTrajectory.txt");
    return 0;
}

EuRoC.yaml配置文件 #

%YAML:1.0

# Camera calibration and distortion parameters (OpenCV)
Camera.fx: 458.654
Camera.fy: 457.296
Camera.cx: 367.215
Camera.cy: 248.375

Camera.k1: -0.28340811
Camera.k2: 0.07395907
Camera.p1: 0.00019359
Camera.p2: 1.76187114e-05

# Camera frames framerate
Camera.fps: 20.0

# stereo baseline times fx
Camera.bf: 47.06390888817197

# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 1

# Close/Far threshold. Baseline times.
ThDepth: 40.0

# ORB Parameters
ORBextractor.nFeatures: 2000
ORBextractor.scaleFactor: 1.2
ORBextractor.nLevels: 8
ORBextractor.iniThFAST: 20
ORBextractor.minThFAST: 7

1. 验证仿真图像质量与格式匹配 #

• 用rqt_image_view查看左右相机的image_raw话题，确认：
  • 图像纹理细节清晰，无过度模糊（Gazebo默认抗锯齿可能抹平特征边缘）
  • 图像格式与EuRoC.yaml中Camera.RGB参数匹配（RGB/BGR不能搞反）
  • 分辨率严格为752x480，与URDF和配置文件完全一致

2. 强制对齐相机内参 #

• 订阅camera_info话题，检查输出的内参矩阵是否与EuRoC.yaml完全一致：

  rostopic echo /stereo/left/camera_info -n 1
  

• 若Gazebo自动计算内参存在偏差，直接在URDF的相机插件中手动指定内参：

  <plugin name="left_camera_controller" filename="libgazebo_ros_camera.so">
    <!-- 保留原有配置 -->
    <intrinsics>
      <fx>458.654</fx>
      <fy>457.296</fy>
      <cx>367.215</cx>
      <cy>248.375</cy>
    </intrinsics>
  </plugin>
  

3. 调整ORB特征提取参数适配仿真图像 #

仿真图像通常比真实图像平滑，放宽特征检测阈值：

• 将ORBextractor.iniThFAST从20降至10，ORBextractor.minThFAST从7降至3
• 临时提升ORBextractor.nFeatures至3000，增加特征检出概率

4. 确保双目图像严格同步 #

• 检查左右相机image_raw话题的时间戳误差，必须控制在1ms以内
• 改用ROS同步器确保左右帧配对，替换原订阅逻辑：

  message_filters::Subscriber<sensor_msgs::Image> sub_left(nodeHandler, "/stereo/left/image_raw", 1);
  message_filters::Subscriber<sensor_msgs::Image> sub_right(nodeHandler, "/stereo/right/image_raw", 1);
  typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image> ExactSyncPolicy;
  message_filters::Synchronizer<ExactSyncPolicy> sync(ExactSyncPolicy(10), sub_left, sub_right);
  sync.registerCallback(boost::bind(&ImageGrabber::GrabStereo, &igb, _1, _2));
  

5. 优化Gazebo渲染与光照 #

• 在world文件中增强环境光照，提升图像对比度：

  <light name="sun" type="directional">
    <cast_shadows>true</cast_shadows>
    <pose>0 0 10 0 0 0</pose>
    <diffuse>1.0 1.0 1.0 1</diffuse>
    <specular>0.1 0.1 0.1 1</specular>
    <attenuation>
      <range>1000</range>
      <constant>0.9</constant>
      <linear>0.01</linear>
      <quadratic>0.001</quadratic>
    </attenuation>
    <direction>-0.5 -0.5 -1</direction>
  </light>
  

• 关闭Gazebo抗锯齿：编辑~/.gazebo/gui.ini，设置anti_aliasing=0

6. 直接调试特征提取过程 #

修改ORB-SLAM3代码，在特征提取后保存带标记的图像，确认是否真的无特征：

// 在ORBextractor::Extract函数末尾添加
cv::Mat img_with_features = img.clone();
for (size_t i=0; i<mvKeys.size(); i++)
{
    cv::circle(img_with_features, mvKeys[i].pt, 2, cv::Scalar(0,255,0), -1);
}
cv::imwrite("sim_feature_test.png", img_with_features);

• 若图像无特征：聚焦图像质量、内参匹配问题
• 若图像有特征但SLAM未使用：检查跟踪模块的帧处理逻辑

内容的提问来源于stack exchange，提问作者Gwen