Adds a fixed sampling points processor. (#96)

cartographer/io/CMakeLists.txt

@@ -4,6 +4,20 @@ google_library(io_cairo_types
     cairo_types.h
 )
 
+google_library(io_fixed_ratio_sampling_points_processor
+  USES_EIGEN
+  USES_GLOG
+  SRCS
+    fixed_ratio_sampling_points_processor.cc
+  HDRS
+    fixed_ratio_sampling_points_processor.h
+  DEPENDS
+    common_fixed_ratio_sampler
+    common_lua_parameter_dictionary
+    common_make_unique
+    io_points_processor
+)
+
 google_library(io_min_max_range_filtering_points_processor
   SRCS
     min_max_range_filtering_points_processor.cc
@@ -74,6 +88,7 @@ google_library(io_points_processor_pipeline_builder
   DEPENDS
     common_lua_parameter_dictionary
     common_make_unique
+    io_fixed_ratio_sampling_points_processor
     io_min_max_range_filtering_points_processor
     io_null_points_processor
     io_pcd_writing_points_processor

cartographer/io/fixed_ratio_sampling_points_processor.cc

@@ -0,0 +1,69 @@
+/*
+ * 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/fixed_ratio_sampling_points_processor.h"
+
+#include "Eigen/Core"
+#include "cartographer/common/make_unique.h"
+#include "glog/logging.h"
+
+namespace cartographer {
+namespace io {
+
+std::unique_ptr<FixedRatioSamplingPointsProcessor>
+FixedRatioSamplingPointsProcessor::FromDictionary(
+    common::LuaParameterDictionary* const dictionary,
+    PointsProcessor* const next) {
+  const double sampling_ratio(dictionary->GetDouble("sampling_ratio"));
+  CHECK_LT(0., sampling_ratio) << "Sampling ratio <= 0 makes no sense.";
+  CHECK_LT(sampling_ratio, 1.) << "Sampling ratio >= 1 makes no sense.";
+  return common::make_unique<FixedRatioSamplingPointsProcessor>(sampling_ratio,
+                                                                next);
+}
+
+FixedRatioSamplingPointsProcessor::FixedRatioSamplingPointsProcessor(
+    const double sampling_ratio, PointsProcessor* next)
+    : sampling_ratio_(sampling_ratio),
+      next_(next),
+      sampler_(new common::FixedRatioSampler(sampling_ratio_)) {}
+
+void FixedRatioSamplingPointsProcessor::Process(
+    std::unique_ptr<PointsBatch> batch) {
+  std::vector<int> to_remove;
+  for (size_t i = 0; i < batch->points.size(); ++i) {
+    if (!sampler_->Pulse()) {
+      to_remove.push_back(i);
+    }
+  }
+  RemovePoints(to_remove, batch.get());
+  next_->Process(std::move(batch));
+}
+
+PointsProcessor::FlushResult FixedRatioSamplingPointsProcessor::Flush() {
+  switch (next_->Flush()) {
+    case PointsProcessor::FlushResult::kFinished:
+      return PointsProcessor::FlushResult::kFinished;
+
+    case PointsProcessor::FlushResult::kRestartStream:
+      sampler_ =
+          common::make_unique<common::FixedRatioSampler>(sampling_ratio_);
+      return PointsProcessor::FlushResult::kRestartStream;
+  }
+  LOG(FATAL);
+}
+
+}  // namespace io
+}  // namespace cartographer

cartographer/io/fixed_ratio_sampling_points_processor.h

@@ -0,0 +1,61 @@
+/*
+ * 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_FIXED_RATIO_SAMPLING_POINTS_PROCESSOR_H_
+#define CARTOGRAPHER_IO_FIXED_RATIO_SAMPLING_POINTS_PROCESSOR_H_
+
+#include <memory>
+
+#include "cartographer/common/fixed_ratio_sampler.h"
+#include "cartographer/common/lua_parameter_dictionary.h"
+#include "cartographer/io/points_processor.h"
+
+namespace cartographer {
+namespace io {
+
+// Only let a fixed 'sampling_ratio' of points through. A 'sampling_ratio' of 1.
+// makes this filter a no-op.
+class FixedRatioSamplingPointsProcessor : public PointsProcessor {
+ public:
+  constexpr static const char* kConfigurationFileActionName =
+      "fixed_ratio_sampler";
+
+  FixedRatioSamplingPointsProcessor(double sampling_ratio,
+                                    PointsProcessor* next);
+
+  static std::unique_ptr<FixedRatioSamplingPointsProcessor> FromDictionary(
+      common::LuaParameterDictionary* dictionary, PointsProcessor* next);
+
+  ~FixedRatioSamplingPointsProcessor() override{};
+
+  FixedRatioSamplingPointsProcessor(const FixedRatioSamplingPointsProcessor&) =
+      delete;
+  FixedRatioSamplingPointsProcessor& operator=(
+      const FixedRatioSamplingPointsProcessor&) = delete;
+
+  void Process(std::unique_ptr<PointsBatch> batch) override;
+  FlushResult Flush() override;
+
+ private:
+  const double sampling_ratio_;
+  PointsProcessor* const next_;
+  std::unique_ptr<common::FixedRatioSampler> sampler_;
+};
+
+}  // namespace io
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_IO_FIXED_RATIO_SAMPLING_POINTS_PROCESSOR_H_

cartographer/io/points_processor_pipeline_builder.cc

@@ -17,6 +17,7 @@
 #include "cartographer/io/points_processor_pipeline_builder.h"
 
 #include "cartographer/common/make_unique.h"
+#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/pcd_writing_points_processor.h"
@@ -28,6 +29,7 @@ namespace cartographer {
 namespace io {
 
 PointsProcessorPipelineBuilder::PointsProcessorPipelineBuilder() {
+  RegisterNonStatic<FixedRatioSamplingPointsProcessor>();
   RegisterNonStatic<MinMaxRangeFiteringPointsProcessor>();
   RegisterNonStatic<PcdWritingPointsProcessor>();
   RegisterNonStatic<PlyWritingPointsProcessor>();