Clean trimming of frozen trajectory data. #1767
Conversation
| @@ -323,6 +323,59 @@ TEST_F(PoseGraph3DTest, EvenSubmapTrimmer) { | |||
| } | |||
| } | |||
|
|
|||
| TEST_F(PoseGraph3DTest, EvenSubmapTrimmerOnFrozenTrajectory) { | |||
There was a problem hiding this comment.
Note: this test-case is based on the existing EvenSubmapTrimmer test case above, with the difference that no constraints are added and the graph is built like we do when loading frozen state. It would fail without this patch.
There's no equivalent test case in pose_graph_2d_test.cc yet, so I did it only for 3D. If we want to extend the 2D test cases, this would be a larger change for separate PRs IMO.
|
Related to the linked RFC PR and the associated PR in cartographer_ros. |
| if (parent_->IsTrajectoryFrozen(submap_id.trajectory_id)) { | ||
| // Remove node_ids that are associated with the frozen submap to be trimmed. | ||
| nodes_to_remove.insert( | ||
| parent_->data_.submap_data.at(submap_id).node_ids.begin(), |
There was a problem hiding this comment.
Do these node_ids always exist? Would it then make sense to get rid of the old style of handling deletion of nodes and replace it by your improved strategy completely? Something along these lines:
- Compute
nodes_to_retainby going through all submaps and adding allnode_idsfor submaps not equal to the givensubmap_idto remove. - Compute
nodes_to_removeas allnode_idsofsubmap_idnot to be retained. - Remove the constraints similar to before, but without the need to build up the
nodes_to_removeas part of it. We could add aCHECKto validate that all retained constraints use nodes not innodes_to_remove.
What do you think?
There was a problem hiding this comment.
Regarding your 3. point: There's already a loop that only keeps constraints for which nodes_to_remove.count(constraint.node_id) == 0 holds. With this in place we don't need to check afterwards, no?
The deletion logic needs to take care of deleting all data that is "exclusively" connected to the submaps that are to be removed. This is achieved by looking at the data that is connected via constraints in the graph. Deleting a frozen trajectory (one without optimization constraints) doesn't work that way and would leave dangling nodes in the graph. This adds an additional logic that uses the `node_ids` field of the submap instead of the constraints if the trajectory is frozen. Signed-off-by: Michael Grupp <[email protected]>
MichaelGrupp
force-pushed
the
upstream-delete-frozen-trajectories
branch
from
5afa6c9 to
c1a4f0f
Compare
Signed-off-by: Michael Grupp <[email protected]>
MichaelGrupp
force-pushed
the
upstream-delete-frozen-trajectories
branch
from
c1a4f0f to
5f328ad
Compare
Signed-off-by: Michael Grupp <[email protected]>
MichaelGrupp
force-pushed
the
upstream-delete-frozen-trajectories
branch
from
a2fa7c3 to
c5e6a85
Compare
There was a problem hiding this comment.
Looks good. Only one nit: Could we update the comment in map_builder.cc? Now setting the nodes contained in a submap is not only needed for 3D, but also 2D: https://github.com/cartographer-project/cartographer/blob/master/cartographer/mapping/map_builder.cc#L362
Maybe simply change it to:
// Information about which nodes belong to which submap is required even without constraints.
Signed-off-by: Michael Grupp <[email protected]>
|
Hi there, I've tried this DeleteTrajectory service to try to remove a loaded frozen trajectory. It works well and the frozen trajectory is deleted. However, the code crashes after the frozen trajectory is deleted. The error is shown as below: terminate called after throwing an instance of 'std::out_of_range' I find it happens in constraint_builder_2d.cc. The line of "(*callback)(result);" I used the latest master for cartographer which has all the updates above. Does anyone have any idea why it happens and how to resolve it? Thanks very much in advance. |
|
@cuicuihaohaoxuexi I'm also confused with this problem, my error is the same as you! |
The deletion logic needs to take care of deleting all data that is
"exclusively" connected to the submaps that are to be removed. This is
achieved by looking at the data that is connected via constraints in
the graph.
Deleting a frozen trajectory (one without optimization constraints)
doesn't work that way and would leave dangling nodes in the graph.
This modifies the logic to use the
node_idsfield of the submapinstead of the constraints.
Signed-off-by: Michael Grupp [email protected]