3D-map is so bad

[Masoumeh]

hi mattieu,

I did mapping, by using an RGBD camera(provides visual oddometry), 3d lidar, imu, wheel odometer, and rtk gps. but as you can see the result is not good in rtabmapviz:
https://www.youtube.com/watch?v=ONypriZz6oE

what is the white line, in the 3d map?

I would be grateful if you guide me how to make it better.

thanks

[matlabbe]

What is the full tf tree? Did you test every parts separately?

[Masoumeh]

rosgraph_bag.png

frames_bag.png

it is not working, as it is clear in the video, the pointclouds starts drifting by the movement of te vehicle, so the 3d map becomes completely distorted.

i am just using visual odometry.(I have rgbd camera, 3d lidar, imu, wheel odometer, imu)
may i uplaod a bag file, and just you check why 3d map is so bad?
how can i send you a video?



  <arg name="navigation"   default="true"/>
  <arg name="localization" default="false"/>
  <arg name="icp_odometry" default="false"/>
  <arg name="rtabmapviz"   default="true"/>
  <arg name="camera"       default="true"/>
  <arg name="lidar2d"      default="false"/>
  <arg name="lidar3d"      default="true"/>
  <arg name="lidar3d_ray_tracing" default="true"/>
  <arg name="slam2d"       default="true"/>       
  <arg name="depth_from_lidar" default="false"/>

  <arg     if="$(arg lidar3d)" name="cell_size" default="0.2"/>
  <arg unless="$(arg lidar3d)" name="cell_size" default="0.05"/>
 
   
   
   
  <arg if="$(arg lidar2d)" name="lidar_args" default="--Reg/Strategy 0 --RGBD/NeighborLinkRefining true --Grid/CellSize $(arg cell_size) --Icp/PointToPlaneRadius 0"/>
  <arg if="$(arg lidar3d)" name="lidar_args" default="--Reg/Strategy 0 --RGBD/NeighborLinkRefining true --ICP/PM true --Icp/PMOutlierRatio 0.7 --Icp/VoxelSize $(arg cell_size) --Icp/MaxCorrespondenceDistance 1 --Icp/PointToPlaneGroundNormalsUp 0.9 --Icp/Iterations 10 --Icp/Epsilon 0.001 --OdomF2M/ScanSubtractRadius $(arg cell_size) --OdomF2M/ScanMaxSize 15000 --Grid/ClusterRadius 1 --Grid/RangeMax 20 --Grid/RayTracing $(arg lidar3d_ray_tracing) --Grid/CellSize $(arg cell_size) --Icp/PointToPlaneRadius 0"/> 





 
  <remap from="/rtabmap/grid_map" to="/map"/>
  <include file="$(find rtabmap_ros)/launch/mainrtabmap_stereo.launch">
    <arg     if="$(arg localization)" name="args" value="--Reg/Force3DoF $(arg slam2d) $(arg lidar_args)" />
    <arg unless="$(arg localization)" name="args" value="--Reg/Force3DoF $(arg slam2d) $(arg lidar_args) -d" /> 
    <arg name="localization"    value="$(arg localization)" />
    <arg name="visual_odometry" value="true" />
    <arg name="approx_sync"     value="$(eval camera or not icp_odometry)" />
    <arg name="imu_topic"       value="/gx5/imu/data" />
    <arg unless="$(arg icp_odometry)" name="odom_topic" value="/odometry/filtered_map"/>   
    <arg name="frame_id"        value="base_link" />
    <arg name="rtabmapviz"      value="$(arg rtabmapviz)" />
    <arg name="gps_topic"       value="/gps/fix"/>

   
    <arg name="subscribe_scan"  value="$(arg lidar2d)" />
    <arg if="$(arg lidar2d)" name="scan_topic"  value="/scan" />
    <arg unless="$(arg lidar2d)" name="scan_topic"  value="/scan_not_used" />   


   
    <arg name="subscribe_scan_cloud"  value="$(arg lidar3d)" />
    <arg if="$(arg lidar3d)" name="scan_cloud_topic" value="/velodyne_points" />
    <arg unless="$(arg lidar3d)" name="scan_cloud_topic" value="/scan_cloud_not_used" />
   
   
    <arg name="depth"             value="$(eval camera and not depth_from_lidar)" />
    <arg name="subscribe_rgb"     value="$(eval camera)" />
    <arg name="rgbd_sync"         value="$(eval camera and not depth_from_lidar)" />
    <arg name="rgb_topic"         value="/camera/color/image_raw" />       
    <arg name="camera_info_topic" value="/camera/color/camera_info" />             
    <arg name="depth_topic"       value="/camera/depth/image_rect_raw" />    
    <arg name="approx_rgbd_sync"  value="false" />
   
   
    <arg name="gen_depth"                  value="$(arg depth_from_lidar)" />
    <arg name="gen_depth_decimation"       value="4" />
    <arg name="gen_depth_fill_holes_size"  value="3" />
    <arg name="gen_depth_fill_iterations"  value="1" />
    <arg name="gen_depth_fill_holes_error" value="0.3" />
   

   
    <arg if="$(arg icp_odometry)" name="icp_odometry" value="true" />
    <arg if="$(arg icp_odometry)" name="odom_guess_frame_id" value="odom" />
    <arg if="$(arg icp_odometry)" name="vo_frame_id" value="icp_odom" />
    <arg unless="$(arg slam2d)"   name="wait_imu_to_init" value="true" />
    <arg if="$(arg lidar3d)"      name="odom_args" value="--Icp/CorrespondenceRatio 0.01"/>
  </include>


.................................................................................................

     
  <arg name="stereo"                    default="false"/>
  <arg     if="$(arg stereo)" name="depth"  default="false"/>
  <arg unless="$(arg stereo)" name="depth"  default="true"/>
  <arg name="subscribe_rgb"  default="$(arg depth)"/>

 
  <arg name="rtabmapviz"              default="true" /> 
  <arg name="rviz"                    default="true" />
 
 
  <arg name="localization"            default="true"/>
  <arg     if="$(arg localization)" name="args"  default=""/>
  <arg unless="$(arg localization)" name="args"  default="--delete_db_on_start"/>
 
 
  <arg name="use_sim_time"            default="true"/>
 
 
 
  <arg name="cfg"                     default="" /> 
  <arg name="gui_cfg"                 default="~/.ros/rtabmap_gui.ini" />
  <arg name="rviz_cfg"                default="$(find rtabmap_ros)/launch/config/rgbd.rviz" />
 
  <arg name="frame_id"                default="base_link" />     
  <arg name="odom_frame_id"           default="odom" />     
  <arg name="odom_frame_id_init"      default=""/>   
  <arg name="map_frame_id"            default="map" />
  <arg name="ground_truth_frame_id"   default=""/>     
  <arg name="ground_truth_base_frame_id" default=""/> 
  <arg name="namespace"               default="rtabmap"/>
  <arg name="database_path"           default="~/.ros/rtabmap.db"/>
  <arg name="queue_size"              default="10"/>
  <arg name="wait_for_transform"      default="0.2"/>
         
  <arg name="rtabmap_args"            default="$(arg args)"/>   
  <arg name="gdb"                     default="false"/>         
  <arg     if="$(arg gdb)" name="launch_prefix" default="xterm -e gdb -q -ex run --args"/>
  <arg unless="$(arg gdb)" name="launch_prefix" default=""/>
  <arg name="output"                  default="screen"/>       
  <arg name="publish_tf_map"          default="false"/>

[Masoumeh]

soory,

I am usin 3d lidar, as well, can be this problem because of lidar???
but i just use it as input of rtabmap

[matlabbe]

Yes a rosbag would be helpful.

[Masoumeh]

dear mattiue.

i have shared a bag file. through google drive. i think you shuld check your email.

thanks a lot

[Masoumeh]

Sorry, did you receive the bag file?

even when we deactivate the camera in the launch file, and use lidar, and icp odometry, in rtabmapviz this issue still exists.

[matlabbe]

Hi,

thx for the bag. The main issue is coming from the IMU, so the orientation estimation from /odometry/filtered is pretty bad. Here is an example showing velodyne points against odom frame:


I removed the odom TF frame from the rosbag using this script (with odom instead of map) and regenerate the odometry filtered with this robot_localization config (note that I recompute the quaternion from the IMU msg, not sure why it was so wrong or maybe it was not in ENU coordinates):

<!-- robot_localization.launch -->
<launch>
  <node pkg="imu_filter_madgwick" type="imu_filter_node" name="imu_filter_node">
    <param name="use_mag" value="false"/>
    <param name="world_frame" value="enu"/>
    <param name="publish_tf" value="true"/>
    <remap from="imu/data_raw" to="/gx5/imu/data"/>
  </node>

   <!-- 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="20"/>
    <param name="sensor_timeout" value="0.1"/>
    <param name="two_d_mode" value="true"/>

    <param name="odom_frame" value="odom"/>
    <param name="base_link_frame" value="base_link"/>
    <param name="world_frame" value="odom"/>

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

    <param name="odom0" value="/warthog_velocity_controller/odom"/>
    <param name="imu0" value="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, false,
                                    false, false, false,
                                    true, false, false,
                                    false, false, false,
                                    false, false, false]</rosparam>

    <rosparam param="imu0_config">[
                                   false, false, false,
                                   false, false, true,
                                   false, false, false,
                                   false, false, false,
                                   false, false, false] </rosparam>
    
    <param name="odom0_differential" value="true"/>
    <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="true"/>

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

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

    <!-- 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>

Here is the same trajectory as above during rotation, you can see that it is a lot more accurate already even without doing SLAM:


This is why I told you "Did you test every parts separately?" before trying rtabmap, as if you are feeding bad data to rtabmap, you will get a bad map.

For the mapping part, I updated demo_husky.launch to get better ICP with those real data with velodyne. Here is the final trajectory:


Commands used (with /odom frame filtered from lab_27jan_2022-01-27-14-21-13.bag and we remap the /odometry/filtered inside the bag to something not used, as our robot_localization.launch above will republish it on the same name):

roscore
rosparam set use_sim_time true
roslaunch rtabmap_ros demo_husky.launch lidar3d:=true
roslaunch robot_localization.launch
rosbag play --clock --pause lab_27jan_2022-01-27-14-21-13.bag /odometry/filtered:=not_used

cheers,
Mathieu

[Masoumeh]

thanks a lot mattieu for your great support,

Query:
you mentioned about IMU that it seems not working properly. how did you identify that?
I have also sensed the same thing, because when i move the vehicle clockwise, yaw should be negative. but I do not see any change in the sign of yaw when i turn left or right (i am transforming quaternion to rpy through a python code).

in a file, i have read that: "An IMU that is placed in its neutral right-side-up (ENU) position on a flat surface will: Measure **+**9.81 meters per second squared for the Z axis".
but my z axis shows -9.8, that I also asked this from clearpath team, they told that this IMU is based on ENU, because it shows -9.8. it means that z is up and because of gravity it shows -9.8.


Query2:
what does "imu_filter_madgwick" do?

query3:
and why in EKF robot-localization, you used . while you have mapped     <remap from="imu/data_raw" to="/gx5/imu/data"/>  in the imu_filter_madgwick part. should not we use /gx5/imu/data in the fusion part?

thanks in advance

[matlabbe]

Hi,

In ENU, z acceleration should be positive +9.81 when standing still on a flat surface (so that measured acceleration +9.81 + gravity -9.81 = 0). The TF frame of the imu is maybe inverted. Normally with IMU, robot_localization yaw estimation should be quite accurate, and with /gx5/imu/data it was pretty bad (I don't know why as I don't know how its quaternion is computed). You can show the velodyne_points with some history in rviz with Odom fixed frame set in Global options. If odom is accurate, the velodyne_points should accumulate like my second screenshot of my previous post, but was accumulating like the first screenshot.

The madgwick filter takes the angular velocity and linear acceleration data from the input IMU msg and computes the quaternion. In this case it recomputes the quaternion of /gx5/imu/data into output /imu/data fed to ekf in my example. The quaternion from /imu/data makes more sense than the one in /gx5/imu/data, so ekf gives good odometry.

cheers,
Mathieu

[Masoumeh]

sorry mattieu,

now just i have connected to the vehicle(warthog robot), and receive imu data from the vehicle when it is motionless:

---
header:
  seq: 70754
  stamp:
    secs: 1643793793
    nsecs: 917505690
  frame_id: "gx5_link"
orientation:
  x: -0.00222070445307
  y: 0.00700274854898
  z: 0.808141171932
  w: 0.588943004608
orientation_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
angular_velocity:
  x: 0.000555756385438
  y: 0.00148306495976
  z: 0.00340826343745
angular_velocity_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
linear_acceleration:
  x: 0.0506784621673
  y: -0.103927881103
  z: -9.82127460122
linear_acceleration_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]


as you can see, it shows z: -9.8, based on your comments, it should be 0, am i correct???

z in imu frame is up!



Kind Regards

[Masoumeh]

sorry mattieu,

would you please check the bag file for the visual odometry as well? in rviz or rtabmapviz
now icp odometry is works fine for me, but visual odometry not working properly!

query:
approx_sync should be fale or true?
in demo it is  mentioned:  <arg name="approx_sync"     value="$(eval camera or not icp_odometry)" />
but in the rtabmap.launchfile it is metioned:  

  <arg     if="$(arg stereo)" name="approx_sync"  default="false"/>
  <arg unless="$(arg stereo)" name="approx_sync"  default="$(arg depth)"/> 

so finally it should be false or true?

Thanks in advance

[matlabbe]

Standing still on a flat surface, if imu is not upside-down, linear acceleration z would show +9.8.


approx_sync should be true if not using icp/visual odometry and if not using both camera and lidar at the same time.


The demo launch file wasn't not meant to do visual odometry. You have to set true this line and false this line, while using icp_odometry argument=true of the launch file to enabled stuff related to odometry computed on rtabmap side (tested with modified rosbag as above):

roscore
rosparam set use_sim_time true
roslaunch robot_localization.launch
roslaunch rtabmap_ros demo_husky.launch lidar3d:=true camera:=true rtabmapviz:=true icp_odometry:=true
rosbag play --clock --pause lab_27jan_2022-01-27-14-21-13.bag \
   /odometry/filtered:=not_used \
   /camera/color/camera_info:=/realsense/color/camera_info \
   /camera/color/image_raw:=/realsense/color/image_raw \
   /camera/depth/image_rect_raw:=/realsense/depth/image_rect_raw

I don't recommend using the RGB-D point cloud from that camera, it is very noisy, even the sky has [wrong] depth! What is the baseline of that camera? For outdoor in large space like that, a higher stereo baseline would be required, or have better disparity estimation than that. It is however fine for visual loop closure detection.

Here is if I limit the depth of the camera to 5 meters (less noisy but range is small):

[Masoumeh]

thanks a lot mattieu,

just you recommend NOt using the RGB-D point cloud, how can i use just one clor for showing that?

it seems we have two imu sensor, i changed the related topics (now it is /imu/data) in our control.yaml file which generate the /odometry filtered topic.
I have uploaded another bag file and shared with you in google drive. so i would be grateful if you check this one as well.
I want to see the result in rviz.

kind regards

[matlabbe]

Hi,

If you need accurate 3d point cloud reconstruction, I would use velodyne for that. The RGB-D camera is still useful for loop closure detection.

That /odometry/filtered doesn't seem better:

[Masoumeh]

sorry mattieu,

1) for doing navigation based on visual odometry, do i need to --Reg/Strategy into 0?!
Do i need to change this parameter everytime based on the odometery (visual OR icp) that i have activated?

2) if I want to use another feature descriptor, how can i activate that one? i want to compare the result in terms of different feature matching/ descriptors?

thanks

[matlabbe]

Hi,

1) For visual odometry, use rgbd_odometry or stereo_odometry, Reg/Strategy will be set automatically for odometry nodes. You would have to set "--Reg/Strategy" only for rtabmap node, depending how you want to compute loop closure transformations.

2) Use Vis/FeatureType parameter to change visual features used in odometry ndoes. Set both Vis/FeatureType and Kp/DetectorStrategy to same thing for rtabmap node (so that Bag-of-words and visual transformation estimation use the same features).

[Masoumeh]

hi mattieu, when i run the python code to remove ofdom frame from the bag, file, it removes, but make the tf tree completey wrong, it has omitted some of the frames!!!

before running python code:


after running python code:



why? is it correct?