Adds a CountingPointsProcessor. (#98)

It outputs the number of points processed at its stage in the pipeline
and is useful for understanding how many points a filter removed.

cartographer/io/CMakeLists.txt

@@ -4,6 +4,18 @@ google_library(io_cairo_types
     cairo_types.h
 )
 
+google_library(io_counting_points_processor
+  USES_GLOG
+  SRCS
+    counting_points_processor.cc
+  HDRS
+    counting_points_processor.h
+  DEPENDS
+    common_lua_parameter_dictionary
+    common_make_unique
+    io_points_processor
+)
+
 google_library(io_fixed_ratio_sampling_points_processor
   USES_EIGEN
   USES_GLOG
@@ -88,6 +100,7 @@ google_library(io_points_processor_pipeline_builder
   DEPENDS
     common_lua_parameter_dictionary
     common_make_unique
+    io_counting_points_processor
     io_fixed_ratio_sampling_points_processor
     io_min_max_range_filtering_points_processor
     io_null_points_processor

cartographer/io/counting_points_processor.cc

@@ -0,0 +1,55 @@
+/*
+ * 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/counting_points_processor.h"
+
+#include "cartographer/common/make_unique.h"
+#include "glog/logging.h"
+
+namespace cartographer {
+namespace io {
+
+CountingPointsProcessor::CountingPointsProcessor(PointsProcessor* next)
+    : num_points_(0), next_(next) {}
+
+std::unique_ptr<CountingPointsProcessor>
+CountingPointsProcessor::FromDictionary(
+    common::LuaParameterDictionary* const dictionary,
+    PointsProcessor* const next) {
+  return common::make_unique<CountingPointsProcessor>(next);
+}
+
+void CountingPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
+  num_points_ += batch->points.size();
+  next_->Process(std::move(batch));
+}
+
+PointsProcessor::FlushResult CountingPointsProcessor::Flush() {
+  switch (next_->Flush()) {
+    case FlushResult::kFinished:
+      LOG(INFO) << "Processed " << num_points_ << " and finishing.";
+      return FlushResult::kFinished;
+
+    case FlushResult::kRestartStream:
+      LOG(INFO) << "Processed " << num_points_ << " and restarting stream.";
+      num_points_ = 0;
+      return FlushResult::kRestartStream;
+  }
+  LOG(FATAL);
+}
+
+}  // namespace io
+}  // namespace cartographer

cartographer/io/counting_points_processor.h

@@ -0,0 +1,52 @@
+/*
+ * 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_COUNTING_POINTS_PROCESSOR_H_
+#define CARTOGRAPHER_IO_COUNTING_POINTS_PROCESSOR_H_
+
+#include "cartographer/common/lua_parameter_dictionary.h"
+#include "cartographer/io/points_processor.h"
+
+namespace cartographer {
+namespace io {
+
+// Passes through points, but keeps track of how many points it saw and output
+// that on Flush.
+class CountingPointsProcessor : public PointsProcessor {
+ public:
+  constexpr static const char* kConfigurationFileActionName = "dump_num_points";
+  explicit CountingPointsProcessor(PointsProcessor* next);
+
+  static std::unique_ptr<CountingPointsProcessor> FromDictionary(
+      common::LuaParameterDictionary* dictionary, PointsProcessor* next);
+
+  ~CountingPointsProcessor() override {}
+
+  CountingPointsProcessor(const CountingPointsProcessor&) = delete;
+  CountingPointsProcessor& operator=(const CountingPointsProcessor&) = delete;
+
+  void Process(std::unique_ptr<PointsBatch> points) override;
+  FlushResult Flush() override;
+
+ private:
+  int64 num_points_;
+  PointsProcessor* next_;
+};
+
+}  // namespace io
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_IO_COUNTING_POINTS_PROCESSOR_H_

cartographer/io/ply_writing_points_processor.h

@@ -27,11 +27,12 @@ class PlyWritingPointsProcessor : public PointsProcessor {
  public:
   constexpr static const char* kConfigurationFileActionName = "write_ply";
   PlyWritingPointsProcessor(const string& filename, PointsProcessor* next);
-  ~PlyWritingPointsProcessor() override {}
 
   static std::unique_ptr<PlyWritingPointsProcessor> FromDictionary(
       common::LuaParameterDictionary* dictionary, PointsProcessor* next);
 
+  ~PlyWritingPointsProcessor() override {}
+
   PlyWritingPointsProcessor(const PlyWritingPointsProcessor&) = delete;
   PlyWritingPointsProcessor& operator=(const PlyWritingPointsProcessor&) =
       delete;

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/counting_points_processor.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"
@@ -29,6 +30,7 @@ namespace cartographer {
 namespace io {
 
 PointsProcessorPipelineBuilder::PointsProcessorPipelineBuilder() {
+  RegisterNonStatic<CountingPointsProcessor>();
   RegisterNonStatic<FixedRatioSamplingPointsProcessor>();
   RegisterNonStatic<MinMaxRangeFiteringPointsProcessor>();
   RegisterNonStatic<PcdWritingPointsProcessor>();