Adds a NestedVectorsById<> class template. (#300)
parent
0bba56428f
commit
2d416589a7
|
@ -17,8 +17,10 @@
|
||||||
#ifndef CARTOGRAPHER_MAPPING_ID_H_
|
#ifndef CARTOGRAPHER_MAPPING_ID_H_
|
||||||
#define CARTOGRAPHER_MAPPING_ID_H_
|
#define CARTOGRAPHER_MAPPING_ID_H_
|
||||||
|
|
||||||
|
#include <algorithm>
|
||||||
#include <ostream>
|
#include <ostream>
|
||||||
#include <tuple>
|
#include <tuple>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
namespace mapping {
|
namespace mapping {
|
||||||
|
@ -55,6 +57,40 @@ inline std::ostream& operator<<(std::ostream& os, const SubmapId& v) {
|
||||||
return os << "(" << v.trajectory_id << ", " << v.submap_index << ")";
|
return os << "(" << v.trajectory_id << ", " << v.submap_index << ")";
|
||||||
}
|
}
|
||||||
|
|
||||||
|
template <typename ValueType, typename IdType>
|
||||||
|
class NestedVectorsById {
|
||||||
|
public:
|
||||||
|
// Appends data to a trajectory, creating trajectories as needed.
|
||||||
|
IdType Append(int trajectory_id, const ValueType& value) {
|
||||||
|
data_.resize(std::max<size_t>(data_.size(), trajectory_id + 1));
|
||||||
|
const IdType id{trajectory_id,
|
||||||
|
static_cast<int>(data_[trajectory_id].size())};
|
||||||
|
data_[trajectory_id].push_back(value);
|
||||||
|
return id;
|
||||||
|
}
|
||||||
|
|
||||||
|
const ValueType& at(const IdType& id) const {
|
||||||
|
return data_.at(id.trajectory_id).at(GetIndex(id));
|
||||||
|
}
|
||||||
|
ValueType& at(const IdType& id) {
|
||||||
|
return data_.at(id.trajectory_id).at(GetIndex(id));
|
||||||
|
}
|
||||||
|
|
||||||
|
int num_trajectories() const { return static_cast<int>(data_.size()); }
|
||||||
|
int num_indices(int trajectory_id) const {
|
||||||
|
return static_cast<int>(data_.at(trajectory_id).size());
|
||||||
|
}
|
||||||
|
|
||||||
|
// TODO(whess): Remove once no longer needed.
|
||||||
|
const std::vector<std::vector<ValueType>> data() const { return data_; }
|
||||||
|
|
||||||
|
private:
|
||||||
|
static int GetIndex(const NodeId& id) { return id.node_index; }
|
||||||
|
static int GetIndex(const SubmapId& id) { return id.submap_index; }
|
||||||
|
|
||||||
|
std::vector<std::vector<ValueType>> data_;
|
||||||
|
};
|
||||||
|
|
||||||
} // namespace mapping
|
} // namespace mapping
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
||||||
|
|
||||||
|
|
|
@ -101,24 +101,18 @@ void SparsePoseGraph::AddScan(
|
||||||
time, range_data_in_pose,
|
time, range_data_in_pose,
|
||||||
Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}),
|
Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}),
|
||||||
trajectory_id, tracking_to_pose});
|
trajectory_id, tracking_to_pose});
|
||||||
trajectory_nodes_.resize(
|
trajectory_nodes_.Append(trajectory_id,
|
||||||
std::max<size_t>(trajectory_nodes_.size(), trajectory_id + 1));
|
mapping::TrajectoryNode{
|
||||||
trajectory_nodes_[trajectory_id].push_back(mapping::TrajectoryNode{
|
&constant_node_data_.back(), optimized_pose,
|
||||||
&constant_node_data_.back(), optimized_pose,
|
});
|
||||||
});
|
|
||||||
++num_trajectory_nodes_;
|
++num_trajectory_nodes_;
|
||||||
trajectory_connectivity_.Add(trajectory_id);
|
trajectory_connectivity_.Add(trajectory_id);
|
||||||
|
|
||||||
if (submap_ids_.count(insertion_submaps.back()) == 0) {
|
if (submap_ids_.count(insertion_submaps.back()) == 0) {
|
||||||
submap_states_.resize(
|
const mapping::SubmapId submap_id =
|
||||||
std::max<size_t>(submap_states_.size(), trajectory_id + 1));
|
submap_states_.Append(trajectory_id, SubmapState());
|
||||||
auto& trajectory_submap_states = submap_states_.at(trajectory_id);
|
submap_ids_.emplace(insertion_submaps.back(), submap_id);
|
||||||
submap_ids_.emplace(
|
submap_states_.at(submap_id).submap = insertion_submaps.back();
|
||||||
insertion_submaps.back(),
|
|
||||||
mapping::SubmapId{trajectory_id,
|
|
||||||
static_cast<int>(trajectory_submap_states.size())});
|
|
||||||
trajectory_submap_states.emplace_back();
|
|
||||||
trajectory_submap_states.back().submap = insertion_submaps.back();
|
|
||||||
}
|
}
|
||||||
const mapping::Submap* const finished_submap =
|
const mapping::Submap* const finished_submap =
|
||||||
insertion_submaps.front()->finished_probability_grid != nullptr
|
insertion_submaps.front()->finished_probability_grid != nullptr
|
||||||
|
@ -162,14 +156,8 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
|
||||||
if (node_id.trajectory_id != submap_id.trajectory_id &&
|
if (node_id.trajectory_id != submap_id.trajectory_id &&
|
||||||
global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
|
global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
|
||||||
constraint_builder_.MaybeAddGlobalConstraint(
|
constraint_builder_.MaybeAddGlobalConstraint(
|
||||||
submap_id,
|
submap_id, submap_states_.at(submap_id).submap, node_id,
|
||||||
submap_states_.at(submap_id.trajectory_id)
|
&trajectory_nodes_.at(node_id).constant_data->range_data_2d.returns,
|
||||||
.at(submap_id.submap_index)
|
|
||||||
.submap,
|
|
||||||
node_id,
|
|
||||||
&trajectory_nodes_.at(node_id.trajectory_id)
|
|
||||||
.at(node_id.node_index)
|
|
||||||
.constant_data->range_data_2d.returns,
|
|
||||||
&trajectory_connectivity_);
|
&trajectory_connectivity_);
|
||||||
} else {
|
} else {
|
||||||
const bool scan_and_submap_trajectories_connected =
|
const bool scan_and_submap_trajectories_connected =
|
||||||
|
@ -190,14 +178,8 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
|
||||||
.point_cloud_pose;
|
.point_cloud_pose;
|
||||||
|
|
||||||
constraint_builder_.MaybeAddConstraint(
|
constraint_builder_.MaybeAddConstraint(
|
||||||
submap_id,
|
submap_id, submap_states_.at(submap_id).submap, node_id,
|
||||||
submap_states_.at(submap_id.trajectory_id)
|
&trajectory_nodes_.at(node_id).constant_data->range_data_2d.returns,
|
||||||
.at(submap_id.submap_index)
|
|
||||||
.submap,
|
|
||||||
node_id,
|
|
||||||
&trajectory_nodes_.at(node_id.trajectory_id)
|
|
||||||
.at(node_id.node_index)
|
|
||||||
.constant_data->range_data_2d.returns,
|
|
||||||
initial_relative_pose);
|
initial_relative_pose);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -206,8 +188,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
|
||||||
void SparsePoseGraph::ComputeConstraintsForOldScans(
|
void SparsePoseGraph::ComputeConstraintsForOldScans(
|
||||||
const mapping::Submap* submap) {
|
const mapping::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);
|
||||||
submap_states_.at(submap_id.trajectory_id).at(submap_id.submap_index);
|
|
||||||
|
|
||||||
const auto& node_data = optimization_problem_.node_data();
|
const auto& node_data = optimization_problem_.node_data();
|
||||||
for (size_t trajectory_id = 0; trajectory_id != node_data.size();
|
for (size_t trajectory_id = 0; trajectory_id != node_data.size();
|
||||||
|
@ -245,20 +226,14 @@ void SparsePoseGraph::ComputeConstraintsForScan(
|
||||||
.size())
|
.size())
|
||||||
: 0};
|
: 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).constant_data;
|
||||||
.at(node_id.node_index)
|
|
||||||
.constant_data;
|
|
||||||
CHECK_EQ(scan_data->trajectory_id, matching_id.trajectory_id);
|
CHECK_EQ(scan_data->trajectory_id, matching_id.trajectory_id);
|
||||||
optimization_problem_.AddTrajectoryNode(
|
optimization_problem_.AddTrajectoryNode(
|
||||||
matching_id.trajectory_id, scan_data->time, pose, optimized_pose);
|
matching_id.trajectory_id, scan_data->time, pose, optimized_pose);
|
||||||
for (const mapping::Submap* submap : insertion_submaps) {
|
for (const mapping::Submap* submap : insertion_submaps) {
|
||||||
const mapping::SubmapId submap_id = GetSubmapId(submap);
|
const mapping::SubmapId submap_id = GetSubmapId(submap);
|
||||||
CHECK(!submap_states_.at(submap_id.trajectory_id)
|
CHECK(!submap_states_.at(submap_id).finished);
|
||||||
.at(submap_id.submap_index)
|
submap_states_.at(submap_id).node_ids.emplace(node_id);
|
||||||
.finished);
|
|
||||||
submap_states_.at(submap_id.trajectory_id)
|
|
||||||
.at(submap_id.submap_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;
|
||||||
|
@ -276,28 +251,22 @@ void SparsePoseGraph::ComputeConstraintsForScan(
|
||||||
Constraint::INTRA_SUBMAP});
|
Constraint::INTRA_SUBMAP});
|
||||||
}
|
}
|
||||||
|
|
||||||
for (size_t trajectory_id = 0; trajectory_id < submap_states_.size();
|
for (int trajectory_id = 0; trajectory_id < submap_states_.num_trajectories();
|
||||||
++trajectory_id) {
|
++trajectory_id) {
|
||||||
for (size_t submap_index = 0;
|
for (int submap_index = 0;
|
||||||
submap_index < submap_states_.at(trajectory_id).size();
|
submap_index < submap_states_.num_indices(trajectory_id);
|
||||||
++submap_index) {
|
++submap_index) {
|
||||||
if (submap_states_.at(trajectory_id).at(submap_index).finished) {
|
const mapping::SubmapId submap_id{trajectory_id, submap_index};
|
||||||
CHECK_EQ(submap_states_.at(trajectory_id)
|
if (submap_states_.at(submap_id).finished) {
|
||||||
.at(submap_index)
|
CHECK_EQ(submap_states_.at(submap_id).node_ids.count(node_id), 0);
|
||||||
.node_ids.count(node_id),
|
ComputeConstraint(node_id, submap_id);
|
||||||
0);
|
|
||||||
ComputeConstraint(node_id,
|
|
||||||
mapping::SubmapId{static_cast<int>(trajectory_id),
|
|
||||||
static_cast<int>(submap_index)});
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if (finished_submap != nullptr) {
|
if (finished_submap != nullptr) {
|
||||||
const mapping::SubmapId finished_submap_id = GetSubmapId(finished_submap);
|
const mapping::SubmapId finished_submap_id = GetSubmapId(finished_submap);
|
||||||
SubmapState& finished_submap_state =
|
SubmapState& finished_submap_state = submap_states_.at(finished_submap_id);
|
||||||
submap_states_.at(finished_submap_id.trajectory_id)
|
|
||||||
.at(finished_submap_id.submap_index);
|
|
||||||
CHECK(!finished_submap_state.finished);
|
CHECK(!finished_submap_state.finished);
|
||||||
// We have a new completed submap, so we look into adding constraints for
|
// We have a new completed submap, so we look into adding constraints for
|
||||||
// old scans.
|
// old scans.
|
||||||
|
@ -391,12 +360,14 @@ void SparsePoseGraph::RunOptimization() {
|
||||||
common::MutexLocker locker(&mutex_);
|
common::MutexLocker locker(&mutex_);
|
||||||
|
|
||||||
const auto& node_data = optimization_problem_.node_data();
|
const auto& node_data = optimization_problem_.node_data();
|
||||||
for (size_t trajectory_id = 0; trajectory_id != node_data.size();
|
for (int trajectory_id = 0;
|
||||||
++trajectory_id) {
|
trajectory_id != static_cast<int>(node_data.size()); ++trajectory_id) {
|
||||||
size_t node_index = 0;
|
int node_index = 0;
|
||||||
const size_t num_nodes = trajectory_nodes_.at(trajectory_id).size();
|
const int num_nodes = trajectory_nodes_.num_indices(trajectory_id);
|
||||||
for (; node_index != node_data[trajectory_id].size(); ++node_index) {
|
for (; node_index != static_cast<int>(node_data[trajectory_id].size());
|
||||||
trajectory_nodes_[trajectory_id][node_index].pose = transform::Embed3D(
|
++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_index].point_cloud_pose);
|
||||||
}
|
}
|
||||||
// Extrapolate all point cloud poses that were added later.
|
// Extrapolate all point cloud poses that were added later.
|
||||||
|
@ -408,9 +379,9 @@ void SparsePoseGraph::RunOptimization() {
|
||||||
const transform::Rigid3d extrapolation_transform =
|
const transform::Rigid3d extrapolation_transform =
|
||||||
new_submap_transforms.back() * old_submap_transforms.back().inverse();
|
new_submap_transforms.back() * old_submap_transforms.back().inverse();
|
||||||
for (; node_index < num_nodes; ++node_index) {
|
for (; node_index < num_nodes; ++node_index) {
|
||||||
trajectory_nodes_[trajectory_id][node_index].pose =
|
const mapping::NodeId node_id{trajectory_id, node_index};
|
||||||
extrapolation_transform *
|
trajectory_nodes_.at(node_id).pose =
|
||||||
trajectory_nodes_[trajectory_id][node_index].pose;
|
extrapolation_transform * trajectory_nodes_.at(node_id).pose;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
optimized_submap_transforms_ = optimization_problem_.submap_data();
|
optimized_submap_transforms_ = optimization_problem_.submap_data();
|
||||||
|
@ -426,7 +397,7 @@ void SparsePoseGraph::RunOptimization() {
|
||||||
std::vector<std::vector<mapping::TrajectoryNode>>
|
std::vector<std::vector<mapping::TrajectoryNode>>
|
||||||
SparsePoseGraph::GetTrajectoryNodes() {
|
SparsePoseGraph::GetTrajectoryNodes() {
|
||||||
common::MutexLocker locker(&mutex_);
|
common::MutexLocker locker(&mutex_);
|
||||||
return trajectory_nodes_;
|
return trajectory_nodes_.data();
|
||||||
}
|
}
|
||||||
|
|
||||||
std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
|
std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
|
||||||
|
@ -437,15 +408,17 @@ std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
|
||||||
transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
|
transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
|
||||||
const int trajectory_id) {
|
const int trajectory_id) {
|
||||||
common::MutexLocker locker(&mutex_);
|
common::MutexLocker locker(&mutex_);
|
||||||
if (submap_states_.size() <= static_cast<size_t>(trajectory_id) ||
|
if (submap_states_.num_trajectories() <= trajectory_id ||
|
||||||
submap_states_.at(trajectory_id).empty()) {
|
submap_states_.num_indices(trajectory_id) == 0) {
|
||||||
return transform::Rigid3d::Identity();
|
return transform::Rigid3d::Identity();
|
||||||
}
|
}
|
||||||
const auto extrapolated_submap_transforms =
|
const auto extrapolated_submap_transforms =
|
||||||
ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory_id);
|
ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory_id);
|
||||||
return extrapolated_submap_transforms.back() *
|
return extrapolated_submap_transforms.back() *
|
||||||
submap_states_.at(trajectory_id)
|
submap_states_
|
||||||
.at(extrapolated_submap_transforms.size() - 1)
|
.at(mapping::SubmapId{
|
||||||
|
trajectory_id,
|
||||||
|
static_cast<int>(extrapolated_submap_transforms.size()) - 1})
|
||||||
.submap->local_pose.inverse();
|
.submap->local_pose.inverse();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -464,15 +437,19 @@ std::vector<transform::Rigid3d> SparsePoseGraph::GetSubmapTransforms(
|
||||||
std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
|
std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
|
||||||
const std::vector<std::vector<sparse_pose_graph::SubmapData>>&
|
const std::vector<std::vector<sparse_pose_graph::SubmapData>>&
|
||||||
submap_transforms,
|
submap_transforms,
|
||||||
const size_t trajectory_id) const {
|
const int trajectory_id) const {
|
||||||
if (trajectory_id >= submap_states_.size()) {
|
if (trajectory_id >= submap_states_.num_trajectories()) {
|
||||||
return {transform::Rigid3d::Identity()};
|
return {transform::Rigid3d::Identity()};
|
||||||
}
|
}
|
||||||
|
|
||||||
// Submaps for which we have optimized poses.
|
// Submaps for which we have optimized poses.
|
||||||
std::vector<transform::Rigid3d> result;
|
std::vector<transform::Rigid3d> result;
|
||||||
for (const auto& state : submap_states_.at(trajectory_id)) {
|
for (int submap_index = 0;
|
||||||
if (trajectory_id < submap_transforms.size() &&
|
submap_index != submap_states_.num_indices(trajectory_id);
|
||||||
|
++submap_index) {
|
||||||
|
const mapping::SubmapId submap_id{trajectory_id, submap_index};
|
||||||
|
const auto& state = submap_states_.at(submap_id);
|
||||||
|
if (static_cast<size_t>(trajectory_id) < submap_transforms.size() &&
|
||||||
result.size() < submap_transforms.at(trajectory_id).size()) {
|
result.size() < submap_transforms.at(trajectory_id).size()) {
|
||||||
// Submaps for which we have optimized poses.
|
// Submaps for which we have optimized poses.
|
||||||
result.push_back(
|
result.push_back(
|
||||||
|
@ -482,11 +459,12 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
|
||||||
} else {
|
} else {
|
||||||
// Extrapolate to the remaining submaps. Accessing 'local_pose' in Submaps
|
// Extrapolate to the remaining submaps. Accessing 'local_pose' in Submaps
|
||||||
// is okay, since the member is const.
|
// is okay, since the member is const.
|
||||||
result.push_back(result.back() *
|
const mapping::SubmapId previous_submap_id{
|
||||||
submap_states_.at(trajectory_id)
|
trajectory_id, static_cast<int>(result.size()) - 1};
|
||||||
.at(result.size() - 1)
|
result.push_back(
|
||||||
.submap->local_pose.inverse() *
|
result.back() *
|
||||||
state.submap->local_pose);
|
submap_states_.at(previous_submap_id).submap->local_pose.inverse() *
|
||||||
|
state.submap->local_pose);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -152,7 +152,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
||||||
std::vector<transform::Rigid3d> ExtrapolateSubmapTransforms(
|
std::vector<transform::Rigid3d> ExtrapolateSubmapTransforms(
|
||||||
const std::vector<std::vector<sparse_pose_graph::SubmapData>>&
|
const std::vector<std::vector<sparse_pose_graph::SubmapData>>&
|
||||||
submap_transforms,
|
submap_transforms,
|
||||||
size_t trajectory_id) const REQUIRES(mutex_);
|
int trajectory_id) const REQUIRES(mutex_);
|
||||||
|
|
||||||
const mapping::proto::SparsePoseGraphOptions options_;
|
const mapping::proto::SparsePoseGraphOptions options_;
|
||||||
common::Mutex mutex_;
|
common::Mutex mutex_;
|
||||||
|
@ -184,7 +184,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
||||||
// before they take part in the background computations.
|
// before they take part in the background computations.
|
||||||
std::map<const mapping::Submap*, mapping::SubmapId> submap_ids_
|
std::map<const mapping::Submap*, mapping::SubmapId> submap_ids_
|
||||||
GUARDED_BY(mutex_);
|
GUARDED_BY(mutex_);
|
||||||
std::vector<std::vector<SubmapState>> submap_states_ GUARDED_BY(mutex_);
|
mapping::NestedVectorsById<SubmapState, mapping::SubmapId> submap_states_
|
||||||
|
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_;
|
||||||
|
@ -196,8 +197,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
||||||
// Deque to keep references valid for the background computation when adding
|
// Deque to keep references valid for the background computation when adding
|
||||||
// new data.
|
// new data.
|
||||||
std::deque<mapping::TrajectoryNode::ConstantData> constant_node_data_;
|
std::deque<mapping::TrajectoryNode::ConstantData> constant_node_data_;
|
||||||
std::vector<std::vector<mapping::TrajectoryNode>> trajectory_nodes_
|
mapping::NestedVectorsById<mapping::TrajectoryNode, mapping::NodeId>
|
||||||
GUARDED_BY(mutex_);
|
trajectory_nodes_ GUARDED_BY(mutex_);
|
||||||
int num_trajectory_nodes_ = 0 GUARDED_BY(mutex_);
|
int num_trajectory_nodes_ = 0 GUARDED_BY(mutex_);
|
||||||
|
|
||||||
// Current submap transforms used for displaying data.
|
// Current submap transforms used for displaying data.
|
||||||
|
|
|
@ -100,23 +100,17 @@ void SparsePoseGraph::AddScan(
|
||||||
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,
|
||||||
transform::Rigid3d::Identity()});
|
transform::Rigid3d::Identity()});
|
||||||
trajectory_nodes_.resize(
|
trajectory_nodes_.Append(
|
||||||
std::max<size_t>(trajectory_nodes_.size(), trajectory_id + 1));
|
trajectory_id,
|
||||||
trajectory_nodes_[trajectory_id].push_back(
|
|
||||||
mapping::TrajectoryNode{&constant_node_data_.back(), optimized_pose});
|
mapping::TrajectoryNode{&constant_node_data_.back(), optimized_pose});
|
||||||
++num_trajectory_nodes_;
|
++num_trajectory_nodes_;
|
||||||
trajectory_connectivity_.Add(trajectory_id);
|
trajectory_connectivity_.Add(trajectory_id);
|
||||||
|
|
||||||
if (submap_ids_.count(insertion_submaps.back()) == 0) {
|
if (submap_ids_.count(insertion_submaps.back()) == 0) {
|
||||||
submap_states_.resize(
|
const mapping::SubmapId submap_id =
|
||||||
std::max<size_t>(submap_states_.size(), trajectory_id + 1));
|
submap_states_.Append(trajectory_id, SubmapState());
|
||||||
auto& trajectory_submap_states = submap_states_.at(trajectory_id);
|
submap_ids_.emplace(insertion_submaps.back(), submap_id);
|
||||||
submap_ids_.emplace(
|
submap_states_.at(submap_id).submap = insertion_submaps.back();
|
||||||
insertion_submaps.back(),
|
|
||||||
mapping::SubmapId{trajectory_id,
|
|
||||||
static_cast<int>(trajectory_submap_states.size())});
|
|
||||||
trajectory_submap_states.emplace_back();
|
|
||||||
trajectory_submap_states.back().submap = insertion_submaps.back();
|
|
||||||
}
|
}
|
||||||
const Submap* const finished_submap =
|
const Submap* const finished_submap =
|
||||||
insertion_submaps.front()->finished ? insertion_submaps.front() : nullptr;
|
insertion_submaps.front()->finished ? insertion_submaps.front() : nullptr;
|
||||||
|
@ -168,16 +162,10 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
|
||||||
|
|
||||||
std::vector<mapping::TrajectoryNode> submap_nodes;
|
std::vector<mapping::TrajectoryNode> submap_nodes;
|
||||||
for (const mapping::NodeId& submap_node_id :
|
for (const mapping::NodeId& submap_node_id :
|
||||||
submap_states_.at(submap_id.trajectory_id)
|
submap_states_.at(submap_id).node_ids) {
|
||||||
.at(submap_id.submap_index)
|
|
||||||
.node_ids) {
|
|
||||||
submap_nodes.push_back(mapping::TrajectoryNode{
|
submap_nodes.push_back(mapping::TrajectoryNode{
|
||||||
trajectory_nodes_.at(submap_node_id.trajectory_id)
|
trajectory_nodes_.at(submap_node_id).constant_data,
|
||||||
.at(submap_node_id.node_index)
|
inverse_submap_pose * trajectory_nodes_.at(submap_node_id).pose});
|
||||||
.constant_data,
|
|
||||||
inverse_submap_pose * trajectory_nodes_.at(submap_node_id.trajectory_id)
|
|
||||||
.at(submap_node_id.node_index)
|
|
||||||
.pose});
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Only globally match against submaps not in this trajectory.
|
// Only globally match against submaps not in this trajectory.
|
||||||
|
@ -196,14 +184,8 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
|
||||||
// yaw component will be handled by the matching procedure in the
|
// yaw component will be handled by the matching procedure in the
|
||||||
// FastCorrelativeScanMatcher, and the given yaw is essentially ignored.
|
// FastCorrelativeScanMatcher, and the given yaw is essentially ignored.
|
||||||
constraint_builder_.MaybeAddGlobalConstraint(
|
constraint_builder_.MaybeAddGlobalConstraint(
|
||||||
submap_id,
|
submap_id, submap_states_.at(submap_id).submap, node_id,
|
||||||
submap_states_.at(submap_id.trajectory_id)
|
&trajectory_nodes_.at(node_id).constant_data->range_data_3d.returns,
|
||||||
.at(submap_id.submap_index)
|
|
||||||
.submap,
|
|
||||||
node_id,
|
|
||||||
&trajectory_nodes_.at(node_id.trajectory_id)
|
|
||||||
.at(node_id.node_index)
|
|
||||||
.constant_data->range_data_3d.returns,
|
|
||||||
submap_nodes, initial_relative_pose.rotation(),
|
submap_nodes, initial_relative_pose.rotation(),
|
||||||
&trajectory_connectivity_);
|
&trajectory_connectivity_);
|
||||||
} else {
|
} else {
|
||||||
|
@ -215,14 +197,8 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
|
||||||
if (node_id.trajectory_id == submap_id.trajectory_id ||
|
if (node_id.trajectory_id == submap_id.trajectory_id ||
|
||||||
scan_and_submap_trajectories_connected) {
|
scan_and_submap_trajectories_connected) {
|
||||||
constraint_builder_.MaybeAddConstraint(
|
constraint_builder_.MaybeAddConstraint(
|
||||||
submap_id,
|
submap_id, submap_states_.at(submap_id).submap, node_id,
|
||||||
submap_states_.at(submap_id.trajectory_id)
|
&trajectory_nodes_.at(node_id).constant_data->range_data_3d.returns,
|
||||||
.at(submap_id.submap_index)
|
|
||||||
.submap,
|
|
||||||
node_id,
|
|
||||||
&trajectory_nodes_.at(node_id.trajectory_id)
|
|
||||||
.at(node_id.node_index)
|
|
||||||
.constant_data->range_data_3d.returns,
|
|
||||||
submap_nodes, initial_relative_pose);
|
submap_nodes, initial_relative_pose);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -230,8 +206,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
|
||||||
|
|
||||||
void SparsePoseGraph::ComputeConstraintsForOldScans(const Submap* submap) {
|
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);
|
||||||
submap_states_.at(submap_id.trajectory_id).at(submap_id.submap_index);
|
|
||||||
|
|
||||||
const auto& node_data = optimization_problem_.node_data();
|
const auto& node_data = optimization_problem_.node_data();
|
||||||
for (size_t trajectory_id = 0; trajectory_id != node_data.size();
|
for (size_t trajectory_id = 0; trajectory_id != node_data.size();
|
||||||
|
@ -268,20 +243,14 @@ void SparsePoseGraph::ComputeConstraintsForScan(
|
||||||
.size())
|
.size())
|
||||||
: 0};
|
: 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).constant_data;
|
||||||
.at(node_id.node_index)
|
|
||||||
.constant_data;
|
|
||||||
CHECK_EQ(scan_data->trajectory_id, matching_id.trajectory_id);
|
CHECK_EQ(scan_data->trajectory_id, matching_id.trajectory_id);
|
||||||
optimization_problem_.AddTrajectoryNode(matching_id.trajectory_id,
|
optimization_problem_.AddTrajectoryNode(matching_id.trajectory_id,
|
||||||
scan_data->time, optimized_pose);
|
scan_data->time, optimized_pose);
|
||||||
for (const Submap* submap : insertion_submaps) {
|
for (const Submap* submap : insertion_submaps) {
|
||||||
const mapping::SubmapId submap_id = GetSubmapId(submap);
|
const mapping::SubmapId submap_id = GetSubmapId(submap);
|
||||||
CHECK(!submap_states_.at(submap_id.trajectory_id)
|
CHECK(!submap_states_.at(submap_id).finished);
|
||||||
.at(submap_id.submap_index)
|
submap_states_.at(submap_id).node_ids.emplace(node_id);
|
||||||
.finished);
|
|
||||||
submap_states_.at(submap_id.trajectory_id)
|
|
||||||
.at(submap_id.submap_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;
|
||||||
|
@ -295,28 +264,22 @@ void SparsePoseGraph::ComputeConstraintsForScan(
|
||||||
Constraint::INTRA_SUBMAP});
|
Constraint::INTRA_SUBMAP});
|
||||||
}
|
}
|
||||||
|
|
||||||
for (size_t trajectory_id = 0; trajectory_id < submap_states_.size();
|
for (int trajectory_id = 0; trajectory_id < submap_states_.num_trajectories();
|
||||||
++trajectory_id) {
|
++trajectory_id) {
|
||||||
for (size_t submap_index = 0;
|
for (int submap_index = 0;
|
||||||
submap_index < submap_states_.at(trajectory_id).size();
|
submap_index < submap_states_.num_indices(trajectory_id);
|
||||||
++submap_index) {
|
++submap_index) {
|
||||||
if (submap_states_.at(trajectory_id).at(submap_index).finished) {
|
const mapping::SubmapId submap_id{trajectory_id, submap_index};
|
||||||
CHECK_EQ(submap_states_.at(trajectory_id)
|
if (submap_states_.at(submap_id).finished) {
|
||||||
.at(submap_index)
|
CHECK_EQ(submap_states_.at(submap_id).node_ids.count(node_id), 0);
|
||||||
.node_ids.count(node_id),
|
ComputeConstraint(node_id, submap_id);
|
||||||
0);
|
|
||||||
ComputeConstraint(node_id,
|
|
||||||
mapping::SubmapId{static_cast<int>(trajectory_id),
|
|
||||||
static_cast<int>(submap_index)});
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if (finished_submap != nullptr) {
|
if (finished_submap != nullptr) {
|
||||||
const mapping::SubmapId finished_submap_id = GetSubmapId(finished_submap);
|
const mapping::SubmapId finished_submap_id = GetSubmapId(finished_submap);
|
||||||
SubmapState& finished_submap_state =
|
SubmapState& finished_submap_state = submap_states_.at(finished_submap_id);
|
||||||
submap_states_.at(finished_submap_id.trajectory_id)
|
|
||||||
.at(finished_submap_id.submap_index);
|
|
||||||
CHECK(!finished_submap_state.finished);
|
CHECK(!finished_submap_state.finished);
|
||||||
// We have a new completed submap, so we look into adding constraints for
|
// We have a new completed submap, so we look into adding constraints for
|
||||||
// old scans.
|
// old scans.
|
||||||
|
@ -410,12 +373,14 @@ void SparsePoseGraph::RunOptimization() {
|
||||||
common::MutexLocker locker(&mutex_);
|
common::MutexLocker locker(&mutex_);
|
||||||
|
|
||||||
const auto& node_data = optimization_problem_.node_data();
|
const auto& node_data = optimization_problem_.node_data();
|
||||||
for (size_t trajectory_id = 0; trajectory_id != node_data.size();
|
for (int trajectory_id = 0;
|
||||||
++trajectory_id) {
|
trajectory_id != static_cast<int>(node_data.size()); ++trajectory_id) {
|
||||||
size_t node_index = 0;
|
int node_index = 0;
|
||||||
const size_t num_nodes = trajectory_nodes_.at(trajectory_id).size();
|
const int num_nodes = trajectory_nodes_.num_indices(trajectory_id);
|
||||||
for (; node_index != node_data[trajectory_id].size(); ++node_index) {
|
for (; node_index != static_cast<int>(node_data[trajectory_id].size());
|
||||||
trajectory_nodes_[trajectory_id][node_index].pose =
|
++node_index) {
|
||||||
|
const mapping::NodeId node_id{trajectory_id, node_index};
|
||||||
|
trajectory_nodes_.at(node_id).pose =
|
||||||
node_data[trajectory_id][node_index].point_cloud_pose;
|
node_data[trajectory_id][node_index].point_cloud_pose;
|
||||||
}
|
}
|
||||||
// Extrapolate all point cloud poses that were added later.
|
// Extrapolate all point cloud poses that were added later.
|
||||||
|
@ -427,9 +392,9 @@ void SparsePoseGraph::RunOptimization() {
|
||||||
const transform::Rigid3d extrapolation_transform =
|
const transform::Rigid3d extrapolation_transform =
|
||||||
new_submap_transforms.back() * old_submap_transforms.back().inverse();
|
new_submap_transforms.back() * old_submap_transforms.back().inverse();
|
||||||
for (; node_index < num_nodes; ++node_index) {
|
for (; node_index < num_nodes; ++node_index) {
|
||||||
trajectory_nodes_[trajectory_id][node_index].pose =
|
const mapping::NodeId node_id{trajectory_id, node_index};
|
||||||
extrapolation_transform *
|
trajectory_nodes_.at(node_id).pose =
|
||||||
trajectory_nodes_[trajectory_id][node_index].pose;
|
extrapolation_transform * trajectory_nodes_.at(node_id).pose;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
optimized_submap_transforms_ = optimization_problem_.submap_data();
|
optimized_submap_transforms_ = optimization_problem_.submap_data();
|
||||||
|
@ -445,7 +410,7 @@ void SparsePoseGraph::RunOptimization() {
|
||||||
std::vector<std::vector<mapping::TrajectoryNode>>
|
std::vector<std::vector<mapping::TrajectoryNode>>
|
||||||
SparsePoseGraph::GetTrajectoryNodes() {
|
SparsePoseGraph::GetTrajectoryNodes() {
|
||||||
common::MutexLocker locker(&mutex_);
|
common::MutexLocker locker(&mutex_);
|
||||||
return trajectory_nodes_;
|
return trajectory_nodes_.data();
|
||||||
}
|
}
|
||||||
|
|
||||||
std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
|
std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
|
||||||
|
@ -456,15 +421,17 @@ std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
|
||||||
transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
|
transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
|
||||||
const int trajectory_id) {
|
const int trajectory_id) {
|
||||||
common::MutexLocker locker(&mutex_);
|
common::MutexLocker locker(&mutex_);
|
||||||
if (submap_states_.size() <= static_cast<size_t>(trajectory_id) ||
|
if (submap_states_.num_trajectories() <= trajectory_id ||
|
||||||
submap_states_.at(trajectory_id).empty()) {
|
submap_states_.num_indices(trajectory_id) == 0) {
|
||||||
return transform::Rigid3d::Identity();
|
return transform::Rigid3d::Identity();
|
||||||
}
|
}
|
||||||
const auto extrapolated_submap_transforms =
|
const auto extrapolated_submap_transforms =
|
||||||
ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory_id);
|
ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory_id);
|
||||||
return extrapolated_submap_transforms.back() *
|
return extrapolated_submap_transforms.back() *
|
||||||
submap_states_.at(trajectory_id)
|
submap_states_
|
||||||
.at(extrapolated_submap_transforms.size() - 1)
|
.at(mapping::SubmapId{
|
||||||
|
trajectory_id,
|
||||||
|
static_cast<int>(extrapolated_submap_transforms.size()) - 1})
|
||||||
.submap->local_pose.inverse();
|
.submap->local_pose.inverse();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -483,15 +450,19 @@ std::vector<transform::Rigid3d> SparsePoseGraph::GetSubmapTransforms(
|
||||||
std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
|
std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
|
||||||
const std::vector<std::vector<sparse_pose_graph::SubmapData>>&
|
const std::vector<std::vector<sparse_pose_graph::SubmapData>>&
|
||||||
submap_transforms,
|
submap_transforms,
|
||||||
const size_t trajectory_id) const {
|
const int trajectory_id) const {
|
||||||
if (trajectory_id >= submap_states_.size()) {
|
if (trajectory_id >= submap_states_.num_trajectories()) {
|
||||||
return {transform::Rigid3d::Identity()};
|
return {transform::Rigid3d::Identity()};
|
||||||
}
|
}
|
||||||
|
|
||||||
// Submaps for which we have optimized poses.
|
// Submaps for which we have optimized poses.
|
||||||
std::vector<transform::Rigid3d> result;
|
std::vector<transform::Rigid3d> result;
|
||||||
for (const auto& state : submap_states_.at(trajectory_id)) {
|
for (int submap_index = 0;
|
||||||
if (trajectory_id < submap_transforms.size() &&
|
submap_index != submap_states_.num_indices(trajectory_id);
|
||||||
|
++submap_index) {
|
||||||
|
const mapping::SubmapId submap_id{trajectory_id, submap_index};
|
||||||
|
const auto& state = submap_states_.at(submap_id);
|
||||||
|
if (static_cast<size_t>(trajectory_id) < submap_transforms.size() &&
|
||||||
result.size() < submap_transforms.at(trajectory_id).size()) {
|
result.size() < submap_transforms.at(trajectory_id).size()) {
|
||||||
// Submaps for which we have optimized poses.
|
// Submaps for which we have optimized poses.
|
||||||
result.push_back(
|
result.push_back(
|
||||||
|
@ -501,11 +472,12 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
|
||||||
} else {
|
} else {
|
||||||
// Extrapolate to the remaining submaps. Accessing 'local_pose' in Submaps
|
// Extrapolate to the remaining submaps. Accessing 'local_pose' in Submaps
|
||||||
// is okay, since the member is const.
|
// is okay, since the member is const.
|
||||||
result.push_back(result.back() *
|
const mapping::SubmapId previous_submap_id{
|
||||||
submap_states_.at(trajectory_id)
|
trajectory_id, static_cast<int>(result.size()) - 1};
|
||||||
.at(result.size() - 1)
|
result.push_back(
|
||||||
.submap->local_pose.inverse() *
|
result.back() *
|
||||||
state.submap->local_pose);
|
submap_states_.at(previous_submap_id).submap->local_pose.inverse() *
|
||||||
|
state.submap->local_pose);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -149,7 +149,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
||||||
std::vector<transform::Rigid3d> ExtrapolateSubmapTransforms(
|
std::vector<transform::Rigid3d> ExtrapolateSubmapTransforms(
|
||||||
const std::vector<std::vector<sparse_pose_graph::SubmapData>>&
|
const std::vector<std::vector<sparse_pose_graph::SubmapData>>&
|
||||||
submap_transforms,
|
submap_transforms,
|
||||||
size_t trajectory_id) const REQUIRES(mutex_);
|
int trajectory_id) const REQUIRES(mutex_);
|
||||||
|
|
||||||
const mapping::proto::SparsePoseGraphOptions options_;
|
const mapping::proto::SparsePoseGraphOptions options_;
|
||||||
common::Mutex mutex_;
|
common::Mutex mutex_;
|
||||||
|
@ -181,7 +181,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
||||||
// before they take part in the background computations.
|
// before they take part in the background computations.
|
||||||
std::map<const mapping::Submap*, mapping::SubmapId> submap_ids_
|
std::map<const mapping::Submap*, mapping::SubmapId> submap_ids_
|
||||||
GUARDED_BY(mutex_);
|
GUARDED_BY(mutex_);
|
||||||
std::vector<std::vector<SubmapState>> submap_states_ GUARDED_BY(mutex_);
|
mapping::NestedVectorsById<SubmapState, mapping::SubmapId> submap_states_
|
||||||
|
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_;
|
||||||
|
@ -193,8 +194,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
||||||
// Deque to keep references valid for the background computation when adding
|
// Deque to keep references valid for the background computation when adding
|
||||||
// new data.
|
// new data.
|
||||||
std::deque<mapping::TrajectoryNode::ConstantData> constant_node_data_;
|
std::deque<mapping::TrajectoryNode::ConstantData> constant_node_data_;
|
||||||
std::vector<std::vector<mapping::TrajectoryNode>> trajectory_nodes_
|
mapping::NestedVectorsById<mapping::TrajectoryNode, mapping::NodeId>
|
||||||
GUARDED_BY(mutex_);
|
trajectory_nodes_ GUARDED_BY(mutex_);
|
||||||
int num_trajectory_nodes_ = 0 GUARDED_BY(mutex_);
|
int num_trajectory_nodes_ = 0 GUARDED_BY(mutex_);
|
||||||
|
|
||||||
// Current submap transforms used for displaying data.
|
// Current submap transforms used for displaying data.
|
||||||
|
|
Loading…
Reference in New Issue