Perform final optimization at the end. (#483)
parent
102a3b0db3
commit
cf13f76c82
|
@ -428,7 +428,6 @@ bool Node::HandleWriteState(
|
|||
}
|
||||
|
||||
void Node::FinishAllTrajectories() {
|
||||
{
|
||||
carto::common::MutexLocker lock(&mutex_);
|
||||
for (auto& entry : is_active_trajectory_) {
|
||||
const int trajectory_id = entry.first;
|
||||
|
@ -438,16 +437,23 @@ void Node::FinishAllTrajectories() {
|
|||
}
|
||||
}
|
||||
}
|
||||
map_builder_bridge_.RunFinalOptimization();
|
||||
}
|
||||
|
||||
void Node::FinishTrajectory(const int trajectory_id) {
|
||||
{
|
||||
carto::common::MutexLocker lock(&mutex_);
|
||||
CHECK(is_active_trajectory_.at(trajectory_id));
|
||||
map_builder_bridge_.FinishTrajectory(trajectory_id);
|
||||
is_active_trajectory_[trajectory_id] = false;
|
||||
}
|
||||
|
||||
void Node::RunFinalOptimization() {
|
||||
{
|
||||
carto::common::MutexLocker lock(&mutex_);
|
||||
for (const auto& entry : is_active_trajectory_) {
|
||||
CHECK(!entry.second);
|
||||
}
|
||||
}
|
||||
// Assuming we are not adding new data anymore, the final optimization
|
||||
// can be performed without holding the mutex.
|
||||
map_builder_bridge_.RunFinalOptimization();
|
||||
}
|
||||
|
||||
|
|
|
@ -56,6 +56,9 @@ class Node {
|
|||
// Finishes a single given trajectory.
|
||||
void FinishTrajectory(int trajectory_id);
|
||||
|
||||
// Runs final optimization. All trajectories have to be finished when calling.
|
||||
void RunFinalOptimization();
|
||||
|
||||
// Starts the first trajectory with the default topics.
|
||||
void StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options);
|
||||
|
||||
|
|
|
@ -50,6 +50,7 @@ void Run() {
|
|||
::ros::spin();
|
||||
|
||||
node.FinishAllTrajectories();
|
||||
node.RunFinalOptimization();
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
|
|
@ -209,15 +209,13 @@ void Run(const std::vector<string>& bag_filenames) {
|
|||
}
|
||||
|
||||
bag.close();
|
||||
// Ensure the clock is republished also during trajectory finalization,
|
||||
// which might take a while.
|
||||
clock_republish_timer.start();
|
||||
node.FinishTrajectory(trajectory_id);
|
||||
clock_republish_timer.stop();
|
||||
}
|
||||
|
||||
// Republish the clock after bag processing has been completed.
|
||||
// Ensure the clock is republished after the bag has been finished, during the
|
||||
// final optimization, serialization, and optional indefinite spinning at the end.
|
||||
clock_republish_timer.start();
|
||||
node.RunFinalOptimization();
|
||||
|
||||
const std::chrono::time_point<std::chrono::steady_clock> end_time =
|
||||
std::chrono::steady_clock::now();
|
||||
|
|
Loading…
Reference in New Issue