outlier_removing_points_processor.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 
21 #include "glog/logging.h"
22 
23 namespace cartographer {
24 namespace io {
25 
26 std::unique_ptr<OutlierRemovingPointsProcessor>
28  common::LuaParameterDictionary* const dictionary,
29  PointsProcessor* const next) {
30  return common::make_unique<OutlierRemovingPointsProcessor>(
31  dictionary->GetDouble("voxel_size"), next);
32 }
33 
35  const double voxel_size, PointsProcessor* next)
36  : voxel_size_(voxel_size),
37  next_(next),
38  state_(State::kPhase1),
40  LOG(INFO) << "Marking hits...";
41 }
42 
44  std::unique_ptr<PointsBatch> batch) {
45  switch (state_) {
46  case State::kPhase1:
47  ProcessInPhaseOne(*batch);
48  break;
49 
50  case State::kPhase2:
51  ProcessInPhaseTwo(*batch);
52  break;
53 
54  case State::kPhase3:
55  ProcessInPhaseThree(std::move(batch));
56  break;
57  }
58 }
59 
61  switch (state_) {
62  case State::kPhase1:
63  LOG(INFO) << "Counting rays...";
66 
67  case State::kPhase2:
68  LOG(INFO) << "Filtering outliers...";
71 
72  case State::kPhase3:
74  << "Voxel filtering and outlier removal must be configured to occur "
75  "after any stages that require multiple passes.";
77  }
78  LOG(FATAL);
79 }
80 
82  const PointsBatch& batch) {
83  for (size_t i = 0; i < batch.points.size(); ++i) {
84  ++voxels_.mutable_value(voxels_.GetCellIndex(batch.points[i]))->hits;
85  }
86 }
87 
89  const PointsBatch& batch) {
90  // TODO(whess): This samples every 'voxel_size' distance and could be improved
91  // by better ray casting, and also by marking the hits of the current range
92  // data to be excluded.
93  for (size_t i = 0; i < batch.points.size(); ++i) {
94  const Eigen::Vector3f delta = batch.points[i] - batch.origin;
95  const float length = delta.norm();
96  for (float x = 0; x < length; x += voxel_size_) {
97  const Eigen::Array3i index =
98  voxels_.GetCellIndex(batch.origin + (x / length) * delta);
99  if (voxels_.value(index).hits > 0) {
100  ++voxels_.mutable_value(index)->rays;
101  }
102  }
103  }
104 }
105 
107  std::unique_ptr<PointsBatch> batch) {
108  constexpr double kMissPerHitLimit = 3;
109  std::unordered_set<int> to_remove;
110  for (size_t i = 0; i < batch->points.size(); ++i) {
111  const VoxelData voxel =
112  voxels_.value(voxels_.GetCellIndex(batch->points[i]));
113  if (!(voxel.rays < kMissPerHitLimit * voxel.hits)) {
114  to_remove.insert(i);
115  }
116  }
117  RemovePoints(to_remove, batch.get());
118  next_->Process(std::move(batch));
119 }
120 
121 } // namespace io
122 } // namespace cartographer
virtual void Process(std::unique_ptr< PointsBatch > points_batch)=0
void RemovePoints(std::unordered_set< int > to_remove, PointsBatch *batch)
Definition: points_batch.cc:22
OutlierRemovingPointsProcessor(double voxel_size, PointsProcessor *next)
virtual FlushResult Flush()=0
void Process(std::unique_ptr< PointsBatch > batch) override
void ProcessInPhaseThree(std::unique_ptr< PointsBatch > batch)
static std::unique_ptr< OutlierRemovingPointsProcessor > FromDictionary(common::LuaParameterDictionary *dictionary, PointsProcessor *next)
std::vector< Eigen::Vector3f > points
Definition: points_batch.h:55


cartographer
Author(s): The Cartographer Authors
autogenerated on Mon Feb 28 2022 22:00:58