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

Support multiple trajectories in LoadMap. (#567)

master
Juraj Oršulić 2017-10-20 16:13:18 +02:00 committed by Wolfgang Hess
parent 0cd0148577
commit f179bd942c
11 changed files with 109 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer/mapping/id.h
View File

@ -249,6 +249,8 @@ class MapById {
// Inserts data (which must not exist already) into a trajectory.
void Insert(const IdType& id, const DataType& data) {
CHECK_GE(id.trajectory_id, 0);
CHECK_GE(GetIndex(id), 0);
auto& trajectory = trajectories_[id.trajectory_id];
trajectory.can_append_ = false;
CHECK(trajectory.data_.emplace(GetIndex(id), data).second);

22
cartographer/mapping/map_builder.cc
View File

@ -177,8 +177,16 @@ void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) {
proto::SparsePoseGraph pose_graph;
CHECK(reader->ReadProto(&pose_graph));
const int map_trajectory_id = AddTrajectoryForDeserialization();
sparse_pose_graph_->FreezeTrajectory(map_trajectory_id);
std::map<int, int> trajectory_remapping;
for (auto& trajectory_proto : *pose_graph.mutable_trajectory()) {
const int new_trajectory_id = AddTrajectoryForDeserialization();
CHECK(trajectory_remapping
.emplace(trajectory_proto.trajectory_id(), new_trajectory_id)
.second)
<< "Duplicate trajectory ID: " << trajectory_proto.trajectory_id();
trajectory_proto.set_trajectory_id(new_trajectory_id);
sparse_pose_graph_->FreezeTrajectory(new_trajectory_id);
}
MapById<SubmapId, transform::Rigid3d> submap_poses;
for (const proto::Trajectory& trajectory_proto : pose_graph.trajectory()) {
@ -205,18 +213,20 @@ void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) {
break;
}
if (proto.has_node()) {
proto.mutable_node()->mutable_node_id()->set_trajectory_id(
trajectory_remapping.at(proto.node().node_id().trajectory_id()));
const transform::Rigid3d node_pose =
node_poses.at(NodeId{proto.node().node_id().trajectory_id(),
proto.node().node_id().node_index()});
sparse_pose_graph_->AddNodeFromProto(map_trajectory_id, node_pose,
proto.node());
sparse_pose_graph_->AddNodeFromProto(node_pose, proto.node());
}
if (proto.has_submap()) {
proto.mutable_submap()->mutable_submap_id()->set_trajectory_id(
trajectory_remapping.at(proto.submap().submap_id().trajectory_id()));
const transform::Rigid3d submap_pose =
submap_poses.at(SubmapId{proto.submap().submap_id().trajectory_id(),
proto.submap().submap_id().submap_index()});
sparse_pose_graph_->AddSubmapFromProto(map_trajectory_id, submap_pose,
proto.submap());
sparse_pose_graph_->AddSubmapFromProto(submap_pose, proto.submap());
}
}
CHECK(reader->eof());

14
cartographer/mapping/sparse_pose_graph.h
View File

@ -77,16 +77,14 @@ class SparsePoseGraph {
// Freezes a trajectory. Poses in this trajectory will not be optimized.
virtual void FreezeTrajectory(int trajectory_id) = 0;
// Adds a 'submap' from a proto with the given 'initial_pose' to the frozen
// trajectory with 'trajectory_id'.
virtual void AddSubmapFromProto(int trajectory_id,
const transform::Rigid3d& initial_pose,
// Adds a 'submap' from a proto with the given 'global_pose' to the
// appropriate frozen trajectory.
virtual void AddSubmapFromProto(const transform::Rigid3d& global_pose,
const proto::Submap& submap) = 0;
// Adds a 'node' from a proto with the given 'pose' to the frozen trajectory
// with 'trajectory_id'.
virtual void AddNodeFromProto(int trajectory_id,
const transform::Rigid3d& pose,
// Adds a 'node' from a proto with the given 'global_pose' to the
// appropriate frozen trajectory.
virtual void AddNodeFromProto(const transform::Rigid3d& global_pose,
const proto::Node& node) = 0;
// Adds a 'trimmer'. It will be used after all data added before it has been

40
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -373,55 +373,55 @@ void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) {
});
}
void SparsePoseGraph::AddSubmapFromProto(const int trajectory_id,
const transform::Rigid3d& initial_pose,
void SparsePoseGraph::AddSubmapFromProto(const transform::Rigid3d& global_pose,
const mapping::proto::Submap& submap) {
if (!submap.has_submap_2d()) {
return;
}
const mapping::SubmapId submap_id = {submap.submap_id().trajectory_id(),
submap.submap_id().submap_index()};
std::shared_ptr<const Submap> submap_ptr =
std::make_shared<const Submap>(submap.submap_2d());
const transform::Rigid2d initial_pose_2d = transform::Project2D(initial_pose);
const transform::Rigid2d global_pose_2d = transform::Project2D(global_pose);
common::MutexLocker locker(&mutex_);
AddTrajectoryIfNeeded(trajectory_id);
const mapping::SubmapId submap_id =
submap_data_.Append(trajectory_id, SubmapData());
AddTrajectoryIfNeeded(submap_id.trajectory_id);
submap_data_.Insert(submap_id, SubmapData());
submap_data_.at(submap_id).submap = submap_ptr;
// Immediately show the submap at the optimized pose.
CHECK_EQ(submap_id,
optimized_submap_transforms_.Append(
trajectory_id, sparse_pose_graph::SubmapData{initial_pose_2d}));
AddWorkItem([this, submap_id, initial_pose_2d]() REQUIRES(mutex_) {
optimized_submap_transforms_.Insert(
submap_id, sparse_pose_graph::SubmapData{global_pose_2d});
AddWorkItem([this, submap_id, global_pose_2d]() REQUIRES(mutex_) {
CHECK_EQ(frozen_trajectories_.count(submap_id.trajectory_id), 1);
submap_data_.at(submap_id).state = SubmapState::kFinished;
optimization_problem_.AddSubmap(submap_id.trajectory_id, initial_pose_2d);
optimization_problem_.InsertSubmap(submap_id, global_pose_2d);
});
}
void SparsePoseGraph::AddNodeFromProto(const int trajectory_id,
const transform::Rigid3d& pose,
void SparsePoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose,
const mapping::proto::Node& node) {
const mapping::NodeId node_id = {node.node_id().trajectory_id(),
node.node_id().node_index()};
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data =
std::make_shared<const mapping::TrajectoryNode::Data>(
mapping::FromProto(node.node_data()));
common::MutexLocker locker(&mutex_);
AddTrajectoryIfNeeded(trajectory_id);
const mapping::NodeId node_id = trajectory_nodes_.Append(
trajectory_id, mapping::TrajectoryNode{constant_data, pose});
AddTrajectoryIfNeeded(node_id.trajectory_id);
trajectory_nodes_.Insert(node_id,
mapping::TrajectoryNode{constant_data, global_pose});
AddWorkItem([this, node_id, pose]() REQUIRES(mutex_) {
AddWorkItem([this, node_id, global_pose]() REQUIRES(mutex_) {
CHECK_EQ(frozen_trajectories_.count(node_id.trajectory_id), 1);
const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
const auto gravity_alignment_inverse = transform::Rigid3d::Rotation(
constant_data->gravity_alignment.inverse());
optimization_problem_.AddTrajectoryNode(
node_id.trajectory_id, constant_data->time,
optimization_problem_.InsertTrajectoryNode(
node_id, constant_data->time,
transform::Project2D(constant_data->local_pose *
gravity_alignment_inverse),
transform::Project2D(pose * gravity_alignment_inverse),
transform::Project2D(global_pose * gravity_alignment_inverse),
constant_data->gravity_alignment);
});
}

5
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -84,10 +84,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
const sensor::FixedFramePoseData& fixed_frame_pose_data);
void FreezeTrajectory(int trajectory_id) override;
void AddSubmapFromProto(int trajectory_id,
const transform::Rigid3d& initial_pose,
void AddSubmapFromProto(const transform::Rigid3d& global_pose,
const mapping::proto::Submap& submap) override;
void AddNodeFromProto(int trajectory_id, const transform::Rigid3d& pose,
void AddNodeFromProto(const transform::Rigid3d& global_pose,
const mapping::proto::Node& node) override;
void AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) override;
void RunFinalOptimization() override;

14
cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc
View File

@ -86,6 +86,15 @@ void OptimizationProblem::AddTrajectoryNode(
NodeData{time, initial_pose, pose, gravity_alignment});
}
void OptimizationProblem::InsertTrajectoryNode(
const mapping::NodeId& node_id, const common::Time time,
const transform::Rigid2d& initial_pose, const transform::Rigid2d& pose,
const Eigen::Quaterniond& gravity_alignment) {
node_data_.Insert(node_id,
NodeData{time, initial_pose, pose, gravity_alignment});
}
void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) {
node_data_.Trim(node_id);
@ -106,6 +115,11 @@ void OptimizationProblem::AddSubmap(const int trajectory_id,
submap_data_.Append(trajectory_id, SubmapData{submap_pose});
}
void OptimizationProblem::InsertSubmap(const mapping::SubmapId& submap_id,
const transform::Rigid2d& submap_pose) {
submap_data_.Insert(submap_id, SubmapData{submap_pose});
}
void OptimizationProblem::TrimSubmap(const mapping::SubmapId& submap_id) {
submap_data_.Trim(submap_id);
}

6
cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h
View File

@ -69,8 +69,14 @@ class OptimizationProblem {
const transform::Rigid2d& initial_pose,
const transform::Rigid2d& pose,
const Eigen::Quaterniond& gravity_alignment);
void InsertTrajectoryNode(const mapping::NodeId& node_id, common::Time time,
const transform::Rigid2d& initial_pose,
const transform::Rigid2d& pose,
const Eigen::Quaterniond& gravity_alignment);
void TrimTrajectoryNode(const mapping::NodeId& node_id);
void AddSubmap(int trajectory_id, const transform::Rigid2d& submap_pose);
void InsertSubmap(const mapping::SubmapId& submap_id,
const transform::Rigid2d& submap_pose);
void TrimSubmap(const mapping::SubmapId& submap_id);
void SetMaxNumIterations(int32 max_num_iterations);

37
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -393,50 +393,49 @@ void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) {
});
}
void SparsePoseGraph::AddSubmapFromProto(const int trajectory_id,
const transform::Rigid3d& initial_pose,
void SparsePoseGraph::AddSubmapFromProto(const transform::Rigid3d& global_pose,
const mapping::proto::Submap& submap) {
if (!submap.has_submap_3d()) {
return;
}
const mapping::SubmapId submap_id = {submap.submap_id().trajectory_id(),
submap.submap_id().submap_index()};
std::shared_ptr<const Submap> submap_ptr =
std::make_shared<const Submap>(submap.submap_3d());
common::MutexLocker locker(&mutex_);
AddTrajectoryIfNeeded(trajectory_id);
const mapping::SubmapId submap_id =
submap_data_.Append(trajectory_id, SubmapData());
AddTrajectoryIfNeeded(submap_id.trajectory_id);
submap_data_.Insert(submap_id, SubmapData());
submap_data_.at(submap_id).submap = submap_ptr;
// Immediately show the submap at the optimized pose.
CHECK_EQ(submap_id,
optimized_submap_transforms_.Append(
trajectory_id, sparse_pose_graph::SubmapData{initial_pose}));
AddWorkItem([this, submap_id, initial_pose]() REQUIRES(mutex_) {
optimized_submap_transforms_.Insert(
submap_id, sparse_pose_graph::SubmapData{global_pose});
AddWorkItem([this, submap_id, global_pose]() REQUIRES(mutex_) {
CHECK_EQ(frozen_trajectories_.count(submap_id.trajectory_id), 1);
submap_data_.at(submap_id).state = SubmapState::kFinished;
optimization_problem_.AddSubmap(submap_id.trajectory_id, initial_pose);
optimization_problem_.InsertSubmap(submap_id, global_pose);
});
}
void SparsePoseGraph::AddNodeFromProto(const int trajectory_id,
const transform::Rigid3d& pose,
void SparsePoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose,
const mapping::proto::Node& node) {
const mapping::NodeId node_id = {node.node_id().trajectory_id(),
node.node_id().node_index()};
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data =
std::make_shared<const mapping::TrajectoryNode::Data>(
mapping::FromProto(node.node_data()));
common::MutexLocker locker(&mutex_);
AddTrajectoryIfNeeded(trajectory_id);
const mapping::NodeId node_id = trajectory_nodes_.Append(
trajectory_id, mapping::TrajectoryNode{constant_data, pose});
AddTrajectoryIfNeeded(node_id.trajectory_id);
trajectory_nodes_.Insert(node_id,
mapping::TrajectoryNode{constant_data, global_pose});
AddWorkItem([this, node_id, pose]() REQUIRES(mutex_) {
AddWorkItem([this, node_id, global_pose]() REQUIRES(mutex_) {
CHECK_EQ(frozen_trajectories_.count(node_id.trajectory_id), 1);
const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
optimization_problem_.AddTrajectoryNode(node_id.trajectory_id,
constant_data->time,
constant_data->local_pose, pose);
optimization_problem_.InsertTrajectoryNode(
node_id, constant_data->time, constant_data->local_pose, global_pose);
});
}

5
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -84,10 +84,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
const sensor::FixedFramePoseData& fixed_frame_pose_data);
void FreezeTrajectory(int trajectory_id) override;
void AddSubmapFromProto(int trajectory_id,
const transform::Rigid3d& initial_pose,
void AddSubmapFromProto(const transform::Rigid3d& global_pose,
const mapping::proto::Submap& submap) override;
void AddNodeFromProto(int trajectory_id, const transform::Rigid3d& pose,
void AddNodeFromProto(const transform::Rigid3d& global_pose,
const mapping::proto::Node& node) override;
void AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) override;
void RunFinalOptimization() override;

18
cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc
View File

@ -89,6 +89,15 @@ void OptimizationProblem::AddTrajectoryNode(
node_data_.Append(trajectory_id, NodeData{time, initial_pose, pose});
}
void OptimizationProblem::InsertTrajectoryNode(
const mapping::NodeId& node_id, const common::Time time,
const transform::Rigid3d& initial_pose, const transform::Rigid3d& pose) {
CHECK_GE(node_id.trajectory_id, 0);
trajectory_data_.resize(std::max(
trajectory_data_.size(), static_cast<size_t>(node_id.trajectory_id) + 1));
node_data_.Insert(node_id, NodeData{time, initial_pose, pose});
}
void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) {
node_data_.Trim(node_id);
@ -112,6 +121,15 @@ void OptimizationProblem::AddSubmap(const int trajectory_id,
submap_data_.Append(trajectory_id, SubmapData{submap_pose});
}
void OptimizationProblem::InsertSubmap(const mapping::SubmapId& submap_id,
const transform::Rigid3d& submap_pose) {
CHECK_GE(submap_id.trajectory_id, 0);
trajectory_data_.resize(
std::max(trajectory_data_.size(),
static_cast<size_t>(submap_id.trajectory_id) + 1));
submap_data_.Insert(submap_id, SubmapData{submap_pose});
}
void OptimizationProblem::TrimSubmap(const mapping::SubmapId& submap_id) {
submap_data_.Trim(submap_id);
}

5
cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h
View File

@ -74,8 +74,13 @@ class OptimizationProblem {
void AddTrajectoryNode(int trajectory_id, common::Time time,
const transform::Rigid3d& initial_pose,
const transform::Rigid3d& pose);
void InsertTrajectoryNode(const mapping::NodeId& node_id, common::Time time,
const transform::Rigid3d& initial_pose,
const transform::Rigid3d& pose);
void TrimTrajectoryNode(const mapping::NodeId& node_id);
void AddSubmap(int trajectory_id, const transform::Rigid3d& submap_pose);
void InsertSubmap(const mapping::SubmapId& submap_id,
const transform::Rigid3d& submap_pose);
void TrimSubmap(const mapping::SubmapId& submap_id);
void SetMaxNumIterations(int32 max_num_iterations);