diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 57d3724..d719f3b 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -195,8 +195,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, .pose.inverse() * optimization_problem_.node_data() .at(node_id.trajectory_id) - .at(node_id.node_index - optimization_problem_.num_trimmed_nodes( - node_id.trajectory_id)) + .at(node_id.node_index) .pose; constraint_builder_.MaybeAddConstraint( submap_id, submap_data_.at(submap_id).submap.get(), node_id, @@ -212,14 +211,9 @@ void SparsePoseGraph::ComputeConstraintsForOldScans( const auto& node_data = optimization_problem_.node_data(); for (size_t trajectory_id = 0; trajectory_id != node_data.size(); ++trajectory_id) { - for (size_t node_data_index = 0; - node_data_index != node_data[trajectory_id].size(); - ++node_data_index) { - const int node_index = - node_data_index + - optimization_problem_.num_trimmed_nodes(trajectory_id); + for (const auto& index_node_data : node_data[trajectory_id]) { const mapping::NodeId node_id{static_cast(trajectory_id), - static_cast(node_index)}; + index_node_data.first}; CHECK(!trajectory_nodes_.at(node_id).trimmed()); if (submap_data.node_ids.count(node_id) == 0) { ComputeConstraint(node_id, submap_id); @@ -247,12 +241,14 @@ void SparsePoseGraph::ComputeConstraintsForScan( const mapping::NodeId node_id{ matching_id.trajectory_id, static_cast(matching_id.trajectory_id) < - optimization_problem_.node_data().size() + optimization_problem_.node_data().size() && + !optimization_problem_.node_data()[matching_id.trajectory_id] + .empty() ? static_cast(optimization_problem_.node_data() .at(matching_id.trajectory_id) - .size()) + - optimization_problem_.num_trimmed_nodes( - matching_id.trajectory_id) + .rbegin() + ->first + + 1) : 0}; const auto& scan_data = trajectory_nodes_.at(node_id).constant_data; optimization_problem_.AddTrajectoryNode( @@ -349,21 +345,19 @@ void SparsePoseGraph::WaitForAllComputations() { common::FromSeconds(1.))) { std::ostringstream progress_info; progress_info << "Optimizing: " << std::fixed << std::setprecision(1) - << 100. * - (constraint_builder_.GetNumFinishedScans() - - num_finished_scans_at_start) / + << 100. * (constraint_builder_.GetNumFinishedScans() - + num_finished_scans_at_start) / (num_trajectory_nodes_ - num_finished_scans_at_start) << "%..."; std::cout << "\r\x1b[K" << progress_info.str() << std::flush; } std::cout << "\r\x1b[KOptimizing: Done. " << std::endl; - constraint_builder_.WhenDone( - [this, ¬ification]( - const sparse_pose_graph::ConstraintBuilder::Result& result) { - common::MutexLocker locker(&mutex_); - constraints_.insert(constraints_.end(), result.begin(), result.end()); - notification = true; - }); + constraint_builder_.WhenDone([this, ¬ification]( + const sparse_pose_graph::ConstraintBuilder::Result& result) { + common::MutexLocker locker(&mutex_); + constraints_.insert(constraints_.end(), result.begin(), result.end()); + notification = true; + }); locker.Await([¬ification]() { return notification; }); } @@ -441,15 +435,12 @@ void SparsePoseGraph::RunOptimization() { const auto& node_data = optimization_problem_.node_data(); for (int trajectory_id = 0; trajectory_id != static_cast(node_data.size()); ++trajectory_id) { - int node_data_index = 0; const int num_nodes = trajectory_nodes_.num_indices(trajectory_id); - int node_index = optimization_problem_.num_trimmed_nodes(trajectory_id); - for (; node_data_index != static_cast(node_data[trajectory_id].size()); - ++node_data_index, ++node_index) { - const mapping::NodeId node_id{trajectory_id, node_index}; + for (const auto& node_data_index : node_data.at(trajectory_id)) { + const mapping::NodeId node_id{trajectory_id, node_data_index.first}; auto& node = trajectory_nodes_.at(node_id); node.pose = - transform::Embed3D(node_data[trajectory_id][node_data_index].pose) * + transform::Embed3D(node_data_index.second.pose) * transform::Rigid3d::Rotation(node.constant_data->gravity_alignment); } // Extrapolate all point cloud poses that were added later. @@ -459,7 +450,12 @@ void SparsePoseGraph::RunOptimization() { optimized_submap_transforms_, trajectory_id); const transform::Rigid3d old_global_to_new_global = local_to_new_global * local_to_old_global.inverse(); - for (; node_index < num_nodes; ++node_index) { + int last_optimized_node_index = + node_data.at(trajectory_id).empty() + ? 0 + : node_data.at(trajectory_id).rbegin()->first; + for (int node_index = last_optimized_node_index + 1; node_index < num_nodes; + ++node_index) { const mapping::NodeId node_id{trajectory_id, node_index}; auto& node_pose = trajectory_nodes_.at(node_id).pose; node_pose = old_global_to_new_global * node_pose; diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc index e43887d..7c9865f 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc @@ -84,25 +84,28 @@ void OptimizationProblem::AddTrajectoryNode( CHECK_GE(trajectory_id, 0); node_data_.resize( std::max(node_data_.size(), static_cast(trajectory_id) + 1)); - node_data_[trajectory_id].push_back(NodeData{time, initial_pose, pose}); trajectory_data_.resize(std::max(trajectory_data_.size(), node_data_.size())); + + auto& trajectory_data = trajectory_data_.at(trajectory_id); + node_data_[trajectory_id].emplace(trajectory_data.next_node_index, + NodeData{time, initial_pose, pose}); + ++trajectory_data.next_node_index; } void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) { - auto& trajectory_data = trajectory_data_.at(node_id.trajectory_id); - // We only allow trimming from the start. - CHECK_EQ(trajectory_data.num_trimmed_nodes, node_id.node_index); auto& node_data = node_data_.at(node_id.trajectory_id); - CHECK(!node_data.empty()); - if (node_id.trajectory_id < static_cast(imu_data_.size())) { - const common::Time node_time = node_data.front().time; + CHECK(node_data.erase(node_id.node_index)); + + if (!node_data.empty() && + node_id.trajectory_id < static_cast(imu_data_.size())) { + auto node_it = node_data.begin(); + const common::Time node_time = node_it->second.time; + auto& imu_data = imu_data_.at(node_id.trajectory_id); while (imu_data.size() > 1 && imu_data[1].time <= node_time) { imu_data.pop_front(); } } - node_data.pop_front(); - ++trajectory_data.num_trimmed_nodes; } void OptimizationProblem::AddSubmap(const int trajectory_id, @@ -143,7 +146,7 @@ void OptimizationProblem::Solve(const std::vector& constraints, // TODO(hrapp): Move ceres data into SubmapData. std::vector>> C_submaps( submap_data_.size()); - std::vector>> C_nodes(node_data_.size()); + std::vector>> C_nodes(node_data_.size()); bool first_submap = true; for (size_t trajectory_id = 0; trajectory_id != submap_data_.size(); ++trajectory_id) { @@ -168,14 +171,15 @@ void OptimizationProblem::Solve(const std::vector& constraints, for (size_t trajectory_id = 0; trajectory_id != node_data_.size(); ++trajectory_id) { const bool frozen = frozen_trajectories.count(trajectory_id); - // Reserve guarantees that data does not move, so the pointers for Ceres - // stay valid. - C_nodes[trajectory_id].reserve(node_data_[trajectory_id].size()); - for (const NodeData& node_data : node_data_[trajectory_id]) { - C_nodes[trajectory_id].push_back(FromPose(node_data.pose)); - problem.AddParameterBlock(C_nodes[trajectory_id].back().data(), 3); + for (const auto& index_node_data : node_data_[trajectory_id]) { + const int node_index = index_node_data.first; + const NodeData& node_data = index_node_data.second; + C_nodes[trajectory_id].emplace(node_index, FromPose(node_data.pose)); + problem.AddParameterBlock(C_nodes[trajectory_id].at(node_index).data(), + 3); if (frozen) { - problem.SetParameterBlockConstant(C_nodes[trajectory_id].back().data()); + problem.SetParameterBlockConstant( + C_nodes[trajectory_id].at(node_index).data()); } } } @@ -192,9 +196,7 @@ void OptimizationProblem::Solve(const std::vector& constraints, .at(constraint.submap_id.submap_index) .data(), C_nodes.at(constraint.node_id.trajectory_id) - .at(constraint.node_id.node_index - - trajectory_data_.at(constraint.node_id.trajectory_id) - .num_trimmed_nodes) + .at(constraint.node_id.node_index) .data()); } @@ -202,27 +204,37 @@ void OptimizationProblem::Solve(const std::vector& constraints, // if odometry is not available. for (size_t trajectory_id = 0; trajectory_id != node_data_.size(); ++trajectory_id) { - for (size_t node_data_index = 1; - node_data_index < node_data_[trajectory_id].size(); - ++node_data_index) { + if (node_data_[trajectory_id].empty()) { + continue; + } + + for (auto node_it = node_data_[trajectory_id].begin();;) { + const int node_index = node_it->first; + const NodeData& node_data = node_it->second; + ++node_it; + if (node_it == node_data_[trajectory_id].end()) { + break; + } + + const int next_node_index = node_it->first; + const NodeData& next_node_data = node_it->second; + + if (next_node_index != node_index + 1) { + continue; + } + const bool odometry_available = trajectory_id < odometry_data_.size() && odometry_data_[trajectory_id].Has( - node_data_[trajectory_id][node_data_index].time) && + node_data_[trajectory_id][next_node_index].time) && odometry_data_[trajectory_id].Has( - node_data_[trajectory_id][node_data_index - 1].time); + node_data_[trajectory_id][node_index].time); const transform::Rigid3d relative_pose = odometry_available - ? odometry_data_[trajectory_id] - .Lookup( - node_data_[trajectory_id][node_data_index - 1].time) - .inverse() * - odometry_data_[trajectory_id].Lookup( - node_data_[trajectory_id][node_data_index].time) - : transform::Embed3D( - node_data_[trajectory_id][node_data_index - 1] - .initial_pose.inverse() * - node_data_[trajectory_id][node_data_index].initial_pose); + ? odometry_data_[trajectory_id].Lookup(node_data.time).inverse() * + odometry_data_[trajectory_id].Lookup(next_node_data.time) + : transform::Embed3D(node_data.initial_pose.inverse() * + next_node_data.initial_pose); problem.AddResidualBlock( new ceres::AutoDiffCostFunction( new SpaCostFunction(Constraint::Pose{ @@ -230,8 +242,8 @@ void OptimizationProblem::Solve(const std::vector& constraints, options_.consecutive_scan_translation_penalty_factor(), options_.consecutive_scan_rotation_penalty_factor()})), nullptr /* loss function */, - C_nodes[trajectory_id][node_data_index - 1].data(), - C_nodes[trajectory_id][node_data_index].data()); + C_nodes[trajectory_id][node_index].data(), + C_nodes[trajectory_id][next_node_index].data()); } } @@ -254,16 +266,14 @@ void OptimizationProblem::Solve(const std::vector& constraints, } for (size_t trajectory_id = 0; trajectory_id != node_data_.size(); ++trajectory_id) { - for (size_t node_data_index = 0; - node_data_index != node_data_[trajectory_id].size(); - ++node_data_index) { - node_data_[trajectory_id][node_data_index].pose = - ToPose(C_nodes[trajectory_id][node_data_index]); + for (auto& index_node_data : node_data_[trajectory_id]) { + index_node_data.second.pose = + ToPose(C_nodes[trajectory_id].at(index_node_data.first)); } } } -const std::vector>& OptimizationProblem::node_data() +const std::vector>& OptimizationProblem::node_data() const { return node_data_; } @@ -273,10 +283,6 @@ const std::vector>& OptimizationProblem::submap_data() return submap_data_; } -int OptimizationProblem::num_trimmed_nodes(int trajectory_id) const { - return trajectory_data_.at(trajectory_id).num_trimmed_nodes; -} - } // namespace sparse_pose_graph } // namespace mapping_2d } // namespace cartographer diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h index 0df075e..c5f0b60 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h @@ -76,20 +76,18 @@ class OptimizationProblem { void Solve(const std::vector& constraints, const std::set& frozen_trajectories); - const std::vector>& node_data() const; + const std::vector>& node_data() const; const std::vector>& submap_data() const; - int num_trimmed_nodes(int trajectory_id) const; - private: struct TrajectoryData { // TODO(hrapp): Remove, once we can relabel constraints. int next_submap_index = 0; - int num_trimmed_nodes = 0; + int next_node_index = 0; }; mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; std::vector> imu_data_; - std::vector> node_data_; + std::vector> node_data_; std::vector odometry_data_; std::vector> submap_data_; std::vector trajectory_data_;