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

Save landmark poses after optimization run. (#896) 

[RFC=0011](https://github.com/googlecartographer/rfcs/blob/master/text/0011-landmarks.md)
master
Alexander Belyaev 2018-02-12 13:15:16 +01:00 committed by GitHub
parent b72bbb20c3
commit ac79f0c034
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 19 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
cartographer/mapping_2d/pose_graph.cc
View File

@ -80,8 +80,8 @@ std::vector<mapping::SubmapId> PoseGraph::InitializeGlobalSubmapPoses(
CHECK(submap_data.BeginOfTrajectory(trajectory_id) != end_it);
const mapping::SubmapId last_submap_id = std::prev(end_it)->id;
if (submap_data_.at(last_submap_id).submap == insertion_submaps.front()) {
// In this case, 'last_submap_id' is the ID of 'insertions_submaps.front()'
// and 'insertions_submaps.back()' is new.
// In this case, 'last_submap_id' is the ID of
// 'insertions_submaps.front()' and 'insertions_submaps.back()' is new.
const auto& first_submap_pose = submap_data.at(last_submap_id).global_pose;
optimization_problem_.AddSubmap(
trajectory_id,
@ -584,6 +584,9 @@ void PoseGraph::RunOptimization() {
old_global_to_new_global * mutable_trajectory_node.global_pose;
}
}
for (const auto& landmark : optimization_problem_.landmark_data()) {
landmark_nodes_[landmark.first].global_landmark_pose = landmark.second;
}
global_submap_poses_ = submap_data;
}

7
cartographer/mapping_2d/pose_graph/optimization_problem.cc
View File

@ -278,7 +278,10 @@ void OptimizationProblem::Solve(
for (const auto& C_node_id_data : C_nodes) {
node_data_.at(C_node_id_data.id).pose = ToPose(C_node_id_data.data);
}
} // namespace pose_graph
for (const auto& C_landmark : C_landmarks) {
landmark_data_[C_landmark.first] = C_landmark.second.ToRigid();
}
}
const mapping::MapById<mapping::NodeId, NodeData>&
OptimizationProblem::node_data() const {
@ -290,7 +293,7 @@ OptimizationProblem::submap_data() const {
return submap_data_;
}
const std::map<std::string, transform::Rigid2d>&
const std::map<std::string, transform::Rigid3d>&
OptimizationProblem::landmark_data() const {
return landmark_data_;
}

4
cartographer/mapping_2d/pose_graph/optimization_problem.h
View File

@ -90,7 +90,7 @@ class OptimizationProblem {
const mapping::MapById<mapping::NodeId, NodeData>& node_data() const;
const mapping::MapById<mapping::SubmapId, SubmapData>& submap_data() const;
const std::map<std::string, transform::Rigid2d>& landmark_data() const;
const std::map<std::string, transform::Rigid3d>& landmark_data() const;
const sensor::MapByTime<sensor::ImuData>& imu_data() const;
const sensor::MapByTime<sensor::OdometryData>& odometry_data() const;
@ -105,7 +105,7 @@ class OptimizationProblem {
mapping::pose_graph::proto::OptimizationProblemOptions options_;
mapping::MapById<mapping::NodeId, NodeData> node_data_;
mapping::MapById<mapping::SubmapId, SubmapData> submap_data_;
std::map<std::string, transform::Rigid2d> landmark_data_;
std::map<std::string, transform::Rigid3d> landmark_data_;
sensor::MapByTime<sensor::ImuData> imu_data_;
sensor::MapByTime<sensor::OdometryData> odometry_data_;
};

3
cartographer/mapping_3d/pose_graph.cc
View File

@ -608,6 +608,9 @@ void PoseGraph::RunOptimization() {
old_global_to_new_global * mutable_trajectory_node.global_pose;
}
}
for (const auto& landmark : optimization_problem_.landmark_data()) {
landmark_nodes_[landmark.first].global_landmark_pose = landmark.second;
}
global_submap_poses_ = submap_data;
// Log the histograms for the pose residuals.

5
cartographer/mapping_3d/pose_graph/optimization_problem.cc
View File

@ -535,7 +535,10 @@ void OptimizationProblem::Solve(
trajectory_data_.at(C_fixed_frame.first).fixed_frame =
C_fixed_frame.second.ToRigid();
}
} // namespace pose_graph
for (const auto& C_landmark : C_landmarks) {
landmark_data_[C_landmark.first] = C_landmark.second.ToRigid();
}
}
const mapping::MapById<mapping::NodeId, NodeData>&
OptimizationProblem::node_data() const {