sparse_pose_graph.cc
Go to the documentation of this file.
1 /*
2  * Copyright 2016 The Cartographer Authors
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
18 
22 #include "glog/logging.h"
23 
24 namespace cartographer {
25 namespace mapping {
26 
27 proto::SparsePoseGraph::Constraint::Tag ToProto(
29  switch (tag) {
30  case SparsePoseGraph::Constraint::Tag::INTRA_SUBMAP:
31  return proto::SparsePoseGraph::Constraint::INTRA_SUBMAP;
32  case SparsePoseGraph::Constraint::Tag::INTER_SUBMAP:
33  return proto::SparsePoseGraph::Constraint::INTER_SUBMAP;
34  }
35  LOG(FATAL) << "Unsupported tag.";
36 }
37 
38 proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
39  common::LuaParameterDictionary* const parameter_dictionary) {
40  proto::SparsePoseGraphOptions options;
41  options.set_optimize_every_n_scans(
42  parameter_dictionary->GetInt("optimize_every_n_scans"));
43  *options.mutable_constraint_builder_options() =
45  parameter_dictionary->GetDictionary("constraint_builder").get());
46  options.set_matcher_translation_weight(
47  parameter_dictionary->GetDouble("matcher_translation_weight"));
48  options.set_matcher_rotation_weight(
49  parameter_dictionary->GetDouble("matcher_rotation_weight"));
50  *options.mutable_optimization_problem_options() =
52  parameter_dictionary->GetDictionary("optimization_problem").get());
53  options.set_max_num_final_iterations(
54  parameter_dictionary->GetNonNegativeInt("max_num_final_iterations"));
55  CHECK_GT(options.max_num_final_iterations(), 0);
56  options.set_global_sampling_ratio(
57  parameter_dictionary->GetDouble("global_sampling_ratio"));
58  return options;
59 }
60 
61 proto::SparsePoseGraph SparsePoseGraph::ToProto() {
62  proto::SparsePoseGraph proto;
63 
64  std::map<NodeId, NodeId> node_id_remapping; // Due to trimming.
65 
66  const auto all_trajectory_nodes = GetTrajectoryNodes();
67  for (size_t trajectory_id = 0; trajectory_id != all_trajectory_nodes.size();
68  ++trajectory_id) {
69  const auto& single_trajectory_nodes = all_trajectory_nodes[trajectory_id];
70  auto* trajectory_proto = proto.add_trajectory();
71 
72  for (size_t old_node_index = 0;
73  old_node_index != single_trajectory_nodes.size(); ++old_node_index) {
74  const auto& node = single_trajectory_nodes[old_node_index];
75  if (!node.trimmed()) {
76  node_id_remapping[NodeId{static_cast<int>(trajectory_id),
77  static_cast<int>(old_node_index)}] =
78  NodeId{static_cast<int>(trajectory_id),
79  static_cast<int>(trajectory_proto->node_size())};
80  auto* node_proto = trajectory_proto->add_node();
81  node_proto->set_timestamp(
82  common::ToUniversal(node.constant_data->time));
83  *node_proto->mutable_pose() = transform::ToProto(
84  node.pose * node.constant_data->tracking_to_pose);
85  }
86  }
87 
88  if (!single_trajectory_nodes.empty()) {
89  const int num_submaps_in_trajectory = num_submaps(trajectory_id);
90  for (int submap_index = 0; submap_index != num_submaps_in_trajectory;
91  ++submap_index) {
92  const SubmapId submap_id{static_cast<int>(trajectory_id), submap_index};
93  *trajectory_proto->add_submap()->mutable_pose() =
95  }
96  }
97  }
98 
99  for (const auto& constraint : constraints()) {
100  auto* const constraint_proto = proto.add_constraint();
101  *constraint_proto->mutable_relative_pose() =
102  transform::ToProto(constraint.pose.zbar_ij);
103  constraint_proto->set_translation_weight(
104  constraint.pose.translation_weight);
105  constraint_proto->set_rotation_weight(constraint.pose.rotation_weight);
106  constraint_proto->mutable_submap_id()->set_trajectory_id(
107  constraint.submap_id.trajectory_id);
108  constraint_proto->mutable_submap_id()->set_submap_index(
109  constraint.submap_id.submap_index);
110 
111  const NodeId node_id = node_id_remapping.at(constraint.node_id);
112  constraint_proto->mutable_scan_id()->set_trajectory_id(
113  node_id.trajectory_id);
114  constraint_proto->mutable_scan_id()->set_scan_index(node_id.node_index);
115 
116  constraint_proto->set_tag(mapping::ToProto(constraint.tag));
117  }
118 
119  return proto;
120 }
121 
122 } // namespace mapping
123 } // namespace cartographer
virtual transform::Rigid3d GetSubmapTransform(const SubmapId &submap_id)=0
virtual std::vector< std::vector< TrajectoryNode > > GetTrajectoryNodes()=0
proto::Rigid2d ToProto(const transform::Rigid2d &transform)
Definition: transform.cc:44
int64 ToUniversal(const Time time)
Definition: time.cc:36
proto::OptimizationProblemOptions CreateOptimizationProblemOptions(common::LuaParameterDictionary *const parameter_dictionary)
virtual std::vector< Constraint > constraints()=0
virtual int num_submaps(int trajectory_id)=0
proto::SparsePoseGraph::Constraint::Tag ToProto(const SparsePoseGraph::Constraint::Tag &tag)
std::unique_ptr< LuaParameterDictionary > GetDictionary(const string &key)
proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(common::LuaParameterDictionary *const parameter_dictionary)
proto::ConstraintBuilderOptions CreateConstraintBuilderOptions(common::LuaParameterDictionary *const parameter_dictionary)


cartographer
Author(s):
autogenerated on Wed Jun 5 2019 21:57:58