Detect terrain slope with filtered odometry and RTAB-map (IMU, LMS111, Husky, R200)

edit

Hi to all,

I'm trying to perform 3D mapping by using Husky A200, filtered odometry (wheel encoders + IMU), Sick laser LMS111 and Real Sense R200 with RTAB.

My purpose is to be able to detect terrain slopes or curvatures.

I tried to run the algorithm like discussed on my previous topic, by placing a slope between the intersection of two rows of grapes as you can see in this picture (the slope is placed on the red flag).

A more closer view:

As you can see, it is not possible to detect it on the 3D map since the robot move over the slope like if it was a normal flat surface. I was expecting to see the robot climbing over the slope or, at least, changing its position along its Z-axis. For the moment, the terrain looks like a flat surface where I places the slope.

Is it possible to find out how to detect slopes or terrain curvatures by using RTAB and filtered odometry?

EDIT:

This is the content of my localization.yaml file used by robot_localization for the filtered odometry. This is my control.launch from Husky and this is the ekf.launch file included in robot_localization package.

odom_frame: odom
base_link_frame: base_link
world_frame: odom

two_d_mode: false

frequency: 50

odom0: husky_velocity_controller/odom
odom0_config: [false, false, false,
               false, false, false,
               true, true, true,
               false, false, true,
               false, false, false]
odom0_differential: false
odom0_queue_size: 10

imu0: imu/data
imu0_config: [false, false, false,
              true, true, true,
              false, false, false,
              true, true, true,
              true, true, true]
imu0_differential: true
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: true

and this is my control.launch file.

<?xml version="1.0"?>
<launch>



  <rosparam command="load" file="$(find husky_control)/config/control.yaml" />

  <node name="base_controller_spawner" pkg="controller_manager" type="spawner" args="husky_joint_publisher husky_velocity_controller --shutdown-timeout 3"/>

  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization">
    <rosparam command="load" file="$(find husky_control)/config/localization.yaml" />
     <param name="two_d_mode" value="false"/>
  <!--  <param name="odom1" value="/vo"/> 
    <rosparam param="odom1_config">[true, true, true, false, false, true, false, false, false, false, false, false, false, false, false]</rosparam> -->
  </node>

  <node pkg="interactive_marker_twist_server" type="marker_server" name="twist_marker_server" output="screen"/>

  <node pkg="twist_mux" type="twist_mux" name="twist_mux">
    <rosparam command="load" file="$(find husky_control)/config/twist_mux.yaml" />
    <remap from="cmd_vel_out" to="husky_velocity_controller/cmd_vel"/>
  </node>

<node pkg="tf" type="static_transform_publisher"  name="base_to_realsense"
      args="-0.3 0 1.1 1.5 3.14 1.5 /front_bumper_link /realsense_frame 100" />
 <node pkg="tf" type="static_transform_publisher"  name="base_to_laser"
      args="-0.3 0 0.6 0 0 0 /front_bumper_link /laser 100" />
<node pkg="tf" type="static_transform_publisher"  name="base_to_color"
      args="0 0 0 0 0 0 /realsense_frame /camera_color_optical_frame 100" />
<node pkg="tf" type="static_transform_publisher"  name="base_to_depth"
      args="0 0 0 0 0 0 /realsense_frame /camera_depth_optical_frame 100" />
<node pkg="tf" type="static_transform_publisher"  name="base_to_imu"
      args="0 0 0 0 0 0 /imu_link /base_imu 100" />

</launch>

EDIT 2: (04 - 09 - 2016)

I can't see any variation in Z values and this is very strange! Can this be related to my XSENS MTi-10 sensor?

This is the output of rostopic echo /odometry ... (more)