12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add 3D localization support. (#632) 

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
cartographer/mapping/map_builder.cc
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());
}

5
cartographer/mapping/sparse_pose_graph.h
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(

8
cartographer/mapping_2d/sparse_pose_graph.cc
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_);

2
cartographer/mapping_2d/sparse_pose_graph.h
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;

8
cartographer/mapping_3d/sparse_pose_graph.cc
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_);

2
cartographer/mapping_3d/sparse_pose_graph.h
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;

5
cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc
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,