ROS rtabmap 不创建点云

Question

我已经开始构建立体摄像系统来重建视野。我在底座上使用两个罗技 C270 网络摄像头来获取图像流。

对于这个项目，我需要尽可能靠近相机光学元件，因此我垂直转动了一台相机。我使用 video_stream_opencv 包来获取和旋转图像，并将它们发送到其他节点。

为了进一步的操作和节省一些硬件资源，我认为在校准，校正等之前需要同步图像和相机信息的时间戳，所以我创建了一个同步节点，使用图像之间的近似同步帧和相机信息消息，它还重新发布具有相同时间戳的数据。 我认为在同步之后就不需要使用 approx_sync，但我认为我错了。 为了测试系统，我还开始使用静态 tf 发布者。

不幸的是我无法从系统中获取点云，但在终端中经常出现警告消息：

[ WARN] [1506963490.361523551]：里程计：0.100000 秒（“wait_for_transform_duration”=0.100000）后无法从 base_link 向左转换（stamp=1506963490.228821）！ Error=".canTransform 在 0.102307 超时后返回为 0.1。"

这是我的启动文件：

<launch>
  <!--*******************************************************************************************-->
  <!-- Global parameters ************************************************************************-->
  <!--*******************************************************************************************-->
  <!-- Camera -->
  <arg name="fps" default="25" />

  <!-- Synchronization -->
  <arg name="syncronizer_namespace" default="/syncronizer" />
  <arg name="left_camera_raw" default="$(arg syncronizer_namespace)/left" />
  <arg name="right_camera_raw" default="$(arg syncronizer_namespace)/right" />
  <arg name="left_camera_info_topic" default="$(arg syncronizer_namespace)/left/camera_info" />
  <arg name="right_camera_info_topic" default="$(arg syncronizer_namespace)/right/camera_info" />

  <!-- Stereo -->
  <arg name="stereo_namespace" default="/stereo_camera" />
  <arg name="left_image_topic" default="$(arg stereo_namespace)/left/image_rect" />
  <arg name="right_image_topic" default="$(arg stereo_namespace)/right/image_rect" />

  <arg name="approx_sync" default="true" />
  <arg name="queue_size" default="5" />

  <!-- Tranfsorm -->
  <arg name="use_static_transform" default="true" />

  <!-- Visual SLAM -->
  <arg name="frame_id" default="base_link" />
  <!-- Fixed frame id, set "base_link" or "base_footprint" if they are published -->
  <arg name="rtabmap" default="true" />
  <arg name="odometry" default="true" />

  <!-- Odometry -->
  <arg name="odom_frame_id" default="odom" />
  <!-- If set, TF is used to get odometry instead of the topic -->
  <arg name="ground_truth_frame_id" default="" />
  <!-- e.g., "world" -->
  <arg name="ground_truth_base_frame_id" default="" />
  <!-- e.g., "tracker", a fake frame matching the frame "frame_id" (but on different TF tree) -->
  <arg name="wait_for_transform" default="true" />
  <arg name="wait_for_transform_duration" default="0.2" />

  <!-- 3D visualization -->
  <arg name="rviz" default="false" />
  <arg name="rtabmapviz" default="true" />

  <arg name="camera_info" default="camera_info" />

  <!--*******************************************************************************************-->
  <!-- Core functionality ***********************************************************************-->
  <!--*******************************************************************************************-->

  <!-- Camera -->
  <group ns="/camera">
    <node pkg="nodelet" type="nodelet" name="stereo_camera_nodelet" args="manager" />

    <!-- Left video stream input -->
    <include file="$(find video_stream_opencv)/launch/camera.launch">
      <arg name="camera_name" value="left" />
      <arg name="camera_info_url" value="file:///$(find reconstruction)/config/left.yaml" />
      <arg name="video_stream_provider" value="1" />
      <arg name="flip_horizontal" value="false" />
      <arg name="flip_vertical" value="false" />
      <arg name="fps" value="$(arg fps)" />
    </include>
    <!-- Right video stream input -->
    <include file="$(find video_stream_opencv)/launch/camera.launch">
      <arg name="camera_name" value="right" />
      <arg name="camera_info_url" value="file:///$(find reconstruction)/config/right.yaml" />
      <arg name="video_stream_provider" value="2" />
      <arg name="flip_horizontal" value="false" />
      <arg name="flip_vertical" value="true" />
      <arg name="fps" value="$(arg fps)" />
    </include>
  </group>

  <!-- Syncronizer -->
  <node name="syncronizer" pkg="reconstruction" type="syncronizer" />

  <!-- Stereo processing -->
  <group ns="/stereo_camera">
    <node pkg="nodelet" type="nodelet" name="stereo_nodelet" args="manager" />

    <node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc">
      <remap from="left/image_raw" to="$(arg left_camera_raw)" />
      <remap from="right/image_raw" to="$(arg right_camera_raw)" />
      <remap from="left/camera_info" to="$(arg left_camera_info_topic)" />
      <remap from="right/camera_info" to="$(arg right_camera_info_topic)" />

      <param name="prefilter_size" value="35" />
      <param name="prefilter_cap" value="11" />
      <param name="correlation_window_size" value="41" />
      <param name="min_disparity" value="-15" />
      <param name="disparity_range" value="160" />
      <param name="uniqueness_ratio" value="0.0" />
      <param name="texture_threshold" value="1000" />
      <param name="speckle_size" value="500" />
      <param name="speckle_range" value="16" />
      <param name="approximate_sync" value="true" />
      <param name="queue_size" value="5" />
    </node>
  </group>

  <!-- Transform -->
  <node if="$(arg use_static_transform)" pkg="tf" type="static_transform_publisher" name="world_to_map" args="0.0 0.0 0.30 0.0 0.0 0.0 /base_link /camera_link 100" />

  <group ns="rtabmap">
    <!-- Stereo Odometry -->
    <node if="$(arg odometry)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen">
      <remap from="left/image_rect" to="$(arg left_image_topic)" />
      <remap from="right/image_rect" to="$(arg right_image_topic)" />
      <remap from="left/camera_info" to="$(arg left_camera_info_topic)" />
      <remap from="right/camera_info" to="$(arg right_camera_info_topic)" />

      <param name="approx_sync" type="bool" value="$(arg approx_sync)" />
      <param name="frame_id" type="string" value="$(arg frame_id)" />
      <param name="odom_frame_id" type="string" value="odom" />
      <param name="queue_size" type="int" value="5" />
    </node>

    <!-- Visual SLAM: args: "delete_db_on_start" and "udebug" -->
    <node if="$(arg rtabmap)" name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start --udebug">
      <remap from="left/image_rect" to="$(arg left_image_topic)" />
      <remap from="right/image_rect" to="$(arg right_image_topic)" />
      <remap from="left/camera_info" to="$(arg left_camera_info_topic)" />
      <remap from="right/camera_info" to="$(arg right_camera_info_topic)" />

      <remap from="odom" to="/rtabmap/odom" />

      <param name="approx_sync" type="bool" value="$(arg approx_sync)" />
      <param name="frame_id" type="string" value="$(arg frame_id)" />
      <param name="queue_size" type="int" value="30" />

      <param name="subscribe_stereo" type="bool" value="true" />
      <param name="subscribe_depth" type="bool" value="false" />
    </node>

    <!-- Visualisation RTAB-Map -->
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
      <remap from="left/image_rect" to="$(arg left_image_topic)" />
      <remap from="right/image_rect" to="$(arg right_image_topic)" />
      <remap from="left/camera_info" to="$(arg left_camera_info_topic)" />
      <remap from="right/camera_info" to="$(arg right_camera_info_topic)" />

      <remap from="odom_info" to="/rtabmap/odom_info" />
      <remap from="odom" to="/rtabmap/odom" />

      <param name="frame_id" type="string" value="$(arg frame_id)" />
      <param name="queue_size" type="int" value="10" />

      <param name="subscribe_stereo" type="bool" value="true" />
      <param name="subscribe_odom_info" type="bool" value="true" />
    </node>
  </group>
</launch>

我还创建了一个 rqt 图来了解节点之间的连接： rqt_graph

如果我们看到 tf 帧也很有帮助： tf frames

我希望我所说的一切都是为了找出我做错了什么，因为这个问题我真的很失望。

Answer 1

好的，我们发现了问题所在。

正如错误所说，里程计无法将左帧转换为帧base_link。您可能希望在 camera_link 和 left 之间添加一个静态转换，如下所示：

<arg name="pi/2" value="1.5707963267948966" />
<node if="$(arg use_static_transform)" pkg="tf" type="static_transform_publisher" name="camera_optical" args="0 0 0 -$(arg pi/2) 0 -$(arg pi/2) /camera_link /left 100" />

... 所以你有 base_link -> camera_link -> left。 请注意，您不需要预先同步，因为 stereo_odometry 和 rtabmap 节点可以直接使用 approx_sync:=true 进行。但是，是的，如果您在所有 image/camera_info 主题上预先同步并设置相同的时间戳，您可以将 approx_sync:=false 用于里程计和 rtabmap。如果之后效果不佳，则可能是由于立体声整流和/或同步不良造成的。

我强烈建议您购买真正的立体相机，该相机会进行立体硬件（而不是软件！）同步，以便在机器人移动时获得良好的效果。

干杯，

马修