12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Change SubmapState.scan_indices -> node_ids (#286) 

Related to #256.

PAIR=wohe
master
Holger Rapp 2017-05-15 16:31:59 +02:00 committed by GitHub
parent f26e4366c9
commit 8392a7e62c
4 changed files with 27 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -215,7 +215,7 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(
submap_states_.at(submap_id.trajectory_id).at(submap_id.submap_index); submap_states_.at(submap_id.trajectory_id).at(submap_id.submap_index);
const int num_nodes = scan_index_to_node_id_.size(); const int num_nodes = scan_index_to_node_id_.size();
for (int scan_index = 0; scan_index < num_nodes; ++scan_index) { for (int scan_index = 0; scan_index < num_nodes; ++scan_index) {
if (submap_state.scan_indices.count(scan_index) == 0) { if (submap_state.node_ids.count(scan_index_to_node_id_[scan_index]) == 0) {
ComputeConstraint(scan_index, submap_id); ComputeConstraint(scan_index, submap_id);
} }
} }
@ -235,9 +235,10 @@ void SparsePoseGraph::ComputeConstraintsForScan(
.pose * .pose *
sparse_pose_graph::ComputeSubmapPose(*matching_submap).inverse() * pose; sparse_pose_graph::ComputeSubmapPose(*matching_submap).inverse() * pose;
CHECK_EQ(scan_index, scan_index_to_node_id_.size()); CHECK_EQ(scan_index, scan_index_to_node_id_.size());
scan_index_to_node_id_.push_back( const mapping::NodeId node_id{
mapping::NodeId{matching_id.trajectory_id, matching_id.trajectory_id,
num_nodes_in_trajectory_[matching_id.trajectory_id]}); num_nodes_in_trajectory_[matching_id.trajectory_id]};
scan_index_to_node_id_.push_back(node_id);
++num_nodes_in_trajectory_[matching_id.trajectory_id]; ++num_nodes_in_trajectory_[matching_id.trajectory_id];
const mapping::TrajectoryNode::ConstantData* const scan_data = const mapping::TrajectoryNode::ConstantData* const scan_data =
trajectory_nodes_[scan_index].constant_data; trajectory_nodes_[scan_index].constant_data;
@ -252,7 +253,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
.finished); .finished);
submap_states_.at(submap_id.trajectory_id) submap_states_.at(submap_id.trajectory_id)
.at(submap_id.submap_index) .at(submap_id.submap_index)
.scan_indices.emplace(scan_index); .node_ids.emplace(node_id);
// Unchanged covariance as (submap <- map) is a translation. // Unchanged covariance as (submap <- map) is a translation.
const transform::Rigid2d constraint_transform = const transform::Rigid2d constraint_transform =
sparse_pose_graph::ComputeSubmapPose(*submap).inverse() * pose; sparse_pose_graph::ComputeSubmapPose(*submap).inverse() * pose;
@ -260,7 +261,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
constexpr double kFakeOrientationCovariance = 1e-6; constexpr double kFakeOrientationCovariance = 1e-6;
constraints_.push_back(Constraint{ constraints_.push_back(Constraint{
submap_id, submap_id,
scan_index_to_node_id_.at(scan_index), node_id,
{transform::Embed3D(constraint_transform), {transform::Embed3D(constraint_transform),
common::ComputeSpdMatrixSqrtInverse( common::ComputeSpdMatrixSqrtInverse(
kalman_filter::Embed3D(covariance, kFakePositionCovariance, kalman_filter::Embed3D(covariance, kFakePositionCovariance,
@ -278,7 +279,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
if (submap_states_.at(trajectory_id).at(submap_index).finished) { if (submap_states_.at(trajectory_id).at(submap_index).finished) {
CHECK_EQ(submap_states_.at(trajectory_id) CHECK_EQ(submap_states_.at(trajectory_id)
.at(submap_index) .at(submap_index)
.scan_indices.count(scan_index), .node_ids.count(node_id),
0); 0);
ComputeConstraint(scan_index, ComputeConstraint(scan_index,
mapping::SubmapId{static_cast<int>(trajectory_id), mapping::SubmapId{static_cast<int>(trajectory_id),
@ -388,9 +389,10 @@ void SparsePoseGraph::RunOptimization() {
const auto& node_data = optimization_problem_.node_data(); const auto& node_data = optimization_problem_.node_data();
const size_t num_optimized_poses = scan_index_to_node_id_.size(); const size_t num_optimized_poses = scan_index_to_node_id_.size();
for (size_t i = 0; i != num_optimized_poses; ++i) { for (size_t i = 0; i != num_optimized_poses; ++i) {
const mapping::NodeId node_id = scan_index_to_node_id_[i];
trajectory_nodes_[i].pose = trajectory_nodes_[i].pose =
transform::Embed3D(node_data.at(scan_index_to_node_id_[i].trajectory_id) transform::Embed3D(node_data.at(node_id.trajectory_id)
.at(scan_index_to_node_id_[i].node_index) .at(node_id.node_index)
.point_cloud_pose); .point_cloud_pose);
} }
// Extrapolate all point cloud poses that were added later. // Extrapolate all point cloud poses that were added later.

4
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -98,10 +98,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
struct SubmapState { struct SubmapState {
const mapping::Submap* submap = nullptr; const mapping::Submap* submap = nullptr;
// Indices of the scans that were inserted into this map together with // IDs of the scans that were inserted into this map together with
// constraints for them. They are not to be matched again when this submap // constraints for them. They are not to be matched again when this submap
// becomes 'finished'. // becomes 'finished'.
std::set<int> scan_indices; std::set<mapping::NodeId> node_ids;
// Whether in the current state of the background thread this submap is // Whether in the current state of the background thread this submap is
// finished. When this transitions to true, all scans are tried to match // finished. When this transitions to true, all scans are tried to match

23
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -211,7 +211,7 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(const Submap* submap) {
submap_states_.at(submap_id.trajectory_id).at(submap_id.submap_index); submap_states_.at(submap_id.trajectory_id).at(submap_id.submap_index);
const int num_nodes = scan_index_to_node_id_.size(); const int num_nodes = scan_index_to_node_id_.size();
for (int scan_index = 0; scan_index < num_nodes; ++scan_index) { for (int scan_index = 0; scan_index < num_nodes; ++scan_index) {
if (submap_state.scan_indices.count(scan_index) == 0) { if (submap_state.node_ids.count(scan_index_to_node_id_[scan_index]) == 0) {
ComputeConstraint(scan_index, submap_id); ComputeConstraint(scan_index, submap_id);
} }
} }
@ -231,9 +231,10 @@ void SparsePoseGraph::ComputeConstraintsForScan(
.pose * .pose *
matching_submap->local_pose().inverse() * pose; matching_submap->local_pose().inverse() * pose;
CHECK_EQ(scan_index, scan_index_to_node_id_.size()); CHECK_EQ(scan_index, scan_index_to_node_id_.size());
scan_index_to_node_id_.push_back( const mapping::NodeId node_id{
mapping::NodeId{matching_id.trajectory_id, matching_id.trajectory_id,
num_nodes_in_trajectory_[matching_id.trajectory_id]}); num_nodes_in_trajectory_[matching_id.trajectory_id]};
scan_index_to_node_id_.push_back(node_id);
++num_nodes_in_trajectory_[matching_id.trajectory_id]; ++num_nodes_in_trajectory_[matching_id.trajectory_id];
const mapping::TrajectoryNode::ConstantData* const scan_data = const mapping::TrajectoryNode::ConstantData* const scan_data =
trajectory_nodes_[scan_index].constant_data; trajectory_nodes_[scan_index].constant_data;
@ -248,13 +249,13 @@ void SparsePoseGraph::ComputeConstraintsForScan(
.finished); .finished);
submap_states_.at(submap_id.trajectory_id) submap_states_.at(submap_id.trajectory_id)
.at(submap_id.submap_index) .at(submap_id.submap_index)
.scan_indices.emplace(scan_index); .node_ids.emplace(node_id);
// Unchanged covariance as (submap <- map) is a translation. // Unchanged covariance as (submap <- map) is a translation.
const transform::Rigid3d constraint_transform = const transform::Rigid3d constraint_transform =
submap->local_pose().inverse() * pose; submap->local_pose().inverse() * pose;
constraints_.push_back( constraints_.push_back(
Constraint{submap_id, Constraint{submap_id,
scan_index_to_node_id_.at(scan_index), node_id,
{constraint_transform, {constraint_transform,
common::ComputeSpdMatrixSqrtInverse( common::ComputeSpdMatrixSqrtInverse(
covariance, options_.constraint_builder_options() covariance, options_.constraint_builder_options()
@ -270,7 +271,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
if (submap_states_.at(trajectory_id).at(submap_index).finished) { if (submap_states_.at(trajectory_id).at(submap_index).finished) {
CHECK_EQ(submap_states_.at(trajectory_id) CHECK_EQ(submap_states_.at(trajectory_id)
.at(submap_index) .at(submap_index)
.scan_indices.count(scan_index), .node_ids.count(node_id),
0); 0);
ComputeConstraint(scan_index, ComputeConstraint(scan_index,
mapping::SubmapId{static_cast<int>(trajectory_id), mapping::SubmapId{static_cast<int>(trajectory_id),
@ -380,10 +381,10 @@ void SparsePoseGraph::RunOptimization() {
const auto& node_data = optimization_problem_.node_data(); const auto& node_data = optimization_problem_.node_data();
const size_t num_optimized_poses = scan_index_to_node_id_.size(); const size_t num_optimized_poses = scan_index_to_node_id_.size();
for (size_t i = 0; i != num_optimized_poses; ++i) { for (size_t i = 0; i != num_optimized_poses; ++i) {
trajectory_nodes_[i].pose = const mapping::NodeId node_id = scan_index_to_node_id_[i];
node_data.at(scan_index_to_node_id_[i].trajectory_id) trajectory_nodes_[i].pose = node_data.at(node_id.trajectory_id)
.at(scan_index_to_node_id_[i].node_index) .at(node_id.node_index)
.point_cloud_pose; .point_cloud_pose;
} }
// Extrapolate all point cloud poses that were added later. // Extrapolate all point cloud poses that were added later.
std::unordered_map<const mapping::Submaps*, transform::Rigid3d> std::unordered_map<const mapping::Submaps*, transform::Rigid3d>

4
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -100,10 +100,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
struct SubmapState { struct SubmapState {
const Submap* submap = nullptr; const Submap* submap = nullptr;
// Indices of the scans that were inserted into this map together with // IDs of the scans that were inserted into this map together with
// constraints for them. They are not to be matched again when this submap // constraints for them. They are not to be matched again when this submap
// becomes 'finished'. // becomes 'finished'.
std::set<int> scan_indices; std::set<mapping::NodeId> node_ids;
// Whether in the current state of the background thread this submap is // Whether in the current state of the background thread this submap is
// finished. When this transitions to true, all scans are tried to match // finished. When this transitions to true, all scans are tried to match