Add finalize option in write state service.

[jihoonl]

to support saving of fully optimized state without killing the cartographer node.

Reopen of #611.

[cschuet]

Thanks. Can you explain how you want to use this? Should I imagine that you have a robot running SLAM, then you want to save the optimized state to disk. Now your cartographer_node is not consuming sensor data anymore as all trajectories have been finished. What do you do now with that cartographer_node? Do you want to start a new trajectory now and localize against the existing ones?

[jihoonl]

Yes, I can start a new trajectory to localize the robot or to extend the existing map without killing the robot. Since our robots should be operatable 24/7, I need to achieve the followings without killing the cartographer_node.

• create a current pbstream snapshot
• update existing pose graph. (by starting a new trajectory)
• localize the robot.(by starting a pure localization trajectory)

Also, I had a trouble to find a moment to save the pose graph after SLAM programmatically when optimization takes longer than robot's actual movement. As a cartographer developer, I can more or less tell when to save the map by checking terminal logs(Remaining work items in queue: xxx). But, I cannot ask the robot operators to read terminal logs to find out when to save the map. Thus, I would like to guarantee that the generated pbstream file is fully optimized by adding finalize option in write_asset service. So that, I can tell robot operators that they can just call this service to generate optmized map.

[cschuet]

Ok, makes sense to me. I am just wondering whether conflating state writing and finishing and running optimization is the right thing. Currently WriteState is a "const" functions so to speak. Setting finalize = true then modifies the state.

We already have a call to finish a trajectory. I am wondering whether it wouldn't be possible to add a ROS method that forces an optimization and returns when the optimization is done. This way you could

1. Issue FinishTrajectory calls for all trajectories.
2. Issue RunOptimization and wait for it to finish.
3. Issue WriteState.

This way other people might be more easily able to reuse these building blocks in a different way for their own use case. WDYT?

[jihoonl]

Sounds good to me. Basically, *2. Issue RunOptimization and wait for it to finish* is a missing step for my usecase.

Then, how about creating a ros service that finishes all trajectories and call RunFinalOptimization()? so I can call WriteState afterward. I think issuing RunOptimization via ros service call would make headaches in cartographer level

[cschuet]

Yes I was also wondering about how difficult it is to make such a force optimization call. I will take a look tomorrow and get back to you. Otherwise a new call that does finish all trajectories and optimizes sounds good to me.

[jihoonl]

Ok. Let me know how your investigation goes.

FYI, this check in constraint builder was being evil when I was trying to refactor HandleWorkQueue logics as we discussed in cartographer-project/cartographer#563.

[ojura]

I agree about adding a RunFinalOptimization service.

Regarding the work queue, I think that RunFinalOptimization (the method in the pose graph) should be pushed to the work queue as well. I have refactored that in my manual constraints branch, but merging that branch is currently postponed. I am very busy atm, but I was planning on singling out some smaller changes I made there as separate PRs. Refactoring RunFinalOptimization is one of these changes. With that change, it plays well with that check!

[cschuet]

Small update: I spent some time today studying the concurrency in PoseGraph / ConstraintBuilder but I need to think about this a little more before I understand it fully.

[cschuet]

@jihoonl To separate the issues from cartographer-project/cartographer#563 from this discussion here, could upload a PR where you show how you tried to refactor HandleWorkQueue() and ran into the

CHECK(when_done_ == nullptr);

check in ConstraintBuilder?

[cschuet]

@ojura I looked at your custom constraints PR. Why do you think refactoring RunFinalOptimization in this way is advantageous?

void SparsePoseGraph::RunFinalOptimization() {
{
  common::MutexLocker locker(&mutex_);
  AddWorkItem([this]() REQUIRES(mutex_) {
    optimization_problem_.SetMaxNumIterations(
      options_.max_num_final_iterations());
    DispatchOptimization();
    });
  WaitForAllComputations();
}

One thing I guess is that the trimmers are not invoked as part of the current RunFinalOptimization.

[ojura]

It's really been a while since I wrote that code so I don't remember right now, but my guess would be the following: I have multiple things (such as AddCustomConstraint) which can trigger optimization (no longer just AddScan/AddNode as it's now called), so I refactored that into DispatchOptimization. RunFinalOptimization didn't play well with that (e.g. if AddCustomConstraint is called a little bit before, or during the final optimization).

I still have a really tight schedule, but I will get back to this when I can :)

[cschuet]

I am closing this PR since we decided not to take this approach.