map_builder.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 
19 #include <cmath>
20 #include <limits>
21 #include <memory>
22 #include <unordered_map>
23 #include <utility>
24 
33 #include "glog/logging.h"
34 
35 namespace cartographer {
36 namespace mapping {
37 
38 proto::MapBuilderOptions CreateMapBuilderOptions(
39  common::LuaParameterDictionary* const parameter_dictionary) {
40  proto::MapBuilderOptions options;
41  options.set_use_trajectory_builder_2d(
42  parameter_dictionary->GetBool("use_trajectory_builder_2d"));
43  options.set_use_trajectory_builder_3d(
44  parameter_dictionary->GetBool("use_trajectory_builder_3d"));
45  options.set_num_background_threads(
46  parameter_dictionary->GetNonNegativeInt("num_background_threads"));
47  *options.mutable_sparse_pose_graph_options() = CreateSparsePoseGraphOptions(
48  parameter_dictionary->GetDictionary("sparse_pose_graph").get());
49  CHECK_NE(options.use_trajectory_builder_2d(),
50  options.use_trajectory_builder_3d());
51  return options;
52 }
53 
54 MapBuilder::MapBuilder(const proto::MapBuilderOptions& options)
55  : options_(options), thread_pool_(options.num_background_threads()) {
56  if (options.use_trajectory_builder_2d()) {
57  sparse_pose_graph_2d_ = common::make_unique<mapping_2d::SparsePoseGraph>(
58  options_.sparse_pose_graph_options(), &thread_pool_);
60  }
61  if (options.use_trajectory_builder_3d()) {
62  sparse_pose_graph_3d_ = common::make_unique<mapping_3d::SparsePoseGraph>(
63  options_.sparse_pose_graph_options(), &thread_pool_);
65  }
66 }
67 
69 
71  const std::unordered_set<string>& expected_sensor_ids,
72  const proto::TrajectoryBuilderOptions& trajectory_options) {
73  const int trajectory_id = trajectory_builders_.size();
74  if (options_.use_trajectory_builder_3d()) {
75  CHECK(trajectory_options.has_trajectory_builder_3d_options());
76  trajectory_builders_.push_back(
77  common::make_unique<CollatedTrajectoryBuilder>(
78  &sensor_collator_, trajectory_id, expected_sensor_ids,
79  common::make_unique<mapping_3d::GlobalTrajectoryBuilder>(
80  trajectory_options.trajectory_builder_3d_options(),
81  trajectory_id, sparse_pose_graph_3d_.get())));
82  } else {
83  CHECK(trajectory_options.has_trajectory_builder_2d_options());
84  trajectory_builders_.push_back(
85  common::make_unique<CollatedTrajectoryBuilder>(
86  &sensor_collator_, trajectory_id, expected_sensor_ids,
87  common::make_unique<mapping_2d::GlobalTrajectoryBuilder>(
88  trajectory_options.trajectory_builder_2d_options(),
89  trajectory_id, sparse_pose_graph_2d_.get())));
90  }
91  return trajectory_id;
92 }
93 
95  const int trajectory_id) const {
96  return trajectory_builders_.at(trajectory_id).get();
97 }
98 
99 void MapBuilder::FinishTrajectory(const int trajectory_id) {
100  sensor_collator_.FinishTrajectory(trajectory_id);
101 }
102 
105 }
106 
107 string MapBuilder::SubmapToProto(const int trajectory_id,
108  const int submap_index,
109  proto::SubmapQuery::Response* const response) {
110  if (trajectory_id < 0 || trajectory_id >= num_trajectory_builders()) {
111  return "Requested submap from trajectory " + std::to_string(trajectory_id) +
112  " but there are only " + std::to_string(num_trajectory_builders()) +
113  " trajectories.";
114  }
115 
116  const int num_submaps = trajectory_builders_.at(trajectory_id)->num_submaps();
117  if (submap_index < 0 || submap_index >= num_submaps) {
118  return "Requested submap " + std::to_string(submap_index) +
119  " from trajectory " + std::to_string(trajectory_id) +
120  " but there are only " + std::to_string(num_submaps) +
121  " submaps in this trajectory.";
122  }
123 
124  const auto submap_data =
125  trajectory_builders_.at(trajectory_id)->GetSubmapData(submap_index);
126  CHECK(submap_data.submap != nullptr);
127  submap_data.submap->ToResponseProto(submap_data.pose, response);
128  return "";
129 }
130 
132  return trajectory_builders_.size();
133 }
134 
136 
137 } // namespace mapping
138 } // namespace cartographer
std::unique_ptr< mapping_3d::SparsePoseGraph > sparse_pose_graph_3d_
Definition: map_builder.h:89
int AddTrajectoryBuilder(const std::unordered_set< string > &expected_sensor_ids, const proto::TrajectoryBuilderOptions &trajectory_options)
Definition: map_builder.cc:70
proto::RangeDataInserterOptions options_
common::ThreadPool thread_pool_
std::vector< std::unique_ptr< mapping::TrajectoryBuilder > > trajectory_builders_
Definition: map_builder.h:93
mapping::SparsePoseGraph * sparse_pose_graph()
Definition: map_builder.cc:135
sensor::Collator sensor_collator_
Definition: map_builder.h:92
proto::MapBuilderOptions CreateMapBuilderOptions(common::LuaParameterDictionary *const parameter_dictionary)
Definition: map_builder.cc:38
std::unique_ptr< mapping_2d::SparsePoseGraph > sparse_pose_graph_2d_
Definition: map_builder.h:88
const proto::MapBuilderOptions options_
Definition: map_builder.h:85
mapping::TrajectoryBuilder * GetTrajectoryBuilder(int trajectory_id) const
Definition: map_builder.cc:94
void FinishTrajectory(int trajectory_id)
Definition: map_builder.cc:99
string SubmapToProto(int trajectory_id, int submap_index, proto::SubmapQuery::Response *response)
Definition: map_builder.cc:107
common::ThreadPool thread_pool_
Definition: map_builder.h:86
mapping::SparsePoseGraph * sparse_pose_graph_
Definition: map_builder.h:90
MapBuilder(const proto::MapBuilderOptions &options)
Definition: map_builder.cc:54
void FinishTrajectory(int trajectory_id)
Definition: collator.cc:36
std::unique_ptr< LuaParameterDictionary > GetDictionary(const string &key)
int GetBlockingTrajectoryId() const
Definition: collator.cc:49
proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(common::LuaParameterDictionary *const parameter_dictionary)


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