Mapping and GPS Orientation Problems

classic Classic list List threaded Threaded
9 messages Options
Reply | Threaded
Open this post in threaded view
|

Mapping and GPS Orientation Problems

jitoobias
Hi,
I am using RTAB-map to create a 2D occupancy map from a 3D LiDAR point cloud (and potentially test the ZED2 camera in the future). I have been able to create a map and but my GPS does not match the orientation of the map. If I try to localize within the map I created my GPS orientation is still not inline.
Mapping, GPS and map not oriented
Note: Robot localization output is red arrows and GPS position is yellow dots. Blue and yellow lines are from RTAB-map /rtabmap/mapGraph. 

- I use the Robot Localization package to fuse my sensor data and provide the map -> Odom and Odom -> base_link TFs.


If I use RTAB-map to provide the Map->Odom TF my robot will follow the map path but the GPS and Robot localization won't.

Note: In my TF tree, it can be seen that I use a TF from datum -> Map. This was done to provide the vehicle an initial position and orientation within the map. Datum is a fixed GPS position of a landmark within the map to provide a global reference for both the vehicle and the map. (Side Note: I have run the mapping without this node applying the datum and the issue is still there.)

If someone can tell me (or provide an example of) the correct way to map using RTAB-map, with a 3D LiDAR, GPS, robot localization, and have everything in the correct orientation that would be amazing, as I have been stuck going around in circles trying to figure this out for weeks.

If you need me to upload the launch files I use to set my RTAB-map parameters or anything else, let me know.
Any help is greatly appreciated.
Reply | Threaded
Open this post in threaded view
|

Re: Mapping and GPS Orientation Problems

matlabbe
Administrator
Hi,

What are ekf_se_odom and ekf_se_map fusing respectively?

Where the yellow dots and red arrows should appear on the map? (It is difficult to see)

If you can provide a rosbag and corresponding launch file, it could be easier to try it on our side.
Reply | Threaded
Open this post in threaded view
|

Re: Mapping and GPS Orientation Problems

jitoobias
Thanks for the reply,

- EKF Odom is fusing IMU (accel, gyro) and Wheel velocities (from encoders)
- EKF map is fusing IMU(accel, gyro), GPS Odom, Wheel velocities (from encoders), and the heading comes from a dual GPS system which is fused as part of the IMU data.

The red arrows and yellow dots should be in the alleyway section of the map. Something to help is when looking at the LiDAR lines and the map you can see a concave shape on the right side of the map, that should hopefully provide a reference for the data. It also should follow the RTAB-Map line (blue) pretty closely.

I am able to provide the files but is there a way I can send it directly to you as my GPS location is used in the bag and launch files and I rather it not be on the forum.

Thanks again for the help.
Reply | Threaded
Open this post in threaded view
|

Re: Mapping and GPS Orientation Problems

matlabbe
Administrator
Hi,

You can click on my user name and click "Send Email to...", a link to a cloud storage service can be provided.

The map published by RTAB-Map is in /map frame. The map frame is published by another node. You would have to match the map frame from rtabmap with the map frame from EKF map. What could be problematic is when a loop closure happens on rtabmap side, the map would be corrected, not sure if the global pose estimated in EKF will match the one rtabmap thinks it is.

Another approach is to use only one EKF node for odometry, then feed /gps/fix topic to rtabmap node. Is GPS published at high rate and always published (like a RTK)? Would you rather use this pose for odomery instead of the IMU+wheel-only odom? Another approach could be to make rtabmap publish map_rtab -> map frame by changing its map_frame_id to "map_rtab", and use odom_frame_id as "map" (the one from EKF map).

cheers,
Mathieu
Reply | Threaded
Open this post in threaded view
|

Re: Mapping and GPS Orientation Problems

jitoobias
This post was updated on .
Hi,
I sent this in an email directly to you last week but don't know if you got
it.

The GPS (GPS/fix) data is coming in at around 90Hz (continuously) but goes
through the Navsat Transform before going to my EKF to provide the
odometry/GPS topic, which is around 15 Hz.
Yes, ideally I would use the GPS fused data for odometry. My understanding
of the Robot Localization package is that the Map and Odom TF both provide
odometry for the vehicle. The map TF is discontinuous and the Odom TF is
continuous.

One thing I have done is subtracted my initial heading from the new heading
data coming through. This starts the GPS going in the correct direction but
still doesn't follow the map. I think that could be due to what you wrote
about the loop closures adjusting the map shape.
I also do have the GPS/topic going into RTAB-map but I don't think it does
anything.

I tried setting RTAB-Maps "map_frame = rtab_map" and "Odom_frame= datum"
and the map was created with the EKF and GPS following the map. The datum
frame got moved but I don't think that matters as that is there to provide
the initial GPS position of the vehicle within the map.

When I tried to localize with the same setup the GPS doesn't follow the map
correctly and the RTAB-map lines (neighbor and loop closure) are not
correctly positioned (I think not in the correct orientation).

If you can provide a cloud storage link, I'll upload the files and
hopefully, you can assist me that way.

Thanks again for all the help
Reply | Threaded
Open this post in threaded view
|

Re: Mapping and GPS Orientation Problems

Masoumeh
This post was updated on .
In reply to this post by jitoobias
Hi jitoobias,

how did you crate the tf between map -> odom, while you are using rtabmap?! did you use map-server???
because if you are using rtabmap, it will be created by that.
I am so confused how to create this tf , while you have rtabmap.  
because rtabmap in localization mode asks for a map in its database, in order to localize the vehicle in the map. now the question is that how did you create this MAP? where has this map been saved? where did you use rtabmap?!

Thanks in advance
Masoumeh
Reply | Threaded
Open this post in threaded view
|

Re: Mapping and GPS Orientation Problems

matlabbe
Administrator
This post was updated on .
In reply to this post by jitoobias
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
Reply | Threaded
Open this post in threaded view
|

Re: Mapping and GPS Orientation Problems

Masoumeh
In reply to this post by jitoobias
Dear jitoobias,

Would you please share the launc file related to instances of robot_localization pakage. I cannot generate the tf between utm and map.

Kind regards
Masoumeh
Reply | Threaded
Open this post in threaded view
|

Re: Mapping and GPS Orientation Problems

jitoobias
There is a parameter in Rtabmap that you can set to false so that it won't publish the TF, called "publish_tf".
For UTM->Map you need to follow the param file "dual_ekf_navsat_example" in the robot localisation package.