Remove flat indices. (#299)
This cleans up scan_index_to_node_id_, num_nodes_in_trajectory_, scan_index. Related to #256.master
parent
34b750977b
commit
0bba56428f
|
@ -97,9 +97,6 @@ void SparsePoseGraph::AddScan(
|
||||||
GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose));
|
GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose));
|
||||||
|
|
||||||
common::MutexLocker locker(&mutex_);
|
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{
|
constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{
|
||||||
time, range_data_in_pose,
|
time, range_data_in_pose,
|
||||||
Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}),
|
Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}),
|
||||||
|
@ -136,9 +133,8 @@ void SparsePoseGraph::AddScan(
|
||||||
}
|
}
|
||||||
|
|
||||||
AddWorkItem([=]() REQUIRES(mutex_) {
|
AddWorkItem([=]() REQUIRES(mutex_) {
|
||||||
ComputeConstraintsForScan(flat_scan_index, matching_submap,
|
ComputeConstraintsForScan(matching_submap, insertion_submaps,
|
||||||
insertion_submaps, finished_submap, pose,
|
finished_submap, pose, covariance);
|
||||||
covariance);
|
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -212,16 +208,23 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(
|
||||||
const auto submap_id = GetSubmapId(submap);
|
const auto submap_id = GetSubmapId(submap);
|
||||||
const auto& submap_state =
|
const auto& submap_state =
|
||||||
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();
|
|
||||||
for (int scan_index = 0; scan_index < num_nodes; ++scan_index) {
|
const auto& node_data = optimization_problem_.node_data();
|
||||||
if (submap_state.node_ids.count(scan_index_to_node_id_[scan_index]) == 0) {
|
for (size_t trajectory_id = 0; trajectory_id != node_data.size();
|
||||||
ComputeConstraint(scan_index_to_node_id_.at(scan_index), submap_id);
|
++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(
|
void SparsePoseGraph::ComputeConstraintsForScan(
|
||||||
const int scan_index, const mapping::Submap* matching_submap,
|
const mapping::Submap* matching_submap,
|
||||||
std::vector<const mapping::Submap*> insertion_submaps,
|
std::vector<const mapping::Submap*> insertion_submaps,
|
||||||
const mapping::Submap* finished_submap, const transform::Rigid2d& pose,
|
const mapping::Submap* finished_submap, const transform::Rigid2d& pose,
|
||||||
const kalman_filter::Pose2DCovariance& covariance) {
|
const kalman_filter::Pose2DCovariance& covariance) {
|
||||||
|
@ -233,12 +236,14 @@ void SparsePoseGraph::ComputeConstraintsForScan(
|
||||||
.at(matching_id.submap_index)
|
.at(matching_id.submap_index)
|
||||||
.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());
|
|
||||||
const mapping::NodeId node_id{
|
const mapping::NodeId node_id{
|
||||||
matching_id.trajectory_id,
|
matching_id.trajectory_id,
|
||||||
num_nodes_in_trajectory_[matching_id.trajectory_id]};
|
static_cast<size_t>(matching_id.trajectory_id) <
|
||||||
scan_index_to_node_id_.push_back(node_id);
|
optimization_problem_.node_data().size()
|
||||||
++num_nodes_in_trajectory_[matching_id.trajectory_id];
|
? static_cast<int>(optimization_problem_.node_data()
|
||||||
|
.at(matching_id.trajectory_id)
|
||||||
|
.size())
|
||||||
|
: 0};
|
||||||
const mapping::TrajectoryNode::ConstantData* const scan_data =
|
const mapping::TrajectoryNode::ConstantData* const scan_data =
|
||||||
trajectory_nodes_.at(node_id.trajectory_id)
|
trajectory_nodes_.at(node_id.trajectory_id)
|
||||||
.at(node_id.node_index)
|
.at(node_id.node_index)
|
||||||
|
|
|
@ -123,7 +123,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
||||||
|
|
||||||
// Adds constraints for a scan, and starts scan matching in the background.
|
// Adds constraints for a scan, and starts scan matching in the background.
|
||||||
void ComputeConstraintsForScan(
|
void ComputeConstraintsForScan(
|
||||||
int scan_index, const mapping::Submap* matching_submap,
|
const mapping::Submap* matching_submap,
|
||||||
std::vector<const mapping::Submap*> insertion_submaps,
|
std::vector<const mapping::Submap*> insertion_submaps,
|
||||||
const mapping::Submap* finished_submap, const transform::Rigid2d& pose,
|
const mapping::Submap* finished_submap, const transform::Rigid2d& pose,
|
||||||
const kalman_filter::Pose2DCovariance& covariance) REQUIRES(mutex_);
|
const kalman_filter::Pose2DCovariance& covariance) REQUIRES(mutex_);
|
||||||
|
@ -186,11 +186,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
||||||
GUARDED_BY(mutex_);
|
GUARDED_BY(mutex_);
|
||||||
std::vector<std::vector<SubmapState>> submap_states_ 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.
|
// Connectivity structure of our trajectories by IDs.
|
||||||
std::vector<std::vector<int>> connected_components_;
|
std::vector<std::vector<int>> connected_components_;
|
||||||
// Trajectory ID to connected component ID.
|
// Trajectory ID to connected component ID.
|
||||||
|
|
|
@ -96,9 +96,6 @@ void SparsePoseGraph::AddScan(
|
||||||
GetLocalToGlobalTransform(trajectory_id) * pose);
|
GetLocalToGlobalTransform(trajectory_id) * pose);
|
||||||
|
|
||||||
common::MutexLocker locker(&mutex_);
|
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{
|
constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{
|
||||||
time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}},
|
time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}},
|
||||||
sensor::Compress(range_data_in_tracking), trajectory_id,
|
sensor::Compress(range_data_in_tracking), trajectory_id,
|
||||||
|
@ -132,9 +129,8 @@ void SparsePoseGraph::AddScan(
|
||||||
}
|
}
|
||||||
|
|
||||||
AddWorkItem([=]() REQUIRES(mutex_) {
|
AddWorkItem([=]() REQUIRES(mutex_) {
|
||||||
ComputeConstraintsForScan(flat_scan_index, matching_submap,
|
ComputeConstraintsForScan(matching_submap, insertion_submaps,
|
||||||
insertion_submaps, finished_submap, pose,
|
finished_submap, pose, covariance);
|
||||||
covariance);
|
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -236,18 +232,24 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(const Submap* submap) {
|
||||||
const auto submap_id = GetSubmapId(submap);
|
const auto submap_id = GetSubmapId(submap);
|
||||||
const auto& submap_state =
|
const auto& submap_state =
|
||||||
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();
|
|
||||||
for (int scan_index = 0; scan_index < num_nodes; ++scan_index) {
|
const auto& node_data = optimization_problem_.node_data();
|
||||||
if (submap_state.node_ids.count(scan_index_to_node_id_[scan_index]) == 0) {
|
for (size_t trajectory_id = 0; trajectory_id != node_data.size();
|
||||||
ComputeConstraint(scan_index_to_node_id_.at(scan_index), submap_id);
|
++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(
|
void SparsePoseGraph::ComputeConstraintsForScan(
|
||||||
const int scan_index, const Submap* matching_submap,
|
const Submap* matching_submap, std::vector<const Submap*> insertion_submaps,
|
||||||
std::vector<const Submap*> insertion_submaps, const Submap* finished_submap,
|
const Submap* finished_submap, const transform::Rigid3d& pose,
|
||||||
const transform::Rigid3d& pose,
|
|
||||||
const kalman_filter::PoseCovariance& covariance) {
|
const kalman_filter::PoseCovariance& covariance) {
|
||||||
GrowSubmapTransformsAsNeeded(insertion_submaps);
|
GrowSubmapTransformsAsNeeded(insertion_submaps);
|
||||||
const mapping::SubmapId matching_id = GetSubmapId(matching_submap);
|
const mapping::SubmapId matching_id = GetSubmapId(matching_submap);
|
||||||
|
@ -257,12 +259,14 @@ void SparsePoseGraph::ComputeConstraintsForScan(
|
||||||
.at(matching_id.submap_index)
|
.at(matching_id.submap_index)
|
||||||
.pose *
|
.pose *
|
||||||
matching_submap->local_pose.inverse() * pose;
|
matching_submap->local_pose.inverse() * pose;
|
||||||
CHECK_EQ(scan_index, scan_index_to_node_id_.size());
|
|
||||||
const mapping::NodeId node_id{
|
const mapping::NodeId node_id{
|
||||||
matching_id.trajectory_id,
|
matching_id.trajectory_id,
|
||||||
num_nodes_in_trajectory_[matching_id.trajectory_id]};
|
static_cast<size_t>(matching_id.trajectory_id) <
|
||||||
scan_index_to_node_id_.push_back(node_id);
|
optimization_problem_.node_data().size()
|
||||||
++num_nodes_in_trajectory_[matching_id.trajectory_id];
|
? static_cast<int>(optimization_problem_.node_data()
|
||||||
|
.at(matching_id.trajectory_id)
|
||||||
|
.size())
|
||||||
|
: 0};
|
||||||
const mapping::TrajectoryNode::ConstantData* const scan_data =
|
const mapping::TrajectoryNode::ConstantData* const scan_data =
|
||||||
trajectory_nodes_.at(node_id.trajectory_id)
|
trajectory_nodes_.at(node_id.trajectory_id)
|
||||||
.at(node_id.node_index)
|
.at(node_id.node_index)
|
||||||
|
|
|
@ -121,7 +121,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
||||||
|
|
||||||
// Adds constraints for a scan, and starts scan matching in the background.
|
// Adds constraints for a scan, and starts scan matching in the background.
|
||||||
void ComputeConstraintsForScan(
|
void ComputeConstraintsForScan(
|
||||||
int scan_index, const Submap* matching_submap,
|
const Submap* matching_submap,
|
||||||
std::vector<const Submap*> insertion_submaps,
|
std::vector<const Submap*> insertion_submaps,
|
||||||
const Submap* finished_submap, const transform::Rigid3d& pose,
|
const Submap* finished_submap, const transform::Rigid3d& pose,
|
||||||
const kalman_filter::PoseCovariance& covariance) REQUIRES(mutex_);
|
const kalman_filter::PoseCovariance& covariance) REQUIRES(mutex_);
|
||||||
|
@ -183,11 +183,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
||||||
GUARDED_BY(mutex_);
|
GUARDED_BY(mutex_);
|
||||||
std::vector<std::vector<SubmapState>> submap_states_ 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.
|
// Connectivity structure of our trajectories by IDs.
|
||||||
std::vector<std::vector<int>> connected_components_;
|
std::vector<std::vector<int>> connected_components_;
|
||||||
// Trajectory ID to connected component ID.
|
// Trajectory ID to connected component ID.
|
||||||
|
|
Loading…
Reference in New Issue