3d-pose is tilting although the copter is not moving at all

[wolfgangschwab]

I'm using cartographer to get the 3D-localization for a copter. As the copter shall start after some waiting time, it is standing still on the ground for some minutes. Unfortunately the pose that cartographer is calculating is tilting after some time. For me it seems that the global slam is coming to the conclusion that the imu is incorrect and calculates an imu-correction. But I cannot find an error in the imu-data.

rviz at the beginning: /home/wolfgang/Pictures/copter_without_movement_start.png After about 14 seconds it tilts:

Why is cartographer expecting that an imu-correction is needed? How can I avoid this tilting? Any quick help is very appreciated.

Data to reproduce:

• rosbag_validate: rosbag validate
• Lua-file is on github: lua
• launch-command: roslaunch cartographer_ros cartographer.launch
• used git-Repository: cartographer_ros
• The rosbag is available here: rosbag

[wolfgangschwab]

Any hints or suggestions are wellcome. :-)

[eithwa]

I used the default configure and it looks normal. Maybe you can try it?

MAP_BUILDER.use_trajectory_builder_3d = true
MAP_BUILDER.num_background_threads = 7

TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 1

POSE_GRAPH.constraint_builder.log_matches = true
POSE_GRAPH.optimization_problem.log_solver_summary = true

POSE_GRAPH.optimization_problem.huber_scale = 5e2
POSE_GRAPH.optimize_every_n_nodes = 320
POSE_GRAPH.constraint_builder.sampling_ratio = 0.03

POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 10

I think the main problem lies in this parameter I'm not sure if the map will still tilt after a longer time

POSE_GRAPH.optimize_every_n_nodes = 320

[wolfgangschwab]

@eithwa , thank you very much for your feedback. I rerun the simulation with the parameters you provided. Yes, the result is, that the tilting does not happen during the time of the rosbag. Letting the simulationt run for a longer time a small tilting occured at about 175 seconds of simulation time and another one at about second 352. It seems to have to do with the global slam that is done later as the parameter POSE_GRAPH.optimize_every_n_nodes is now higher. As the tilting is very small this looks much better than before.

Having a deeper look into the logs:

1. first try with small values (as above):

   Termination:                      CONVERGENCE (Function tolerance reached. |cost_change|/cost: 9.903359e-07 <= 1.000000e-06)
   [ INFO] [1630350811.882581783]: I0830 21:13:31.000000 15168 optimization_problem_3d.cc:578] Gravity was: 9.75875
   [ INFO] [1630350811.882597281]: I0830 21:13:31.000000 15168 optimization_problem_3d.cc:580] IMU correction was: 137.151 deg (0.365276, -0.212097, -0.445897, -0.789154)

   An extrem IMU correction was done. Why does this happen?

2. next try with the latest parameters: At 175s

   Termination:                   NO_CONVERGENCE (Maximum number of iterations reached. Number of iterations: 10.)
   [ INFO] [1630352607.769159054, 175.904000000]: I0830 21:43:27.000000 16786 optimization_problem_3d.cc:578] Gravity was: 9.76668
   [ INFO] [1630352607.769175816, 175.904000000]: I0830 21:43:27.000000 16786 optimization_problem_3d.cc:580] IMU correction was: 2.8768 deg (0.999685, -0.0249821, 0.00152267, -0.00192245)

   At 351s

   Termination:                   NO_CONVERGENCE (Maximum number of iterations reached. Number of iterations: 10.)
   [ INFO] [1630353657.268683891, 351.756000000]: I0830 22:00:57.000000 16786 optimization_problem_3d.cc:578] Gravity was: 9.77712
   [ INFO] [1630353657.268700291, 351.756000000]: I0830 22:00:57.000000 16786 optimization_problem_3d.cc:580] IMU correction was: 1.83688 deg (0.999872, -0.0126566, -0.0024808, -0.00951781)

   Only a small IMU correction is done here. But why is an IMU correction done at all? Is this needed? But now we get NO_CONVERGENCE. This really frightens me.

And additionally I would like to have shorter time distances between the global SLAM corrections as we also have added some code to also use the results of a barometer and a magnetic compass. These important values are use for corrections in the global SLAM calculations.

What does the NO_CONVERGENCE means? How can I avoid this? How can the correction of the IMU been minimized without setting the optimize_very_nodes parameter to very high values?

[eithwa]

I haven't seen anyone use cartographer on a copter, so I am not sure if it can work well when moving in a wide range of altitude. Usually using POSE_GRAPH.optimize_every_n_nodes quickly will not get good results, because it cannot get enough information to optimize the map. Maybe you can try to increase the weight of imu in trajectory_builder_3d.lua.

TRAJECTORY_BUILDER_3D.imu_based.imu_acceleration_weight
TRAJECTORY_BUILDER_3D.imu_based.imu_rotation_weight

In some cases, the last version of cartographer has strange tf corrections. Maybe you can also find out whether the old version can solve this problem. I think the problem may come from the discontinuous topic delivery. https://github.com/cartographer-project/cartographer_ros/issues/1432 https://github.com/cartographer-project/cartographer_ros/issues/1420

[wolfgangschwab]

So, I understand that I should avoid low values of POSE_GRAPH.optimize_every_n_nodes. Thanks for that explanation. I will also try with the older versions.

If Cartographer is not ideal for copters, which tool should I use for copters?

Do I have to worry about the NO_CONVERGENCE, which occurs when I use higher values of POSE_GRAPH.optimize_every_n_nodes?

[eithwa]

https://github.com/cartographer-project/cartographer_ros/issues/1201 I found a problem similar to yours, maybe it will help you

[wolfgangschwab]

Yes, indeed. The other issue sounds very similar. Thanks for the link.

So you have any idea how problematic a NO_CONVERGENCE is?

[eithwa]

Yes, indeed. The other issue sounds very similar. Thanks for the link.

So you have any idea how problematic a NO_CONVERGENCE is?

I don’t know the reason for NO_CONVERGENCE, sorry

[wolfgangschwab]

Yesterday there was a meeting with teams that participated in the Subterranean Challenge. Many used Cartographer and as I understood, they all found the problem described here. So for me it seems this is a reasonable problem. Leaving a robot for some time at the start position and start moving it some time later is a big problem in Cartographer.

Does anybody have an idea how this could be fixed in cartographer?

[eithwa]

POSE_GRAPH.optimization_problem.use_online_imu_extrinsics_in_3d = false

[JiaoxianDu]

I convert my pointcloud2 topic to laserscan and run 2d mapping, got the same problem. At the beginning it seems work fine, but after a while all scan points rotate 90 degrees, and my car model just "insert" into the groud vertically in rviz, and I rotate the whole scene for 90 degrees and find that the space relationship between car model and pointcloud is right, so It just regards the X or Y axis as the Z axis for some reason ... Though it works truly fine while running official museum demo.

[JiaoxianDu]

@eithwa But your answers under https://github.com/cartographer-project/cartographer_ros/issues/1587 solve my problem, so It's due to imu simulation bug of gazebo. Thanks gratefully.

[JiaoxianDu]

I convert my pointcloud2 topic to laserscan and run 2d mapping, got the same problem. At the beginning it seems work fine, but after a while all scan points rotate 90 degrees, and my car model just "insert" into the groud vertically in rviz, and I rotate the whole scene for 90 degrees and find that the space relationship between car model and pointcloud is right, so It just regards the X or Y axis as the Z axis for some reason ... Though it works truly fine while running official museum demo.

In 2d mapping (gazebo simulation), to set use_imu_data as false is the solution. But in 3d mapping, still have the problem. All pointclouds are around Z axis, but program is trying to track the other 2 axis.

[eithwa]

There are some problems with imu's simulation on gazebo, so I don't think you can use cartographer to simulate 3d slam on gazebo.

[JiaoxianDu]

我將我的 pointcloud2 主題轉換為 Laserscan 並運行 2D 映射，遇到了相同的問題。一開始看起來工作正常，但過了一會兒所有掃描點都旋轉了 90 度，而我的汽車模型只是在 rviz 中垂直「插入」地面，我將整個場景旋轉 90 度，發現之間的空間關係汽車模型和點雲是正確的，所以出於某種原因它只是將X 或Y 軸視為Z 軸......儘管在運行官方博物館演示時它工作得很好。

在二維映射（涼亭模擬）中，設定 use_imu_data為false是解決方案。但在3D映射中，仍存在問題。所有點雲都圍繞著 Z 軸，但程式正在嘗試追蹤其他 2 個軸。

There are some problems with imu's simulation on gazebo, so I don't think you can use cartographer to simulate 3d slam on gazebo.

I came across a tutorial on performing LiDAR simulation and mapping in Gazebo, which uses the libgazebo_ros_imu_sensor.so plugin instead of libgazebo_ros_imu.so. In this case, the tracking_frame can also be set to the IMU coordinate system --- previously it was base_footprint, and mapping works correctly. I believe that libgazebo_ros_imu.so may have some issues with the simulation of the direction of gravity, while libgazebo_ros_imu_sensor.so does not have this problem. Here are some codes for those have the same problem, need to be added to the robot urdf file.

  <!--imu -->
  <link name="imu_base_link">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <box size="0.03 0.03 0.03" />
      </geometry>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <box size="0.03 0.03 0.03" />
      </geometry>
    </collision>
    <inertial>
      <mass value="0.001" />
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
    </inertial>
  </link>

  <gazebo reference="imu_base_link">
    <material>Gazebo/FlatBlack</material>
    <turnGravityOff>true</turnGravityOff>
    <visualize>$(arg imu_visual)</visualize>
  </gazebo>

  <joint name="imu_platform_joint" type="fixed">
    <parent link="base_link" />
    <child link="imu_base_link" />
    <origin xyz="0.05 0 0.065" rpy="0 0 0" />
    <axis xyz="0 0 1" />
  </joint>

  <gazebo reference="imu_base_link">
    <gravity>true</gravity>
    <sensor name="imu_sensor" type="imu">
      <always_on>true</always_on>
      <update_rate>200</update_rate>
      <visualize>$(arg imu_visual)</visualize>
      <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
        <topicName>/imu</topicName>
        <bodyName>imu_base_link</bodyName>
        <updateRateHZ>200.0</updateRateHZ>
        <gaussianNoise>0.00329</gaussianNoise>
        <xyzOffset>0 0 0</xyzOffset>
        <rpyOffset>0 0 0</rpyOffset>
        <frameName>imu_base_link</frameName>
      </plugin>
      <pose>0 0 0 0 0 0</pose>
    </sensor>
  </gazebo>