Adds a voxel filtering and moving objects removing points processor. (#99)

cartographer/io/CMakeLists.txt

@@ -49,6 +49,19 @@ google_library(io_null_points_processor
     io_points_processor
 )
 
+google_library(io_outlier_removing_points_processor
+  USES_GLOG
+  SRCS
+    outlier_removing_points_processor.cc
+  HDRS
+    outlier_removing_points_processor.h
+  DEPENDS
+    common_lua_parameter_dictionary
+    common_make_unique
+    io_points_processor
+    mapping_3d_hybrid_grid
+)
+
 google_library(io_pcd_writing_points_processor
   USES_GLOG
   SRCS
@@ -104,6 +117,7 @@ google_library(io_points_processor_pipeline_builder
     io_fixed_ratio_sampling_points_processor
     io_min_max_range_filtering_points_processor
     io_null_points_processor
+    io_outlier_removing_points_processor
     io_pcd_writing_points_processor
     io_ply_writing_points_processor
     io_points_processor

cartographer/io/outlier_removing_points_processor.cc

@@ -0,0 +1,119 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "cartographer/io/outlier_removing_points_processor.h"
+
+#include "cartographer/common/lua_parameter_dictionary.h"
+#include "cartographer/common/make_unique.h"
+#include "glog/logging.h"
+
+namespace cartographer {
+namespace io {
+
+std::unique_ptr<OutlierRemovingPointsProcessor>
+OutlierRemovingPointsProcessor::FromDictionary(
+    common::LuaParameterDictionary* const dictionary,
+    PointsProcessor* const next) {
+  return common::make_unique<OutlierRemovingPointsProcessor>(
+      dictionary->GetDouble("voxel_size"), next);
+}
+
+OutlierRemovingPointsProcessor::OutlierRemovingPointsProcessor(
+    const double voxel_size, PointsProcessor* next)
+    : voxel_size_(voxel_size),
+      next_(next),
+      state_(State::kPhase1),
+      voxels_(voxel_size_, Eigen::Vector3f::Zero()) {
+  LOG(INFO) << "Marking hits...";
+}
+
+void OutlierRemovingPointsProcessor::Process(
+    std::unique_ptr<PointsBatch> points) {
+  switch (state_) {
+    case State::kPhase1:
+      ProcessInPhaseOne(*points);
+      break;
+
+    case State::kPhase2:
+      ProcessInPhaseTwo(*points);
+      break;
+
+    case State::kPhase3:
+      ProcessInPhaseThree(std::move(points));
+      break;
+  }
+}
+
+PointsProcessor::FlushResult OutlierRemovingPointsProcessor::Flush() {
+  switch (state_) {
+    case State::kPhase1:
+      LOG(INFO) << "Counting rays...";
+      state_ = State::kPhase2;
+      return FlushResult::kRestartStream;
+
+    case State::kPhase2:
+      LOG(INFO) << "Filtering outliers...";
+      state_ = State::kPhase3;
+      return FlushResult::kRestartStream;
+
+    case State::kPhase3:
+      CHECK(next_->Flush() == FlushResult::kFinished)
+          << "Voxel filtering and outlier removal must be configured to occur "
+             "after any stages that require multiple passes.";
+      return FlushResult::kFinished;
+  }
+  LOG(FATAL);
+}
+
+void OutlierRemovingPointsProcessor::ProcessInPhaseOne(
+    const PointsBatch& batch) {
+  for (size_t i = 0; i < batch.points.size(); ++i) {
+    ++voxels_.mutable_value(voxels_.GetCellIndex(batch.points[i]))->hits;
+  }
+}
+
+void OutlierRemovingPointsProcessor::ProcessInPhaseTwo(
+    const PointsBatch& batch) {
+  // TODO(whess): This samples every 'voxel_size' distance and could be improved
+  // by better ray casting, and also by marking the hits of the current laser
+  // fan to be excluded.
+  for (size_t i = 0; i < batch.points.size(); ++i) {
+    const Eigen::Vector3f delta = batch.points[i] - batch.origin;
+    const float length = delta.norm();
+    for (float x = 0; x < length; x += voxel_size_) {
+      const auto index =
+          voxels_.GetCellIndex(batch.origin + (x / length) * delta);
+      if (voxels_.value(index).hits > 0) {
+        ++voxels_.mutable_value(index)->rays;
+      }
+    }
+  }
+}
+
+void OutlierRemovingPointsProcessor::ProcessInPhaseThree(
+    std::unique_ptr<PointsBatch> batch) {
+  constexpr double kMissPerHitLimit = 3;
+  std::vector<int> to_remove;
+  for (size_t i = 0; i < batch->points.size(); ++i) {
+    const auto voxel = voxels_.value(voxels_.GetCellIndex(batch->points[i]));
+    to_remove.push_back(!(voxel.rays < kMissPerHitLimit * voxel.hits));
+  }
+  RemovePoints(to_remove, batch.get());
+  next_->Process(std::move(batch));
+}
+
+}  // namespace io
+}  // namespace cartographer

cartographer/io/outlier_removing_points_processor.h

@@ -0,0 +1,87 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CARTOGRAPHER_IO_OUTLIER_REMOVING_POINTS_PROCESSOR_H_
+#define CARTOGRAPHER_IO_OUTLIER_REMOVING_POINTS_PROCESSOR_H_
+
+#include "cartographer/common/lua_parameter_dictionary.h"
+#include "cartographer/io/points_processor.h"
+#include "cartographer/mapping_3d/hybrid_grid.h"
+
+namespace cartographer {
+namespace io {
+
+// Voxel filters the data and only passes on points that we believe are on
+// non-moving objects.
+class OutlierRemovingPointsProcessor : public PointsProcessor {
+ public:
+  constexpr static const char* kConfigurationFileActionName =
+      "voxel_filter_and_remove_moving_objects";
+
+  OutlierRemovingPointsProcessor(double voxel_size, PointsProcessor* next);
+
+  static std::unique_ptr<OutlierRemovingPointsProcessor> FromDictionary(
+      common::LuaParameterDictionary* dictionary, PointsProcessor* next);
+
+  ~OutlierRemovingPointsProcessor() override {}
+
+  OutlierRemovingPointsProcessor(const OutlierRemovingPointsProcessor&) =
+      delete;
+  OutlierRemovingPointsProcessor& operator=(
+      const OutlierRemovingPointsProcessor&) = delete;
+
+  void Process(std::unique_ptr<PointsBatch> batch) override;
+  FlushResult Flush() override;
+
+ private:
+  // To reduce memory consumption by not having to keep all rays in memory, we
+  // filter outliers in three phases each going over all data: First we compute
+  // all voxels containing any hits, then we compute the rays passing through
+  // each of these voxels, and finally we output all hits in voxels that are
+  // considered obstructed.
+  struct VoxelData {
+    int hits = 0;
+    int rays = 0;
+  };
+  enum class State {
+    kPhase1,
+    kPhase2,
+    kPhase3,
+  };
+
+  // First phase counts the number of hits per voxel.
+  void ProcessInPhaseOne(const PointsBatch& batch);
+
+  // Second phase counts how many rays pass through each voxel. This is only
+  // done for voxels that contain hits. This is to reduce memory consumption by
+  // not adding data to free voxels.
+  void ProcessInPhaseTwo(const PointsBatch& batch);
+
+  // Third phase produces the output containing all inliers. We consider each
+  // hit an inlier if it is inside a voxel that has a sufficiently high
+  // hit-to-ray ratio.
+  void ProcessInPhaseThree(std::unique_ptr<PointsBatch> batch);
+
+  const double voxel_size_;
+  PointsProcessor* const next_;
+  State state_;
+  mapping_3d::HybridGridBase<VoxelData> voxels_;
+};
+
+}  // namespace io
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_IO_OUTLIER_REMOVING_POINTS_PROCESSOR_H_

cartographer/io/points_processor_pipeline_builder.cc

@@ -21,6 +21,7 @@
 #include "cartographer/io/fixed_ratio_sampling_points_processor.h"
 #include "cartographer/io/min_max_range_filtering_points_processor.h"
 #include "cartographer/io/null_points_processor.h"
+#include "cartographer/io/outlier_removing_points_processor.h"
 #include "cartographer/io/pcd_writing_points_processor.h"
 #include "cartographer/io/ply_writing_points_processor.h"
 #include "cartographer/io/xray_points_processor.h"
@@ -33,6 +34,7 @@ PointsProcessorPipelineBuilder::PointsProcessorPipelineBuilder() {
   RegisterNonStatic<CountingPointsProcessor>();
   RegisterNonStatic<FixedRatioSamplingPointsProcessor>();
   RegisterNonStatic<MinMaxRangeFiteringPointsProcessor>();
+  RegisterNonStatic<OutlierRemovingPointsProcessor>();
   RegisterNonStatic<PcdWritingPointsProcessor>();
   RegisterNonStatic<PlyWritingPointsProcessor>();
   RegisterNonStatic<XRayPointsProcessor>();