Multiple camera mapping
Posted by
jacklu333333 on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Multiple-camera-mapping-tp8365.html
Hi, I am tried using two cameras for rtabmap to have better odometry. I refer to the example launch file of "demo_two_kinects.launch." But the rtabmap did not work and it is not publishing the map topic.
Yet, the cameras I got are one Realsense L515 and one Astra camera. Therefore I remap all the input accordingly to the example file.
I check the input of odometry of RGDB for both cameras they are both published by rgbd_odometry node.
I am wondering what might be the cause? Is it because of the difference of frame_id for both cameras?
The following is my launch file.
<launch>
<include file="$(find realsense2_camera)/launch/rs_camera.launch">
<arg name="camera" value="l515" />
<arg name="depth_width" value="640" />
<arg name="depth_height" value="480" />
<arg name="color_width" value="1280" />
<arg name="color_height" value="720" />
<arg name="color_fps" value="30" />
<arg name="gyro_fps" value="200" />
<arg name="accel_fps" value="200" />
<arg name="enable_gyro" value="true" />
<arg name="enable_accel" value="true" />
<arg name="unite_imu_method" value="linear_interpolation" />
<arg name="initial_reset" value="true" />
<arg name="enable_confidence" value="false" />
<arg name="topic_odom_in" value="/odometry/filtered" />
<!-- <arg name="topic_odom_in" value="/odom" /> -->
<!-- <arg name="tf_publish_rate" value="100" /> -->
<arg name="enable_sync" value="true" />
<arg name="align_depth" value="true" />
</include>
<env name="TURTLEBOT_3D_SENSOR" value="astra" />
<include file="$(find turtlebot_bringup)/launch/3dsensor.launch">
<arg name="depth_registration" value="true"/>
</include>
<include file="$(find turtlebot_navigation)/launch/includes/move_base.launch.xml"/>
<arg name="imu_topic" default="/rtabmap/imu"/>
<!-- Choose visualization -->
<arg name="rviz" default="false" />
<arg name="rtabmapviz" default="false" />
<arg name="strategy" default="1" />
<arg name="feature" default="6" />
<arg name="nn" default="3" />
<arg name="max_depth" default="4.0" />
<arg name="min_inliers" default="20" />
<arg name="inlier_distance" default="0.02" />
<arg name="local_map" default="1000" />
<arg name="odom_info_data" default="true" />
<arg name="wait_for_transform" default="true" />
<!-- sync rgb/depth images per camera -->
<group ns="camera1">
<node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync camera1_nodelet_manager">
<remap from="rgb/image" to="/l515/color/image_raw"/>
<remap from="depth/image" to="/l515/aligned_depth_to_color/image_raw"/>
<remap from="rgb/camera_info" to="/l515/color/camera_info"/>
</node>
</group>
<group ns="camera2">
<node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync camera2_nodelet_manager">
<remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
<remap from="depth/image" to="/camera/depth_registered/image_raw"/>
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
</node>
</group>
<group ns="rtabmap">
<!-- Odometry -->
<node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
<remap from="rgbd_image0" to="/camera1/rgbd_image"/>
<remap from="rgbd_image1" to="/camera2/rgbd_image"/>
<param name="subscribe_rgbd" type="bool" value="true"/>
<param name="frame_id" type="string" value="odom"/>
<param name="rgbd_cameras" type="int" value="2"/>
<param name="wait_for_transform" type="bool" value="$(arg wait_for_transform)"/>
<param name="Odom/Strategy" type="string" value="$(arg strategy)"/>
<param name="OdomF2M/BundleAdjustment" type="string" value="0"/> <!-- should be 0 for multi-cameras -->
<param name="Vis/EstimationType" type="string" value="0"/> <!-- should be 0 for multi-cameras -->
<param name="Vis/FeatureType" type="string" value="$(arg feature)"/>
<param name="Vis/CorGuessWinSize" type="string" value="0"/>
<param name="Vis/CorNNType" type="string" value="$(arg nn)"/>
<param name="Vis/MaxDepth" type="string" value="$(arg max_depth)"/>
<param name="Vis/MinInliers" type="string" value="$(arg min_inliers)"/>
<param name="Vis/InlierDistance" type="string" value="$(arg inlier_distance)"/>
<param name="OdomF2M/MaxSize" type="string" value="$(arg local_map)"/>
<param name="Odom/FillInfoData" type="string" value="$(arg odom_info_data)"/>
</node>
<param name="odom_frame_id" type="string" value="odom"/>
<remap from="odom" to="/rtabmap/rgbd_odom"/>
<remap from="imu" to="$(arg imu_topic)"/>
<param name="publish_tf" type="bool" value="false"/>
<!-- Visual SLAM (robot side) -->
<!-- args: "delete_db_on_start" and "udebug" -->
<node if="false" name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgbd" type="bool" value="true"/>
<param name="rgbd_cameras" type="int" value="2"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="gen_scan" type="bool" value="true"/>
<param name="wait_for_transform" type="bool" value="$(arg wait_for_transform)"/>
<param name="map_negative_poses_ignored" type="bool" value="false"/> <!-- refresh grid map even if we are not moving-->
<param name="map_negative_scan_empty_ray_tracing" type="bool" value="false"/> <!-- don't fill empty space between the generated scans-->
<remap from="imu" to="$(arg imu_topic)"/>
<param name="map_frame_id" type="string" value="map"/>
<param name="odom_frame_id" type="string" value="odom"/>
<param name="publish_tf" type="bool" value="true"/>
<param name="odom_tf_angular_variance" type="double" value="0.001"/>
<param name="odom_tf_linear_variance" type="double" value="0.001"/>
<remap from="grid_map" to="/map"/>
<remap from="rgbd_image0" to="/camera1/rgbd_image"/>
<remap from="rgbd_image1" to="/camera2/rgbd_image"/>
<param name="Grid/FromDepth" type="string" value="false"/>
<param name="Vis/EstimationType" type="string" value="0"/> <!-- should be 0 for multi-cameras -->
<param name="Vis/MinInliers" type="string" value="10"/>
<param name="Vis/InlierDistance" type="string" value="$(arg inlier_distance)"/>
</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">
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgbd" type="bool" value="true"/>
<param name="subscribe_odom_info" type="bool" value="$(arg odom_info_data)"/>
<param name="frame_id" type="string" value="base_link"/>
<param name="rgbd_cameras" type="int" value="2"/>
<param name="wait_for_transform" type="bool" value="$(arg wait_for_transform)"/>
<remap from="rgbd_image0" to="/camera1/rgbd_image"/>
<remap from="rgbd_image1" to="/camera2/rgbd_image"/>
</node>
</group>
<!-- Visualization RVIZ -->
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbd.rviz"/>
</launch>