sparse_pose_graph_test.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 <memory>
21 #include <random>
22 
32 #include "gmock/gmock.h"
33 
34 namespace cartographer {
35 namespace mapping_2d {
36 namespace {
37 
38 class SparsePoseGraphTest : public ::testing::Test {
39  protected:
40  SparsePoseGraphTest() : thread_pool_(1) {
41  // Builds a wavy, irregularly circular point cloud that is unique
42  // rotationally. This gives us good rotational texture and avoids any
43  // possibility of the CeresScanMatcher preferring free space (>
44  // kMinProbability) to unknown space (== kMinProbability).
45  for (float t = 0.f; t < 2.f * M_PI; t += 0.005f) {
46  const float r = (std::sin(20.f * t) + 2.f) * std::sin(t + 2.f);
47  point_cloud_.emplace_back(r * std::sin(t), r * std::cos(t), 0.f);
48  }
49 
50  {
51  auto parameter_dictionary = common::MakeDictionary(R"text(
52  return {
53  resolution = 0.05,
54  half_length = 21.,
55  num_range_data = 1,
56  output_debug_images = false,
57  range_data_inserter = {
58  insert_free_space = true,
59  hit_probability = 0.53,
60  miss_probability = 0.495,
61  },
62  })text");
63  submaps_ = common::make_unique<Submaps>(
64  CreateSubmapsOptions(parameter_dictionary.get()));
65  }
66 
67  {
68  auto parameter_dictionary = common::MakeDictionary(R"text(
69  return {
70  optimize_every_n_scans = 1000,
71  constraint_builder = {
72  sampling_ratio = 1.,
73  max_constraint_distance = 6.,
74  adaptive_voxel_filter = {
75  max_length = 1e-2,
76  min_num_points = 1000,
77  max_range = 50.,
78  },
79  min_score = 0.5,
80  global_localization_min_score = 0.6,
81  loop_closure_translation_weight = 1.,
82  loop_closure_rotation_weight = 1.,
83  log_matches = true,
84  fast_correlative_scan_matcher = {
85  linear_search_window = 3.,
86  angular_search_window = 0.1,
87  branch_and_bound_depth = 3,
88  },
89  ceres_scan_matcher = {
90  occupied_space_weight = 20.,
91  translation_weight = 10.,
92  rotation_weight = 1.,
93  ceres_solver_options = {
94  use_nonmonotonic_steps = true,
95  max_num_iterations = 50,
96  num_threads = 1,
97  },
98  },
99  fast_correlative_scan_matcher_3d = {
100  branch_and_bound_depth = 3,
101  full_resolution_depth = 3,
102  rotational_histogram_size = 30,
103  min_rotational_score = 0.1,
104  linear_xy_search_window = 4.,
105  linear_z_search_window = 4.,
106  angular_search_window = 0.1,
107  },
108  ceres_scan_matcher_3d = {
109  occupied_space_weight_0 = 20.,
110  translation_weight = 10.,
111  rotation_weight = 1.,
112  only_optimize_yaw = true,
113  ceres_solver_options = {
114  use_nonmonotonic_steps = true,
115  max_num_iterations = 50,
116  num_threads = 1,
117  },
118  },
119  },
120  matcher_translation_weight = 1.,
121  matcher_rotation_weight = 1.,
122  optimization_problem = {
123  acceleration_weight = 1.,
124  rotation_weight = 1e2,
125  huber_scale = 1.,
126  consecutive_scan_translation_penalty_factor = 0.,
127  consecutive_scan_rotation_penalty_factor = 0.,
128  log_solver_summary = true,
129  ceres_solver_options = {
130  use_nonmonotonic_steps = false,
131  max_num_iterations = 200,
132  num_threads = 1,
133  },
134  },
135  max_num_final_iterations = 200,
136  global_sampling_ratio = 0.01,
137  })text");
138  sparse_pose_graph_ = common::make_unique<SparsePoseGraph>(
139  mapping::CreateSparsePoseGraphOptions(parameter_dictionary.get()),
140  &thread_pool_);
141  }
142 
144  }
145 
146  void MoveRelativeWithNoise(const transform::Rigid2d& movement,
147  const transform::Rigid2d& noise) {
148  current_pose_ = current_pose_ * movement;
149  const sensor::PointCloud new_point_cloud = sensor::TransformPointCloud(
150  point_cloud_,
151  transform::Embed3D(current_pose_.inverse().cast<float>()));
152  const mapping::Submap* const matching_submap =
153  submaps_->Get(submaps_->matching_index());
154  std::vector<const mapping::Submap*> insertion_submaps;
155  for (int insertion_index : submaps_->insertion_indices()) {
156  insertion_submaps.push_back(submaps_->Get(insertion_index));
157  }
158  const sensor::RangeData range_data{
159  Eigen::Vector3f::Zero(), new_point_cloud, {}};
160  const transform::Rigid2d pose_estimate = noise * current_pose_;
161  constexpr int kTrajectoryId = 0;
162  submaps_->InsertRangeData(TransformRangeData(
163  range_data, transform::Embed3D(pose_estimate.cast<float>())));
164  sparse_pose_graph_->AddScan(
166  pose_estimate, kTrajectoryId, matching_submap, insertion_submaps);
167  }
168 
169  void MoveRelative(const transform::Rigid2d& movement) {
170  MoveRelativeWithNoise(movement, transform::Rigid2d::Identity());
171  }
172 
174  std::unique_ptr<Submaps> submaps_;
175  common::ThreadPool thread_pool_;
176  std::unique_ptr<SparsePoseGraph> sparse_pose_graph_;
178 };
179 
180 TEST_F(SparsePoseGraphTest, EmptyMap) {
181  sparse_pose_graph_->RunFinalOptimization();
182  const auto nodes = sparse_pose_graph_->GetTrajectoryNodes();
183  EXPECT_THAT(nodes.size(), ::testing::Eq(0));
184 }
185 
186 TEST_F(SparsePoseGraphTest, NoMovement) {
187  MoveRelative(transform::Rigid2d::Identity());
188  MoveRelative(transform::Rigid2d::Identity());
189  MoveRelative(transform::Rigid2d::Identity());
190  sparse_pose_graph_->RunFinalOptimization();
191  const auto nodes = sparse_pose_graph_->GetTrajectoryNodes();
192  ASSERT_THAT(nodes.size(), ::testing::Eq(1));
193  EXPECT_THAT(nodes[0].size(), ::testing::Eq(3));
194  EXPECT_THAT(nodes[0][0].pose,
195  transform::IsNearly(transform::Rigid3d::Identity(), 1e-2));
196  EXPECT_THAT(nodes[0][1].pose,
197  transform::IsNearly(transform::Rigid3d::Identity(), 1e-2));
198  EXPECT_THAT(nodes[0][2].pose,
199  transform::IsNearly(transform::Rigid3d::Identity(), 1e-2));
200 }
201 
202 TEST_F(SparsePoseGraphTest, NoOverlappingScans) {
203  std::mt19937 rng(0);
204  std::uniform_real_distribution<double> distribution(-1., 1.);
205  std::vector<transform::Rigid2d> poses;
206  for (int i = 0; i != 4; ++i) {
207  MoveRelative(transform::Rigid2d({0.25 * distribution(rng), 5.}, 0.));
208  poses.emplace_back(current_pose_);
209  }
210  sparse_pose_graph_->RunFinalOptimization();
211  const auto nodes = sparse_pose_graph_->GetTrajectoryNodes();
212  ASSERT_THAT(nodes.size(), ::testing::Eq(1));
213  for (int i = 0; i != 4; ++i) {
214  EXPECT_THAT(poses[i],
215  IsNearly(transform::Project2D(nodes[0][i].pose), 1e-2))
216  << i;
217  }
218 }
219 
220 TEST_F(SparsePoseGraphTest, ConsecutivelyOverlappingScans) {
221  std::mt19937 rng(0);
222  std::uniform_real_distribution<double> distribution(-1., 1.);
223  std::vector<transform::Rigid2d> poses;
224  for (int i = 0; i != 5; ++i) {
225  MoveRelative(transform::Rigid2d({0.25 * distribution(rng), 2.}, 0.));
226  poses.emplace_back(current_pose_);
227  }
228  sparse_pose_graph_->RunFinalOptimization();
229  const auto nodes = sparse_pose_graph_->GetTrajectoryNodes();
230  ASSERT_THAT(nodes.size(), ::testing::Eq(1));
231  for (int i = 0; i != 5; ++i) {
232  EXPECT_THAT(poses[i],
233  IsNearly(transform::Project2D(nodes[0][i].pose), 1e-2))
234  << i;
235  }
236 }
237 
238 TEST_F(SparsePoseGraphTest, OverlappingScans) {
239  std::mt19937 rng(0);
240  std::uniform_real_distribution<double> distribution(-1., 1.);
241  std::vector<transform::Rigid2d> ground_truth;
242  std::vector<transform::Rigid2d> poses;
243  for (int i = 0; i != 5; ++i) {
244  const double noise_x = 0.1 * distribution(rng);
245  const double noise_y = 0.1 * distribution(rng);
246  const double noise_orientation = 0.1 * distribution(rng);
247  transform::Rigid2d noise({noise_x, noise_y}, noise_orientation);
248  MoveRelativeWithNoise(
249  transform::Rigid2d({0.15 * distribution(rng), 0.4}, 0.), noise);
250  ground_truth.emplace_back(current_pose_);
251  poses.emplace_back(noise * current_pose_);
252  }
253  sparse_pose_graph_->RunFinalOptimization();
254  const auto nodes = sparse_pose_graph_->GetTrajectoryNodes();
255  ASSERT_THAT(nodes.size(), ::testing::Eq(1));
256  transform::Rigid2d true_movement =
257  ground_truth.front().inverse() * ground_truth.back();
258  transform::Rigid2d movement_before = poses.front().inverse() * poses.back();
259  transform::Rigid2d error_before = movement_before.inverse() * true_movement;
260  transform::Rigid3d optimized_movement =
261  nodes[0].front().pose.inverse() * nodes[0].back().pose;
262  transform::Rigid2d optimized_error =
263  transform::Project2D(optimized_movement).inverse() * true_movement;
264  EXPECT_THAT(std::abs(optimized_error.normalized_angle()),
265  ::testing::Lt(std::abs(error_before.normalized_angle())));
266  EXPECT_THAT(optimized_error.translation().norm(),
267  ::testing::Lt(error_before.translation().norm()));
268 }
269 
270 } // namespace
271 } // namespace mapping_2d
272 } // namespace cartographer
PointCloud TransformPointCloud(const PointCloud &point_cloud, const transform::Rigid3f &transform)
Definition: point_cloud.cc:25
common::ThreadPool thread_pool_
std::unique_ptr< SparsePoseGraph > sparse_pose_graph_
Rigid3< double > Rigid3d
Rigid3< T > Embed3D(const Rigid2< T > &transform)
Definition: transform.h:110
RangeData TransformRangeData(const RangeData &range_data, const transform::Rigid3f &transform)
Definition: range_data.cc:41
transform::Rigid3d pose
Rigid2< T > Project2D(const Rigid3< T > &transform)
Definition: transform.h:103
static Rigid2< FloatType > Identity()
Rigid2< double > Rigid2d
std::unique_ptr< LuaParameterDictionary > MakeDictionary(const string &code)
Time FromUniversal(const int64 ticks)
Definition: time.cc:34
proto::SubmapsOptions CreateSubmapsOptions(common::LuaParameterDictionary *const parameter_dictionary)
Definition: 2d/submaps.cc:87
sensor::PointCloud point_cloud_
std::vector< Eigen::Vector3f > PointCloud
Definition: point_cloud.h:30
transform::Rigid2d current_pose_
std::unique_ptr< Submaps > submaps_
proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(common::LuaParameterDictionary *const parameter_dictionary)


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