voxel_filter.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 
22 
23 namespace cartographer {
24 namespace sensor {
25 
26 namespace {
27 
28 PointCloud FilterByMaxRange(const PointCloud& point_cloud,
29  const float max_range) {
30  PointCloud result;
31  for (const Eigen::Vector3f& point : point_cloud) {
32  if (point.norm() <= max_range) {
33  result.push_back(point);
34  }
35  }
36  return result;
37 }
38 
39 PointCloud AdaptivelyVoxelFiltered(
40  const proto::AdaptiveVoxelFilterOptions& options,
41  const PointCloud& point_cloud) {
42  if (point_cloud.size() <= options.min_num_points()) {
43  // 'point_cloud' is already sparse enough.
44  return point_cloud;
45  }
46  PointCloud result = VoxelFiltered(point_cloud, options.max_length());
47  if (result.size() >= options.min_num_points()) {
48  // Filtering with 'max_length' resulted in a sufficiently dense point cloud.
49  return result;
50  }
51  // Search for a 'low_length' that is known to result in a sufficiently
52  // dense point cloud. We give up and use the full 'point_cloud' if reducing
53  // the edge length by a factor of 1e-2 is not enough.
54  for (float high_length = options.max_length();
55  high_length > 1e-2f * options.max_length(); high_length /= 2.f) {
56  float low_length = high_length / 2.f;
57  result = VoxelFiltered(point_cloud, low_length);
58  if (result.size() >= options.min_num_points()) {
59  // Binary search to find the right amount of filtering. 'low_length' gave
60  // a sufficiently dense 'result', 'high_length' did not. We stop when the
61  // edge length is at most 10% off.
62  while ((high_length - low_length) / low_length > 1e-1f) {
63  const float mid_length = (low_length + high_length) / 2.f;
64  const PointCloud candidate = VoxelFiltered(point_cloud, mid_length);
65  if (candidate.size() >= options.min_num_points()) {
66  low_length = mid_length;
67  result = candidate;
68  } else {
69  high_length = mid_length;
70  }
71  }
72  return result;
73  }
74  }
75  return result;
76 }
77 
78 } // namespace
79 
80 PointCloud VoxelFiltered(const PointCloud& point_cloud, const float size) {
81  VoxelFilter voxel_filter(size);
82  voxel_filter.InsertPointCloud(point_cloud);
83  return voxel_filter.point_cloud();
84 }
85 
86 VoxelFilter::VoxelFilter(const float size) : voxels_(size) {}
87 
88 void VoxelFilter::InsertPointCloud(const PointCloud& point_cloud) {
89  for (const Eigen::Vector3f& point : point_cloud) {
90  auto* const value = voxels_.mutable_value(voxels_.GetCellIndex(point));
91  if (*value == 0) {
92  point_cloud_.push_back(point);
93  *value = 1;
94  }
95  }
96 }
97 
99 
100 proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions(
101  common::LuaParameterDictionary* const parameter_dictionary) {
102  proto::AdaptiveVoxelFilterOptions options;
103  options.set_max_length(parameter_dictionary->GetDouble("max_length"));
104  options.set_min_num_points(
105  parameter_dictionary->GetNonNegativeInt("min_num_points"));
106  options.set_max_range(parameter_dictionary->GetDouble("max_range"));
107  return options;
108 }
109 
111  const proto::AdaptiveVoxelFilterOptions& options)
112  : options_(options) {}
113 
115  return AdaptivelyVoxelFiltered(
116  options_, FilterByMaxRange(point_cloud, options_.max_range()));
117 }
118 
119 } // namespace sensor
120 } // namespace cartographer
const PointCloud & point_cloud() const
Definition: voxel_filter.cc:98
proto::RangeDataInserterOptions options_
Eigen::Array3i GetCellIndex(const Eigen::Vector3f &point) const
Definition: hybrid_grid.h:428
PointCloud VoxelFiltered(const PointCloud &point_cloud, const float size)
Definition: voxel_filter.cc:80
void InsertPointCloud(const PointCloud &point_cloud)
Definition: voxel_filter.cc:88
AdaptiveVoxelFilter(const proto::AdaptiveVoxelFilterOptions &options)
PointCloud Filter(const PointCloud &point_cloud) const
ValueType * mutable_value(const Eigen::Array3i &index)
Definition: hybrid_grid.h:283
std::vector< Eigen::Vector3f > PointCloud
Definition: point_cloud.h:30
proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions(common::LuaParameterDictionary *const parameter_dictionary)
const proto::AdaptiveVoxelFilterOptions options_
Definition: voxel_filter.h:68
float value
mapping_3d::HybridGridBase< uint8 > voxels_
Definition: voxel_filter.h:50


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