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

Deserialize trajectory nodes. (#569)

master
Juraj Oršulić 2017-10-06 16:16:25 +02:00 committed by Wolfgang Hess
parent 6708930bbf
commit 2f332eca28
7 changed files with 73 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
cartographer/mapping/id.h
View File

@ -31,6 +31,13 @@ struct NodeId {
int trajectory_id;
int node_index;
bool operator==(const NodeId& other) const {
return std::forward_as_tuple(trajectory_id, node_index) ==
std::forward_as_tuple(other.trajectory_id, other.node_index);
}
bool operator!=(const NodeId& other) const { return !operator==(other); }
bool operator<(const NodeId& other) const {
return std::forward_as_tuple(trajectory_id, node_index) <
std::forward_as_tuple(other.trajectory_id, other.node_index);

9
cartographer/mapping/map_builder.cc
View File

@ -218,6 +218,15 @@ void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) {
if (!reader->ReadProto(&proto)) {
break;
}
if (proto.has_node()) {
const auto& pose_graph_node =
pose_graph.trajectory(proto.node().node_id().trajectory_id())
.node(proto.node().node_id().node_index());
const transform::Rigid3d pose =
transform::ToRigid3(pose_graph_node.pose());
sparse_pose_graph_->AddNodeFromProto(map_trajectory_id, pose,
proto.node());
}
if (proto.has_submap()) {
const transform::Rigid3d submap_pose = transform::ToRigid3(
pose_graph.trajectory(proto.submap().submap_id().trajectory_id())

6
cartographer/mapping/sparse_pose_graph.h
View File

@ -83,6 +83,12 @@ class SparsePoseGraph {
const transform::Rigid3d& initial_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,
const proto::Node& node) = 0;
// Adds a 'trimmer'. It will be used after all data added before it has been
// included in the pose graph.
virtual void AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) = 0;

26
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -439,6 +439,32 @@ void SparsePoseGraph::AddSubmapFromProto(const int trajectory_id,
});
}
void SparsePoseGraph::AddNodeFromProto(const int trajectory_id,
const transform::Rigid3d& pose,
const mapping::proto::Node& node) {
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_);
trajectory_connectivity_state_.Add(trajectory_id);
const mapping::NodeId node_id = trajectory_nodes_.Append(
trajectory_id, mapping::TrajectoryNode{constant_data, pose});
AddWorkItem([this, node_id, 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,
transform::Project2D(constant_data->initial_pose *
gravity_alignment_inverse),
transform::Project2D(pose * gravity_alignment_inverse),
constant_data->gravity_alignment);
});
}
void SparsePoseGraph::AddTrimmer(
std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) {
common::MutexLocker locker(&mutex_);

2
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -86,6 +86,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
void AddSubmapFromProto(int trajectory_id,
const transform::Rigid3d& initial_pose,
const mapping::proto::Submap& submap) override;
void AddNodeFromProto(int trajectory_id, const transform::Rigid3d& pose,
const mapping::proto::Node& node) override;
void AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) override;
void RunFinalOptimization() override;
std::vector<std::vector<int>> GetConnectedTrajectories() override;

21
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -458,6 +458,27 @@ void SparsePoseGraph::AddSubmapFromProto(const int trajectory_id,
});
}
void SparsePoseGraph::AddNodeFromProto(const int trajectory_id,
const transform::Rigid3d& pose,
const mapping::proto::Node& node) {
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_);
trajectory_connectivity_state_.Add(trajectory_id);
const mapping::NodeId node_id = trajectory_nodes_.Append(
trajectory_id, mapping::TrajectoryNode{constant_data, pose});
AddWorkItem([this, node_id, 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->initial_pose, pose);
});
}
void SparsePoseGraph::AddTrimmer(
std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) {
common::MutexLocker locker(&mutex_);

2
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -87,6 +87,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
void AddSubmapFromProto(int trajectory_id,
const transform::Rigid3d& initial_pose,
const mapping::proto::Submap& submap) override;
void AddNodeFromProto(int trajectory_id, const transform::Rigid3d& pose,
const mapping::proto::Node& node) override;
void AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) override;
void RunFinalOptimization() override;
std::vector<std::vector<int>> GetConnectedTrajectories() override;