Continous shift in z-direction

[maxehrhardt]

Hey,

I run your code with a VLP16 connected and leave it in a stationary place. With your default launch file I get a constant movement of the base_link and the keyframe in positive z-direction though. It looks like the keyframe is always a bit ahead of the base_link.

Already great thanks for help.

[koide3]

In your setting, is the LIDAR be able to see the ground or ceil? Without observing them such a drift may happen. If you can put a screenshot here, it would be helpful to find the cause.

[maxehrhardt]

Hey,

I included one image of all points recorded and one of only the filtered points. I set the distance threshold to only see ground. After around 20s there is already a quite big drift.

[lmunier]

Hey,

I have the same problem as you @maxehrhardt . If I let the robot static it will tilt the generated map. I have the same velodyne as you (VLP16) and as soon as I move the robot, he will move on the "new" ground reference.

As you can see on the attached image, all the generated point cloud from the velodyne is tilting in few seconds. Did you find any solution?

Thank you for your help and best regards, Louis

[lmunier]

As you can see, after a longer period without moving the robot, the map has a big tilt angle.

[koide3]

Hi, Can you try to increase "keyframe_delta_time" in your launch file to a large number (e.g., 10000)? This parameter prevents the scan matching from stucking, but I suppose it may cause such a drift if you run the system for a long time.

[lmunier]

Hi,

Thank you ! It solves the problem.

As far as I understand, this parameter will just influence when the people/robot with the sensor does not move and an object/people is moving around. We will not see the moving object by putting this value too high, isn't it ?

Bets regards, Louis

[maxehrhardt]

Hi, setting this parameter up also solved the issue with the drift for me.

Thanks

[nyangshawbin]

Hi @koide3 ,

I have similarly experienced a drift regarding the keyframe, and it gets worse as time progresses. I have tried troubleshooting by increasing the "keyframe_delta_time" to 20000 as recommended but it didnt help.

The corridor seemed to be mapped twice from both the robot and the diverged keyframe. Would like to find out what are some ways to troubleshoot this. Similarly, isit possible to feed our own odom into hdl_graph_slam?

Thanks in advance!

[koide3]

Hi @nyangshawbin , It might be due to some errors in NDT. It's worth trying to use GICP instead of NDT_OMP because I recently found that GICP in PCL 1.8 (or later) is much accurate than NDT. Note that if you are on Ubuntu 16, the system default PCL has a buggy GICP.

Regarding odometry, yes, you can feed your own odom into this package. As illustrated in the following figure, the odometry estimation and mapping nodelets are separated. You can remove scan_matching_odometry_nodelet and publish your own odom data to /odom topic

https://raw.githubusercontent.com/koide3/hdl_graph_slam/master/imgs/nodelets.png

[nyangshawbin]

Hi @koide3 , Yup, I am using GICP (on Ubuntu 18) as I found out it gives a better map than NDT_OMP. I am using this package offline on a rosbag with a Velodyne HDL-32E set up. So the deviation of keyframe away from the main robot model is purely a scan matching issue? Apart from changing the scan registration method are they any other parameters which I can tune to mitigate this deviation.

Thanks for your help regarding the odometry, I will try that out!

[tim-fan]

Hello, I just saw the comment about using odometry, and thought I'd add; I tried using my own odometry as well, and found I had to make some changes in HdlGraphSlamNodelet to make it work. Specifically the nodelet uses an exact time synchronizer to match odom with point cloud messages, however in my case none of my odom messages had exact timestamp matches with my scans, so the cloud callback was never triggered. I worked around this by switching to an approximate time synchronizer. Here's the diff:

diff --git a/apps/hdl_graph_slam_nodelet.cpp b/apps/hdl_graph_slam_nodelet.cpp
index 453d617..e801fb1 100644
--- a/apps/hdl_graph_slam_nodelet.cpp
+++ b/apps/hdl_graph_slam_nodelet.cpp
@@ -17,6 +17,7 @@
 #include <pcl_ros/point_cloud.h>
 #include <message_filters/subscriber.h>
 #include <message_filters/time_synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
 #include <tf_conversions/tf_eigen.h>
 #include <tf/transform_listener.h>

@@ -61,6 +62,7 @@ namespace hdl_graph_slam {
 class HdlGraphSlamNodelet : public nodelet::Nodelet {
 public:
   typedef pcl::PointXYZI PointT;
+  typedef message_filters::sync_policies::ApproximateTime<nav_msgs::Odometry, sensor_msgs::PointCloud2> ApproxSyncPolicy;

   HdlGraphSlamNodelet() {}
   virtual ~HdlGraphSlamNodelet() {}
@@ -105,7 +107,7 @@ public:
     // subscribers
     odom_sub.reset(new message_filters::Subscriber<nav_msgs::Odometry>(mt_nh, "/odom", 256));
     cloud_sub.reset(new message_filters::Subscriber<sensor_msgs::PointCloud2>(mt_nh, "/filtered_points", 32));
-    sync.reset(new message_filters::TimeSynchronizer<nav_msgs::Odometry, sensor_msgs::PointCloud2>(*odom_sub, *cloud_sub, 32));
+    sync.reset(new message_filters::Synchronizer<ApproxSyncPolicy>(ApproxSyncPolicy(32), *odom_sub, *cloud_sub));
     sync->registerCallback(boost::bind(&HdlGraphSlamNodelet::cloud_callback, this, _1, _2));
     imu_sub = nh.subscribe("/gpsimu_driver/imu_data", 1024, &HdlGraphSlamNodelet::imu_callback, this);
     floor_sub = nh.subscribe("/floor_detection/floor_coeffs", 1024, &HdlGraphSlamNodelet::floor_coeffs_callback, this);
@@ -138,7 +140,7 @@ private:
    * @param cloud_msg
    */
   void cloud_callback(const nav_msgs::OdometryConstPtr& odom_msg, const sensor_msgs::PointCloud2::ConstPtr& cloud_msg) {
-    const ros::Time& stamp = odom_msg->header.stamp;
+    const ros::Time& stamp = cloud_msg->header.stamp;
     Eigen::Isometry3d odom = odom2isometry(odom_msg);

     pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
@@ -909,7 +911,7 @@ private:

   std::unique_ptr<message_filters::Subscriber<nav_msgs::Odometry>> odom_sub;
   std::unique_ptr<message_filters::Subscriber<sensor_msgs::PointCloud2>> cloud_sub;
-  std::unique_ptr<message_filters::TimeSynchronizer<nav_msgs::Odometry, sensor_msgs::PointCloud2>> sync;
+  std::unique_ptr<message_filters::Synchronizer<ApproxSyncPolicy>> sync;

   ros::Subscriber gps_sub;
   ros::Subscriber nmea_sub;

[nyangshawbin]

Hello @tim-fan , thanks for the advice, however, I have some enquiries. Your modification is done to solve the issue of unsynchronized timestamp between your odometry and point cloud messages? Meaning is if a setup is synchronized in terms of timestamp, I can simply replace the scan_matching_odometry_nodelet with my published odom as mentioned by @koide3 .

If so, I would like to get some guidance on how can i 'remove' the scan_matching_odometry_nodelet so that i can provide my own /odom from my rosbag.

[tim-fan]

Hi, yes that's true - I needed to make those modifications because my odometry messages did not have the same timestamps as my scan messages. If your odometry comes from scan matching then this probably wont be an issue for you.

Regarding how to remove the scan matching node, I was using a copy of hdl_graph_slam.launch, and simply commented out the scan_matching_odometry_nodelet from that file. Then I made sure my odometry was being published to the /odom topic.

[nyangshawbin]

Hi, i was running hdl_graph_slam on a rosbag, which already has its own /odom topic.

I am a little confused, does this mean that if i were to run the default HDL slam configuration, the odom (which will be fed into the hdl_graph_slam_nodelet) will come from the scan_matching_odometry_nodelet and not the /odom I already in my rosbag?

[tim-fan]

hdl_graph_slam_nodelet subscribes to /odom, and will use any messages published there.

If you run the default HDL graph slam config, then scan_matching_odometry_nodelet will publish to /odom. If you are also publishing to /odom from a bag, then you will have two sources of odometry publishing to /odom, which is not what you want.

To avoid this, you can remove scan_matching_odometry_nodelet from your launch file. Now your bag will be the only source of odometry, and everything should work.

[nyangshawbin]

Hi, i have commented out the scan_matching_odometry_nodelet so that the hdl_graph_slam_nodelet is able to use the /odom topic directly from my rosbag.

Im able to see the point clouds in Rviz, but mapping doesnt seems to be done. Below is a tf tree where the 'keyframe' frame is missing.

[tim-fan]

Sounds like you're making progress :) It might help if you post the output of rqt_graph as well.

My first guess as to why it wouldn't be working now would go back to my original comment - if the timestamps of the odometry messages do not exactly match the timestamps of the scans, then no scans will be processed (due to exact-timestamp synchronisation in hdl_graph_slam_nodelet. In this case the patch I provided would help.

Can you describe how you generated your odometry? Is it based off the robot's wheel encoders, or from an external scan matching algorithm, or something else?

[nyangshawbin]

Hi, this is the rqt after i removed the scan_matching_odometry_nodelet. Dont mind the /odom_noise for now, although i plan to remap /odom into /odom_noise later to better simulated noise.

Currently, all my topics are recorded from gazebo simulation, hence I assumed the timestamp would be synchronized by default. Nevertheless, i will give your patch a try to see if it works:)

------------------------Update-------------------------- It worked! With your approximate time sync! I guess even though topics were recorded at the same time in a simulation, the timestamp still doesnt match exactly. Thanks alot!!

[tim-fan]

Good to hear 😃 Note now that #128 is merged this should work on the master branch, without needing to apply the diff I posted. Did this resolve the original issue you posted (the drift in z)?

[Johntec]

Hi all, @koide3 regarding using the odom message instead of the scan matching, when using UTM odom from GPS messages, and changing them with the odom messages from scan matching nodelet, there will be a problem or showing them in rviz. The problem is that the UTM positions are too large and cant be seen in the rviz, they are too far from the origin, any idea how to solve this issue?

[nyangshawbin]

Hi @koide3 ,

I have similarly experienced a drift regarding the keyframe, and it gets worse as time progresses. I have tried troubleshooting by increasing the "keyframe_delta_time" to 20000 as recommended but it didnt help.

The corridor seemed to be mapped twice from both the robot and the diverged keyframe. Would like to find out what are some ways to troubleshoot this. Similarly, isit possible to feed our own odom into hdl_graph_slam?

Thanks in advance!

@tim-fan This drift was probably due to my poor tuning. Its solved now by using back the default parameters. Thanks!

[koide3]

@Johntec , UTM coords can be very huge and visualization apps often fail to directly handle them. To avoid this problem, I shift UTM coords so that the very first data comes to the origin. I think you need to do this kind of trick in your program to display your data on rviz. https://github.com/koide3/hdl_graph_slam/blob/0228e86b7f4f9649515b0217440694bc1fdf7f20/apps/hdl_graph_slam_nodelet.cpp#L323

[Johntec]

Thank you @koide3 for the reply. It is confusing, I am also using the gps/navsat which uses the same utm position with odom, i have set the use gps parameter to true, so technically, the first gps should be the origion of the map which is also the utm position of odom, but it doesnt work. I tried by publishing my odom(utm) messages, and disabled scan matching and also tried by using the scan matching nodelet and only commented the https://github.com/koide3/hdl_graph_slam/blob/0228e86b7f4f9649515b0217440694bc1fdf7f20/apps/scan_matching_odometry_nodelet.cpp#L114 that also didnt work. any idea?