ROS rtabmap 不创建点云
ROS rtabmap does not create point cloud
我已经开始构建立体相机系统来重建视野。我在底座上使用两个罗技 C270 网络摄像头来获取图像流。
对于该项目，有必要尽可能靠近相机镜头，所以我将一台相机垂直转动。我使用 video_stream_opencv 包来获取和旋转图像，并将它们发送到其他节点。
由于后续操作和节省一些硬件资源，我认为在校准，校正等之前同步图像和相机信息的时间戳是必要的，所以我创建了一个同步节点，它使用之间的近似同步图像帧和相机信息消息，它还重新发布具有相同时间戳的数据。
我以为同步后就不需要使用approx_sync了，但我想我错了。
为了测试系统，我也开始使用静态 tf 发布器。
不幸的是我无法从系统中获取点云，但在终端中经常出现警告信息：
[ WARN] [1506963490.361523551]: odometry: Could not get transform from base_link to left (stamp=1506963490.228821) after 0.100000 seconds ("wait_for_transform_duration"=0.100000)! Error=". canTransform returned after 0.102307 timeout was 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
我希望我说的一切都是为了找出我错了什么，因为这个问题我真的很失望。
好的，我们找到问题所在了。
As the error is saying, odometry cannot transform frame left to frame base_link. You may want to add a static transform between camera_link and left as this:
<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" />
... so you have base_link -> camera_link -> left.
Note that you don't need to pre-synchronize as stereo_odometry and rtabmap nodes can do it directly with approx_sync:=true. But yes, if you pre-syncrhonize and set the same timestamp on all image/camera_info topics, you can use approx_sync:=false for odometry and rtabmap. If you have poor results after that, it may be caused by bad stereo rectification and/or synchronization.
I strongly suggest that you get a real stereo camera that does stereo hardware (not software!) synchronization to get good results when the robot is moving.
cheers,
Mathieu
我已经开始构建立体相机系统来重建视野。我在底座上使用两个罗技 C270 网络摄像头来获取图像流。
对于该项目，有必要尽可能靠近相机镜头，所以我将一台相机垂直转动。我使用 video_stream_opencv 包来获取和旋转图像，并将它们发送到其他节点。
由于后续操作和节省一些硬件资源，我认为在校准，校正等之前同步图像和相机信息的时间戳是必要的，所以我创建了一个同步节点，它使用之间的近似同步图像帧和相机信息消息，它还重新发布具有相同时间戳的数据。 我以为同步后就不需要使用approx_sync了，但我想我错了。 为了测试系统，我也开始使用静态 tf 发布器。
不幸的是我无法从系统中获取点云，但在终端中经常出现警告信息：
[ WARN] [1506963490.361523551]: odometry: Could not get transform from base_link to left (stamp=1506963490.228821) after 0.100000 seconds ("wait_for_transform_duration"=0.100000)! Error=". canTransform returned after 0.102307 timeout was 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
我希望我说的一切都是为了找出我错了什么，因为这个问题我真的很失望。
好的，我们找到问题所在了。
As the error is saying, odometry cannot transform frame left to frame base_link. You may want to add a static transform between camera_link and left as this:
<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" />
... so you have base_link -> camera_link -> left. Note that you don't need to pre-synchronize as stereo_odometry and rtabmap nodes can do it directly with approx_sync:=true. But yes, if you pre-syncrhonize and set the same timestamp on all image/camera_info topics, you can use approx_sync:=false for odometry and rtabmap. If you have poor results after that, it may be caused by bad stereo rectification and/or synchronization.
I strongly suggest that you get a real stereo camera that does stereo hardware (not software!) synchronization to get good results when the robot is moving.
cheers,
Mathieu