ROS rtabmap erstellt keine Punktwolke

Question

Ich habe begonnen, ein Stereokamerasystem zu bauen, um das Sichtfeld zu rekonstruieren. Ich verwende zwei Logitech C270 Webcameras auf einem Standfuß, um die Bildströme zu erhalten.

Für das Projekt ist es notwendig, die Kameraoptik so nah wie möglich zu halten, also habe ich eine Kamera vertikal gedreht. Ich verwende das video_stream_opencv-Paket, um die Bilder zu erhalten und zu drehen und sie auch an die anderen Knoten zu senden.

Wegen der weiteren Operationen und um einige Hardwareressourcen zu sparen, dachte ich, es ist notwendig, die Zeitstempel der Bilder und Kameras vor der Kalibrierung, Korrektur usw. zu synchronisieren, also habe ich einen Synchronisationsknoten erstellt, der ungefähre Synchronisation zwischen den verwendet Bilderrahmen und Kamera-Info-Nachrichten, und es veröffentlicht auch die Daten mit den gleichen Zeitstempel. Ich dachte, dass nach der Synchronisation nicht notwendig sein wird, um die approx_sync zu verwenden, aber ich denke, ich habe mich geirrt. Um das System zu testen, habe ich auch einen statischen tf-Publisher verwendet.

Leider konnte ich nicht Punktwolke aus dem System, aber im Terminal erscheint eine Warnmeldung häufig erhalten:

[Warnen] [1506963490,361523551]: Odometrie: Könnte erhalten von base_link nicht verwandeln nach links (stamp = 1506963490.228821) nach 0,100000 Sekunden ("wait_for_transform_duration" = 0,100000)! Error = ". CanTransform zurückgegeben nach 0,102307 Timeout war 0,1."

Hier ist meine Startdatei:

<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>

ich auch ein rqt Diagramm erstellt, um die Verbindungen zwischen den Knoten zu verstehen: rqt_graph

Und es ist auch hilfreich, wenn Wir sehen die tf Frames: tf frames

Ich hoffe, ich habe alles gesagt, um herauszufinden, was ich falsch gemacht habe, ich bin wirklich wegen dieses Problems enttäuscht.

Answer 1

Ok, wir haben herausgefunden, was das Problem war.

Wie der Fehler sagt, kann die Odometrie den Frame nicht in Frame base_link umwandeln. Sie können eine statische Transformation zwischen camera_link und links, wie dies hinzufügen möchten:

<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 haben Sie base_link -> camera_link -> links. Beachten Sie, dass Sie keine Vorsynchronisierung als stereo_odometry durchführen müssen und rtabmap-Knoten dies direkt mit approx_sync: = true tun können. Aber ja, wenn Sie den gleichen Zeitstempel für alle image/camera_info-Themen voraussynchronisieren und setzen, können Sie approx_sync: = false für Odometrie und rtabmap verwenden. Wenn Sie danach schlechte Ergebnisse haben, kann dies auf eine schlechte Stereo-Gleichrichtung und/oder Synchronisation zurückzuführen sein.

Ich empfehle dringend, dass Sie eine echte Stereo-Kamera, die Stereo-Hardware (nicht Software!) Synchronisation, um gute Ergebnisse zu erhalten, wenn der Roboter sich bewegt.

prost,

Mathieu