Trim node data. (#331)

Also, remove trimmed nodes from proto serialization.

PAIR=wohe
master
Holger Rapp 2017-06-12 15:20:49 +02:00 committed by GitHub
parent f68da37c6e
commit 521666ce55
3 changed files with 44 additions and 27 deletions

View File

@ -61,6 +61,36 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
proto::SparsePoseGraph SparsePoseGraph::ToProto() {
proto::SparsePoseGraph proto;
std::map<NodeId, NodeId> node_id_remapping; // Due to trimming.
const auto all_trajectory_nodes = GetTrajectoryNodes();
for (size_t trajectory_id = 0; trajectory_id != all_trajectory_nodes.size();
++trajectory_id) {
const auto& single_trajectory_nodes = all_trajectory_nodes[trajectory_id];
auto* trajectory_proto = proto.add_trajectory();
for (size_t old_node_index = 0;
old_node_index != single_trajectory_nodes.size(); ++old_node_index) {
const auto& node = single_trajectory_nodes[old_node_index];
if (!node.trimmed()) {
node_id_remapping[NodeId{trajectory_id, old_node_index}] =
NodeId{trajectory_id, trajectory_proto->node_size()};
auto* node_proto = trajectory_proto->add_node();
node_proto->set_timestamp(
common::ToUniversal(node.constant_data->time));
*node_proto->mutable_pose() = transform::ToProto(
node.pose * node.constant_data->tracking_to_pose);
}
}
if (!single_trajectory_nodes.empty()) {
for (const auto& transform : GetSubmapTransforms(trajectory_id)) {
*trajectory_proto->add_submap()->mutable_pose() =
transform::ToProto(transform);
}
}
}
for (const auto& constraint : constraints()) {
auto* const constraint_proto = proto.add_constraint();
*constraint_proto->mutable_relative_pose() =
@ -73,34 +103,14 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
constraint_proto->mutable_submap_id()->set_submap_index(
constraint.submap_id.submap_index);
const NodeId node_id = node_id_remapping.at(constraint.node_id);
constraint_proto->mutable_scan_id()->set_trajectory_id(
constraint.node_id.trajectory_id);
constraint_proto->mutable_scan_id()->set_scan_index(
constraint.node_id.node_index);
node_id.trajectory_id);
constraint_proto->mutable_scan_id()->set_scan_index(node_id.node_index);
constraint_proto->set_tag(mapping::ToProto(constraint.tag));
}
const auto all_trajectory_nodes = GetTrajectoryNodes();
for (size_t trajectory_id = 0; trajectory_id != all_trajectory_nodes.size();
++trajectory_id) {
const auto& trajectory_nodes = all_trajectory_nodes[trajectory_id];
auto* trajectory_proto = proto.add_trajectory();
for (const auto& node : trajectory_nodes) {
auto* node_proto = trajectory_proto->add_node();
node_proto->set_timestamp(common::ToUniversal(node.constant_data->time));
*node_proto->mutable_pose() =
transform::ToProto(node.pose * node.constant_data->tracking_to_pose);
}
if (!trajectory_nodes.empty()) {
for (const auto& transform : GetSubmapTransforms(trajectory_id)) {
*trajectory_proto->add_submap()->mutable_pose() =
transform::ToProto(transform);
}
}
}
return proto;
}

View File

@ -47,9 +47,10 @@ struct TrajectoryNode {
};
common::Time time() const { return constant_data->time; }
bool trimmed() const { return constant_data == nullptr; }
// This must be a shared_ptr. If the data is used for visualization while the
// node is being deleted, it must survive until all use finishes.
// node is being trimmed, it must survive until all use finishes.
std::shared_ptr<const Data> constant_data;
transform::Rigid3d pose;

View File

@ -200,7 +200,8 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(
++node_index) {
const mapping::NodeId node_id{static_cast<int>(trajectory_id),
static_cast<int>(node_index)};
if (submap_data.node_ids.count(node_id) == 0) {
if (!trajectory_nodes_.at(node_id).trimmed() &&
submap_data.node_ids.count(node_id) == 0) {
ComputeConstraint(node_id, submap_id);
}
}
@ -542,14 +543,19 @@ void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
// TODO(hrapp): Make 'Submap' object thread safe and remove submap data in
// there.
// TODO(whess): Mark the 'nodes_to_remove' as pruned and remove their data.
// Also make sure we no longer try to scan match against it.
// Mark the 'nodes_to_remove' as trimmed and remove their data.
for (const mapping::NodeId& node_id : nodes_to_remove) {
CHECK(!parent_->trajectory_nodes_.at(node_id).trimmed());
parent_->trajectory_nodes_.at(node_id).constant_data.reset();
}
// TODO(whess): The optimization problem should no longer include the submap
// and the removed nodes.
// TODO(whess): If the first submap is gone, we want to tie the first not
// yet trimmed submap to be set fixed to its current pose.
// TODO(hrapp): Delete related IMU data.
}
} // namespace mapping_2d