T265 and D435i launch example

[codeforge]

Hi,
i'm new to ros and rtabmap and i'm trying to use both t265 and d435i to 3d mapping. Do you have a sample launch file to start with?


Thanks

[codeforge]

No one can help me?
Searching online i made a mix of lauch files in one file. Could someone check this file below and say me if i'm on the right way?
I'm using ros kinect on ubuntu 16.04, and as first command i start rs_d400_and_t265.launch that i found in librealsense2 on github, i change just the serial number and camera name (i set camera1 name to t265 and camera2 to d400) sections from my sensors:
roslaunch realsense2_camera rs_camera.launch align_depth:=true unite_imu_method:="linear_interpolation"

roslaunch realsense2_camera rs_d400_and_t265.launch align_depth:=true unite_imu_method:=linear_interpolation

then i start the imu_filter_magic as decribed in http://wiki.ros.org/rtabmap_ros/Tutorials/HandHeldMapping:

rosrun imu_filter_madgwick imu_filter_node _use_mag:=false _publish_tf:=false _world_frame:="enu" /imu/data_raw:=/t265/imu /imu/data:=/rtabmap/imu

and after all i started my modified rtabmap.launch using the same wiki about hendheldmapping for d435i:

roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="--delete_db_on_start --Optimizer/GravitySigma 0.3" depth_topic:=/d400/aligned_depth_to_color/image_raw rgb_topic:=/d400/color/image_raw camera_info_topic:=/camera/color/camera_info approx_sync:=false wait_imu_to_init:=true imu_topic:=/rtabmap/imu

and this is my rtabmap.launch:

<launch>
  <arg name="gui_cfg"           default="~/.ros/rtabmap_gui.ini" />
  <arg name="launch_prefix"     default=""/>
  <arg name="output"            default="screen"/>
  <arg name="imu_topic"         default="/imu/data"/>
  <!-- <node pkg="tf" type="static_transform_publisher" name="base_to_t265_tf" args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /t265_link 100" /> -->
  <node pkg="tf" type="static_transform_publisher" name="d400_to_t265_tf" args="0.0 0.0 0.0 -3.14 0.0 0.0 /t265_link /d400_link 100" />

  <group ns="rtabmap">
  	<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
    	<param name="odom_frame_id"        type="string" value="t265_odom_frame"/> <!-- the fixed frame used by robot_localization -->
    	<param name="odom_tf_angular_variance" type="double" value="0.0005"/>
    	<param name="odom_tf_linear_variance"  type="double" value="0.0001"/> <!-- 1 cm stddev error -->

      <param name="frame_id" type="string" value="t265_link"/>
      <param name="subscribe_depth" type="bool" value="true"/>
      <param name="subscribe_odom_info" type="bool" value="false"/>
    	<!-- Add param name="subscribe_odom_info" type="bool" value="true"/> to fix problems with rtabmap subscribing to odom_info -->
      <!--  <remap from="odom" to="/odometry/filtered"/> -->
      <!--remap from="rgb/image" to="/camera/color/image_raw"/-->
    	<!--remap from="depth/image" to="/camera/aligned_depth_to_color/image_raw"/-->
    	<!--remap from="rgb/camera_info" to="/camera/color/camera_info"/-->
      <remap from="rgb/image" to="/d400/color/image_raw"/>
      <remap from="depth/image" to="/d400/aligned_depth_to_color/image_raw"/>
      <remap from="rgb/camera_info" to="/d400/color/camera_info"/>
    	<param name="queue_size" type="int" value="100"/>
      <!-- RTAB-Map's parameters -->
    	<param name="RGBD/NeighborLinkRefining" type="string" value="true"/>
    	<param name="RGBD/ProximityBySpace" 	type="string" value="true"/>
    	<param name="RGBD/AngularUpdate"    	type="string" value="0.01"/>
    	<param name="RGBD/LinearUpdate"     	type="string" value="0.01"/>
    	<param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
    	<param name="Reg/Force3DoF"      	type="string" value="true"/> <!--same as optimizer/slam2D -->
    	<param name="Reg/Strategy"          	type="string" value="1"/> <!-- 1=ICP -->
    	<param name="Reg/Force3DoF"         	type="string" value="true"/>
    	<param name="Vis/MinInliers"        	type="string" value="5"/>
    	<param name="Vis/InlierDistance"    	type="string" value="0.1"/>
    	<param name="Rtabmap/TimeThr"       	type="string" value="700"/>
    	<param name="Mem/RehearsalSimilarity"   type="string" value="0.45"/>
    	<!-- <param name="Grid/CellSize" type="string" value="0.05"/> -->
    	<param name="Kp/MaxFeatures" type="string" value="400"/>
      <param name="Optimizer/Strategy" type="string" value="2"/> <!-- 0 is TORO, Default G20-->
      <param name="RGBD/OptimizeMaxError" type="string" value="0"/> <!--When using TORO, seto to 0,otherwise set to 1-->
      <param name="Kp/DetectorStrategy" type="string" value="6"/> <!--0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF(default) 7=BRISK 8=GFTT/ORB 9=KAZE-->
      <param name="RGBD/LoopClosureReextractFeatures" type="string" value="true"/> <!--extract features even if there are some already in the node-->
      <!--  <param name="Grid/FromDepth" type="string" value="false"/> -->
      <!-- <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="10"/> -->
      <param name="Vis/FeatureType" type="string" value="6"/> <!--suppress warning,want to be same as KP/detectorstrategy -->
    </node>

    <!-- Visualisation RTAB-Map -->
    <node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg gui_cfg)" output="$(arg output)" launch-prefix="$(arg launch_prefix)">
      <!-- <node pkg="rviz" type="rviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd.rviz"/> -->
      <!-- <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb" output="$(arg output)"> -->
      <param name="subscribe_rgbd"   	type="bool"   value="false"/>
      <param name="subscribe_odom_info" type="bool" value="false"/>
      <!-- <param name="frame_id"         	type="string" value="d400_link"/> -->
      <param name="odom_frame_id"    	type="string" value="t265_link"/>
      <param name="wait_for_transform_duration" type="double"   value="0.4"/>
      <param name="queue_size"       	type="int"	value="50"/>
      <param name="approx_sync"      	type="bool"   value="true"/>



      <!--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"/-->
      <!--remap from="rgbd_image"   to="/camera/rgb/image_rect_color"/-->
      <remap from="rgb/image" to="/d400/color/image_raw"/>
      <remap from="depth/image" to="/d400/aligned_depth_to_color/image_raw"/>
      <remap from="rgb/camera_info" to="/d400/color/camera_info"/>
      <remap from="odom"      to="/odometry/filtered"/>
  	</node>
  </group>
</launch>

I get always scan error/warning becouse, if i understood well, i'm using optimizer/strategy as 2, but if i change it to 1 or 0 i get bad result in 3d mapping. With this configuration i get slow good result in 3d mapping, but i want to know if it's all correct or if i made some mistakes.

Thanks

[matlabbe]

Hi,

Can you show the TF tree?

rtabmap node would simply subscribe to D435's RGB image + aligned depth image + camera_info, and the output odometry from T265.

cheers,
Mathieu

[codeforge]

Hi,
i played with args and params, this is my new setup. Now i can get good result but the mapping is really slow, in terminal i  see this:

rtabmap (113): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1927s, Maps update=0.0029s pub=0.0000s (local map=15, WM=42)

it's like using only D435i sensor. I'm doing it correctly?
Moreover i get this error and warning messages in terminal:

[ WARN] (2019-09-02 13:22:28.648) SensorData.cpp:718::uncompressDataConst() Requested laser scan data, but the sensor data (71) doesn't have laser scan.
[ WARN] (2019-09-02 13:22:28.648) SensorData.cpp:718::uncompressDataConst() Requested laser scan data, but the sensor data (124) doesn't have laser scan.
[ERROR] (2019-09-02 13:22:28.721) RegistrationIcp.cpp:1206::computeTransformationImpl() Laser scans empty?!? (new[124]=0 old[71]=0)

I don't use lidar sensor, i just using T265 and D435i...

first command:

roslaunch realsense2_camera rs_d435i_and_t265.launch

second command:

roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="--delete_db_on_start --Optimizer/GravitySigma 0.3" depth_topic:=/d435i/aligned_depth_to_color/image_raw rgb_topic:=/d435i/color/image_raw camera_info_topic:=/d435i/color/camera_info approx_sync:=false

this is the content of file rs_d435i_and_t265.launch:

<launch>
  <node pkg="tf" type="static_transform_publisher" name="d435i_to_t265_tf" args="0.0 0.0 0.0 -3.14 0.0 0.0 /t265_link /d435i_link 100" />

  <arg name="serial_no_camera1"    			default="myserial1"/> 			<!-- Note: Replace with actual serial number -->
  <arg name="serial_no_camera2"    			default="myserial2"/> 			<!-- Note: Replace with actual serial number -->
  <arg name="camera1"              			default="t265"/>		<!-- Note: Replace with camera name -->
  <arg name="camera2"              			default="d435i"/>		<!-- Note: Replace with camera name -->
  <arg name="tf_prefix_camera1"         default="t265"/>
  <arg name="tf_prefix_camera2"         default="d435i"/>
  <arg name="initial_reset"             default="false"/>
  <arg name="enable_fisheye"            default="true"/>
  <arg name="color_width"               default="640"/>
  <arg name="color_height"              default="480"/>
  <arg name="depth_width"               default="640"/>
  <arg name="depth_height"              default="480"/>
  <arg name="clip_distance"             default="-2"/>
  <arg name="topic_odom_in"             default="odom_in"/>
  <arg name="calib_odom_file"           default=""/>
  <arg name="enable_sync"               default="true"/>

  <group ns="$(arg camera1)">
    <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
      <arg name="serial_no"             value="myserial1"/>
      <arg name="tf_prefix"         		value="t265"/>
      <arg name="initial_reset"         value="$(arg initial_reset)"/>
      <arg name="enable_fisheye1"       value="$(arg enable_fisheye)"/>
      <arg name="enable_fisheye2"       value="$(arg enable_fisheye)"/>
      <arg name="topic_odom_in"         value="$(arg topic_odom_in)"/>
      <arg name="calib_odom_file"       value="$(arg calib_odom_file)"/>
      <arg name="unite_imu_method"      value="linear_interpolation"/>
      <arg name="align_depth"           value="true"/>
      <arg name="fisheye_width"         default="848"/>
      <arg name="fisheye_height"        default="800"/>
      <arg name="fisheye_fps"           default="30"/>
      <arg name="gyro_fps"              default="200"/>
      <arg name="accel_fps"             default="62"/>
      <arg name="enable_gyro"           default="true"/>
      <arg name="enable_accel"          default="true"/>
      <arg name="enable_sync"           default="true"/>
      <arg name="linear_accel_cov"      default="0.01"/>
    </include>
  </group>

  <group ns="$(arg camera2)">
    <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
      <arg name="serial_no"             value="myserial2"/>
      <arg name="tf_prefix"		          value="d435i"/>
      <arg name="initial_reset"         value="$(arg initial_reset)"/>
      <arg name="align_depth"           value="true"/>
      <arg name="filters"               value="pointcloud"/>
      <arg name="color_width"           value="$(arg color_width)"/>
      <arg name="color_height"          value="$(arg color_height)"/>
      <arg name="depth_width"           value="$(arg depth_width)"/>
      <arg name="depth_height"          value="$(arg depth_height)"/>
      <arg name="clip_distance"         value="$(arg clip_distance)"/>
    </include>
  </group>
</launch>

and this is the content of file rtabmap.launch:

<launch>
  <arg name="gui_cfg"           default="~/.ros/rtabmap_gui.ini" />
  <arg name="launch_prefix"     default=""/>
  <arg name="output"            default="screen"/>
  <arg name="imu_topic"         default="/imu/data"/>
  <arg name="rtabmapviz"              default="true" />
  <arg name="rviz"                    default="false" />
  <arg name="rviz_cfg"                default="$(find rtabmap_ros)/launch/config/rgbd.rviz" />
  <arg name="localization"            default="false"/>
  <!-- sim time for convenience, if playing a rosbag -->
  <arg name="use_sim_time"            default="false"/>

  <group ns="rtabmap">
      <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
      <param name="frame_id" type="string" value="t265_link"/>
      <param name="odom_frame_id"        type="string" value="t265_odom_frame"/>

      <remap from="rgb/image" to="/d435i/color/image_raw"/>
      <remap from="depth/image" to="/d435i/aligned_depth_to_color/image_raw"/>
      <remap from="rgb/camera_info" to="/d435i/color/camera_info"/>


      <param name="Kp/DetectorStrategy" type="string" value="6"/>
      <param name="RGBD/LoopClosureReextractFeatures" type="string" value="true"/>
      <param name="Vis/FeatureType" type="string" value="6"/>
    </node>

    <!-- Visualisation RTAB-Map -->

    <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(arg rviz_cfg)"/>
    <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb" output="$(arg output)">

      <param name="decimation"  type="double" value="4"/>
      <param name="voxel_size"  type="double" value="0.0"/>
      <param name="approx_sync" type="bool"   value="$(arg approx_sync)"/>
    </node>

    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg gui_cfg)" output="$(arg output)" launch-prefix="$(arg launch_prefix)">

      <param name="frame_id"             type="string" value="t265_link"/>
      <param name="odom_frame_id"        type="string" value="t265_odom_frame"/>
      <param name="queue_size"           type="int"    value="100"/>
      <param name="approx_sync"          type="bool"   value="false"/>
    </node>

  </group>
</launch>

[matlabbe]

Hi,

The arguments "rtabmap_args", "depth_topic", etc. are not used anywhere in your rtabmap.launch, so no need to set them when launching it. You could also remove points_xyzrgb node, as it seems not used.

Beside that, the rtabmap.launch file seems okay. Not sure why you get "Requested laser scan data, but the sensor data (71) doesn't have laser scan." errors though, which version of rtabmap are you using? That one "RegistrationIcp.cpp:1206::computeTransformationImpl" is called if Reg/Strategy is 1 or 2, but you don't seem to enable ICP in your launch file, it should not show up!? Do you have a resulting rtabmap.db to share?

For the /t265_link -> /d435i_link static transform, not sure how it is possible that both cameras are physically superposed. I would expect at least a Z value if one camera is over the other.

When you say it is slow, what do you mean? The input odometry from T265 should be quite fast (~30 Hz). If you want the map to be updated more frequently, increase Rtabmap/DetectionRate parameter (default is 1 Hz).

cheers,
Mathieu

[TOM_RB]

hi
i am trying to set three depth cams and one t265 for mapping and then localization and path planning ..
can you please help me by sending your final launch file (after modifications) >> ?