Switch trajectory_nodes in SPG2D to MapById. (#592)
parent
bcd1be92b1
commit
fdda1dd091
|
@ -129,6 +129,8 @@ class Range {
|
|||
|
||||
// Reminiscent of std::map, but indexed by 'IdType' which can be 'NodeId' or
|
||||
// 'SubmapId'.
|
||||
// Note: This container will only ever contain non-empty trajectories. Trimming
|
||||
// the last remaining node of a trajectory drops the trajectory.
|
||||
template <typename IdType, typename DataType>
|
||||
class MapById {
|
||||
private:
|
||||
|
@ -296,6 +298,9 @@ class MapById {
|
|||
trajectory.can_append_ = false;
|
||||
}
|
||||
trajectory.data_.erase(it);
|
||||
if (trajectory.data_.empty()) {
|
||||
trajectories_.erase(id.trajectory_id);
|
||||
}
|
||||
}
|
||||
|
||||
bool Contains(const IdType& id) const {
|
||||
|
|
|
@ -469,30 +469,33 @@ void SparsePoseGraph::RunOptimization() {
|
|||
|
||||
const auto& submap_data = optimization_problem_.submap_data();
|
||||
const auto& node_data = optimization_problem_.node_data();
|
||||
for (auto node_it = node_data.begin(); node_it != node_data.end();) {
|
||||
const int trajectory_id = node_it->id.trajectory_id;
|
||||
const auto trajectory_end = node_data.EndOfTrajectory(trajectory_id);
|
||||
const int num_nodes = trajectory_nodes_.num_indices(trajectory_id);
|
||||
for (; node_it != trajectory_end; ++node_it) {
|
||||
const mapping::NodeId node_id = node_it->id;
|
||||
auto& node = trajectory_nodes_.at(node_id);
|
||||
node.global_pose =
|
||||
transform::Embed3D(node_it->data.pose) *
|
||||
transform::Rigid3d::Rotation(node.constant_data->gravity_alignment);
|
||||
for (const int trajectory_id : node_data.trajectory_ids()) {
|
||||
for (const auto& node : node_data.trajectory(trajectory_id)) {
|
||||
auto& mutable_trajectory_node = trajectory_nodes_.at(node.id);
|
||||
mutable_trajectory_node.global_pose =
|
||||
transform::Embed3D(node.data.pose) *
|
||||
transform::Rigid3d::Rotation(
|
||||
mutable_trajectory_node.constant_data->gravity_alignment);
|
||||
}
|
||||
// Extrapolate all point cloud poses that were added later.
|
||||
|
||||
// Extrapolate all point cloud poses that were not included in the
|
||||
// 'optimization_problem_' yet.
|
||||
const auto local_to_new_global =
|
||||
ComputeLocalToGlobalTransform(submap_data, trajectory_id);
|
||||
const auto local_to_old_global = ComputeLocalToGlobalTransform(
|
||||
optimized_submap_transforms_, trajectory_id);
|
||||
const transform::Rigid3d old_global_to_new_global =
|
||||
local_to_new_global * local_to_old_global.inverse();
|
||||
const int last_optimized_node_index = std::prev(node_it)->id.node_index;
|
||||
for (int node_index = last_optimized_node_index + 1; node_index < num_nodes;
|
||||
++node_index) {
|
||||
const mapping::NodeId node_id{trajectory_id, node_index};
|
||||
auto& node_pose = trajectory_nodes_.at(node_id).global_pose;
|
||||
node_pose = old_global_to_new_global * node_pose;
|
||||
|
||||
const mapping::NodeId last_optimized_node_id =
|
||||
std::prev(node_data.EndOfTrajectory(trajectory_id))->id;
|
||||
auto node_it =
|
||||
std::next(trajectory_nodes_.FindChecked(last_optimized_node_id));
|
||||
for (; node_it != trajectory_nodes_.EndOfTrajectory(trajectory_id);
|
||||
++node_it) {
|
||||
auto& mutable_trajectory_node = trajectory_nodes_.at(node_it->id);
|
||||
mutable_trajectory_node.global_pose =
|
||||
old_global_to_new_global * mutable_trajectory_node.global_pose;
|
||||
}
|
||||
}
|
||||
optimized_submap_transforms_ = submap_data;
|
||||
|
@ -500,8 +503,17 @@ void SparsePoseGraph::RunOptimization() {
|
|||
|
||||
std::vector<std::vector<mapping::TrajectoryNode>>
|
||||
SparsePoseGraph::GetTrajectoryNodes() {
|
||||
// TODO(cschuet): Rewrite downstream code and switch to MapById.
|
||||
common::MutexLocker locker(&mutex_);
|
||||
return trajectory_nodes_.data();
|
||||
std::vector<std::vector<mapping::TrajectoryNode>> nodes;
|
||||
for (const int trajectory_id : trajectory_nodes_.trajectory_ids()) {
|
||||
nodes.resize(trajectory_id + 1);
|
||||
for (const auto& node : trajectory_nodes_.trajectory(trajectory_id)) {
|
||||
nodes.at(trajectory_id).resize(node.id.node_index + 1);
|
||||
nodes.at(trajectory_id).at(node.id.node_index) = node.data;
|
||||
}
|
||||
}
|
||||
return nodes;
|
||||
}
|
||||
|
||||
std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
|
||||
|
|
|
@ -209,8 +209,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
|||
GUARDED_BY(mutex_);
|
||||
|
||||
// Data that are currently being shown.
|
||||
mapping::NestedVectorsById<mapping::TrajectoryNode, mapping::NodeId>
|
||||
trajectory_nodes_ GUARDED_BY(mutex_);
|
||||
mapping::MapById<mapping::NodeId, mapping::TrajectoryNode> trajectory_nodes_
|
||||
GUARDED_BY(mutex_);
|
||||
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
|
||||
|
||||
// Current submap transforms used for displaying data.
|
||||
|
|
|
@ -508,28 +508,29 @@ void SparsePoseGraph::RunOptimization() {
|
|||
|
||||
const auto& submap_data = optimization_problem_.submap_data();
|
||||
const auto& node_data = optimization_problem_.node_data();
|
||||
for (auto node_it = node_data.begin(); node_it != node_data.end();) {
|
||||
const int trajectory_id = node_it->id.trajectory_id;
|
||||
const auto trajectory_end = node_data.EndOfTrajectory(trajectory_id);
|
||||
const int num_nodes = trajectory_nodes_.num_indices(trajectory_id);
|
||||
for (; node_it != trajectory_end; ++node_it) {
|
||||
const mapping::NodeId node_id = node_it->id;
|
||||
auto& node = trajectory_nodes_.at(node_id);
|
||||
node.global_pose = node_it->data.pose;
|
||||
for (const int trajectory_id : node_data.trajectory_ids()) {
|
||||
for (const auto& node : node_data.trajectory(trajectory_id)) {
|
||||
trajectory_nodes_.at(node.id).global_pose = node.data.pose;
|
||||
}
|
||||
// Extrapolate all point cloud poses that were added later.
|
||||
|
||||
// Extrapolate all point cloud poses that were not included in the
|
||||
// 'optimization_problem_' yet.
|
||||
const auto local_to_new_global =
|
||||
ComputeLocalToGlobalTransform(submap_data, trajectory_id);
|
||||
const auto local_to_old_global = ComputeLocalToGlobalTransform(
|
||||
optimized_submap_transforms_, trajectory_id);
|
||||
const transform::Rigid3d old_global_to_new_global =
|
||||
local_to_new_global * local_to_old_global.inverse();
|
||||
const int last_optimized_node_index = std::prev(node_it)->id.node_index;
|
||||
for (int node_index = last_optimized_node_index + 1; node_index < num_nodes;
|
||||
++node_index) {
|
||||
const mapping::NodeId node_id{trajectory_id, node_index};
|
||||
auto& node_pose = trajectory_nodes_.at(node_id).global_pose;
|
||||
node_pose = old_global_to_new_global * node_pose;
|
||||
|
||||
const mapping::NodeId last_optimized_node_id =
|
||||
std::prev(node_data.EndOfTrajectory(trajectory_id))->id;
|
||||
auto node_it =
|
||||
std::next(trajectory_nodes_.FindChecked(last_optimized_node_id));
|
||||
for (; node_it != trajectory_nodes_.EndOfTrajectory(trajectory_id);
|
||||
++node_it) {
|
||||
auto& mutable_trajectory_node = trajectory_nodes_.at(node_it->id);
|
||||
mutable_trajectory_node.global_pose =
|
||||
old_global_to_new_global * mutable_trajectory_node.global_pose;
|
||||
}
|
||||
}
|
||||
optimized_submap_transforms_ = submap_data;
|
||||
|
@ -542,8 +543,17 @@ void SparsePoseGraph::RunOptimization() {
|
|||
|
||||
std::vector<std::vector<mapping::TrajectoryNode>>
|
||||
SparsePoseGraph::GetTrajectoryNodes() {
|
||||
// TODO(cschuet): Rewrite downstream code and switch to MapById.
|
||||
common::MutexLocker locker(&mutex_);
|
||||
return trajectory_nodes_.data();
|
||||
std::vector<std::vector<mapping::TrajectoryNode>> nodes;
|
||||
for (const int trajectory_id : trajectory_nodes_.trajectory_ids()) {
|
||||
nodes.resize(trajectory_id + 1);
|
||||
for (const auto& node : trajectory_nodes_.trajectory(trajectory_id)) {
|
||||
nodes.at(trajectory_id).resize(node.id.node_index + 1);
|
||||
nodes.at(trajectory_id).at(node.id.node_index) = node.data;
|
||||
}
|
||||
}
|
||||
return nodes;
|
||||
}
|
||||
|
||||
std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
|
||||
|
|
|
@ -213,8 +213,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
|||
GUARDED_BY(mutex_);
|
||||
|
||||
// Data that are currently being shown.
|
||||
mapping::NestedVectorsById<mapping::TrajectoryNode, mapping::NodeId>
|
||||
trajectory_nodes_ GUARDED_BY(mutex_);
|
||||
mapping::MapById<mapping::NodeId, mapping::TrajectoryNode> trajectory_nodes_
|
||||
GUARDED_BY(mutex_);
|
||||
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
|
||||
|
||||
// Current submap transforms used for displaying data.
|
||||
|
|
Loading…
Reference in New Issue