Outdoor 3D mapping with GPS and IMU

[acp]

Dear people.

I have a jetson tx2 and a ZED camera. I have installed ros kinetic and rtabmap-ros.

A launch file has been created base on a existing ones from zed-ros, rtabmap-ros.

The questions are:

  1.-How can I know that rtabmap - ros is a good choice for 3D mapping and drone navigation?

  2.-How easy or difficult is to integrate GPS and IMU readings?

  3.-what parameters I need to tune to get a god 3D map for drone navigation.


I am looking forward to your replay.

[matlabbe]

Hi,

1. Look at the paper RTAB-Map as an Open-Source Lidar and Visual SLAM Library for Large-Scale and Long-Term Online Operation, in particular results on EuRoC dataset. To know if it is a good choice, are you planning to do navigation indoor or outdoor? RTAB-Map is more tested indoor.

2. IMU reading can be fed to rtabmap's odometry nodes (e.g., rgbd_odometry, stereo_odometry), but only used by VIO approaches. So you have to build rtabmap with OKVIS or MSCKF_VIO dependencies, then select Odom/Strategy=6 or 8 respectively.

3. A good 3D map depends on localization and disparity accuracy. Localization is covered in paper referred above. For disparity, I would refer to cv::StereoBM (rtabmap's default disparity approach) or how ZED sdk is generating the depth image. The default parameters or RTAB-Map are tuned for hand-held scanning, which is similar to a drone. If you have images with higher resolution than 640x480, parameters GFTT/MinDistance or GFTT/BlockSize could be increased.

cheers,
Mathieu

[acp]

Hi Mathieu

Thank you for your replay

Well, the idea is outdoor mapping and navigation. And,  the ZED-SDK gives a depth disparity image, actually I have build up a 3D  indoor map.


I took a look at the paper and I can see that:
Overall, ORB2-RTAB performs better on six of the 11 sequences when compared to other RTAB-Map’s odometry
approaches, but is the second most computationally expensive approach.  OKVIS and MSCKF are the only
approaches able to track the whole V2-03-difficult sequence.

It means that rtabmap it is a good option, right?

As far I do understand is that PX4 fuses the GPS position with IMU using a extended kalman filtering (EKF) to produce an estimate with a full state covariance matrix. This odometry message can be feed to rtabmap. right?

Well my idea was to first use the output of PX4 to see how the map is build and then fuse it with zed position estimation. What do you think?


I am looking forward to your replay

[acp]

Hi Mathieu


I just recorded  a bag file with the following topics:

rosbag record -O subset /stereo_camera/rgb/image_rect_color /stereo_camera/depth/depth_registered /stereo_camera/depth/camera_info /stereo_camera/depth/camera_info /stereo_camera/odom /stereo_camera/rgb/camera_info /tf /mavros/odometry/odom /mavros/local_position/velocity /mavros/local_position/pose /mavros/local_position/odom /mavros/global_position/local /mavros/global_position/global  /mavros/battery /mavros/trajectory/path /mavros/trajectory/generated  -b 0


When I run the bag file with the rtabmap setup as follows:

    <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
      <arg name="rtabmap_args" value="--delete_db_on_start" />       
       <arg name="rgb_topic"               value="/stereo_camera/$(arg rgb_topic)" />
      <arg name="depth_topic"             value="/stereo_camera/$(arg depth_topic)" />
      <arg name="camera_info_topic"       value="/stereo_camera/$(arg camera_info_topic)" />
      <arg name="depth_camera_info_topic" value="/stereo_camera/$(arg depth_camera_info_topic)" />
      <arg name="frame_id"                  value="$(arg camera_frame)" />
      <arg name="approx_sync"               value="false" />
      <arg name="visual_odometry"           value="false" />
      <arg name="odom_topic"                value="/stereo_camera/odom" />
     
     

         
         
         
         
            
                  
         

         

         
          

         
         
          


    </include>


The map is quite poor when I use

<arg name="odom_topic"    value="/stereo_camera/odom" /> 

however when I run it with

<arg name="odom_topic"     value="/mavros/local_position/odom" />

     the publish frequency of "/mavros/local_position/odom"  is less than 1 and I loose the transformation /map -> /odom. And, I do not get the xyz coordinates as I do when I run it with "/stereo_camera/odom"


I think this is because the ZED camera gives a transformation /map -> /odom.  


1.- How cam I increase the frequency of:

            "/mavros/local_position/odom"

2.-How can I get a good "/mavros/local_position/odom"

3.-maybe I just need to fuse:

                 "/mavros/local_position/odom"   with "/stereo_camera/odom"

4.-how can I use OKVIS or MSCKF_VIO with   "/mavros/local_position/odom"   and "/stereo_camera/odom"


I am kind of lost :)


I am looking forward to your replay :)

Thank you :)

[matlabbe]

Hi,

If you use rtabmap with zed's odometry, make sure to disable /map -> /odom tf from zed wrapper, by setting publish_map_tf to false. Only rtabmap should publish /map -> /odom.

the publish frequency of "/mavros/local_position/odom"  is less than 1

Is it 1 Hz? IF so it is not published enough fast. Odometry should be published over 10 Hz. For questions 1 and 2, I cannot really help, look at mavros documentation or ask those specific questions on ROS Answers.

3. Fusion of multiple odometry sources can be not trivial, robot_localization package can be used for that though.

4. I think you should start by using only one odometry source (e.g., zed odometry), you should be able to have already a relatively good map. Make sure you don't have multiple nodes publishing the same /odom -> /base_link TF frame.

Overall, if you can share a rosbag, it could help to debug.

cheers,
Mathieu

[acp]

Dear Mathieu.


Thank you for your answer.


1.-I have tried to upload a .bag file however it is too big :) Is there a way to share it here :). I will try to upload it in the public site of the Institute and share the link :).

2.-Just to ask if the right topic from mavros to feed rtabmap to build up the map is:

            <arg name="odom_topic"                value="/mavros/local_position/odom" />

3.-When I take a look at "rostopic hz /mavros/local_position/odom" I get:

               average rate: 0.650
              min: 1.539s max: 1.539s std dev: 0.00000s window: 2
               no new messages
              average rate: 0.655
            min: 1.515s max: 1.539s std dev: 0.01202s window: 3

4.-When I take a look at "rostopic echo /mavros/local_position/odom" I get:

           
   header:
  seq: 72
  stamp:
    secs: 1548858290
    nsecs: 758918336
  frame_id: "map"
child_frame_id: "base_link"
pose:
  pose:
    position:
      x: -0.00200466089882
      y: 0.00498028472066
      z: 0.308459073305
    orientation:
      x: -0.0208578280061
      y: 0.0280280862562
      z: -0.515654499607
      w: -0.856083999632
  covariance: [1e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-06]
twist:
  twist:
    linear:
      x: -0.0269354181798
      y: -0.0513042242524
      z: 0.113977489076
    angular:
      x: 0.264631807804
      y: 0.000432345404988
      z: -0.233702689409
  covariance: [0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001]

As you can see the position is zero and does not change when the drone is moving as it does with the odom from the zed camera.

4.- Well I have tried to start a mission planner with qgrouncontrol, I can see the following:

      GPS Count 17
     GPS lock 3D DGPS lock
     HDOP 0.7
    VDOP 1.1
   Course over ground 21.4

       -However, when I set up or add way points in mission plan. I could successfully load the mission and when I wanted to start the mission I got the same error:

     -""Unable to start vehicle not ready""

    -When I take a look at mavlink inspector I can not see GLOBAL_POSITION_INT




5.-When I write the  launch file "roslaunch mavros px4.launch fcu_url:="/dev/ttyTHS1:57600" gcs_url:="udp:=//@my_inet_addr" I get:


   * Qgroundcontro says communication regain
   * I get no erros



6.- I am thinking that there is a relation between the the drone not be able to get GLOBAL_POSITION_INT and the  /mavros/local_position/odom.


What do you think? any clue about this issue :)



Regards,

[acp]

Hi Mathew


Just to answer to my questions :)


1.-I managed to increase the rate of pixhawk's publishing topics like "/mavros/local_position/odom" by simply adding to the pixhawk sd card the following:  


1. I have created a file called extras.txt in /fs/etc/extras.txt

2.-I added the following lines

mavlink stop-all
mavlink start -d /dev/ttyS1 -b 921600 -r 20000 -m onboard
mavlink stream -d /dev/ttyS0 -s ATTITUDE -r 200
mavlink stream -d /dev/ttyS0 -s HIGHRES_IMU -r 200

I now get rate of 25 :)

3.- I have manage to get GLOBAL_POSITION_INT in the qgroundcontrol station by simply calibrating the pixhawk mini again using the Qgroundcontrol station.

And, to get that variable one has to be outside and the GPS must be lock.




4.- I have a question concerning the  transformation /odom -> /base_link. If I disable th tf transform from the zed wrapper how can I get the transformation /odom -> /base_link


5.-And, just to asking if the right topic from mavros to feed rtabmap to build up the map is:

            <arg name="odom_topic"                value="/mavros/local_position/odom" />


Regards and thank you

[acp]

Hi Mathiu

When you say.

2. IMU reading can be fed to rtabmap's odometry nodes (e.g., rgbd_odometry, stereo_odometry), but only used by VIO approaches. So you have to build rtabmap with OKVIS or MSCKF_VIO dependencies, then select Odom/Strategy=6 or 8 respectively.


I am trying to fuse the odom from zed camera with the imu from the pixhawk.


More specifically.  

the fusion launch file: odom_zed_imu_fusion.launch looks like this:

odom_zed_imu_fusion.launch

<launch>  


<arg name="frame_id"                default="zed_center" />
  <arg name="imu_ignore_acc"          default="true" />
  <arg name="imu_remove_gravitational_acceleration" default="true" />

  <!-- Localization-only mode -->
  <arg name="localization"      default="false"/>
  <arg     if="$(arg localization)" name="rtabmap_args"  default=""/>
  <arg unless="$(arg localization)" name="rtabmap_args"  default="--delete_db_on_start"/>  
  <!-- Odometry fusion (EKF), refer to demo launch file in robot_localization for more info -->
  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true" output="screen">

      <param name="frequency" value="50"/>
      <param name="sensor_timeout" value="0.1"/>
      <param name="two_d_mode" value="false"/>

      <param name="odom_frame" value="odom"/>
      <param name="base_link_frame" value="$(arg frame_id)"/>
      <param name="world_frame" value="odom"/>

      <param name="transform_time_offset" value="0.0"/>

      <param name="odom0" value="/stereo_camera/odom"/>
      <param name="imu0" value="/mavros/imu/data"/> 

      <!-- The order of the values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. -->
      <rosparam param="odom0_config">[true, true, true,
                                      false, false, false,
                                      true, true, true,
                                      false, false, false,
                                      false, false, false]</rosparam>

      <rosparam     if="$(arg imu_ignore_acc)" param="imu0_config">[
                                     false, false, false,
                                     true,  true,  true,
                                     false, false, false,
                                     true,  true,  true,
                                     false,  false,  false] </rosparam>
      <rosparam unless="$(arg imu_ignore_acc)" param="imu0_config">[
                                     false, false, false,
                                     true,  true,  true,
                                     false, false, false,
                                     true,  true,  true,
                                     true,  true,  true] </rosparam>  
      
      <param name="odom0_differential" value="false"/>
      <param name="imu0_differential" value="false"/>

      <param name="odom0_relative" value="true"/>
      <param name="imu0_relative" value="true"/>

      <param name="imu0_remove_gravitational_acceleration" value="$(arg imu_remove_gravitational_acceleration)"/>

      <param name="print_diagnostics" value="true"/>

      <!-- ======== ADVANCED PARAMETERS ======== -->
      <param name="odom0_queue_size" value="5"/>
      <param name="imu0_queue_size" value="50"/> 

      <!-- The values are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz,
           vroll, vpitch, vyaw, ax, ay, az. -->
      <rosparam param="process_noise_covariance">[0.005, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0.005, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0.006, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0.003, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0.003, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0.006, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0.0025, 0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0.0025, 0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0.004, 0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0.001, 0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.001, 0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.002, 0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.001, 0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.001, 0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.0015]</rosparam>

      <!-- The values are ordered as x, y,
           z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. -->
           <rosparam param="initial_estimate_covariance">[1e-9, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    1e-9, 0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    1e-9, 0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    1e-9, 0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    1e-9, 0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-9,  0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-9,  0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1e-9,  0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1e-9, 0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1e-9, 0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]</rosparam>

    </node>





</launch>

I have recorded a rosbag file and when I run the command:

rosbag play --clock subset and then the launch the rtabmap launch file to build the 3d map:

<!-- RTAB-map Node-->
    <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
      <arg name="rtabmap_args"		value="--delete_db_on_start" />	
       <arg name="rgb_topic"               	value="/stereo_camera/$(arg rgb_topic)" />
      <arg name="depth_topic"             	value="/stereo_camera/$(arg depth_topic)" />
      <arg name="camera_info_topic"       	value="/stereo_camera/$(arg camera_info_topic)" />
      <arg name="depth_camera_info_topic" 	value="/stereo_camera/$(arg depth_camera_info_topic)" />
      <arg name="frame_id"                  value="$(arg camera_frame)" />
      <param name="odom_frame_id"          type="string" value="odom"/>
      <arg name="approx_sync"               value="false" />
      <arg name="visual_odometry"           value="false" />
      <!--arg name="odom_topic"                value="/odometry/filtered" /-->
      <arg name="odom_topic"                value="/stereo_camera/odom" />
      <!--remap from="odom"            to="/odometry/filtered"/-->
      <!--arg name="odom_topic"                value="/mavros/local_position/odom" /-->

<!--from here-->
<!-- RTAB-Map's parameters -->
         <param name="Rtabmap/TimeThr" type="string" value="700"/>
         <param name="Rtabmap/DetectionRate" type="string" value="1"/>
         
         <param name="Kp/WordsPerImage" type="string" value="200"/>
         <param name="Kp/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
         <param name="Kp/DetectorStrategy" type="string" value="0"/>   <!-- use SURF -->
         <param name="Kp/NNStrategy" type="string" value="1"/>         <!-- kdTree -->
         <param name="Kp/MaxFeatures" type="string" value="-1"/>
          

         <param name="SURF/HessianThreshold" type="string" value="1000"/>

         <param name="LccBow/MinInliers" type="string" value="10"/>
         <param name="LccBow/EstimationType" type="string" value="1"/> <!-- 3D->2D (PnP) -->

         <param name="LccReextract/Activated" type="string" value="true"/>
         <param name="LccReextract/MaxWords" type="string" value="500"/>
         <param name="LccReextract/MaxDepth" type="string" value="10"/> 

         <param name="RGBD/OptimizeStrategy" type="string" value="2"/> <!-- g2o=1, GTSAM=2 -->
   <param name="RGBD/OptimizeRobust" type="string" value="true"/>
   <param name="RGBD/OptimizeMaxError" type="string" value="0"/> <!-- should be 0 if RGBD/OptimizeRobust is true -->


    </include>

I get the following problems:


1.- When I use /stereo_camera/odom the RTABMAP publish a transform /map->/odom, however the map looks really bad :)


2.-when I run the rtabmap launch file with /odometry/filtered

  RTABMAP does not  publish any  transform /map->/odom,

What I am doing wrong? any help I will appreciate it


THANK YOU :)

[matlabbe]

Hi,

You would have to debug the odometry part before feeding odometry to rtabmap. In rviz, you could try starting only robot_localization to show TF odom->base_link and see if the resulting frame makes sense while moving zed+IMU. You may also show the Zed point cloud at the same time to see if objects are not moving when the zed is moving.

When /map->/odom is not published, you may have warnings on the terminal saying that rtabmap didn't receive topics since 5 seconds. If there are no warnings, rtabmap may have difficulty to synchronize the input topics. Do you have a rosbag to share with only robot_localization started with zed and IMU?

cheers,
Mathieu

[acp]

Hi Mathieu


I just run the robot localization:  odom_vo_imu_fusion.launch


and I run this with the bag file: http://www.fit.vutbr.cz/~plascencia/subset.bag and this launch file:  zed_stereo_fusion_bag_play.launch



And the data base is this.

 http://www.fit.vutbr.cz/~plascencia/rtabmap.db 


Thank you :)

[acp]

Hi Matheiu,


How can I  fed IMU  to rtabmap's odometry nodes (e.g., rgbd_odometry, stereo_odometry) using VIO approaches. How can I  build rtabmap with OKVIS or MSCKF_VIO dependencies, then how can I  select Odom/Strategy=6 or 8 respectively.


Regards and thank you :)

[matlabbe]

Hi,
I tried to tear down your launch files into a single one to test four combinations described below:

<?xml version="1.0"?>

<launch>
  <arg name="frame_id"                default="base_link" />
  <arg name="rgb_topic"               default="/stereo_camera/rgb/image_rect_color" />
  <arg name="depth_topic"             default="/stereo_camera/depth/depth_registered" />
  <arg name="camera_info_topic"       default="/stereo_camera/rgb/camera_info" />
  <arg name="imu_topic"               default="/mavros/imu/data" />
  <arg name="imu_ignore_acc"          default="true" />
  <arg name="imu_remove_gravitational_acceleration" default="true" />
  <arg name="sensor_fusion" default="true"/>
  <arg name="zed_odom" default="false"/>

  <param name="use_sim_time" value="true"/> <!-- just when playing a rosbag -->

  <group unless="$(arg sensor_fusion)">
    <!-- Publish odom tf when not using sensor fusion with zed odom msg -->
    <node if="$(arg zed_odom)" pkg="rtabmap_ros" type="odom_msg_to_tf" name="odom_msg_to_tf">
      <remap from="odom" to="/stereo_camera/odom"/>
      <param name="frame_id" type="string" value="$(arg frame_id)"/>
    </node>
  </group>

  <group ns="rtabmap">
    <node unless="$(arg zed_odom)" pkg="rtabmap_ros" type="rgbd_odometry" name="visual_odometry" output="screen">
      <remap from="rgb/image"       to="$(arg rgb_topic)"/>
      <remap from="depth/image"     to="$(arg depth_topic)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>

      <param name="frame_id"               type="string" value="$(arg frame_id)"/>
      <param     if="$(arg sensor_fusion)" name="publish_tf"  type="bool"   value="false"/>
      <param unless="$(arg sensor_fusion)" name="publish_tf"  type="bool"   value="true"/>
      <param name="publish_null_when_lost" type="bool"   value="false"/>
      <param name="approx_sync"            type="bool"   value="false"/>

      <param name="Odom/ResetCountdown"    type="string" value="1"/>
    </node>

    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
      <param name="frame_id"        type="string" value="$(arg frame_id)"/>
      <param name="approx_sync"              type="bool"   value="false"/>

      <param     if="$(arg sensor_fusion)" name="odom_frame_id"            type="string" value="odom"/>
      <param     if="$(arg sensor_fusion)" name="odom_tf_angular_variance" type="double" value="0.01"/>
      <param     if="$(arg sensor_fusion)" name="odom_tf_linear_variance"  type="double" value="0.01"/>
      <!-- those following remaps are ignored when sensor_fusion is true (because odom_frame_id is set above) -->
      <remap     if="$(arg zed_odom)" from="odom" to="/stereo_camera/odom"/>
      <remap unless="$(arg zed_odom)" from="odom" to="/rtabmap/odom"/>

      <remap from="rgb/image"       to="$(arg rgb_topic)"/>
      <remap from="depth/image"     to="$(arg depth_topic)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>   
    </node>
 
    <node name="rtabmapviz" pkg="rtabmap_ros" type="rtabmapviz" output="screen">
      <param name="frame_id"         type="string" value="$(arg frame_id)"/>
      <param name="subscribe_depth"  type="bool"   value="true"/>
      <param name="approx_sync"      type="bool"   value="false"/>
      <param name="odom_frame_id"    type="string" value="odom"/>
      <remap from="rgb/image"       to="$(arg rgb_topic)"/>
      <remap from="depth/image"     to="$(arg depth_topic)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/> 
    </node>

  </group>

  <!-- Odometry fusion (EKF), refer to demo launch file in robot_localization for more info -->
  <node if="$(arg sensor_fusion)" pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true" output="screen">

      <param name="frequency" value="35"/>
      <param name="sensor_timeout" value="0.1"/>
      <param name="two_d_mode" value="false"/>

      <param name="odom_frame" value="odom"/>
      <param name="base_link_frame" value="$(arg frame_id)"/>
      <param name="world_frame" value="odom"/>

      <param name="transform_time_offset" value="0.0"/>

      <param     if="$(arg zed_odom)" name="odom0" value="/stereo_camera/odom"/>
      <param unless="$(arg zed_odom)" name="odom0" value="/rtabmap/odom"/>
      <param name="imu0" value="$(arg imu_topic)"/> 

      <!-- The order of the values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. -->
      <rosparam param="odom0_config">[true, true, true,
                                      false, false, false,
                                      true, true, true,
                                      false, false, false,
                                      false, false, false]</rosparam>

      <rosparam     if="$(arg imu_ignore_acc)" param="imu0_config">[
                                     false, false, false,
                                     true,  true,  true,
                                     false, false, false,
                                     true,  true,  true,
                                     false,  false,  false] </rosparam>
      <rosparam unless="$(arg imu_ignore_acc)" param="imu0_config">[
                                     false, false, false,
                                     true,  true,  true,
                                     false, false, false,
                                     true,  true,  true,
                                     true,  true,  true] </rosparam>  
      
      <param name="odom0_differential" value="false"/>
      <param name="imu0_differential" value="false"/>

      <param name="odom0_relative" value="true"/>
      <param name="imu0_relative" value="true"/>

      <param name="imu0_remove_gravitational_acceleration" value="$(arg imu_remove_gravitational_acceleration)"/>

      <param name="print_diagnostics" value="true"/>

      <!-- ======== ADVANCED PARAMETERS ======== -->
      <param name="odom0_queue_size" value="5"/>
      <param name="imu0_queue_size" value="50"/> 

      <!-- The values are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz,
           vroll, vpitch, vyaw, ax, ay, az. -->
      <rosparam param="process_noise_covariance">[0.005, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0.005, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0.006, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0.003, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0.003, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0.006, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0.0025, 0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0.0025, 0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0.004, 0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0.001, 0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.001, 0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.002, 0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.001, 0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.001, 0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.0015]</rosparam>

      <!-- The values are ordered as x, y,
           z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. -->
           <rosparam param="initial_estimate_covariance">[1e-9, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    1e-9, 0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    1e-9, 0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    1e-9, 0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    1e-9, 0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-9,  0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-9,  0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1e-9,  0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1e-9, 0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1e-9, 0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]</rosparam>

    </node>

</launch>

Rtabmap VO-only:

$ roslaunch zed_stereo_fusion_bag_play.launch sensor_fusion:=false zed_odom:=false
$ rosbag play --clock subset.bag

Rtabmap VO + IMU fusion:

$ roslaunch zed_stereo_fusion_bag_play.launch sensor_fusion:=true zed_odom:=false
$ rosbag play --clock subset.bag

Zed VO-only:

$ roslaunch zed_stereo_fusion_bag_play.launch sensor_fusion:=false zed_odom:=true
$ rosbag play --clock subset.bag

Zed VO + IMU fusion:

$ roslaunch zed_stereo_fusion_bag_play.launch sensor_fusion:=true zed_odom:=true
$ rosbag play --clock subset.bag

Results are pretty similar, though with zed there is a big jump when the camera is landing at the end.

VIO To enable MSCKF or OKVIS, we should build rtabmap with them. To install them to be compatible with rtabmap, look at this line for MSCKF and this line and those lines for OKVIS (there are some patches to apply on them). Then when make rtabmap, we should see the following in the cmake status:

--   With okvis                = YES
--   With msckf_vio            = YES 

Then to use one of them with rtabmap under stereo_odometry node:

<node pkg="rtabmap_ros" type="stereo_odometry" name="visual_odometry">
   ...
   <param name="Odom/Strategy" value="6"/> <!-- 6=OKVIS, 8=MSCKF_VIO-->
   <remap from="imu" to="/mavros/imu/data"/>
</node>

However, I strongly recommend to use their respective packages under ROS to configure them more easily and to debug them alone. You will not have to rebuild rtabmap that way, only their ros packages. You will only have to feed their output odometry topic (or corresponding TF) from their packages to rtabmap node.

cheers,
Mathieu

[acp]

Hi Mathieu.


Two things:

1-


Thank you very much for your kind and helpul explanation, however I add few lines and  run your launchfile with a new bagfile


Rtabmap VO + IMU fusion + /odometry/filtered
$roslaunch zed_wrapper zed_mathieu_stereo_fusion_bag_play.launch sensor_fusion:=true zed_odom:=false filter:=true
$rosbag play --clock outdoor.bag


And the results are as follows: (when can clearly see that the map is not good)





Launchfile:
zed_mathieu_stereo_fusion_bag_play.launch


Bagfile:
http://www.fit.vutbr.cz/~plascencia/outdoor.bag



Files for the drone urdf
qdrone_c.xacro


qdrone.xacro


qdrone.gazebo


materials.xacro

component_snippets.xacro


qdrone_base.xacro

macros.xacro

zed.urdf

http://www.fit.vutbr.cz/~plascencia/dronebae.dae




2.-
On the other hand,

  a.-) Do you really recommend to use either  MSCKF or OKVIS?

  b.-) If yes, you mean to insttall MSCKF or OKVIS ros packages, right? And which one of both do you recomend? which ros package, since I am using kinetic.  I have installed rtabmap from binaries.



I guess I may need this line, right?
Then to use one of them with rtabmap under stereo_odometry node:

<node pkg="rtabmap_ros" type="stereo_odometry" name="visual_odometry">
   ...
   <param name="Odom/Strategy" value="6"/> <!-- 6=OKVIS, 8=MSCKF_VIO-->
   <remap from="imu" to="/mavros/imu/data"/>
</node>

[matlabbe]

Hi,

1. I played a little with the bag, zed and rtabmap odom seems to work relatively the same. When enabling sensor fusion with IMU, the trajectory looks the same (which is normal as the xyz is taken from the visual odometry), but the rotation of the clouds is very bad sometimes. It means the robot_localization config is not good, the imu is poorly synchronized with the camera and/or TF between imu and camera is wrong. If you want to to VIO, I think using a VIO approach would be better than trying to do fusion with robot_localization.

2. You should not use stereo_odometry if you are using MSCKF or OKVIS ros packages. They would give an output like zed odometry that can be connected to rtabmap node. rtabmap doesn't need to be recompiled when using MSCKF or OKVIS ros packages. Another recent package that could be interesting to try: VINS-Fusion. I don't have enough experience with VIO to tell you which one to use. I've found with the standard EuRoC dataset that MSCKF seems quite faster than OKVIS. For VINS-Fusion I don't know (quite recent), but their results look impressive. As stated on the page of VINS-fusion:

VIO is not only a software algorithm, it heavily relies on hardware quality. For beginners, we recommend you to run VIO with professional equipment, which contains global shutter cameras and hardware synchronization.

For that reason I have never being able to try those VIO approaches live (not a dataset) as I don't have  synchronized IMU + stereo camera hardware. I cannot really help on how to calibrate IMU/camera to make them compatible with those approaches.

cheers,
Mathieu

[acp]

Hi Mathieu

Well based on what you have explained, I would prefer to use sensor fusion. However, you said that the trajectories looks the same.

I do not understand, I have used the rosbag file:  http://www.fit.vutbr.cz/~plascencia/outdoor.bag 

And, I have ploted in a single plot the following topics:

1.  /stereo_camera/odom
2. /stereo_camera/rtabmap/odom
3. /odometry/filtered
4. /mavros/local_position/odom

As you can see in the following two pictures:







As you can see the trajectories do not look the same. however they look similar.


Moreover, I would like to use the sensor fusion, but you have said the the robot_localization config is not good, the imu is poorly synchronized with the camera and/or TF between imu and camera is wrong

I have done  camera and imu calibration using kalibr  https://github.com/ethz-asl/kalibr/wiki/camera-imu-calibration


I have used /mavros/imu/data_raw and not /mavros/imu/data. I hope it is ok :)

The calibration is here: http://official-rtab-map-forum.206.s1.nabble.com/file/n5559/camchain-imucam-homeacpcatkin_wskalibr-cdedynamicsdata.yaml


Do you know how to set the calibration into the sensor fusion launch file????


I have another question.
I want to make a 3D outdoor rtabmap for localization using a real flying  drone. What do you recommend?
 

[acp]

Hi Mathieu.


1.-When enabling sensor fusion with IMU, the trajectory looks the same (which is normal as the xyz is taken from the visual odometry), but the rotation of the clouds is very bad sometimes. It means the robot_localization config is not good, the imu is poorly synchronized with the camera and/or TF between imu and camera is wrong. If you want to to VIO, I think using a VIO approach would be better than trying to do fusion with robot_localization.

      *   I have calibrated the /mavros/imu/data_raw with the  zed camera using mono images.

     *  Do you thing if I insert that calibration matrix into the roslaunch file, the problem would be solved?

     *  Just wondering if the ZED camera is a good choice for the drone and rtabmap?

    *  What do you think about ZEDm, it has an option to integrate the IMU readings.


2.- You should not use stereo_odometry if you are using MSCKF or OKVIS ros packages. They would give an output like zed odometry that can be connected to rtabmap node. rtabmap doesn't need to be recompiled when using MSCKF.

    *  Based on what you told that that VIO systems is not just Software but also hardware, I think this option may not be considered right now.


3.-Another recent package that could be interesting to try: VINS-Fusion.

    * Yea, it looks interesting :) thank you, I may have a look and maybe also give it a try.



4.- How can I solve the following warning?

   *  [ WARN] (2019-02-13 10:47:51.015) OdometryF2M.cpp:469::computeTransform() Registration failed: "Not enough inliers 10/20 (matches=35) between -1 and 231"




 Thank you and I am looking forward to your replay.