cartographer 3d loop closure
955 views
Skip to first unread message
Mathieu Bolduc
Nov 2, 2016, 5:39:23 PM11/2/16
to google-cartographer
Hello!
First,
thank you for releasing this open source project! Ive been doing a test integration of it inside our machine learning platform (MLDB.ai). However Ive been unable to make loop closure work so far, and I was wondering if anyone could point me in the right direction to fix it.
We've captured 3D lidar data going around the block clockwise and fed it to cartographer. This is the result so far:
Please disregard the color scheme of the point cloud. The red crosses represent the trajectory nodes, and it should overlap in the top middle. It ends up drifting by a few meters, but nothing that loop closure shouldn't be able to fix.
However, if I understand the verbose output of the final optimization, it seems to think the match is almost perfect:
Solver Summary (v 1.12.0-eigen-(3.2.93)-lapack-eigensparse-openmp)
Original Reduced
Parameter blocks 6245 6244
Parameters 21855 21852
Effective parameters 18732 18729
Residual blocks 12251 12251
Residual 54909 54909
Minimizer TRUST_REGION
Sparse linear algebra library EIGEN_SPARSE
Trust region strategy LEVENBERG_MARQUARDT
Given Used
Linear solver SPARSE_SCHUR SPARSE_SCHUR
Threads 32 32
Linear solver threads 1 1
Linear solver ordering AUTOMATIC 1552, 4692
Cost:
Initial 9.194945e-03
Final 9.194945e-03
Change 0.000000e+00
Minimizer iterations 1
Successful steps 1
Unsuccessful steps 0
Time (in seconds):
Preprocessor 0.0382
Residual evaluation 0.0029
Jacobian evaluation 0.0092
Linear solver 0.1200
Minimizer 0.1473
Postprocessor 0.0012
Total 0.1867
Termination: CONVERGENCE (Function tolerance reached. |cost_change|/cost: 2.485712e-08 <= 1.000000e-06)
Do you have any tips or advice in where to look next to solve the loop closure?
Also if I may ask another question, in the ROS integration it is mentioned that 3D mapping requires IMU data where 2D does not. Could you provide more details?
Again thank you for releasing this and for any help or advice you can give me.
Best regards,
Mathieu Marquis Bolduc
MLDB.ai
Mathieu Bolduc
Nov 2, 2016, 5:41:16 PM11/2/16
to google-cartographer
Here is the missing picture, sorry.
Holger Rapp
Nov 2, 2016, 8:11:52 PM11/2/16
to Mathieu Bolduc, google-cartographer
On Wed, Nov 2, 2016 at 2:39 PM, Mathieu Bolduc <mbo...@mldb.ai> wrote:
Hello!First,thank you for releasing this open source project! Ive been doing a test integration of it inside our machine learning platform (MLDB.ai).
That sounds very interesting. Could you outline a bit more what your project is and what you want to use Cartographer for?
However Ive been unable to make loop closure work so far, and I was wondering if anyone could point me in the right direction to fix it.
We've captured 3D lidar data going around the block clockwise and fed it to cartographer. This is the result so far:
Please disregard the color scheme of the point cloud. The red crosses represent the trajectory nodes, and it should overlap in the top middle. It ends up drifting by a few meters, but nothing that loop closure shouldn't be able to fix.
The image is really hard to parse for me. For example, what is the length of the total trajectory? The point cloud also looks pretty low res, but it is hard to know for sure. What lidar is that?
The loop closure search window is configurable: https://github.com/googlecartographer/cartographer/blob/master/configuration_files/sparse_pose_graph.lua#L50. If you did not change it, it will search 4 meters in xy and 1m in z for a loop closure. That said, you probably need to tune the '*score*' values in the linked file for your resolution. It defines when a loop closure is 'good enough' to be added as a constraint. While you run the SLAM, it periodically outputs the number of loop closure constraint and their score in a ASCII histogram.
This alone does not help too much. I suggest you put a github repo with your config and a bag somewhere so we can roslaunch your setup or/and pastebin the complete output somewhere for analysis.
Do you have any tips or advice in where to look next to solve the loop closure?Also if I may ask another question, in the ROS integration it is mentioned that 3D mapping requires IMU data where 2D does not. Could you provide more details?
In 2D we can do without IMU - you just assume all scans are parallel to the ground plane and try to scan match harder - i.e. do not predict movement forward. In 3D we require the IMU to figure out where gravity is - to gravity align the z axis. If you do not have an IMU, have only movement in xy and want to do 3D you can just fake imu messages that are z-up with only gravity in it and increase the scan matching for local SLAM. Your milage may vary quality wise.
Again thank you for releasing this and for any help or advice you can give me.Best regards,Mathieu Marquis BolducMLDB.ai
--
You received this message because you are subscribed to the Google Groups "google-cartographer" group.
To unsubscribe from this group and stop receiving emails from it, send an email to google-cartographer+unsub...@googlegroups.com.
To post to this group, send email to google-cartographer@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/google-cartographer/eab1ca71-7f5a-46e5-82a3-1070ee2f2166%40googlegroups.com.
For more options, visit https://groups.google.com/d/optout.
Google Germany GmbH
Erika-Mann-Straße 33
80331 München
Registergericht und -nummer: Hamburg, HRB 86891
Sitz der Gesellschaft: Hamburg
Geschäftsführer: Matthew Scott Sucherman, Paul Terence Manicle
Mathieu Bolduc
Nov 3, 2016, 12:19:29 PM11/3/16
to google-cartographer, mbo...@mldb.ai
Hi,
Thank you for the quick answer!
The end goal of this project is to compress lidar data in order to transmit it from all the various sources that are popping in the city
(self-driving cars, drones and others) in order to build a real-time, 3d map of the city. On top of which we'll run models using our ML platform to allow all sort of queries.
Im testing to see if we can use cartographer for the mapping phase, either before or after compression. We find the occupancy-space approach very appealing.
We've captured this test data using a Velodyne VLP-16 puck.
Ive found out that I wasnt loading all parameters correctly for the sparse pose graph, which explain why loop closure wasnt doing any work. However now that its
running, the results are rather unsettling. I ran it on only a few seconds of input data while walking down the street. If I disable sparse graph optimization (by setting
optimize_every_n_scans and max_num_final_iterations to 0), the transformed point cloud and trajectory looks like the attached picture.Its a top down view, and looks pretty correct, the trajectory is on the side walk, you can see the two rows of parked cars and the trail from the moving ones. Trajectory nodes go from
{ t: [0.000000, 0.000000, 0.000000], q: [1.000000, 0.000000, 0.000000, 0.000000] } to { t: [-0.665780, 8.028970, -0.257078], q: [0.992933, 0.005923, 0.003877, -0.118469] }
However if I allow the sparse graph optimization to run with default settings, then the trajectories end up going from
{ t: [0.180539, -0.853876, -0.033034], q: [0.721486, -0.692233, 0.007359, 0.014724] } to { t: [-0.759318, 7.275198, -0.274126], q: [0.721486, -0.692234, 0.007359, 0.014722] }
I find it somewhat weird that it would change the first trajectory node, since that its the only origin. If I put everything in the reference system of the first node, the nodes go as this:
{ t: [0.000000, 0.000000, 0.000000], q: [1.000000, 0.000000, 0.000000, 0.000000] } to { t: [-0.841979, 0.605166, 8.120847], q: [1.000000, -0.000000, 0.000001, -0.000001] }
So it seems that the sparse graph optimization ended up stacking everything vertically. Do you have any idea what would cause that?
Regarding the 3D IMU, do you think it would be possible to allow the optimization to search for the proper vertical as well? Is it a question of stability or performance?
Thanks again for any help or advice you can give me!
Mathieu
To post to this group, send email to google-ca...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/google-cartographer/eab1ca71-7f5a-46e5-82a3-1070ee2f2166%40googlegroups.com.
For more options, visit https://groups.google.com/d/optout.
Mathieu Bolduc
Nov 3, 2016, 1:34:56 PM11/3/16
to google-cartographer
postscriptum:
And the whole system is still wierdly rotated.
Holger Rapp
Nov 3, 2016, 6:06:14 PM11/3/16
to Mathieu Bolduc, google-cartographer
I have a few theories as of what is going on. For example, you said you had an IMU and it sounds you hand carried the laser. What data do you provide the system to correct for gravity alignment, i.e. alignment of the scans? Also, a single VLP16 has an opening angle of 30deg - you will not see the floor close to the device. This means you will not get data constraining you in the direction of the laser spin axis. How do you solve that?
Knowing what the issues are you are seeing is really hard without more insight. For example, properly configured loop closure should never mess up locale SLAM, but can move all nodes. If you want more useful insights, please make the data and your configuration available somewhere, so we can see and test your tuning.
--
You received this message because you are subscribed to the Google Groups "google-cartographer" group.
To unsubscribe from this group and stop receiving emails from it, send an email to google-cartographer+unsub...@googlegroups.com.
To post to this group, send email to google-cartographer@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/google-cartographer/23852938-6d85-41bf-9839-be9d943335d3%40googlegroups.com.
Mathieu Bolduc
Nov 4, 2016, 5:34:31 PM11/4/16
to google-cartographer, mbo...@mldb.ai
This dataset was not captured with an IMU, we're in the process of adding IMU and GPS to our test setup. I passed a constant gravity vector to cartographer (as is done in one of the tests). Which introduce a fair amount of error (since we couldnt keep the sensor perfectly level), however the fan insertion in cartographer seems to deal with it extremely well (grats on that!), with drift becoming noticeable only after several hundred meters. We see a large amount of floor on each scan - we start seeing the floor 8m away while the range is 100m. When looking at the voxel submap, the slope of the street is captured.
I'll pause investigating the loop closure until we get IMU data. Its puzzling that the fan insertion deals with our noisy data so much better than the loop closure. But even just getting the voxel submap(s) from Cartographer has been really useful, so thank you again!
Holger Rapp
Nov 4, 2016, 7:47:03 PM11/4/16
to Mathieu Bolduc, google-cartographer
On Fri, Nov 4, 2016 at 2:34 PM, Mathieu Bolduc <mbo...@mldb.ai> wrote:
This dataset was not captured with an IMU, we're in the process of adding IMU and GPS to our test setup. I passed a constant gravity vector to cartographer (as is done in one of the tests). Which introduce a fair amount of error (since we couldnt keep the sensor perfectly level), however the fan insertion in cartographer seems to deal with it extremely well (grats on that!), with drift becoming noticeable only after several hundred meters. We see a large amount of floor on each scan - we start seeing the floor 8m away while the range is 100m. When looking at the voxel submap, the slope of the street is captured.I'll pause investigating the loop closure until we get IMU data. Its puzzling that the fan insertion deals with our noisy data so much better than the loop closure.
Yes, it is. I believe that this is mostly an issue of tuning, but it is hard to know without seeing your data and config.
But even just getting the voxel submap(s) from Cartographer has been really useful, so thank you again!
You are welcome, keep us posted please!
--
You received this message because you are subscribed to the Google Groups "google-cartographer" group.
To unsubscribe from this group and stop receiving emails from it, send an email to google-cartographer+unsub...@googlegroups.com.
To post to this group, send email to google-cartographer@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/google-cartographer/337f285b-0226-4f81-85e6-bedef57ce2de%40googlegroups.com.
Sam Thomas
Apr 29, 2017, 7:59:25 PM4/29/17
to google-cartographer
Hi Mathieu,
Did you have any success adding gps?
Reply all
Reply to author
Forward
0 new messages