Map is flipped when enable GPS and linear_acc

[tkkim-robot]

Thanks a lot for the great source code. But I have a problem when enable GPS and linear_acc (IMU).

When I use my custom rosbag file to run hdl_graph_slam with enabling GPS and linear_acc, the map is flipped just after few seconds. The trees and the buildings are registered below the ground plane. When only enable GPS, it works well. When enable both sensors, it happens. Watch the map on the top, the entire shape is correct, only the road is flipped.

You can see the video here: https://youtu.be/UVNYSuga61Y

I think it is related to the registration process. Because the fitness_score is lower than the threshold, even it is flipped, so the registration is happen. It is just my pure guess.

How to solve this problem??

Best.

[koide3]

Hi @ktk1501 , What happens if you enable only linear_acc? If you see a flipped map with linear_acc, I guess it is a problem of the definition of the relative pose between LIDAR and IMU. Please confirm if the pose definition on tf is correct.

[tkkim-robot]

@koide3

Thanks for you reply. I found map is also flipped when only enable linear_acc. I think tf is wrong as you said.

I can find "base_link" to "velodyne" static tf in the launch file. But I can't find any tf of IMU. Where can I set the IMU tf??

My rosbag file has topic :

• /velodyne_points
• /gpsimu_driver/imu_data
• /gpsimu_driver/nmea_sentence

The "imu" and "nmea_sentence" topic name was not the name above, but I had modified its name by simple python code.

How can I set IMU tf??

Best.

[koide3]

@ktk1501 ,

You can define tf transformations like the following line: https://github.com/koide3/hdl_graph_slam/blob/1b8085383529ecb1eb5d01380e63e2df94e17a40/launch/hdl_graph_slam_kit.launch#L13

[tkkim-robot]

Thanks for reply.

I had tried to set the static_transform_publisher with imu, but any rotation does not work. Here is the static_tf I used.

<node pkg="tf" type="static_transform_publisher" name="lidar2base_publisher" args="0 0 3 0 0 0 base_link lidar 10" />
  <node pkg="tf" type="static_transform_publisher" name="imu2base_publisher" args="0 0 1 3.14 0 0 base_link imu 10" />

For checking imu linear_acc data, z is always about -9.8. So I thought if tf is problem, I only need to change "yaw". So I tried "yaw" to be set 0, 1.57, 3.14, and everthing doesn't work.

I think IMU data has nothing wrong. Because on the right corner, I can see the base_link turns left, and suddenly optimized to the right because of GPS. The road is flipped, so base_link turns left according to IMU, means IMU has correct data.

If you want to see the full video, I can give you a link tomorrow.

How to solve this problem?

Best.

[koide3]

It could be a bug in the program. I took a look at the code, and maybe in the following line, "Eigen::Vector3d::UnitZ()" should be "- Eigen::Vector3d::UnitZ()"... Could you try to change it and see if it solves the problem?

https://github.com/koide3/hdl_graph_slam/blob/ae0fceb260e6e5e2cb7bffd5ce92f9ce73c70fc7/apps/hdl_graph_slam_nodelet.cpp#L426

[tkkim-robot]

Oh, I was looking at the same code today. I tried your comments and it works well now. I opened a pull request. I also linked the test video. Please have a look and merge it.

Thanks for the great works!!

Best.

[lmunier]

Hello,

first of all, thank you for this great job! I know that this issue is closed, but I had it with the new version and the "- Eigen::Vector3d::UnitZ()". I think that it comes from the IMU measurement and the way it interprets the gravity (as a negativ or positiv value). So it will depend on the IMU.

Best regards, Louis

[koide3]

Hi @lmunier , I agree with you that the definition of the acceleration direction depends on the IMU. I would add a param to switch the interpretation of the acc direction.