From d8eb94b4a92d7aeb84c43a73f94e7905eb17148f Mon Sep 17 00:00:00 2001 From: Holger Rapp Date: Mon, 26 Jun 2017 16:38:08 +0200 Subject: [PATCH] Delete NodeData and ImuData from trimmed submaps in the OptimizationProblem (#364) Related to #283. PAIR=wohe --- cartographer/mapping_2d/sparse_pose_graph.cc | 35 +++++----- .../sparse_pose_graph/optimization_problem.cc | 67 +++++++++++++------ .../sparse_pose_graph/optimization_problem.h | 12 +++- 3 files changed, 76 insertions(+), 38 deletions(-) diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index af6f10c..867fe9c 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -190,9 +190,9 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, .pose.inverse() * optimization_problem_.node_data() .at(node_id.trajectory_id) - .at(node_id.node_index) + .at(node_id.node_index - optimization_problem_.num_trimmed_nodes( + node_id.trajectory_id)) .point_cloud_pose; - constraint_builder_.MaybeAddConstraint( submap_id, submap_data_.at(submap_id).submap.get(), node_id, &trajectory_nodes_.at(node_id).constant_data->range_data_2d.returns, @@ -207,12 +207,16 @@ 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_index = 0; node_index != node_data[trajectory_id].size(); - ++node_index) { + 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); const mapping::NodeId node_id{static_cast(trajectory_id), static_cast(node_index)}; - if (!trajectory_nodes_.at(node_id).trimmed() && - submap_data.node_ids.count(node_id) == 0) { + CHECK(!trajectory_nodes_.at(node_id).trimmed()); + if (submap_data.node_ids.count(node_id) == 0) { ComputeConstraint(node_id, submap_id); } } @@ -241,7 +245,9 @@ void SparsePoseGraph::ComputeConstraintsForScan( optimization_problem_.node_data().size() ? static_cast(optimization_problem_.node_data() .at(matching_id.trajectory_id) - .size()) + .size()) + + optimization_problem_.num_trimmed_nodes( + matching_id.trajectory_id) : 0}; const auto& scan_data = trajectory_nodes_.at(node_id).constant_data; optimization_problem_.AddTrajectoryNode( @@ -386,13 +392,14 @@ 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_index = 0; + int node_data_index = 0; const int num_nodes = trajectory_nodes_.num_indices(trajectory_id); - for (; node_index != static_cast(node_data[trajectory_id].size()); - ++node_index) { + 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}; trajectory_nodes_.at(node_id).pose = transform::Embed3D( - node_data[trajectory_id][node_index].point_cloud_pose); + node_data[trajectory_id][node_data_index].point_cloud_pose); } // Extrapolate all point cloud poses that were added later. const auto local_to_new_global = ComputeLocalToGlobalTransform( @@ -583,15 +590,13 @@ void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed( for (const mapping::NodeId& node_id : nodes_to_remove) { CHECK(!parent_->trajectory_nodes_.at(node_id).trimmed()); parent_->trajectory_nodes_.at(node_id).constant_data.reset(); + parent_->optimization_problem_.TrimTrajectoryNode(node_id); } - // TODO(whess): The optimization problem should no longer include the submap - // and the removed nodes. + // TODO(whess): The optimization problem should no longer include the submap. // TODO(whess): If the first submap is gone, we want to tie the first not // yet trimmed submap to be set fixed to its current pose. - - // TODO(hrapp): Delete related IMU data. } } // namespace mapping_2d diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc index 4bd0d87..51417de 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc @@ -80,6 +80,22 @@ void OptimizationProblem::AddTrajectoryNode( std::max(node_data_.size(), static_cast(trajectory_id) + 1)); node_data_[trajectory_id].push_back( NodeData{time, initial_point_cloud_pose, point_cloud_pose}); + trajectory_data_.resize(std::max(trajectory_data_.size(), node_data_.size())); +} + +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()); + const common::Time node_time = node_data.front().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, @@ -107,7 +123,7 @@ void OptimizationProblem::Solve(const std::vector& constraints) { // Set the starting point. // 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()); for (size_t trajectory_id = 0; trajectory_id != submap_data_.size(); ++trajectory_id) { for (size_t submap_index = 0; @@ -128,12 +144,12 @@ void OptimizationProblem::Solve(const std::vector& constraints) { } for (size_t trajectory_id = 0; trajectory_id != node_data_.size(); ++trajectory_id) { - C_nodes[trajectory_id].resize(node_data_[trajectory_id].size()); - for (size_t node_index = 0; node_index != node_data_[trajectory_id].size(); - ++node_index) { - C_nodes[trajectory_id][node_index] = - FromPose(node_data_[trajectory_id][node_index].point_cloud_pose); - problem.AddParameterBlock(C_nodes[trajectory_id][node_index].data(), 3); + // 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.point_cloud_pose)); + problem.AddParameterBlock(C_nodes[trajectory_id].back().data(), 3); } } @@ -150,27 +166,31 @@ 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) + .at(constraint.node_id.node_index - + trajectory_data_.at(constraint.node_id.trajectory_id) + .num_trimmed_nodes) .data()); } // Add penalties for changes between consecutive scans. for (size_t trajectory_id = 0; trajectory_id != node_data_.size(); ++trajectory_id) { - for (size_t node_index = 1; node_index < node_data_[trajectory_id].size(); - ++node_index) { + for (size_t node_data_index = 1; + node_data_index < node_data_[trajectory_id].size(); + ++node_data_index) { problem.AddResidualBlock( new ceres::AutoDiffCostFunction( new SpaCostFunction(Constraint::Pose{ - transform::Embed3D(node_data_[trajectory_id][node_index - 1] - .initial_point_cloud_pose.inverse() * - node_data_[trajectory_id][node_index] - .initial_point_cloud_pose), + transform::Embed3D( + node_data_[trajectory_id][node_data_index - 1] + .initial_point_cloud_pose.inverse() * + node_data_[trajectory_id][node_data_index] + .initial_point_cloud_pose), options_.consecutive_scan_translation_penalty_factor(), options_.consecutive_scan_rotation_penalty_factor()})), nullptr /* loss function */, - C_nodes[trajectory_id][node_index - 1].data(), - C_nodes[trajectory_id][node_index].data()); + C_nodes[trajectory_id][node_data_index - 1].data(), + C_nodes[trajectory_id][node_data_index].data()); } } @@ -195,15 +215,16 @@ void OptimizationProblem::Solve(const std::vector& constraints) { } for (size_t trajectory_id = 0; trajectory_id != node_data_.size(); ++trajectory_id) { - for (size_t node_index = 0; node_index != node_data_[trajectory_id].size(); - ++node_index) { - node_data_[trajectory_id][node_index].point_cloud_pose = - ToPose(C_nodes[trajectory_id][node_index]); + 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].point_cloud_pose = + ToPose(C_nodes[trajectory_id][node_data_index]); } } } -const std::vector>& OptimizationProblem::node_data() +const std::vector>& OptimizationProblem::node_data() const { return node_data_; } @@ -213,6 +234,10 @@ 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 50ee2f8..06f4c94 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h @@ -63,6 +63,7 @@ class OptimizationProblem { void AddTrajectoryNode(int trajectory_id, common::Time time, const transform::Rigid2d& initial_point_cloud_pose, const transform::Rigid2d& point_cloud_pose); + void TrimTrajectoryNode(const mapping::NodeId& node_id); void AddSubmap(int trajectory_id, const transform::Rigid2d& submap_pose); void SetMaxNumIterations(int32 max_num_iterations); @@ -70,14 +71,21 @@ class OptimizationProblem { // Computes the optimized poses. void Solve(const std::vector& constraints); - 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 num_trimmed_nodes = 0; + }; mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; std::vector> imu_data_; - std::vector> node_data_; + std::vector> node_data_; std::vector> submap_data_; + std::vector trajectory_data_; }; } // namespace sparse_pose_graph