Add VerticalRangeFilteringPointsProcessor. (#1636)

Add an extra filter for 3D data.
Solve the problem with 2D maps created from 3D sensors.

cartographer/io/points_processor_pipeline_builder.cc

@@ -29,6 +29,7 @@
 #include "cartographer/io/pcd_writing_points_processor.h"
 #include "cartographer/io/ply_writing_points_processor.h"
 #include "cartographer/io/probability_grid_points_processor.h"
+#include "cartographer/io/vertical_range_filtering_points_processor.h"
 #include "cartographer/io/xray_points_processor.h"
 #include "cartographer/io/xyz_writing_points_processor.h"
 #include "cartographer/mapping/proto/trajectory.pb.h"
@@ -84,6 +85,7 @@ void RegisterBuiltInPointsProcessors(
   RegisterPlainPointsProcessor<FixedRatioSamplingPointsProcessor>(builder);
   RegisterPlainPointsProcessor<FrameIdFilteringPointsProcessor>(builder);
   RegisterPlainPointsProcessor<MinMaxRangeFilteringPointsProcessor>(builder);
+  RegisterPlainPointsProcessor<VerticalRangeFilteringPointsProcessor>(builder);
   RegisterPlainPointsProcessor<OutlierRemovingPointsProcessor>(builder);
   RegisterPlainPointsProcessor<ColoringPointsProcessor>(builder);
   RegisterPlainPointsProcessor<IntensityToColorPointsProcessor>(builder);

cartographer/io/vertical_range_filtering_points_processor.cc

@@ -0,0 +1,59 @@
+/*
+ * 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/vertical_range_filtering_points_processor.h"
+
+#include "absl/memory/memory.h"
+#include "cartographer/common/lua_parameter_dictionary.h"
+#include "cartographer/io/points_batch.h"
+
+namespace cartographer {
+namespace io {
+
+std::unique_ptr<VerticalRangeFilteringPointsProcessor>
+VerticalRangeFilteringPointsProcessor::FromDictionary(
+    common::LuaParameterDictionary* const dictionary,
+    PointsProcessor* const next) {
+  return absl::make_unique<VerticalRangeFilteringPointsProcessor>(
+      dictionary->GetDouble("min_z"), dictionary->GetDouble("max_z"),
+      next);
+}
+
+VerticalRangeFilteringPointsProcessor::VerticalRangeFilteringPointsProcessor(
+    const double min_z, const double max_z,
+    PointsProcessor* next)
+    : min_z_(min_z), max_z_(max_z),
+      next_(next) {}
+
+void VerticalRangeFilteringPointsProcessor::Process(
+    std::unique_ptr<PointsBatch> batch) {
+  absl::flat_hash_set<int> to_remove;
+  for (size_t i = 0; i < batch->points.size(); ++i) {
+    const float distance_z = batch->points[i].position.z() - batch->origin.z();
+    if (!(min_z_ <= distance_z && distance_z <= max_z_) ) {
+      to_remove.insert(i);
+    }
+  }
+  RemovePoints(to_remove, batch.get());
+  next_->Process(std::move(batch));
+}
+
+PointsProcessor::FlushResult VerticalRangeFilteringPointsProcessor::Flush() {
+  return next_->Flush();
+}
+
+}  // namespace io
+}  // namespace cartographer

cartographer/io/vertical_range_filtering_points_processor.h

@@ -0,0 +1,58 @@
+/*
+ * 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_VERTICAL_RANGE_FILTERING_POINTS_PROCESSOR_H_
+#define CARTOGRAPHER_IO_VERTICAL_RANGE_FILTERING_POINTS_PROCESSOR_H_
+
+#include <memory>
+
+#include "cartographer/common/lua_parameter_dictionary.h"
+#include "cartographer/io/points_processor.h"
+
+namespace cartographer {
+namespace io {
+
+// Filters all points which distance in the Z direction from their 'origin'
+// exceeds 'max_z' or 'min_z'.
+class VerticalRangeFilteringPointsProcessor : public PointsProcessor {
+ public:
+  constexpr static const char* kConfigurationFileActionName =
+      "vertical_range_filter";
+  VerticalRangeFilteringPointsProcessor(double min_z, double max_z,
+                                        PointsProcessor* next);
+  static std::unique_ptr<VerticalRangeFilteringPointsProcessor> FromDictionary(
+      common::LuaParameterDictionary* dictionary, PointsProcessor* next);
+
+  ~VerticalRangeFilteringPointsProcessor() override {}
+
+  VerticalRangeFilteringPointsProcessor(
+      const VerticalRangeFilteringPointsProcessor&) = delete;
+  VerticalRangeFilteringPointsProcessor& operator=(
+      const VerticalRangeFilteringPointsProcessor&) = delete;
+
+  void Process(std::unique_ptr<PointsBatch> batch) override;
+  FlushResult Flush() override;
+
+ private:
+  const double min_z_;
+  const double max_z_;
+  PointsProcessor* const next_;
+};
+
+}  // namespace io
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_IO_VERTICAL_RANGE_FILTERING_POINTS_PROCESSOR_H_