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

Remove flat indices. (#299) 

This cleans up scan_index_to_node_id_, num_nodes_in_trajectory_,
scan_index. Related to #256.
master
Wolfgang Hess 2017-05-19 14:01:59 +02:00 committed by GitHub
parent 34b750977b
commit 0bba56428f
4 changed files with 43 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -97,9 +97,6 @@ void SparsePoseGraph::AddScan(
GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose));
common::MutexLocker locker(&mutex_);
const int flat_scan_index = num_trajectory_nodes_;
CHECK_LT(flat_scan_index, std::numeric_limits<int>::max());
constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{
time, range_data_in_pose,
Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}),
@ -136,9 +133,8 @@ void SparsePoseGraph::AddScan(
}
AddWorkItem([=]() REQUIRES(mutex_) {
ComputeConstraintsForScan(flat_scan_index, matching_submap,
insertion_submaps, finished_submap, pose,
covariance);
ComputeConstraintsForScan(matching_submap, insertion_submaps,
finished_submap, pose, covariance);
});
}
@ -212,16 +208,23 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(
const auto submap_id = GetSubmapId(submap);
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_state.node_ids.count(scan_index_to_node_id_[scan_index]) == 0) {
ComputeConstraint(scan_index_to_node_id_.at(scan_index), submap_id);
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) {
const mapping::NodeId node_id{static_cast<int>(trajectory_id),
static_cast<int>(node_index)};
if (submap_state.node_ids.count(node_id) == 0) {
ComputeConstraint(node_id, submap_id);
}
}
}
}
void SparsePoseGraph::ComputeConstraintsForScan(
const int scan_index, const mapping::Submap* matching_submap,
const mapping::Submap* matching_submap,
std::vector<const mapping::Submap*> insertion_submaps,
const mapping::Submap* finished_submap, const transform::Rigid2d& pose,
const kalman_filter::Pose2DCovariance& covariance) {
@ -233,12 +236,14 @@ void SparsePoseGraph::ComputeConstraintsForScan(
.at(matching_id.submap_index)
.pose *
sparse_pose_graph::ComputeSubmapPose(*matching_submap).inverse() * pose;
CHECK_EQ(scan_index, scan_index_to_node_id_.size());
const mapping::NodeId node_id{
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];
static_cast<size_t>(matching_id.trajectory_id) <
optimization_problem_.node_data().size()
? static_cast<int>(optimization_problem_.node_data()
.at(matching_id.trajectory_id)
.size())
: 0};
const mapping::TrajectoryNode::ConstantData* const scan_data =
trajectory_nodes_.at(node_id.trajectory_id)
.at(node_id.node_index)

7
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -123,7 +123,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Adds constraints for a scan, and starts scan matching in the background.
void ComputeConstraintsForScan(
int scan_index, const mapping::Submap* matching_submap,
const mapping::Submap* matching_submap,
std::vector<const mapping::Submap*> insertion_submaps,
const mapping::Submap* finished_submap, const transform::Rigid2d& pose,
const kalman_filter::Pose2DCovariance& covariance) REQUIRES(mutex_);
@ -186,11 +186,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
GUARDED_BY(mutex_);
std::vector<std::vector<SubmapState>> submap_states_ GUARDED_BY(mutex_);
// Mapping to flat indices to aid the transition to per-trajectory data
// structures.
std::map<int, int> num_nodes_in_trajectory_ GUARDED_BY(mutex_);
std::vector<mapping::NodeId> scan_index_to_node_id_ GUARDED_BY(mutex_);
// Connectivity structure of our trajectories by IDs.
std::vector<std::vector<int>> connected_components_;
// Trajectory ID to connected component ID.

38
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -96,9 +96,6 @@ void SparsePoseGraph::AddScan(
GetLocalToGlobalTransform(trajectory_id) * pose);
common::MutexLocker locker(&mutex_);
const int flat_scan_index = num_trajectory_nodes_;
CHECK_LT(flat_scan_index, std::numeric_limits<int>::max());
constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{
time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}},
sensor::Compress(range_data_in_tracking), trajectory_id,
@ -132,9 +129,8 @@ void SparsePoseGraph::AddScan(
}
AddWorkItem([=]() REQUIRES(mutex_) {
ComputeConstraintsForScan(flat_scan_index, matching_submap,
insertion_submaps, finished_submap, pose,
covariance);
ComputeConstraintsForScan(matching_submap, insertion_submaps,
finished_submap, pose, covariance);
});
}
@ -236,18 +232,24 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(const Submap* submap) {
const auto submap_id = GetSubmapId(submap);
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_state.node_ids.count(scan_index_to_node_id_[scan_index]) == 0) {
ComputeConstraint(scan_index_to_node_id_.at(scan_index), submap_id);
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) {
const mapping::NodeId node_id{static_cast<int>(trajectory_id),
static_cast<int>(node_index)};
if (submap_state.node_ids.count(node_id) == 0) {
ComputeConstraint(node_id, submap_id);
}
}
}
}
void SparsePoseGraph::ComputeConstraintsForScan(
const int scan_index, const Submap* matching_submap,
std::vector<const Submap*> insertion_submaps, const Submap* finished_submap,
const transform::Rigid3d& pose,
const Submap* matching_submap, std::vector<const Submap*> insertion_submaps,
const Submap* finished_submap, const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance) {
GrowSubmapTransformsAsNeeded(insertion_submaps);
const mapping::SubmapId matching_id = GetSubmapId(matching_submap);
@ -257,12 +259,14 @@ void SparsePoseGraph::ComputeConstraintsForScan(
.at(matching_id.submap_index)
.pose *
matching_submap->local_pose.inverse() * pose;
CHECK_EQ(scan_index, scan_index_to_node_id_.size());
const mapping::NodeId node_id{
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];
static_cast<size_t>(matching_id.trajectory_id) <
optimization_problem_.node_data().size()
? static_cast<int>(optimization_problem_.node_data()
.at(matching_id.trajectory_id)
.size())
: 0};
const mapping::TrajectoryNode::ConstantData* const scan_data =
trajectory_nodes_.at(node_id.trajectory_id)
.at(node_id.node_index)

7
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -121,7 +121,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Adds constraints for a scan, and starts scan matching in the background.
void ComputeConstraintsForScan(
int scan_index, const Submap* matching_submap,
const Submap* matching_submap,
std::vector<const Submap*> insertion_submaps,
const Submap* finished_submap, const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance) REQUIRES(mutex_);
@ -183,11 +183,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
GUARDED_BY(mutex_);
std::vector<std::vector<SubmapState>> submap_states_ GUARDED_BY(mutex_);
// Mapping to flat indices to aid the transition to per-trajectory data
// structures.
std::map<int, int> num_nodes_in_trajectory_ GUARDED_BY(mutex_);
std::vector<mapping::NodeId> scan_index_to_node_id_ GUARDED_BY(mutex_);
// Connectivity structure of our trajectories by IDs.
std::vector<std::vector<int>> connected_components_;
// Trajectory ID to connected component ID.