kalman_local_trajectory_builder_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 <memory>
20 #include <random>
21 
22 #include "Eigen/Core"
31 #include "glog/logging.h"
32 #include "gmock/gmock.h"
33 
34 namespace cartographer {
35 namespace mapping_3d {
36 namespace {
37 
38 class KalmanLocalTrajectoryBuilderTest : public ::testing::Test {
39  protected:
40  struct TrajectoryNode {
43  };
44 
45  void SetUp() override { GenerateBubbles(); }
46 
47  proto::LocalTrajectoryBuilderOptions CreateTrajectoryBuilderOptions() {
48  auto parameter_dictionary = common::MakeDictionary(R"text(
49  return {
50  min_range = 0.5,
51  max_range = 50.,
52  scans_per_accumulation = 1,
53  voxel_filter_size = 0.05,
54 
55  high_resolution_adaptive_voxel_filter = {
56  max_length = 0.7,
57  min_num_points = 200,
58  max_range = 50.,
59  },
60 
61  low_resolution_adaptive_voxel_filter = {
62  max_length = 0.7,
63  min_num_points = 200,
64  max_range = 50.,
65  },
66 
67  ceres_scan_matcher = {
68  occupied_space_weight_0 = 5.,
69  occupied_space_weight_1 = 20.,
70  translation_weight = 0.1,
71  rotation_weight = 0.3,
72  only_optimize_yaw = false,
73  ceres_solver_options = {
74  use_nonmonotonic_steps = true,
75  max_num_iterations = 20,
76  num_threads = 1,
77  },
78  },
79  motion_filter = {
80  max_time_seconds = 0.2,
81  max_distance_meters = 0.02,
82  max_angle_radians = 0.001,
83  },
84  submaps = {
85  high_resolution = 0.2,
86  high_resolution_max_range = 50.,
87  low_resolution = 0.5,
88  num_range_data = 45000,
89  range_data_inserter = {
90  hit_probability = 0.7,
91  miss_probability = 0.4,
92  num_free_space_voxels = 0,
93  },
94  },
95 
96  use = "KALMAN",
97  kalman_local_trajectory_builder = {
98  use_online_correlative_scan_matching = false,
99  real_time_correlative_scan_matcher = {
100  linear_search_window = 0.2,
101  angular_search_window = math.rad(1.),
102  translation_delta_cost_weight = 1e-1,
103  rotation_delta_cost_weight = 1.,
104  },
105  pose_tracker = {
106  orientation_model_variance = 5e-4,
107  position_model_variance = 0.000654766,
108  velocity_model_variance = 0.053926,
109  imu_gravity_time_constant = 1.,
110  imu_gravity_variance = 1e-4,
111  num_odometry_states = 1,
112  },
113 
114  scan_matcher_variance = 1e-6,
115  odometer_translational_variance = 1e-7,
116  odometer_rotational_variance = 1e-7,
117  },
118  optimizing_local_trajectory_builder = {
119  high_resolution_grid_weight = 5.,
120  low_resolution_grid_weight = 15.,
121  velocity_weight = 4e1,
122  translation_weight = 1e2,
123  rotation_weight = 1e3,
124  odometry_translation_weight = 1e4,
125  odometry_rotation_weight = 1e4,
126  },
127  }
128  )text");
129  return CreateLocalTrajectoryBuilderOptions(parameter_dictionary.get());
130  }
131 
132  void GenerateBubbles() {
133  std::mt19937 prng(42);
134  std::uniform_real_distribution<float> distribution(-1.f, 1.f);
135 
136  for (int i = 0; i != 100; ++i) {
137  const float x = distribution(prng);
138  const float y = distribution(prng);
139  const float z = distribution(prng);
140  bubbles_.push_back(10. * Eigen::Vector3f(x, y, z).normalized());
141  }
142  }
143 
144  // Computes the earliest intersection of the ray 'from' to 'to' with the
145  // axis-aligned cube with edge length 30 and center at the origin. Assumes
146  // that 'from' is inside the cube.
147  Eigen::Vector3f CollideWithBox(const Eigen::Vector3f& from,
148  const Eigen::Vector3f& to) {
149  float first = 100.f;
150  if (to.x() > from.x()) {
151  first = std::min(first, (15.f - from.x()) / (to.x() - from.x()));
152  } else if (to.x() < from.x()) {
153  first = std::min(first, (-15.f - from.x()) / (to.x() - from.x()));
154  }
155  if (to.y() > from.y()) {
156  first = std::min(first, (15.f - from.y()) / (to.y() - from.y()));
157  } else if (to.y() < from.y()) {
158  first = std::min(first, (-15.f - from.y()) / (to.y() - from.y()));
159  }
160  if (to.z() > from.z()) {
161  first = std::min(first, (15.f - from.z()) / (to.z() - from.z()));
162  } else if (to.z() < from.z()) {
163  first = std::min(first, (-15.f - from.z()) / (to.z() - from.z()));
164  }
165  return first * (to - from) + from;
166  }
167 
168  // Computes the earliest intersection of the ray 'from' to 'to' with all
169  // bubbles. Returns 'to' if no intersection exists.
170  Eigen::Vector3f CollideWithBubbles(const Eigen::Vector3f& from,
171  const Eigen::Vector3f& to) {
172  float first = 1.f;
173  constexpr float kBubbleRadius = 0.5f;
174  for (const Eigen::Vector3f& center : bubbles_) {
175  const float a = (to - from).squaredNorm();
176  const float beta = (to - from).dot(from - center);
177  const float c =
178  (from - center).squaredNorm() - kBubbleRadius * kBubbleRadius;
179  const float discriminant = beta * beta - a * c;
180  if (discriminant < 0.f) {
181  continue;
182  }
183  const float solution = (-beta - std::sqrt(discriminant)) / a;
184  if (solution < 0.f) {
185  continue;
186  }
187  first = std::min(first, solution);
188  }
189  return first * (to - from) + from;
190  }
191 
192  sensor::RangeData GenerateRangeData(const transform::Rigid3d& pose) {
193  // 360 degree rays at 16 angles.
194  sensor::PointCloud directions_in_rangefinder_frame;
195  for (int r = -8; r != 8; ++r) {
196  for (int s = -250; s != 250; ++s) {
197  directions_in_rangefinder_frame.push_back(
198  Eigen::AngleAxisf(M_PI * s / 250., Eigen::Vector3f::UnitZ()) *
199  Eigen::AngleAxisf(M_PI / 12. * r / 8., Eigen::Vector3f::UnitY()) *
200  Eigen::Vector3f::UnitX());
201  // Second orthogonal rangefinder.
202  directions_in_rangefinder_frame.push_back(
203  Eigen::AngleAxisf(M_PI / 2., Eigen::Vector3f::UnitX()) *
204  Eigen::AngleAxisf(M_PI * s / 250., Eigen::Vector3f::UnitZ()) *
205  Eigen::AngleAxisf(M_PI / 12. * r / 8., Eigen::Vector3f::UnitY()) *
206  Eigen::Vector3f::UnitX());
207  }
208  }
209  // We simulate a 30 m edge length box around the origin, also containing
210  // 50 cm radius spheres.
211  sensor::PointCloud returns_in_world_frame;
212  for (const Eigen::Vector3f& direction_in_world_frame :
213  sensor::TransformPointCloud(directions_in_rangefinder_frame,
214  pose.cast<float>())) {
215  const Eigen::Vector3f origin =
216  pose.cast<float>() * Eigen::Vector3f::Zero();
217  returns_in_world_frame.push_back(CollideWithBubbles(
218  origin, CollideWithBox(origin, direction_in_world_frame)));
219  }
220  return {Eigen::Vector3f::Zero(),
221  sensor::TransformPointCloud(returns_in_world_frame,
222  pose.inverse().cast<float>()),
223  {}};
224  }
225 
226  void AddLinearOnlyImuObservation(const common::Time time,
227  const transform::Rigid3d& expected_pose) {
228  const Eigen::Vector3d gravity =
229  expected_pose.rotation().inverse() * Eigen::Vector3d(0., 0., 9.81);
230  local_trajectory_builder_->AddImuData(time, gravity,
231  Eigen::Vector3d::Zero());
232  }
233 
234  std::vector<TrajectoryNode> GenerateCorkscrewTrajectory() {
235  std::vector<TrajectoryNode> trajectory;
236  common::Time current_time = common::FromUniversal(12345678);
237  // One second at zero velocity.
238  for (int i = 0; i != 5; ++i) {
239  current_time += common::FromSeconds(0.3);
240  trajectory.push_back(
241  TrajectoryNode{current_time, transform::Rigid3d::Identity()});
242  }
243  // Corkscrew translation and constant velocity rotation.
244  for (double t = 0.; t <= 0.6; t += 0.006) {
245  current_time += common::FromSeconds(0.3);
246  trajectory.push_back(TrajectoryNode{
247  current_time,
248  transform::Rigid3d::Translation(Eigen::Vector3d(
249  std::sin(4. * t), 1. - std::cos(4. * t), 1. * t)) *
250  transform::Rigid3d::Rotation(Eigen::AngleAxisd(
251  0.3 * t, Eigen::Vector3d(1., -1., 2.).normalized()))});
252  }
253  return trajectory;
254  }
255 
256  void VerifyAccuracy(const std::vector<TrajectoryNode>& expected_trajectory,
257  double expected_accuracy) {
258  int num_poses = 0;
259  for (const TrajectoryNode& node : expected_trajectory) {
260  AddLinearOnlyImuObservation(node.time, node.pose);
261  const auto range_data = GenerateRangeData(node.pose);
262  if (local_trajectory_builder_->AddRangefinderData(
263  node.time, range_data.origin, range_data.returns) != nullptr) {
264  const auto pose_estimate = local_trajectory_builder_->pose_estimate();
265  EXPECT_THAT(pose_estimate.pose, transform::IsNearly(node.pose, 1e-1));
266  ++num_poses;
267  LOG(INFO) << "num_poses: " << num_poses;
268  }
269  }
270  }
271 
272  std::unique_ptr<KalmanLocalTrajectoryBuilder> local_trajectory_builder_;
273  std::vector<Eigen::Vector3f> bubbles_;
274 };
275 
276 TEST_F(KalmanLocalTrajectoryBuilderTest,
277  MoveInsideCubeUsingOnlyCeresScanMatcher) {
279  new KalmanLocalTrajectoryBuilder(CreateTrajectoryBuilderOptions()));
280  VerifyAccuracy(GenerateCorkscrewTrajectory(), 1e-1);
281 }
282 
283 } // namespace
284 } // namespace mapping_3d
285 } // namespace cartographer
PointCloud TransformPointCloud(const PointCloud &point_cloud, const transform::Rigid3f &transform)
Definition: point_cloud.cc:25
static Rigid3 Rotation(const AngleAxis &angle_axis)
std::vector< Eigen::Vector3f > bubbles_
Rigid3< double > Rigid3d
UniversalTimeScaleClock::time_point Time
Definition: time.h:44
transform::Rigid3d pose
std::unique_ptr< LuaParameterDictionary > MakeDictionary(const string &code)
Time FromUniversal(const int64 ticks)
Definition: time.cc:34
Duration FromSeconds(const double seconds)
Definition: time.cc:24
Eigen::Vector2f to
Definition: 3d/submaps.cc:35
Eigen::Vector2f from
Definition: 3d/submaps.cc:34
proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions(common::LuaParameterDictionary *const parameter_dictionary)
proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions(common::LuaParameterDictionary *const parameter_dictionary)
std::vector< Eigen::Vector3f > PointCloud
Definition: point_cloud.h:30
static Rigid3 Translation(const Vector &vector)
std::unique_ptr< KalmanLocalTrajectoryBuilder > local_trajectory_builder_


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