Support for loading saved submaps and pose graph for long-term SLAM.

[ajakhotia]

Hi Authors and other Contributors Let me start by thanking you all for writing Cartographer and open-sourcing it. It adds immense value to the robotics society as it is probably the only SLAM algorithm as of now that is field-ready. Great job!

I am using Cartographer with our in-house framework to create a solution for a long-term 2D mapping and localization. For the implementation to be truly long-term, I need to add the ability of being able to save the pose of the robot, sub-maps and the pose-graph(SPA) on shutdown and load them back into cartographer on subsequent startup of the software. I will be beginning my efforts on this very soon and was wondering if you guys have any thoughts, pointers to references or inputs before I begin. I would be more than happy to collaborate as suitable.

Here's a quick brain dump of what I think needs to be done to accomplish this:

• Serialization, saving, loading and construction of sub-maps in local trajectory builder(front-end) and sparse pose graph in global trajectory builder.

• Saving and loading of robot pose(with covariance?) and instantiation of map builder with this pose as the starting point.

• A global matching scheme to match the new scans to the loaded sub-maps in the vicinity(scaled based on covariance?) of the seed pose before cartographer resurrects and operates as normal. May be?

• A strategy to appropriately kick-start the internal kalman filters. Not sure how the time-jump will affect those.

• Swapping in new laser frames to replace stale frames in the pose-graph that are no longer representative of the environment. Also limiting the insertion of laser scans to situations when new area is being explored or when swapping out an stale scan . This will

  1. Avoid growth of pose-graph over course of time to an intractable number of constraints.
  2. Make the pose-graph adaptive to the long-term, slow changes in the environment.

Please do share your thoughts that you think might help. I will be updating this post as I think of more items.

Thanks

-- Anurag

[SirVer]

@wohe @macmason @yutakaoka We had several discussions with TRI about this very idea, but afaik no real work has started on this. @TRI: Guys, what is your status on this?

[haidai]

I'm also looking a similar case but we're using cartographer components and data types as a starting point and not worrying as much about the submaps for now since it's hard enough to do good global localization and figuring out how to edit graphs without having to do recursively also. Might be a helpful direction in your case too?

[yutakaoka]

This is exactly what we want to do. At this moment we just tried exist functions on multi-trajectory mapping. Now we are setting up cloud environment and robots, after that we really want to proceed this work. I think this thread is a good starting point to discuss more on the topic.

I am just curious about how to manage different configuration sensors to create a shared map.

Thanks, Yutaka

[macmason]

Hey folks,

TRI's got some cycles free to think about this again. I thought we had a fairly detailed work plan at one point, but I can't find it anymore. Submap / HybridGrid serialization to start?

[mtau87]

Yes, I think submap serialization is a good start point.

[macmason]

Design question:

@ajakhotia's list of steps includes serializing local trajectory builders. I think that's overkill: we could reasonably introduce the constraint that you have to start a new trajectory when you load a map from disk, so we'd only need the sparse pose graph.

I think. @wohe @damonkohler @SirVer thoughts?

[damonkohler]

That makes sense, @macmason, and that was our original plan. The original action plan was posted on the mailing list.

[macmason]

Thanks @damonkohler. That list suggests that sparse pose graph constraints haven't been open-sourced, but exist; does that mean that step is best left to the Googlers, or should I take it on?

[damonkohler]

@macmason, the SparsePoseGraph proto is already open sourced with an associated ToProto. Deserializing it into a new SparsePoseGraph has not yet been done and we'd love to see you take that on :)

[Mofef]

Hello everyone,

thank you for open sourcing this project! We at magazino are impressed by the performance of cartographer and hope to use it for continuous slam in the future. Therefore we would like to contribute to the API for loading and saving the state of the cartographer (and whatever else is necessary for it).

So where can I start? @macmason did you already start with the deserialization of SparsePoseGraph? What else is there to be done, @damonkohler?

Cheers, Moritz

[macmason]

@Mofef I have some work in progress on that, yes. My suggestion would be to look into how to detect whether or not a given submap is a candidate for deletion; I think that's the next hard part. If @damonkohler disagrees, do what he says instead.

[wohe]

@macmason I think not adding a trajectory builder for a deserialized trajectory is a good idea, but right now there is no distinction between trajectories and their builders, e.g. in mapping/map_builder.h. This must be addressed first, e.g. by changing the interface to allow enumerating all trajectories, only some of which have a builder.

[wohe]

@macmason @Mofef There are still two pieces missing for serialization. Each trajectory currently has a number of point clouds (currently called laser fans but they could also be from a depth camera so should be renamed), some submaps, and constraints between the two. A trajectory can also have associated IMU data and in the future we might add more data, e.g. odometry. We are missing:

• serialization of constraints if multiple trajectories are present
• combining the code that serializes individual pieces into serialization of all data (fans, submaps, constraints, IMU data)

[macmason]

@wohe working on ToProto right now, in fact.

[Mofef]

@wohe Does that imply that serialization of constraints in case of just one trajectory is working already?

Please correct me if I'm wrong:

• In the current implementation there is always just one trajectory
• Another trajectory should be generated for
  • a kidnapped robot re-localizing in a existing map (based on an old trajectory)
  • scenarios with more than one robot

@macmason concering deletion of submaps: Was there already a decision that this is how the map size should be limited? Might it also be an option to merge/stich several submaps into one? (potentially weighting newer ones higher than older ones)

[wohe]

@Mofef Correct, the function SparsePoseGraph::ToProto() works for the single trajectory case, and the proto message was already designed with the multi-trajectory case in mind.

Regarding deletion of submaps: I think it makes sense to add support for deleting submaps. Right now each submap is from a small time interval, a property you would lose if stitched. Also, I would like to see support for pure localization in the future, and I'm thinking about implementing it as running a trajectory builder, but only keeping a rolling window of n submaps. This would be pretty simple to do once we have code for dropping submaps.

[macmason]

@Mofef there hasn't been any particular decision on how keep the submap count from growing. My first thought was to delete those submaps that have been completely "covered up" by newly-arrived data, on the theory that old information is less interesting.

Much more interesting things are possible...that's part of why I'm interested in this.

[macmason]

@wohe I've done some digging here, and I have a design question. My thinking was to extend SparsePoseGraph::ToProto to fully use the multi-trajectory capabilities of the proto (and cleaning up the TODOs you've left for yourself).

However, this is pretty awkward to implement: the native Constraint objects don't know what trajectory they're from, so you're reduced to tricky indexing to try to back that out from elsewhere. My feeling is that SparsePoseGraph shouldn't actually be tracking that information; after all, it doesn't actually care about trajectories, only submaps and constraints.

That would suggest that SparsePoseGraph's proto should in fact keep all the constraints and trajectory nodes in one big flat list, and that the association of those entries into trajectories should be done one level up, when we serialize the entire MapBuilder.

Thoughts?

[wohe]

@macmason Unfortunately, this is not possible. The optimization problem needs to know how to tie poses from a trajectory together. mapping_3d::sparse_pose_graph::OptimizationProblem::Solve() shows best what is necessary: trajectory nodes are stored interleaved, but have to be connected inside of each trajectory via IMU measurements. 2D is similar but cannot take IMU into account yet. The same will be true if we add support for odometry to the global SLAM problem: we need to know which odometry data is for which subset of nodes.

This means that without changing the way the data is stored, we would need to map between interleaved indices into the flat list of nodes (or submaps) and index pairs including a trajectory ID.

Alternatively, one could first continue to refactor the way data is kept: Right now each node knows its trajectory. We could change this to keeping node data separate per trajectory. Constraint objects would then change to use index pairs.

[brandon-northcutt]

Hello Cartographer team,

I have a few weeks to dedicate to working on this issue here at Toyota Research Institute. What I see as good block of next steps:

1) Load one of the gold standard curated sensor collections into Cartographer 2) Create a method to stream the various Cartographer data structure protobufs to disk (hopefully in a way that can be reused later for streaming optomized mapping data between servers) 3) Write a method to read this stream back into memory and construct the necessary data structures 4) combine all these steps in a unit test to compare the pre-serialization data structures to the post deserialization data structures to ensure they match.

I would appreciate an comments and advice to set me out on the right foot. I'm nearly done reading through the issues, comments, google group archive, and journal papers on the algorithms.

[dcconner]

@brandon-northcutt We are interested in this as well, but are just getting started with Cartographer.

I think your serialization and unit test approach makes sense, but I have a few questions about the design of map saving and loading relative to cartographer operation.

Q1) The first question I have is related to saving and loading maps is in regards to the map origin. I presume (again, we are just getting started, so I may be off base) that the optimization updates prior sub-map poses relative to the current robot pose estimate in order to avoid having the local pose "jump". This would make sense for online, but for saving and loading a map, it would seem to be important to have a "fixed" origin for "home base".

First, is my understanding correct?

Would it be possible to define a given sub-map as the "base" and update poses relative to that within the optimization, or at least when the map is saved.

Q2) Are sub-maps a fixed resolution and size relative to one another. Would it be possible to save and load a single "composite" map as a global submap, and then use that to initialize optimization.

Q3) Is it necessary to save the "Laser fans", or only the sub-maps and poses? (@wohe comment 21-March)

[Mofef]

@dcconner about Q1: As far as I can see the origin of Submap 0 is fixed to the global origin. At the moment this behaviour is hard coded. Q2: Submaps have a fixed resulution, The size is only fixed in terms of a fixed number of scans that are added to it (these are both global parameters), not in terms of X-by-Y pixels/voxels. Loading of submaps is not yet implemented at all. However I would also be interested what the authors think about that "single composite submap at initialization" idea. Q3: After the constraint builder was using the Laser fans to generate constraints they can be thrown away.

[wohe]

@brandon-northcutt Sounds good to me. A PR implementing "2." should now be possible by feeding the outputs of the various serialization functions through #241.

@dcconner Combining submaps or different submap resolutions are out of scope for this issue. This of course does not mean that we could not support it, it just seems unrelated to adding serialization support: The goal here is to serialize and reload the current state of Cartographer. For this to work, we must serialize the range data, otherwise we cannot match this data against future submaps anymore.

[dcconner]

@wohe My reading of @ajakhotia issue was different. I read the issue as being similar to my interest in saving data after a mapping run, and reusing it as a starting point for further exploration. I'm not necessarily interested in recreating the original cartographer state with all of its trajectory data, and allow completely fresh optimizations.

While combining submaps, or using a single submap, is not necessary to my task, I'm not sure serialization and deserialization of the entire cartographer state is either.

Let's say I have a large buiding with three hallways, and I map two of three hallways today, and then need to come back tomorrow. If I'm happy with map quality of day 1, then I would be content optimizing new day 2 data relative a "fixed" sub map set from day 1. Day 2 would be a completely separate trajectory with no necessary constraints from the prior days trajectory. I would just want the new data to be optimized relative to overlapping portions of the old data.

While it would be better to have a way of allowing new data to "correct" old maps and adjust their relative poses, I would be OK having old maps fixed relative to one another so long as they are available for doing loop closure with new submaps.

Based on my reading of paper and comments so far, without diving into the details of code, it seems that post optimization, we only need the sparse graph with submaps and constraints, and do not need all of the trajectory data with laser fan to do what I thought the initial issue from @ajakhotia was asking for. Thus, we could only focus on serializing parts of the data.

This is an obvious loss of data, but seems more practical for sharing maps over time.

So I guess my questions it what is the minimal set of data (with some reasonable loss of information) to allow us to re-initialize and continue extending a map from a completely different run?

[brandon-northcutt]

@dcconner First off, I'm new to this project too, so please take the primary contributor's opinions more seriously. ;)

Q1) Cartographer supports multiple trajectories well and will make them relative to a common origin if a high-quality constraint (scan --> submap) can be found. The first pose of the first trajectory is used as a common trajectory origin. My understanding is that submap probability grids are fixed in size and are considered finished after being filled with several adjacent scans (where inter-pose error is considered minimal) optimization will cause sub-maps to jump relative to one another particularly when large loop closures occur, and I think this does cause the current robot pose estimate to jump. It is certainly possible to choose the origin of a specific submap as the world origin, but this is just an abstraction because optimization may change it's value.

Q2) I think submaps are fixed size. They certainly can be aggregated and saved, as is done in Cartographer ROS to produce the finished .pgm and .yaml files. However, I do not think this can be used to seed optimization of a future Cartographer instance because the full sparse pose graph, set of submaps, set of constraints, and scan data are needed for optimization. I.e., once it is flattened, it cannot participate in future trajectory optimizations and you'll have to localize against it by some other means.

Q3) Constraints are computed between scans and submaps. There is currently no implementation of scan or submap deletion. Since constraints are computed from scan to submap, optimization will lose information if we delete scans or submaps (old scans to new submaps vs new scans to old submaps). Certainly we will want to implement a method of considering areas of the map "optimized" so that we may delete old scans, but there will be some heavy lifting in making sure there are no dangling areas that would have benefited from future loop closures.

[brandon-northcutt]

@wohe Thanks. I'll get started on a PR for 2. using #241

[SirVer]

This discussion in this issue became quite broad. I am trying to organize and focus the lifelong effort a bit better and pulled out all sub-tasks that I could identify in this issue into separate issues tracked under the newly created lifelong milestone.

I also pulled out the two feature requests from @dcconner into separate issues to be discussed there.

[SirVer]

This is now implemented. We still do not serialize range data, IMU and odometry. This is tracked in #253.