0
$\begingroup$

Rosanswers logo

I'm continuing my work on 3D map building as specified in my old topic.

Finally, I mounted my sensors (IMU, SICK and RealSense R200) on a mobile robot (Husky A200) an I'm doing some acquisitions outdoor; in particular, the IMU is mounted on the center of the robot frame, the SICK laser is mounted 10 centimeters at the top of the IMU sensor and the RealSense is mounted 40 centimeters on the top of the laser sensor.

Now, I have the /odometry/filtered topic which is a combination of wheels encoders and IMU data. I added the laser information in this way:

  <arg name="subscribe_scan"          default="true"/>
  <arg name="scan_topic"              default="/scan"/>

inside the rgbd_mapping.launch file and I run the RTAB algorithm with these commands:

$ roscore
$ rosparam set use_sim_time true
$ roslaunch rtabmap_ros rgbd_mapping.launch rgb_topic:=/camera/color/image_raw depth_registered_topic:=/camera/depth/image_raw camera_info_topic:=/camera/color/camera_info compressed:=true frame_id:=base_link rtabmap_args:="--delete_db_on_start" estimation:=1
$ rosbag play --clock test1.bag (the bag contains both realsense and laser data)

How can I add the filtered odometry which contains also the IMU data? Is there anything which I have to change/add in the rgbd_mapping.launch file?

Moreover, I tried to check the tf and frames for all sensors and I cannot see the laser frame. Moreover, the real sense frame is not a child of base_link.

How can I fix this frame problem? This is the frames.pdf file. The frame tree is suitable for the RTAB algorithm?

What happens if the robot has to face terrain disparity? The /odometry/filtered based on IMU sensor is able to compensate the disparity by adjusting the laser and the real sense data according with the tilt angle?

This is my rgbd_mapping.launch file:

<launch>
  <!-- Your RGB-D sensor should be already started with "depth_registration:=true".
        Examples:
           $ roslaunch freenect_launch freenect.launch depth_registration:=true 
           $ roslaunch openni2_launch openni2.launch depth_registration:=true -->
 
  <!-- Choose visualization -->
  <arg name="rviz"                    default="false" />
  <arg name="rtabmapviz"              default="true" /> 
  
  <!-- Corresponding config files -->
  <arg name="rtabmapviz_cfg"          default="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" />
  <arg name="rviz_cfg"                default="-d $(find rtabmap_ros)/launch/config/rgbd.rviz" />
  
  <arg name="frame_id"                default="camera_link"/>   <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
  <arg name="time_threshold"          default="0"/>             <!-- (ms) If not 0 ms, memory management is used to keep processing time on this fixed limit. -->
  <arg name="optimize_from_last_node" default="false"/>         <!-- Optimize the map from the last node. Should be true on multi-session mapping and when time threshold is set -->
  <arg name="database_path"           default="~/.ros/rtabmap.db"/>
  <arg name="rtabmap_args"            default=""/>              <!-- delete_db_on_start, udebug -->
  <arg name="launch_prefix"           default=""/>              <!-- for debugging purpose, it fills launch-prefix tag of the nodes -->
   
  <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
  <arg name="depth_registered_topic"  default="/camera/depth_registered/image_raw" />
  <arg name="camera_info_topic"       default="/camera/rgb/camera_info" />
  <arg name="compressed"              default="false"/>
  <arg name="convert_depth_to_mm"     default="true"/>
   
  <arg name="subscribe_scan"          default="true"/>         <!-- Assuming 2D scan if set, rtabmap will do 3DoF mapping instead of 6DoF -->
  <arg name="scan_topic"              default="/scan"/>
  
  <arg name="visual_odometry"         default="true"/>          <!-- Generate visual odometry -->
  <arg name="odom_topic"              default="/odom"/>         <!-- Odometry topic used if visual_odometry is false -->
   
  <arg name="namespace"               default="rtabmap"/>
  <arg name="wait_for_transform"      default="0.1"/>
  
  <!-- Odometry parameters: -->
  <arg name="strategy"            default="0" />       <!-- Strategy: 0=BOW (bag-of-words) 1=Optical Flow -->
  <arg name="feature"             default="6" />       <!-- Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK -->
  <arg name="estimation"          default="0" />       <!-- Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP) -->
  <arg name="nn"                  default="3" />       <!-- Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE (SIFT, SURF), 2=FLANN_LSH, 3=BRUTEFORCE (ORB/FREAK/BRIEF/BRISK) -->
  <arg name="max_depth"           default="0" />       <!-- Maximum features depth (m) -->
  <arg name="min_inliers"         default="20" />      <!-- Minimum visual correspondences to accept a transformation (m) -->
  <arg name="inlier_distance"     default="0.1" />     <!-- RANSAC maximum inliers distance (m) -->
  <arg name="local_map"           default="1000" />    <!-- Local map size: number of unique features to keep track -->
  <arg name="variance_inliers"    default="true"/>    <!-- Variance from inverse of inliers count -->  
        
  <!-- Nodes -->
  <group ns="$(arg namespace)">
  
    <node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="compressed in:=$(arg rgb_topic) raw out:=$(arg rgb_topic)" />
    <node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="compressedDepth in:=$(arg depth_registered_topic) raw out:=$(arg depth_registered_topic)" />
  
    <!-- Odometry -->
    <node if="$(arg visual_odometry)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen" launch-prefix="$(arg launch_prefix)">
      <remap from="rgb/image"       to="$(arg rgb_topic)"/>
      <remap from="depth/image"     to="$(arg depth_registered_topic)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
      
      <param name="frame_id"                 type="string" value="$(arg frame_id)"/>
      <param name="wait_for_transform_duration"       type="double"   value="$(arg wait_for_transform)"/>
      
      <param name="Odom/Strategy"            type="string" value="$(arg strategy)"/> 
      <param name="Odom/FeatureType"         type="string" value="$(arg feature)"/>  
      <param name="OdomBow/NNType"           type="string" value="$(arg nn)"/>
      <param name="Odom/EstimationType"      type="string" value="$(arg estimation)"/> 
      <param name="Odom/MaxDepth"            type="string" value="$(arg max_depth)"/>  
      <param name="Odom/MinInliers"          type="string" value="$(arg min_inliers)"/> 
      <param name="Odom/InlierDistance"      type="string" value="$(arg inlier_distance)"/>       
      <param name="OdomBow/LocalHistorySize" type="string" value="$(arg local_map)"/> 
      <param name="Odom/FillInfoData"        type="string" value="true"/> 
      <param name="Odom/VarianceFromInliersCount" type="string" value="$(arg variance_inliers)"/>  
    </node>
  
    <!-- Visual SLAM (robot side) -->
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
      <param name="subscribe_depth"    type="bool"   value="true"/>
      <param name="subscribe_laserScan"     type="bool"   value="$(arg subscribe_scan)"/>
      <param name="frame_id"           type="string" value="$(arg frame_id)"/>
      <param name="wait_for_transform_duration" type="double"   value="$(arg wait_for_transform)"/>
      <param name="database_path"      type="string" value="$(arg database_path)"/>
    
      <remap from="rgb/image"       to="$(arg rgb_topic)"/>
      <remap from="depth/image"     to="$(arg depth_registered_topic)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
      <remap from="scan"            to="$(arg scan_topic)"/>
      <remap unless="$(arg visual_odometry)" from="odom"  to="$(arg odom_topic)"/>
      
      <param name="Rtabmap/TimeThr"           type="string" value="$(arg time_threshold)"/>
      <param name="RGBD/OptimizeFromGraphEnd" type="string" value="$(arg optimize_from_last_node)"/>
      <param name="LccBow/MinInliers"         type="string" value="10"/>
      <param name="LccBow/InlierDistance"     type="string" value="$(arg inlier_distance)"/>
      <param name="LccBow/EstimationType"     type="string" value="$(arg estimation)"/> 
      <param name="LccBow/VarianceFromInliersCount" type="string" value="$(arg variance_inliers)"/>
      <param name="Mem/SaveDepth16Format"     type="string" value="$(arg convert_depth_to_mm)"/>
      
      <!-- when 2D scan is set -->
      <param if="$(arg subscribe_scan)" name="RGBD/OptimizeSlam2D"          type="string" value="true"/>
      <param if="$(arg subscribe_scan)" name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/>
      <param if="$(arg subscribe_scan)" name="LccIcp/Type"                  type="string" value="2"/> 
      <param if="$(arg subscribe_scan)" name="LccIcp2/CorrespondenceRatio"  type="string" value="0.25"/>
    </node>
  
    <!-- Visualisation RTAB-Map -->
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="$(arg rtabmapviz_cfg)" output="screen" launch-prefix="$(arg launch_prefix)">
      <param name="subscribe_depth"     type="bool"   value="true"/>
      <param name="subscribe_laserScan"      type="bool"   value="$(arg subscribe_scan)"/>
      <param name="subscribe_odom_info" type="bool"   value="$(arg visual_odometry)"/>
      <param name="frame_id"            type="string" value="$(arg frame_id)"/>
      <param name="wait_for_transform_duration"  type="double"   value="$(arg wait_for_transform)"/>
    
      <remap from="rgb/image"       to="$(arg rgb_topic)"/>
      <remap from="depth/image"     to="$(arg depth_registered_topic)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
      <remap from="scan"            to="$(arg scan_topic)"/>
      <remap unless="$(arg visual_odometry)" from="odom"  to="$(arg odom_topic)"/>
    </node>
  
  </group>
  
  <!-- Visualization RVIZ -->
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="$(arg rviz_cfg)"/>
  <!-- sync cloud with odometry and voxelize the point cloud (for fast visualization in rviz) -->
  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="standalone_nodelet"  args="manager" output="screen"/>
  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap_ros/data_odom_sync standalone_nodelet">
    <remap from="rgb/image_in"        to="$(arg rgb_topic)"/>
    <remap from="depth/image_in"      to="$(arg depth_registered_topic)"/>
    <remap from="rgb/camera_info_in"  to="$(arg camera_info_topic)"/>
    <remap if="$(arg visual_odometry)"     from="odom_in"  to="rtabmap/odom"/>
    <remap unless="$(arg visual_odometry)" from="odom_in"  to="$(arg odom_topic)"/>
    
    <remap from="rgb/image_out"       to="data_odom_sync/image"/>
    <remap from="depth/image_out"     to="data_odom_sync/depth"/>
    <remap from="rgb/camera_info_out" to="data_odom_sync/camera_info"/>
    <remap from="odom_out"            to="odom_sync"/>
  </node>
  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap_ros/point_cloud_xyzrgb standalone_nodelet">
    <remap from="rgb/image"       to="data_odom_sync/image"/>
    <remap from="depth/image"     to="data_odom_sync/depth"/>
    <remap from="rgb/camera_info" to="data_odom_sync/camera_info"/>
    <remap from="cloud"           to="voxel_cloud" />

    <param name="decimation" type="double" value="2"/>
    <param name="voxel_size" type="double" value="0.02"/>
  </node>
<node pkg="tf" type="static_transform_publisher"  name="base_to_realsense"
      args="0 0 0 -1.5707963 0 -1.5707963 /base_link /realsense_frame 100" />
<node pkg="tf" type="static_transform_publisher"  name="base_to_laser"
      args="0 0 0.1 0 0 0 /base_link /laser 100" />
<node pkg="tf" type="static_transform_publisher"  name="base_to_color"
      args="0 0 0.6 -1.5707963 0 -1.5707963 /base_link /camera_color_optical_frame 100" />
<node pkg="tf" type="static_transform_publisher"  name="color_to_depth"
      args="0 0 0 0 0 0 /camera_color_optical_frame /camera_depth_optical_frame 100" />
</launch>

Originally posted by Marcus Barnet on ROS Answers with karma: 287 on 2016-08-23

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

First, you need to add a calibrated tf between your base_link and your realsense_frame. You can manually measure this and verify it. The robot needs to know where the camera is relative to its other parts.

You may also want to look at the robot_localization package to fuse your rtabmap odometry with the IMU and wheel odometry.


Originally posted by mjedmonds with karma: 58 on 2016-08-23

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by Marcus Barnet on 2016-08-24:
rosrun tf static_transform_publisher 0 0 0.50 0 0 0 base_link realsense_frame 100

rosrun tf static_transform_publisher 0 0 0.1 0 0 0 base_link laser 100

Do you think it should be correct?

/odometry/filtered is the output of robot_localization, how can I use it as input for RTAB?

Comment by Marcus Barnet on 2016-08-24:
/odometry/filtered already contains the wheel odometry fused with the IMU data. I need to use this as input for RTAB, but I don't know how to set this inside rgbd_mapping.launch file.

Comment by matlabbe on 2016-08-24:
Using /odometry/filtered as odometry:

<arg name="visual_odometry" default="false"/> <!-- Generate visual odometry -->
<arg name="odom_topic" default="/odometry/filtered"/> <!-- Odometry topic used if visual_odometry is false -->

Comment by Marcus Barnet on 2016-08-24:
Do you think the /odometry/filtered with IMU will improve the rtab algorithm by adding compensation for terrain disparity? I mean for example if the robot climbs or tilt on agricultural terrain.

Comment by matlabbe on 2016-08-24:
Not sure what do you mean by terrain disparity, but with IMU you would have 6DoF odometry, instead of 3DoF if using only wheel encoders.

Note also that if you are on a 3D terrain, don't set subscribe_scan to true, as rtabmap assumes you are on a 2d terrain (indoor) if it is true.

Comment by Marcus Barnet on 2016-08-24:
On agricultural terrain, there can be ditches that will cause the robot and the camera to bottom out, and slopes that will tip the robot and the camera over. With 6FoF, the RTAB can compensate this problems by building a more accurate 3D map?

Comment by Marcus Barnet on 2016-08-24:
Moreover, if I'm on a 3D terrain, can't I use /scan topic, then? I noticed that using the laser, the map is improved with more features. Is there a way to add laser information on 3D terrain? Can't I compensate the scan information by adding the IMU data to achieve a 3D laser visualization?

Comment by mjedmonds on 2016-08-24:
It seems more appropriate to have bottom-out and tipping avoidance be part of your navigation planner, but I'm not an expert on the topic.

However, the way I use robot_localization, I fuse the IMU with the wheel odometry and visual odometry from rtabmap. See the robot_localization package

Comment by Marcus Barnet on 2016-08-24:
Thank you for the suggestion. I've read the documentation about the robot_localization package but I didn't understand how it is possible to fuse the filtered odometry (wheels + IMU) with the visual odometry. How do you do this?

Comment by mjedmonds on 2016-08-24:
You'll add a line like this:

<param name="odom1" value="rtabmap/odom"/>

Comment by mjedmonds on 2016-08-24:
with a corresponding config like this:

<rosparam param="odom1_config">[true, true, false, false, false, true, false, false, false, false, false, false, false, false, false]</rosparam>

Comment by Marcus Barnet on 2016-08-24:
You mean I can do it directly inside my rgbd_mapping.launch file? rtabmap/odom is the visual odometry computed by RTAB? Because I think I can specify only one odometry topic inside the rgbd_mapping.launch file.

Comment by mjedmonds on 2016-08-24:
No, you set those parameters in the robot_localization launch file that's already fusing your IMU and wheel odometery. See an example robot_localization launch file from the package.

Comment by Marcus Barnet on 2016-08-24:
The problem is that the robot_localization package is launched by husky_base base.launch file which launches control.launch which starts robot_localization here. How can I modify it?

Comment by mjedmonds on 2016-08-24:
Not a problem, put the odom configuration in between the tags for robot_localization in the file you linked

Comment by Marcus Barnet on 2016-08-24:
Ok I'm going to try it in a while. But if I do this, I will have to record again the bag files by including the Odom configuration in robot_localization? Or can I do it by using the old bag files?

Comment by mjedmonds on 2016-08-24:
Yes, this is a fundamental change to the odometry/filtered topic, so you will have to rerecord your bags. Any change to the robot's configuration will require a rerecording of any bags.

Comment by Marcus Barnet on 2016-08-24:
So, during the recording, do I have to launch also roslaunch rtabmap_ros rgbd_mapping.launch since I need to generete the RTAB visual odometry needed by robot_localization? Is it correct? Or do I need to launch another file?

Comment by mjedmonds on 2016-08-24:
Yes, that sounds correct, though you know more about your exact configuration than I do.

Comment by Marcus Barnet on 2016-08-24:
I added to robot_localization the visual odometry rtabmap/odom as suggested. In rgbd_mapping.launch I had to set to false the visual_odometry: since I had to specify the /odometry/filtered topic.

Comment by Marcus Barnet on 2016-08-24:
But, now, when I launch the rgbd_mapping.launch file, will it create the visual odometry rtabmap/odom even if I set it to false? Does it make sense to use visual odometry in robot_localization if it is not generated by rgbd_mapping? Or the visual odometry is always published by RTAB?

Comment by matlabbe on 2016-08-24:
Be aware that you should set publish_tf to false for the visual odometry if you want to integrate to robot_localization (which also publishes /odom->/base_link). You may also prefer to copy the <node> you want in another launch file than using rgbd_mapping.launch.

Comment by matlabbe on 2016-08-24:
You can take also a look at the sensor_fusion.launch example for an integration of IMU/vo and robot_localization.

Comment by Marcus Barnet on 2016-08-25:
I'm getting confusing. When I lunch my robot roslaunch husky_base base.launch, base.launch launches control.launch which launches robot_localization package in this way.

Comment by Marcus Barnet on 2016-08-25:
This is my modified control.launch. This is my rgbd_mapping.launch. How can I use the sensor_fusion.launch? I see it calls again ekf_localization_node. Should I don't use the control.launch file?

Comment by Marcus Barnet on 2016-08-25:
If I use the modified control.launch, robot_localization will require the visual odometry, too, in order to generete /odometry/filtered. But RTAB won't generete the visual_odometry since it is already set to false because I use /odometry/filtered in rgbd_mapping.launch. Is it correct?

Comment by matlabbe on 2016-08-25:
It would be less complicated using the rtabmap's nodes directly, not the launch files. You may copy the nodes in control.launch if you want. rgbd_mapping.launch and sensor_fusion.launch files are "examples" on how to use the nodes.

Comment by Marcus Barnet on 2016-08-25:
I included the RTAB node and this is the control-new.launch file, is it OK? I don't know if I have to use /vo or rtabmap/odom as parameter in robot_localization node. How to pass the correct visual odometry to robot_localization?help me please

Comment by mjedmonds on 2016-08-25:
The numbering for each fused source should start at 0, as the robot_localization documentation indicates. So if rtabmap is your own odometry source, you should set it be 'odom0' and 'odom0_config'. However, you said you were also integrating wheel odometry, which can be a twist or odometry input.

Comment by mjedmonds on 2016-08-25:
My main point is that you should try to understand the hierarchy of your setup and set the proper value.

Comment by Marcus Barnet on 2016-08-25:
In localization.yaml, I have odom0: husky_velocity_controller/odom and imu0: imu/data, so the rtabmap should be odom1 inside control-new.launch. The question is: the value for odom1 must be /vo or rtabmap/odom?

Comment by Marcus Barnet on 2016-08-25:
I can't understand why in sensor_fusion sample, there is <remap from="odom" to="/vo"/> and also <remap from="odom" to="/odometry/filtered"/>. My control-new.launch file is totally wrong? Should I put the RTAB inside the localization.yaml file?

Comment by mjedmonds on 2016-08-25:
It should be the value of the odometry topic from rtabmap, which is /rtabmap/odom if you have rtabmap namespace'd, or /odom if you don't (your control-new.launch has it namespace'd).

Comment by matlabbe on 2016-08-25:
in your control-new.launch, there is a remap <remap from="odom" to="/vo"/>, so you should use /vo for robot_localization. Without this remap, you would use /rtabmap/odom.

Comment by Marcus Barnet on 2016-08-25:
Thank you for your help! And <remap from="odom" to="/odometry/filtered"/> is correct inside the control-new.launch file? I think not since /odometry/filtered is generated by robot_localization. Should I remove this line from launch-new.launch file?

Comment by Marcus Barnet on 2016-08-25:
Ok, Matlabbe, thanks you, I didn't read your last message. So I just have to use the /vo value. Should I delete the <remap from="odom" to="/odometry/filtered"/> from control-launch.launch? I think it has no sense in this case, is it correct?

Comment by matlabbe on 2016-08-25:
For rtabmap node, you should keep <remap from="odom" to="/odometry/filtered"/> as it is the odometry input.

Comment by Marcus Barnet on 2016-08-25:
So, my final control-new.launch file is correct, now? I was also thinking to the frame_id which is set to base_link which should be my Husky robot frame. Is it a correct assignment?

Comment by matlabbe on 2016-08-25:
frame_id should be your robot base frame (the same used by your wheel odometry). Use <param name="odom1" value="/vo"/> as you kept the remap. For odom1_config, refer to robot_localization documentation (/vo is 6 DoF).

Comment by Marcus Barnet on 2016-08-25:
After recording the bag file, do I have to use only odometry/filtered as input to rgbd_mapping.launch? The odometry/filtered will contain the combined odometry between wheels encoders, Imu and visual odometry?

Comment by Marcus Barnet on 2016-08-25:
I recorded a new bag file with control_new.launch. I launched rgbd_mapping.launch to build the offline map but the map is totally messed up. Can it be for transformations errors?

Comment by Marcus Barnet on 2016-08-25:
I get no errors for rgbd_mapping.launch, but the map is not built correctly. I think it should be a transformation error, but I can't find the reason!

Comment by Marcus Barnet on 2016-08-26:
I opened another topic in other to better focus on the map building since the odometry filtered should be OK now!

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.