PointsProcessor can now ask for another pass over the data. (#69)

* PointsProcessor can now ask for another pass.
* Added MinMaxFilteringPointsProcessor to have a user for RemovePoints.

cartographer/io/CMakeLists.txt

@@ -4,6 +4,16 @@ google_library(io_cairo_types
     cairo_types.h
 )
 
+google_library(io_min_max_range_filtering_points_processor
+  SRCS
+    min_max_range_filtering_points_processor.cc
+  HDRS
+    min_max_range_filtering_points_processor.h
+  DEPENDS
+    io_points_batch
+    io_points_processor
+)
+
 google_library(io_null_points_processor
   HDRS
     null_points_processor.h
@@ -13,6 +23,8 @@ google_library(io_null_points_processor
 
 google_library(io_points_batch
   USES_EIGEN
+  SRCS
+    points_batch.cc
   HDRS
     points_batch.h
   DEPENDS

cartographer/io/min_max_range_filtering_points_processor.cc

@@ -0,0 +1,46 @@
+/*
+ * 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/min_max_range_filtering_points_processor.h"
+
+#include "cartographer/io/points_batch.h"
+
+namespace cartographer {
+namespace io {
+
+MinMaxRangeFiteringPointsProcessor::MinMaxRangeFiteringPointsProcessor(
+    const double min_range, const double max_range, PointsProcessor* next)
+    : min_range_(min_range), max_range_(max_range), next_(next) {}
+
+void MinMaxRangeFiteringPointsProcessor::Process(
+    std::unique_ptr<PointsBatch> batch) {
+  std::vector<int> to_remove;
+  for (size_t i = 0; i < batch->points.size(); ++i) {
+    const float range = (batch->points[i] - batch->origin).norm();
+    if (!(min_range_ <= range && range <= max_range_)) {
+      to_remove.push_back(i);
+    }
+  }
+  RemovePoints(to_remove, batch.get());
+  next_->Process(std::move(batch));
+}
+
+PointsProcessor::FlushResult MinMaxRangeFiteringPointsProcessor::Flush() {
+  return next_->Flush();
+}
+
+}  // namespace io
+}  // namespace cartographer

cartographer/io/min_max_range_filtering_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_MIN_MAX_RANGE_FILTERING_POINTS_PROCESSOR_H_
+#define CARTOGRAPHER_IO_MIN_MAX_RANGE_FILTERING_POINTS_PROCESSOR_H_
+
+#include <memory>
+
+#include "cartographer/io/points_processor.h"
+
+namespace cartographer {
+namespace io {
+
+// Filters all points that are farther away from their 'origin' as 'max_range'
+// or closer than 'min_range'.
+class MinMaxRangeFiteringPointsProcessor : public PointsProcessor {
+ public:
+  MinMaxRangeFiteringPointsProcessor(double min_range, double max_range,
+                                     PointsProcessor* next);
+  ~MinMaxRangeFiteringPointsProcessor() override {}
+
+  MinMaxRangeFiteringPointsProcessor(
+      const MinMaxRangeFiteringPointsProcessor&) = delete;
+  MinMaxRangeFiteringPointsProcessor& operator=(
+      const MinMaxRangeFiteringPointsProcessor&) = delete;
+
+  void Process(std::unique_ptr<PointsBatch> batch) override;
+  FlushResult Flush() override;
+
+ private:
+  const double min_range_;
+  const double max_range_;
+  PointsProcessor* const next_;
+};
+
+}  // namespace io
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_IO_MIN_MAX_RANGE_FILTERING_POINTS_PROCESSOR_H_

cartographer/io/null_points_processor.h

@@ -28,8 +28,8 @@ class NullPointsProcessor : public PointsProcessor {
   NullPointsProcessor() {}
   ~NullPointsProcessor() override {}
 
-  void Process(const PointsBatch& points_batch) override {}
-  void Flush() override {}
+  void Process(std::unique_ptr<PointsBatch> points_batch) override {}
+  FlushResult Flush() override { return FlushResult::kFinished; }
 };
 
 }  // namespace io

cartographer/io/points_batch.cc

@@ -0,0 +1,33 @@
+/*
+ * 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/points_batch.h"
+
+namespace cartographer {
+namespace io {
+
+void RemovePoints(std::vector<int> to_remove, PointsBatch* batch) {
+  std::sort(to_remove.begin(), to_remove.end(), std::greater<int>());
+  for (const int index : to_remove) {
+    batch->points.erase(batch->points.begin() + index);
+    if (!batch->colors.empty()) {
+      batch->colors.erase(batch->colors.begin() + index);
+    }
+  }
+}
+
+}  // namespace io
+}  // namespace cartographer

cartographer/io/points_batch.h

@@ -26,6 +26,9 @@
 namespace cartographer {
 namespace io {
 
+// A point's color.
+using Color = std::array<uint8_t, 3>;
+
 // A number of points, captured around the same 'time' and by a
 // sensor at the same 'origin'.
 struct PointsBatch {
@@ -49,10 +52,12 @@ struct PointsBatch {
   int trajectory_index;
 
   std::vector<Eigen::Vector3f> points;
-  std::vector<Eigen::Vector3f> normals;
-  std::vector<Eigen::Matrix<uint8_t, 3, 1>> colors;
+  std::vector<Color> colors;
 };
 
+// Removes the indices in 'to_remove' from 'batch'.
+void RemovePoints(std::vector<int> to_remove, PointsBatch* batch);
+
 }  // namespace io
 }  // namespace cartographer
 

cartographer/io/points_processor.h

@@ -17,24 +17,34 @@
 #ifndef CARTOGRAPHER_IO_POINTS_PROCESSOR_H_
 #define CARTOGRAPHER_IO_POINTS_PROCESSOR_H_
 
+#include <memory>
+
 #include "cartographer/io/points_batch.h"
 
 namespace cartographer {
 namespace io {
 
 // A processor in a pipeline. It processes a 'points_batch' and hands it to the
-// next processor in the pipeline. Once 'flush' is called no more data will be
-// send through the pipeline.
+// next processor in the pipeline.
 class PointsProcessor {
  public:
+  enum class FlushResult {
+    kRestartStream,
+    kFinished,
+  };
+
   PointsProcessor() {}
   virtual ~PointsProcessor() {}
 
   PointsProcessor(const PointsProcessor&) = delete;
   PointsProcessor& operator=(const PointsProcessor&) = delete;
 
-  virtual void Process(const PointsBatch& points_batch) = 0;
-  virtual void Flush() = 0;
+  // Receive a 'points_batch', process it and pass it on.
+  virtual void Process(std::unique_ptr<PointsBatch> points_batch) = 0;
+
+  // Some implementations will perform expensive computations and others that do
+  // multiple passes over the data might ask for restarting the stream.
+  virtual FlushResult Flush() = 0;
 };
 
 }  // namespace io

cartographer/io/xray_points_processor.cc

@@ -83,17 +83,25 @@ XRayPointsProcessor::XRayPointsProcessor(const double voxel_size,
       transform_(transform),
       voxels_(voxel_size, Eigen::Vector3f::Zero()) {}
 
-void XRayPointsProcessor::Process(const PointsBatch& batch) {
-  for (const auto& point : batch.points) {
+void XRayPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
+  for (const auto& point : batch->points) {
     const Eigen::Vector3f camera_point = transform_ * point;
     *voxels_.mutable_value(voxels_.GetCellIndex(camera_point)) = true;
   }
-  next_->Process(batch);
+  next_->Process(std::move(batch));
 }
 
-void XRayPointsProcessor::Flush() {
+PointsProcessor::FlushResult XRayPointsProcessor::Flush() {
   WriteImage();
-  return next_->Flush();
+  switch (next_->Flush()) {
+    case FlushResult::kRestartStream:
+      LOG(FATAL) << "X-Ray generation must be configured to occur after any "
+                    "stages that require multiple passes";
+
+    case FlushResult::kFinished:
+      return FlushResult::kFinished;
+  }
+  LOG(FATAL);
 }
 
 void XRayPointsProcessor::WriteImage() {

cartographer/io/xray_points_processor.h

@@ -35,8 +35,8 @@ class XRayPointsProcessor : public PointsProcessor {
 
   ~XRayPointsProcessor() override {}
 
-  void Process(const PointsBatch& batch) override;
-  void Flush() override;
+  void Process(std::unique_ptr<PointsBatch> batch) override;
+  FlushResult Flush() override;
 
  private:
   using Voxels = mapping_3d::HybridGridBase<bool>;

cartographer/sensor/CMakeLists.txt

@@ -60,6 +60,7 @@ google_library(sensor_data
   HDRS
     data.h
   DEPENDS
+    common_time
     kalman_filter_pose_tracker
     sensor_laser
     transform_rigid_transform