How can I tune the parameter to modify my own imu data?

[DriftingTimmy]

I used cartographer to build my map using 3d lidar data and imu data. Because of some reasons, my trajectory keep climbing while the programme run, but the real environment has little difference in Z direction. And I think my imu is not that reliable, how can I change the parameter to make the trajectory on the same level plane?

Here is the constraint_list shown in my rviz:

And here is my configuration file: configuration.zip

One way I think may work is tuning the rotation_weight in ceres_scan_matcher, but I am not sure.

[wohe]

From the Cartographer ROS issue template:

It would help us tremendously if you provide the following so that we can easily reproduce your issue:

1. A link to a Git repository containing a branch of cartographer_ros containing all the configuration, launch, and URDF files required to reproduce your issue.
2. A link to a bag file we can use to reproduce your issue.

That said, if the trajectory keeps climbing, the gravity estimation is probably not working well.

1. Most likely cause is bad calibration of the relative orientation between IMU and LIDAR in the URDF. #361 is trying to help in this situation. Assuming the URDF is good enough to allow for loop closures to be detected, you can move along some path and the trajectory will climb due to the error in calibration. Now you turn by 180 degrees and move forward again back to where you started. Assuming loop closures are detected, the optimization problem can now compute a correction of the IMU orientation.

2. You can of course also try to tune the Ceres scan matcher and imu_gravity_time_constant, but unless your submaps look distorted, I think this is less likely to be an issue. You might want to have a look at acceleration_weight and rotation_weight in the optimization_problem which are the weights given to IMU data in the pose graph optimization.

[DriftingTimmy]

Thanks for your reply. You remind me of my configuration of my urdf, actually I can not get the real orientation of my imu and my lidar, so I just use the estimation of my own. Now I m trying to tune the parameter to make the map run well. If I have further problem, I will upload my bagfile, because the bagfile is too big... Thanks again.

[DriftingTimmy]

And here is one more question for the task I'm working on : Can cartographer work well in an outdoor and complex environment especially using 3D Lidar as the main sensor? Please let me know if there exists a way to modify the algorithm to make it work in a more complicate environment. I tried some outdoor data I got and I can hardly get a satisfied result. Thanks very much~ @wohe @SirVer

[damonkohler]

Cartographer is known to work very well outdoors. Complex environments are much more favorable than simple repetitive ones. I suspect your performance issues are related to sensor configuration (e.g. URDF) and tuning.

If you need assistance tuning, please follow the instructions in the issue template (mentioned by @wohe above) and we will try to help you as soon as time allows.

[DriftingTimmy]

I m glad to hear u say so! And I m not sure if I can upload my bagfile because it is too big (5G even after compressed), the bag lasts for about 20 minutes and the first half looks ok, but after a cross the trajectory begin to drift and the programme seems to stop and lasts for hours without optimization.

Here is my configuration, and I just run the demo_test.launch file. cartographer_ros.zip Because my bagfile can provide /tf and /tf_static, so I delete the ros_state_publisher node. And here is the problem, I just show the constraints lists because the trajectory can be hardly seen.

By the way, do u know how to upload a huge bagfile ?

Thanks very much! @damonkohler

[damonkohler]

You can host your bag file anyway you like (e.g. Google Drive or Dropbox). Sharing your GitHub fork with the configurations and launch files checked also makes it much easier for us to investigate your issue.

[DriftingTimmy]

I finally upload my bagfile through google drive! Thanks very much! But there are some errors in my github, so I first give the link of my bagfile : https://drive.google.com/open?id=0B-HHtJ5SoiGgX1lmT2Q3bHlfYm8

And my configuration file has uploaded as .zip above, maybe I need a little time to fix my rep. Thanks very much.

[DriftingTimmy]

Sorry for loss of command! I just run the 'demo_test.launch' file for my project! And other files are my test file!

[damonkohler]

Looking at your bag, I noticed two things:

1. It crashed because your IMU is not aligned with your tracking frame. Did you disable that check? If so, that's at least part of your problem. Change the tracking frame to be your IMU. You can continue publishing map <- base_link if you like. That's a separate concern.
2. Once I fixed that issue, it appears that your IMU to LIDAR alignment is off. The car (I guess it's a car?) is always climbing a bit. You should improve quality by calibrating the alignment between the IMU and the LIDAR.

[DriftingTimmy]

Yes, because when I ran the bagfile, cartographer warned that the distance between the imu and Lidar will make the result imprecise. So I just change the threshold to make the programme run.

if (sensor_to_tracking != nullptr) { CHECK(sensor_to_tracking->translation().norm() < 1) << "The IMU frame must be colocated with the tracking frame. " "Transforming linear acceleration into the tracking frame will " "otherwise be imprecise."; trajectorybuilder->AddImuData( sensor_id, time, sensor_to_tracking->rotation() ToEigen(msg->linear_acceleration), sensor_to_tracking->rotation() ToEigen(msg->angular_velocity)); }

I changed the threshold to 1 in the sensor_bridge.cc . Sorry for forgetting to mention that issue.

And once I finished my bagfile, I wait for the final optimization for several hours but I can not get the result. Do you know why? And can you show your result of my data? Thanks a lot~

One question is that the IMU and Lidar need to be so closed? I check the threshold to be 1e-5, which I think is not easy to gurantee.

Thanks for your suggestion and I will try to calib my IMU and LIDAR to get a better result. Further question may be asked because I m trying to use and learn this amazing SLAM algorithm!

[ludennis]

Hi all,

I am trying to calibrate the IMU, and am able to have cartographer output the "IMU correction" message to give me the quaternion value. However, I am having trouble figuring out how to use this number in my urdf to correct my IMU. Specifically, I am not even sure if the quaternion pertains to the tf from IMU to lidar or from lidar to IMU, or IMU to base_link. Perhaps more description on what the quaternion value means in respect to IMU would do.

Thanks in advance,

Dennis

[ojura]

Have a look at this line to see how IMU data is transformed from the IMU to the tracking frame: https://github.com/googlecartographer/cartographer_ros/blob/c0e713c404d3087cec3dbe07e35bd162ce802dc0/cartographer_ros/cartographer_ros/sensor_bridge.cc#L130

To apply the IMU orientation correction quaternion, you would have an additional multiplication from the left (i.e. correction_quaternion * sensor_to_tracking->rotation() * imu data).

Therefore, you can check that you edited your URDF correctly if sensor_to_tracking_edited_urdf->rotation() == correction_quaternion * sensor_to_tracking_original_urdf->rotation().

[ludennis]

@ojura Thanks! I got the IMU to be calibrated and checked.

So, now the problem is that Cartographer continues to correct my corrected IMU. (See this video). I still want Cartographer to correct my IMU, but not to do so when my IMU is corrected. I am guessing I should modify POSE_GRAPH.optimization_problem.acceleration_weight and POSE_GRAPH.optimization_problem.rotation_weight and TRAJECTORY_BUILDER_3D.imu_gravity_time_constant or maybe another solution would do?

Thank you!

Dennis

[ojura]

Yeah, I got that as well. Seems like Cartographer's correction quaternion doesn't fully converge (when re-running complete SLAM again with the corrections from previous iterations). Cartographer always outputs a correction quaternion which is a rotation of about half a degree for me. TODO investigate this.

[ludennis]

Hmm, only half a degree for you? I am getting over 5 degrees of correction here in the video the IMU seems correct:

[INFO] [1535779221.344208450, 1535097152.736092384]: I0901 13:20:21.000000 8907 local_trajectory_builder_3d.cc:86] IMU not yet initialized. [ INFO] [1535779231.843297657, 1535097186.769224742]: I0901 13:20:31.000000 8973 optimization_problem_3d.cc:563] IMU correction was: 15.6116 deg (0.990734, -0.00709178, -0.000948437, 0.135627) [ INFO] [1535779247.029615521, 1535097220.596634522]: I0901 13:20:47.000000 8980 optimization_problem_3d.cc:563] IMU correction was: 11.8896 deg (0.994622, -0.00189059, 0.0130392, 0.102729) [ INFO] [1535779263.450916091, 1535097253.426262807]: I0901 13:21:03.000000 8974 optimization_problem_3d.cc:563] IMU correction was: 7.14785 deg (0.998055, -0.00196241, 0.00900004, 0.0616519) [ INFO] [1535779266.643966983, 1535097258.756602127]: I0901 13:21:06.000000 8978 optimization_problem_3d.cc:563] IMU correction was: 7.46526 deg (0.997879, -0.00193765, 0.00829395, 0.064541) [ INFO] [1535779266.693936177, 1535097258.756602127]: I0901 13:21:06.000000 8978 optimization_problem_3d.cc:563] IMU correction was: 7.46526 deg (0.997879, -0.00193765, 0.00829395, 0.064541)

And thanks @ojura for the investigation, if there's anything I can help you with the investigation, please let me know. I will be more than happy to help. Thanks!

[ludennis]

Hi @ojura, just an update on imu data. So it turns out that I set the wrong tracking_frame in my assets_writer lua file. I have tracking_frame = 'base_imu' in my backpack 3d lua file, while I have tracking_frame = 'base_link' in my assets_writer lua file. And while I have this mistake in tracking_frame, I kept on changing my tf (or urdf) between the lidar and IMU, and have been misled to think that the weird/bad map that I was producing was caused by a bad IMU, or even by Cartographer's IMU correction.

Needless to say, it was my own fault in producing an error like this. I have been able to produce good map ever since. Thank you very much for your support and respond!

Dennis

[ojura]

Good to hear! What is the magnitude of the new correction after you apply the previously outputted one in the urdf? Have you tried iterating? (Note that you have to multiply all the previous corrections together.)

[ludennis]

@ojura, yeah! I got the IMU correction to be under the magnitude of 1 degree. And I did this by collecting a rosbag with a very small loop, and controlling the global matcher to only optimize at the location where the loop closes. After 2 or 3 times of obtaining and applying the IMU correction this way, the IMU correction was cut to under 1 degree.

[Sanderi44]

@ludennis Thank you for looking into this! I have been trying to figure out how to basically do exactly the same thing (reduce the IMU correction to zero). However, I cannot seem to figure out what to apply that quaternion to. How did you apply that quaternion in your urdf? Is it the base_link to imu, imu to tracking_frame or something else? This never really got resolved.

Thanks so much!

[ludennis]

@Sanderi44,

Here's a snippet of my .urdf file:

Here are the steps I took:

1) Recorded a bag file that has a small loop-closure with topics /velodyne_points and /data/imu, 2) Measured the distance between lidar (velodyne) and imu, and recorded the xyz difference in the .urdf file (If you, like me, have added a base_link, then you would need to find out the distance between base_link and imu and then base_link and velodyne, 3) Set POSE_GRAPH.optimization_problem.log_solver_summary = true in my .lua file, 4) Ran [offline_mapping_node_name].launch | grep IMU, 5) Adjust POSE_GRAPH.optimize_every_n_nodes so that IMU correction only happen once at the loop-closure, 6) Recorded the quaternion of IMU correction, 7) Used tf.transformations in python's interactive mode (python then import tf.transformations) to convert quaternion of IMU correction to RPY (roll, pitch, yaw), 8) Recorded the RPY in the .urdf file, 9) Ran [offline_mapping_node_name].launch | grep IMU, again, 10) Recorded the quaternion of IMU correction, 11) Applied new quaternion to the previous one with tf.transformations.quaternion_multiply([first_quaternion], [second_quaternion]) 12) Used tf.transformations to convert quaternion to rpy 13) Updated the RPY in the .urdf file 14) Repeated steps 9)-13) until the IMU correction angel is less than 1 degree.

I personally don't think this is the best way of calibrating the imu (my colleague told me what I was doing was lidar-imu fusion. Was it?), but these were the steps I took. Didn't have to worry about these afterwards. Hope this helps. Let me know if you need anything!

Dennis

[Sanderi44]

Thank you @ludennis! This is a great tutorial! Just to be clear, the new rpy values are applied to the base_link to tracking_imu joint right?

[ojura]

@Sanderi44 please carefully read my message from above, https://github.com/googlecartographer/cartographer/issues/440#issuecomment-417559625.

When Cartographer receives an IMU measurement, it takes the IMU sensor frame ID from the IMU message (probably imu_link for you) and obtains a transform from the IMU sensor frame to the tracking frame, which can be any frame in your tf tree (usually base_link), as long as it is colocated with your IMU frame (so no translation, but it can have an arbitrary rotation). The IMU measurements are transformed to the tracking frame in the line I highlighted in the message above.

There isn't a universal answer to your question. It depends on how your tf tree looks like. For @ludennis' example, yes, it could be applied to the base_link->tracking_imu transform. Or, to tracking_imu->base_imu. You can check if you got it right by comparing transforms like I described above.

[ojura]

This gives me an idea. Maybe it would be useful to have an additional option in Cartographer ROS to specify that additional correction quaternion for pre-multiplying the IMU data without having to modify transforms. This would enable users to just directly copy and use the correction quaternion suggested in the solver's summary.

[ojura]

@Sanderi44, check out https://github.com/googlecartographer/cartographer_ros/pull/1025 (https://github.com/larics/cartographer_ros/tree/imu-correction). It should enable using the correction quaternion verbatim, without having to fiddle with the TF tree.

[Sanderi44]

@ojura Thank you for the advice and for adding that commit. Very helpful! I will try out this solution and let you know how it goes.

[ojura]

@Sanderi44 have you had any luck with using the IMU correction quaternion?

[Sanderi44]

@ojura Thanks for checking in. I applied that commit to my code and did the following: 1) set the correction quaternion to (1,0,0,0) in the yaml 2) played back a dataset in which I travel in a loop, setting the pose graph to update at the end of the loop. During this playback, the point cloud is not aligned with the ground plane until the pose graph is updated. 3) set the correction quaternion to the resulting IMU correction value.
4) played back again and the new IMU correction was still very big (> 40 deg, sometimes greater than 100 deg).

Trying it with different update rates results in the same thing. However, the orientation of the point cloud when viewing in rviz is aligned correctly with the ground plane and the 2d projected map that it creates looks very good (i.e. walls are thin, no warping, ground plane looks aligned to horizontal map). It seems kind of strange that it would continue to output a large IMU correction

Not sure if it is related, but the vertical accuracy of the resulting trajectory is much noisier than the horizontal accuracy. The first image shows the horizontal trajectory versus the true trajectory. The second shows the same time thing with the Z axis included.

[ojura]

40 degrees?! Ouch, something is very much off. IIRC we have observed convergence to around half a degree or less in this thread.

[Sanderi44]

I agree, something is very wrong. Also, I have a small update to that result. In my previous post, I had more than one submap for every pose graph update. This time, I tried doing only one submap per pose graph update. I was able to get the result down to 0.5 degrees, playing it back again with the new quaternion, and the same settings. However, when I play back another dataset, with the same setup, in the same place, I once again get large IMU correction results. If it helps, I can post my setup, rosbag validate, etc.

[ojura]

Feel free to post your bags and setup. I can’t promise to have a look at it in soonish, but someone else might be able to.

[Sanderi44]

@ojura after much fretting, I was able to figure out what was going on. There are two IMU's on my assembly, and I had the wrong one plugged in. That's why it was not matching up. Now I am able to get very low values for the IMU correction. I am, however, still getting the same error in the vertical axis. Do you have any hints as to how to improve that?

[ludennis]

@Sanderi44

Even having calibrated my imu, cartographer will still need to correct my z-axis (slowly drifting up or down). I haven't found a solution to correct it. Cartographer corrects the imu, but I would still see a straight road being slightly slanted after running.

Btw, did you use MATLAB to generate the 3d plot?

Dennis

[Liying0824]

@ojura ,I used cartographer to build my map using 3d lidar data and imu data. It looks like the first submap build properly. But I found IMU correction have a big jumps per OptimizationProblem3D update, IMU correction was very big (> 20 deg, sometimes greater than 100 deg). I don't know what happened, and IMU correction how to calculate exactly. I don't find specific calculation code of IMU correction in cartographer, can you tall me where have computation process of IMU correction in cartographer? Thanks

ying

[ojura]

@Liying0824 Have a look at https://github.com/googlecartographer/cartographer/blob/f73758e3a54fbb0821be4e40d5815116416a76af/cartographer/mapping/internal/optimization/optimization_problem_3d.cc#L357

[Fanny-one]

I am trying to calibrate the IMU, and am able to have cartographer output the "IMU correction" message to give me the quaternion value. However, I am having trouble figuring out how to use this number in my urdf to correct my IMU. Specifically, I am not even sure if the quaternion pertains to the tf from IMU to lidar or from lidar to IMU, or IMU to base_link. Perhaps more description on what the quaternion value means in respect to IMU would do.

Hi, @ludennis I have the same trouble. Is it right that the quaternion only pertains to the tf from IMU to base_link? I don't think the operation could calibrate lidar and I don't know how to calibrate lidar. Can you give me help? Thanks in advance.