From 7d43eaa08c9106d9f6ede2027d81baeeb992f8b7 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Mon, 15 May 2017 14:09:45 +0200 Subject: [PATCH] Keep node data per trajectory in the optimization problem. (#282) Related to #256. PAIR=SirVer --- cartographer/mapping_2d/sparse_pose_graph.cc | 55 ++++++++------- .../sparse_pose_graph/optimization_problem.cc | 68 ++++++++----------- .../sparse_pose_graph/optimization_problem.h | 8 +-- cartographer/mapping_3d/sparse_pose_graph.cc | 56 ++++++++------- .../sparse_pose_graph/optimization_problem.cc | 64 ++++++++--------- .../sparse_pose_graph/optimization_problem.h | 6 +- .../optimization_problem_test.cc | 2 +- 7 files changed, 120 insertions(+), 139 deletions(-) diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index f6a9c63..3dfc3b3 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -102,9 +102,6 @@ void SparsePoseGraph::AddScan( const int trajectory_id = trajectory_ids_.at(trajectory); const int flat_scan_index = trajectory_nodes_.size(); CHECK_LT(flat_scan_index, std::numeric_limits::max()); - scan_index_to_node_id_.push_back( - mapping::NodeId{trajectory_id, num_nodes_in_trajectory_[trajectory_id]}); - ++num_nodes_in_trajectory_[trajectory_id]; constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{ time, range_data_in_pose, @@ -167,12 +164,15 @@ void SparsePoseGraph::AddImuData(const mapping::Submaps* trajectory, void SparsePoseGraph::ComputeConstraint(const int scan_index, const mapping::SubmapId& submap_id) { - const transform::Rigid2d relative_pose = - optimization_problem_.submap_data() - .at(submap_id.trajectory_id) - .at(submap_id.submap_index) - .pose.inverse() * - optimization_problem_.node_data().at(scan_index).point_cloud_pose; + const mapping::NodeId node_id = scan_index_to_node_id_.at(scan_index); + const transform::Rigid2d relative_pose = optimization_problem_.submap_data() + .at(submap_id.trajectory_id) + .at(submap_id.submap_index) + .pose.inverse() * + optimization_problem_.node_data() + .at(node_id.trajectory_id) + .at(node_id.node_index) + .point_cloud_pose; const mapping::Submaps* const scan_trajectory = trajectory_nodes_[scan_index].constant_data->trajectory; @@ -186,8 +186,7 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index, submap_states_.at(submap_id.trajectory_id) .at(submap_id.submap_index) .submap, - scan_index_to_node_id_.at(scan_index), scan_index, - &trajectory_connectivity_, + node_id, scan_index, &trajectory_connectivity_, &trajectory_nodes_[scan_index].constant_data->range_data_2d.returns); } else { const bool scan_and_submap_trajectories_connected = @@ -202,7 +201,7 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index, submap_states_.at(submap_id.trajectory_id) .at(submap_id.submap_index) .submap, - scan_index_to_node_id_.at(scan_index), scan_index, + node_id, scan_index, &trajectory_nodes_[scan_index].constant_data->range_data_2d.returns, relative_pose); } @@ -212,14 +211,11 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index, void SparsePoseGraph::ComputeConstraintsForOldScans( const mapping::Submap* submap) { const auto submap_id = GetSubmapId(submap); - const auto& node_data = optimization_problem_.node_data(); - CHECK_GT(node_data.size(), 0); - CHECK_LT(node_data.size(), std::numeric_limits::max()); - const int num_nodes = node_data.size(); + const auto& submap_state = + submap_states_.at(submap_id.trajectory_id).at(submap_id.submap_index); + const int num_nodes = scan_index_to_node_id_.size(); for (int scan_index = 0; scan_index < num_nodes; ++scan_index) { - if (submap_states_.at(submap_id.trajectory_id) - .at(submap_id.submap_index) - .scan_indices.count(scan_index) == 0) { + if (submap_state.scan_indices.count(scan_index) == 0) { ComputeConstraint(scan_index, submap_id); } } @@ -238,12 +234,17 @@ void SparsePoseGraph::ComputeConstraintsForScan( .at(matching_id.submap_index) .pose * sparse_pose_graph::ComputeSubmapPose(*matching_submap).inverse() * pose; - CHECK_EQ(scan_index, optimization_problem_.node_data().size()); + CHECK_EQ(scan_index, scan_index_to_node_id_.size()); + scan_index_to_node_id_.push_back( + mapping::NodeId{matching_id.trajectory_id, + num_nodes_in_trajectory_[matching_id.trajectory_id]}); + ++num_nodes_in_trajectory_[matching_id.trajectory_id]; const mapping::TrajectoryNode::ConstantData* const scan_data = trajectory_nodes_[scan_index].constant_data; + CHECK_EQ(trajectory_ids_.at(scan_data->trajectory), + matching_id.trajectory_id); optimization_problem_.AddTrajectoryNode( - trajectory_ids_.at(scan_data->trajectory), scan_data->time, pose, - optimized_pose); + matching_id.trajectory_id, scan_data->time, pose, optimized_pose); for (const mapping::Submap* submap : insertion_submaps) { const mapping::SubmapId submap_id = GetSubmapId(submap); CHECK(!submap_states_.at(submap_id.trajectory_id) @@ -385,10 +386,12 @@ void SparsePoseGraph::RunOptimization() { common::MutexLocker locker(&mutex_); const auto& node_data = optimization_problem_.node_data(); - const size_t num_optimized_poses = node_data.size(); + const size_t num_optimized_poses = scan_index_to_node_id_.size(); for (size_t i = 0; i != num_optimized_poses; ++i) { trajectory_nodes_[i].pose = - transform::Rigid3d(transform::Embed3D(node_data[i].point_cloud_pose)); + transform::Embed3D(node_data.at(scan_index_to_node_id_[i].trajectory_id) + .at(scan_index_to_node_id_[i].node_index) + .point_cloud_pose); } // Extrapolate all point cloud poses that were added later. std::unordered_map @@ -478,8 +481,8 @@ std::vector SparsePoseGraph::ExtrapolateSubmapTransforms( } else if (result.empty()) { result.push_back(transform::Rigid3d::Identity()); } else { - // Extrapolate to the remaining submaps. Accessing local_pose() in - // Submaps is okay, since the member is const. + // Extrapolate to the remaining submaps. Accessing local_pose() in Submaps + // is okay, since the member is const. result.push_back(result.back() * submap_states_.at(trajectory_id) .at(result.size() - 1) diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc index 0c409dc..24d845c 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc @@ -66,7 +66,7 @@ void OptimizationProblem::AddImuData(const int trajectory_id, const Eigen::Vector3d& angular_velocity) { CHECK_GE(trajectory_id, 0); imu_data_.resize( - std::max(submap_data_.size(), static_cast(trajectory_id) + 1)); + std::max(imu_data_.size(), static_cast(trajectory_id) + 1)); imu_data_[trajectory_id].push_back( mapping_3d::ImuData{time, linear_acceleration, angular_velocity}); } @@ -75,8 +75,11 @@ void OptimizationProblem::AddTrajectoryNode( const int trajectory_id, const common::Time time, const transform::Rigid2d& initial_point_cloud_pose, const transform::Rigid2d& point_cloud_pose) { - node_data_.push_back(NodeData{trajectory_id, time, initial_point_cloud_pose, - point_cloud_pose}); + 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_point_cloud_pose, point_cloud_pose}); } void OptimizationProblem::AddSubmap(const int trajectory_id, @@ -98,20 +101,13 @@ void OptimizationProblem::Solve(const std::vector& constraints) { return; } - // Compute the indices of consecutive nodes for each trajectory. - std::vector> nodes_per_trajectory(submap_data_.size()); - for (size_t j = 0; j != node_data_.size(); ++j) { - nodes_per_trajectory.at(node_data_[j].trajectory_id).push_back(j); - } - ceres::Problem::Options problem_options; ceres::Problem problem(problem_options); // Set the starting point. // TODO(hrapp): Move ceres data into SubmapData. std::vector>> C_submaps(submap_data_.size()); - std::vector>> C_nodes( - nodes_per_trajectory.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; @@ -130,15 +126,13 @@ void OptimizationProblem::Solve(const std::vector& constraints) { } } } - for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size(); + for (size_t trajectory_id = 0; trajectory_id != node_data_.size(); ++trajectory_id) { - C_nodes[trajectory_id].resize(nodes_per_trajectory[trajectory_id].size()); - for (size_t node_index = 0; - node_index != nodes_per_trajectory[trajectory_id].size(); + 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_[nodes_per_trajectory[trajectory_id][node_index]] - .point_cloud_pose); + FromPose(node_data_[trajectory_id][node_index].point_cloud_pose); problem.AddParameterBlock(C_nodes[trajectory_id][node_index].data(), 3); } } @@ -166,28 +160,22 @@ void OptimizationProblem::Solve(const std::vector& constraints) { options_.consecutive_scan_translation_penalty_factor(), options_.consecutive_scan_rotation_penalty_factor()); - for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size(); + for (size_t trajectory_id = 0; trajectory_id != node_data_.size(); ++trajectory_id) { - if (nodes_per_trajectory[trajectory_id].empty()) { - continue; - } - for (size_t node_index = 1; - node_index != nodes_per_trajectory[trajectory_id].size(); + for (size_t node_index = 1; node_index < node_data_[trajectory_id].size(); ++node_index) { constexpr double kUnusedPositionPenalty = 1.; constexpr double kUnusedOrientationPenalty = 1.; problem.AddResidualBlock( - new ceres::AutoDiffCostFunction< - SpaCostFunction, 3, 3, 3>(new SpaCostFunction(Constraint::Pose{ - transform::Embed3D( - node_data_[nodes_per_trajectory[trajectory_id] - [node_index - 1]] - .initial_point_cloud_pose.inverse() * - node_data_[nodes_per_trajectory[trajectory_id][node_index]] - .initial_point_cloud_pose), - kalman_filter::Embed3D(consecutive_pose_change_penalty_matrix, - kUnusedPositionPenalty, - kUnusedOrientationPenalty)})), + 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), + kalman_filter::Embed3D(consecutive_pose_change_penalty_matrix, + kUnusedPositionPenalty, + kUnusedOrientationPenalty)})), nullptr /* loss function */, C_nodes[trajectory_id][node_index - 1].data(), C_nodes[trajectory_id][node_index].data()); @@ -213,18 +201,18 @@ void OptimizationProblem::Solve(const std::vector& constraints) { ToPose(C_submaps[trajectory_id][submap_index]); } } - for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size(); + for (size_t trajectory_id = 0; trajectory_id != node_data_.size(); ++trajectory_id) { - for (size_t node_index = 0; - node_index != nodes_per_trajectory[trajectory_id].size(); + for (size_t node_index = 0; node_index != node_data_[trajectory_id].size(); ++node_index) { - node_data_[nodes_per_trajectory[trajectory_id][node_index]] - .point_cloud_pose = ToPose(C_nodes[trajectory_id][node_index]); + node_data_[trajectory_id][node_index].point_cloud_pose = + ToPose(C_nodes[trajectory_id][node_index]); } } } -const std::vector& OptimizationProblem::node_data() const { +const std::vector>& OptimizationProblem::node_data() + const { return node_data_; } diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h index cca2e2f..50ee2f8 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h @@ -18,6 +18,7 @@ #define CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_ #include +#include #include #include @@ -27,7 +28,6 @@ #include "cartographer/common/time.h" #include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h" -#include "cartographer/mapping_2d/submaps.h" #include "cartographer/mapping_3d/imu_integration.h" namespace cartographer { @@ -35,8 +35,6 @@ namespace mapping_2d { namespace sparse_pose_graph { struct NodeData { - // TODO(whess): Keep nodes per trajectory instead. - int trajectory_id; common::Time time; transform::Rigid2d initial_point_cloud_pose; transform::Rigid2d point_cloud_pose; @@ -72,13 +70,13 @@ 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; private: mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; std::vector> imu_data_; - std::vector node_data_; + std::vector> node_data_; std::vector> submap_data_; }; diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index ec2e33a..b7c37ee 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -100,9 +100,6 @@ void SparsePoseGraph::AddScan( const int trajectory_id = trajectory_ids_.at(trajectory); const int flat_scan_index = trajectory_nodes_.size(); CHECK_LT(flat_scan_index, std::numeric_limits::max()); - scan_index_to_node_id_.push_back( - mapping::NodeId{trajectory_id, num_nodes_in_trajectory_[trajectory_id]}); - ++num_nodes_in_trajectory_[trajectory_id]; constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{ time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}, @@ -167,12 +164,15 @@ void SparsePoseGraph::AddImuData(const mapping::Submaps* trajectory, void SparsePoseGraph::ComputeConstraint(const int scan_index, const mapping::SubmapId& submap_id) { - const transform::Rigid3d relative_pose = - optimization_problem_.submap_data() - .at(submap_id.trajectory_id) - .at(submap_id.submap_index) - .pose.inverse() * - optimization_problem_.node_data().at(scan_index).point_cloud_pose; + const mapping::NodeId node_id = scan_index_to_node_id_.at(scan_index); + const transform::Rigid3d relative_pose = optimization_problem_.submap_data() + .at(submap_id.trajectory_id) + .at(submap_id.submap_index) + .pose.inverse() * + optimization_problem_.node_data() + .at(node_id.trajectory_id) + .at(node_id.node_index) + .point_cloud_pose; const mapping::Submaps* const scan_trajectory = trajectory_nodes_[scan_index].constant_data->trajectory; @@ -186,8 +186,7 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index, submap_states_.at(submap_id.trajectory_id) .at(submap_id.submap_index) .submap, - scan_index_to_node_id_.at(scan_index), scan_index, - &trajectory_connectivity_, trajectory_nodes_); + node_id, scan_index, &trajectory_connectivity_, trajectory_nodes_); } else { const bool scan_and_submap_trajectories_connected = reverse_connected_components_.count(scan_trajectory_id) > 0 && @@ -201,22 +200,18 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index, submap_states_.at(submap_id.trajectory_id) .at(submap_id.submap_index) .submap, - scan_index_to_node_id_.at(scan_index), scan_index, trajectory_nodes_, - relative_pose); + node_id, scan_index, trajectory_nodes_, relative_pose); } } } void SparsePoseGraph::ComputeConstraintsForOldScans(const Submap* submap) { const auto submap_id = GetSubmapId(submap); - const auto& node_data = optimization_problem_.node_data(); - CHECK_GT(node_data.size(), 0); - CHECK_LT(node_data.size(), std::numeric_limits::max()); - const int num_nodes = node_data.size(); + const auto& submap_state = + submap_states_.at(submap_id.trajectory_id).at(submap_id.submap_index); + const int num_nodes = scan_index_to_node_id_.size(); for (int scan_index = 0; scan_index < num_nodes; ++scan_index) { - if (submap_states_.at(submap_id.trajectory_id) - .at(submap_id.submap_index) - .scan_indices.count(scan_index) == 0) { + if (submap_state.scan_indices.count(scan_index) == 0) { ComputeConstraint(scan_index, submap_id); } } @@ -235,12 +230,17 @@ void SparsePoseGraph::ComputeConstraintsForScan( .at(matching_id.submap_index) .pose * matching_submap->local_pose().inverse() * pose; - CHECK_EQ(scan_index, optimization_problem_.node_data().size()); + CHECK_EQ(scan_index, scan_index_to_node_id_.size()); + scan_index_to_node_id_.push_back( + mapping::NodeId{matching_id.trajectory_id, + num_nodes_in_trajectory_[matching_id.trajectory_id]}); + ++num_nodes_in_trajectory_[matching_id.trajectory_id]; const mapping::TrajectoryNode::ConstantData* const scan_data = trajectory_nodes_[scan_index].constant_data; - optimization_problem_.AddTrajectoryNode( - trajectory_ids_.at(scan_data->trajectory), scan_data->time, - optimized_pose); + CHECK_EQ(trajectory_ids_.at(scan_data->trajectory), + matching_id.trajectory_id); + optimization_problem_.AddTrajectoryNode(matching_id.trajectory_id, + scan_data->time, optimized_pose); for (const Submap* submap : insertion_submaps) { const mapping::SubmapId submap_id = GetSubmapId(submap); CHECK(!submap_states_.at(submap_id.trajectory_id) @@ -378,9 +378,12 @@ void SparsePoseGraph::RunOptimization() { common::MutexLocker locker(&mutex_); const auto& node_data = optimization_problem_.node_data(); - const size_t num_optimized_poses = node_data.size(); + const size_t num_optimized_poses = scan_index_to_node_id_.size(); for (size_t i = 0; i != num_optimized_poses; ++i) { - trajectory_nodes_[i].pose = node_data[i].point_cloud_pose; + trajectory_nodes_[i].pose = + node_data.at(scan_index_to_node_id_[i].trajectory_id) + .at(scan_index_to_node_id_[i].node_index) + .point_cloud_pose; } // Extrapolate all point cloud poses that were added later. std::unordered_map @@ -459,6 +462,7 @@ std::vector SparsePoseGraph::ExtrapolateSubmapTransforms( return {transform::Rigid3d::Identity()}; } + // Submaps for which we have optimized poses. std::vector result; for (const auto& state : submap_states_.at(trajectory_id)) { if (trajectory_id < submap_transforms.size() && diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc index cd970c8..6e1a30e 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc @@ -84,7 +84,7 @@ void OptimizationProblem::AddImuData(const int trajectory_id, const Eigen::Vector3d& angular_velocity) { CHECK_GE(trajectory_id, 0); imu_data_.resize( - std::max(submap_data_.size(), static_cast(trajectory_id) + 1)); + std::max(imu_data_.size(), static_cast(trajectory_id) + 1)); imu_data_[trajectory_id].push_back( ImuData{time, linear_acceleration, angular_velocity}); } @@ -92,7 +92,10 @@ void OptimizationProblem::AddImuData(const int trajectory_id, void OptimizationProblem::AddTrajectoryNode( const int trajectory_id, const common::Time time, const transform::Rigid3d& point_cloud_pose) { - node_data_.push_back(NodeData{trajectory_id, time, point_cloud_pose}); + 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, point_cloud_pose}); } void OptimizationProblem::AddSubmap(const int trajectory_id, @@ -114,12 +117,6 @@ void OptimizationProblem::Solve(const std::vector& constraints) { return; } - // Compute the indices of consecutive nodes for each trajectory. - std::vector> nodes_per_trajectory(submap_data_.size()); - for (size_t j = 0; j != node_data_.size(); ++j) { - nodes_per_trajectory.at(node_data_[j].trajectory_id).push_back(j); - } - ceres::Problem::Options problem_options; ceres::Problem problem(problem_options); @@ -136,7 +133,7 @@ void OptimizationProblem::Solve(const std::vector& constraints) { CHECK(!submap_data_[0].empty()); // TODO(hrapp): Move ceres data into SubmapData. std::vector> C_submaps(submap_data_.size()); - std::vector> C_nodes(nodes_per_trajectory.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; @@ -158,14 +155,12 @@ void OptimizationProblem::Solve(const std::vector& constraints) { } } } - for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size(); + for (size_t trajectory_id = 0; trajectory_id != node_data_.size(); ++trajectory_id) { - for (size_t node_index = 0; - node_index != nodes_per_trajectory[trajectory_id].size(); + for (size_t node_index = 0; node_index != node_data_[trajectory_id].size(); ++node_index) { C_nodes[trajectory_id].emplace_back( - node_data_[nodes_per_trajectory[trajectory_id][node_index]] - .point_cloud_pose, + node_data_[trajectory_id][node_index].point_cloud_pose, translation_parameterization(), common::make_unique(), &problem); } @@ -196,34 +191,29 @@ void OptimizationProblem::Solve(const std::vector& constraints) { // Add constraints based on IMU observations of angular velocities and // linear acceleration. - for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size(); + for (size_t trajectory_id = 0; trajectory_id != node_data_.size(); ++trajectory_id) { - const std::vector& flat_indices = - nodes_per_trajectory[trajectory_id]; const std::deque& imu_data = imu_data_.at(trajectory_id); - CHECK(!flat_indices.empty()); CHECK(!imu_data.empty()); + // TODO(whess): Add support for empty trajectories. + const auto& node_data = node_data_[trajectory_id]; + CHECK(!node_data.empty()); // Skip IMU data before the first node of this trajectory. auto it = imu_data.cbegin(); - while ((it + 1) != imu_data.cend() && - (it + 1)->time <= node_data_[flat_indices[0]].time) { + while ((it + 1) != imu_data.cend() && (it + 1)->time <= node_data[0].time) { ++it; } - for (size_t node_index = 1; node_index < flat_indices.size(); - ++node_index) { + for (size_t node_index = 1; node_index < node_data.size(); ++node_index) { auto it2 = it; const IntegrateImuResult result = - IntegrateImu(imu_data, node_data_[flat_indices[node_index - 1]].time, - node_data_[flat_indices[node_index]].time, &it); - if (node_index + 1 < flat_indices.size()) { - const common::Time first_time = - node_data_[flat_indices[node_index - 1]].time; - const common::Time second_time = - node_data_[flat_indices[node_index]].time; - const common::Time third_time = - node_data_[flat_indices[node_index + 1]].time; + IntegrateImu(imu_data, node_data[node_index - 1].time, + node_data[node_index].time, &it); + if (node_index + 1 < node_data.size()) { + const common::Time first_time = node_data[node_index - 1].time; + const common::Time second_time = node_data[node_index].time; + const common::Time third_time = node_data[node_index + 1].time; const common::Duration first_duration = second_time - first_time; const common::Duration second_duration = third_time - second_time; const common::Time first_center = first_time + first_duration / 2; @@ -283,18 +273,18 @@ void OptimizationProblem::Solve(const std::vector& constraints) { C_submaps[trajectory_id][submap_index].ToRigid(); } } - for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size(); + for (size_t trajectory_id = 0; trajectory_id != node_data_.size(); ++trajectory_id) { - for (size_t node_index = 0; - node_index != nodes_per_trajectory[trajectory_id].size(); + for (size_t node_index = 0; node_index != node_data_[trajectory_id].size(); ++node_index) { - node_data_[nodes_per_trajectory[trajectory_id][node_index]] - .point_cloud_pose = C_nodes[trajectory_id][node_index].ToRigid(); + node_data_[trajectory_id][node_index].point_cloud_pose = + C_nodes[trajectory_id][node_index].ToRigid(); } } } -const std::vector& OptimizationProblem::node_data() const { +const std::vector>& OptimizationProblem::node_data() + const { return node_data_; } diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h index 8f316ea..25e0930 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h @@ -35,8 +35,6 @@ namespace mapping_3d { namespace sparse_pose_graph { struct NodeData { - // TODO(whess): Keep nodes per trajectory instead. - int trajectory_id; common::Time time; transform::Rigid3d point_cloud_pose; }; @@ -73,14 +71,14 @@ 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; private: mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; FixZ fix_z_; std::vector> imu_data_; - std::vector node_data_; + std::vector> node_data_; std::vector> submap_data_; double gravity_constant_ = 9.8; }; diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc index 1dc681b..3bc8ad6 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc @@ -155,7 +155,7 @@ TEST_F(OptimizationProblemTest, ReducesNoise) { double translation_error_before = 0.; double rotation_error_before = 0.; - const auto& node_data = optimization_problem_.node_data(); + const auto& node_data = optimization_problem_.node_data().at(0); for (int j = 0; j != kNumNodes; ++j) { translation_error_before += (test_data[j].ground_truth_pose.translation() - node_data[j].point_cloud_pose.translation())