Add 3D localization support. ()

This adds information about which nodes are contained in which
deserialized submaps. This is necessary for the rotational scan
matcher.

Also skips frozen trajectories when tying together nodes of a
trajectory with IMU data which is unavailable in this case.
master
Wolfgang Hess 2017-11-07 11:26:41 +01:00 committed by GitHub
parent 8186316d77
commit 088681f9ec
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
7 changed files with 46 additions and 0 deletions

View File

@ -229,6 +229,22 @@ void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) {
sparse_pose_graph_->AddSubmapFromProto(submap_pose, proto.submap());
}
}
// Add information about which nodes belong to which submap.
for (const proto::SparsePoseGraph::Constraint& constraint_proto :
pose_graph.constraint()) {
if (constraint_proto.tag() !=
mapping::proto::SparsePoseGraph::Constraint::INTRA_SUBMAP) {
continue;
}
const NodeId node_id{
trajectory_remapping.at(constraint_proto.node_id().trajectory_id()),
constraint_proto.node_id().node_index()};
const SubmapId submap_id{
trajectory_remapping.at(constraint_proto.submap_id().trajectory_id()),
constraint_proto.submap_id().submap_index()};
sparse_pose_graph_->AddNodeToSubmap(node_id, submap_id);
}
CHECK(reader->eof());
}

View File

@ -87,6 +87,11 @@ class SparsePoseGraph {
virtual void AddNodeFromProto(const transform::Rigid3d& global_pose,
const proto::Node& node) = 0;
// Adds information that 'node_id' was inserted into 'submap_id'. The submap
// has to be deserialized first.
virtual void AddNodeToSubmap(const NodeId& node_id,
const SubmapId& submap_id) = 0;
// Adds serialized constraints. The corresponding trajectory nodes and submaps
// have to be deserialized before calling this function.
virtual void AddSerializedConstraints(

View File

@ -427,6 +427,14 @@ void SparsePoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose,
});
}
void SparsePoseGraph::AddNodeToSubmap(const mapping::NodeId& node_id,
const mapping::SubmapId& submap_id) {
common::MutexLocker locker(&mutex_);
AddWorkItem([this, node_id, submap_id]() REQUIRES(mutex_) {
submap_data_.at(submap_id).node_ids.insert(node_id);
});
}
void SparsePoseGraph::AddSerializedConstraints(
const std::vector<Constraint>& constraints) {
common::MutexLocker locker(&mutex_);

View File

@ -88,6 +88,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
const mapping::proto::Submap& submap) override;
void AddNodeFromProto(const transform::Rigid3d& global_pose,
const mapping::proto::Node& node) override;
void AddNodeToSubmap(const mapping::NodeId& node_id,
const mapping::SubmapId& submap_id) override;
void AddSerializedConstraints(
const std::vector<Constraint>& constraints) override;
void AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) override;

View File

@ -441,6 +441,14 @@ void SparsePoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose,
});
}
void SparsePoseGraph::AddNodeToSubmap(const mapping::NodeId& node_id,
const mapping::SubmapId& submap_id) {
common::MutexLocker locker(&mutex_);
AddWorkItem([this, node_id, submap_id]() REQUIRES(mutex_) {
submap_data_.at(submap_id).node_ids.insert(node_id);
});
}
void SparsePoseGraph::AddSerializedConstraints(
const std::vector<Constraint>& constraints) {
common::MutexLocker locker(&mutex_);

View File

@ -88,6 +88,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
const mapping::proto::Submap& submap) override;
void AddNodeFromProto(const transform::Rigid3d& global_pose,
const mapping::proto::Node& node) override;
void AddNodeToSubmap(const mapping::NodeId& node_id,
const mapping::SubmapId& submap_id) override;
void AddSerializedConstraints(
const std::vector<Constraint>& constraints) override;
void AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) override;

View File

@ -216,6 +216,11 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
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);
if (frozen_trajectories.count(trajectory_id) != 0) {
// We skip frozen trajectories.
node_it = trajectory_end;
continue;
}
TrajectoryData& trajectory_data = trajectory_data_.at(trajectory_id);
problem.AddParameterBlock(trajectory_data.imu_calibration.data(), 4,