Re: Mapping and GPS Orientation Problems

Posted by matlabbe on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Mapping-and-GPS-Orientation-Problems-tp8090p8403.html

Hi,

Below is a copy of the email I sent privately (I removed location stuff) talking about fusing GPS into rtabmap's graph optimization. For GPS fusion with robot_localization, this is something I've never tried myself, so I cannot really help more. What I can say is that with GPS fused in rtabmap's graph, the orientation of the map shoul match the real world coordinates, but origin of the map will start at (0,0), which would correspond to the first GPS value received.

<-- email copy -->
I've found a problem in the /gps/fix topic, the altitude seems to be the bearing. This is causing some wrong altitude errors (from 0 to 359 meters!) and trajectory alignment in DatabaseViewer doesn't like that. I attached a python node to republish gps data with altitude 0. I couldn't modify my catkin workspace with all your packages, so I tested the bag like below (the zed topics were also missing, so I did a 3D Lidar-only test) .

Based on this post, but using wheel odometry:
--------------------------------

$ roslaunch rtabmap_ros rtabmap.launch    \
   use_sim_time:=true \
   depth:=false \
   subscribe_scan_cloud:=true \
   frame_id:=base_link \
   scan_cloud_topic:=/velodyne_points \
   scan_cloud_max_points:=15000  \
   visual_odometry:=false \
   icp_odometry:=false \
   approx_sync:=true \
   odom_frame_id:=odom \
   odom_tf_angular_variance:=0.0005 \
   odom_tf_linear_variance:=0.001 \
   gps_topic:=/gps/fix2 \
   args:="-d  \
      --RGBD/CreateOccupancyGrid false \
      --Rtabmap/DetectionRate 2 \
      --Odom/ScanKeyFrameThr 0.8 \
      --OdomF2M/ScanMaxSize 10000  \
      --OdomF2M/ScanSubtractRadius 0.5   \
      --Icp/PM true \
      --Icp/VoxelSize 0.5   \
      --Icp/MaxTranslation 2   \
      --Icp/MaxCorrespondenceDistance 1.5 \
      --Icp/PMOutlierRatio 0.7 \
      --Icp/Iterations 10 \
      --Icp/PointToPlane false \
      --Icp/PMMatcherKnn 3 \
      --Icp/PMMatcherEpsilon 1 \
      --Icp/Epsilon 0.0001 \
      --Icp/PointToPlaneK 0 \
      --Icp/PointToPlaneRadius 0 \
      --Icp/CorrespondenceRatio 0.01 \
      --Optimizer/PriorsIgnored false"

$ python repub_gps.py

$ rosbag play --clock --pause 2021-05-25-12-10-14.bag

-----------------------------
Following this post, I exported both gps (green) and poses (blue) in KML format:
[screenshot omitted, may contain sensitive data]

If you don't want to use GPS priors in graph optimization, set "--Optimizer/PriorsIgnored true" above (the screenshot above was with priors ignored to show difference between gps and slam poses). In Graph View, you can right-click and enable ENU orientation to see in the same orientation than on google map.

The other trajectory with gps used in graph optimization (priors not ignored):


python code to fix gps:

#!/usr/bin/env python
import rospy
from sensor_msgs.msg import NavSatFix

def callback(data):
    global pub
    data.altitude = 0
    pub.publish(data)
    
def listener():
    global pub
    rospy.init_node('repub', anonymous=True)
    pub = rospy.Publisher('/gps/fix2', NavSatFix, queue_size=10)
    rospy.Subscriber("/gps/fix", NavSatFix, callback)
    rospy.spin()

if __name__ == '__main__':
    listener()

For reference, the topics in the bag:

rosbag info 2021-05-25-11-22-04_map.bag 
path:        2021-05-25-11-22-04_map.bag
version:     2.0
duration:    2:38s (158s)
start:       May 24 2021 19:22:04.88 (1621898524.88)
end:         May 24 2021 19:24:43.01 (1621898683.01)
size:        643.9 MB
messages:    65694
compression: none [785/785 chunks]
types:       nav_msgs/Odometry       [cd5e73d190d741a2f92e81eda573aca7]
             sensor_msgs/Imu         [6a62c6daae103f4ff57a132d6f95cec2]
             sensor_msgs/LaserScan   [90c7ef2dc6895d81024acba2ac42f369]
             sensor_msgs/NavSatFix   [2d3a8cd499b9b4a0249fb98fd05cfa48]
             sensor_msgs/PointCloud2 [1158d486dd51d683ce2f1be655c3c181]
             tf2_msgs/TFMessage      [94810edda583a504dfda3829e70d7eec]
topics:      /gps/fix           12385 msgs    : sensor_msgs/NavSatFix  
             /imu/sensor_data   12354 msgs    : sensor_msgs/Imu        
             /odometry/wheel     2962 msgs    : nav_msgs/Odometry      
             /scan               3136 msgs    : sensor_msgs/LaserScan  
             /tf                26882 msgs    : tf2_msgs/TFMessage      (3 connections)
             /tf_static             2 msgs    : tf2_msgs/TFMessage      (2 connections)
             /ts_cloud           4837 msgs    : sensor_msgs/PointCloud2
             /velodyne_points    3136 msgs    : sensor_msgs/PointCloud2