Support by-floor XRay generation. (#126)

cartographer/io/CMakeLists.txt

@@ -123,6 +123,7 @@ google_library(io_points_processor_pipeline_builder
     io_points_processor
     io_xray_points_processor
     io_xyz_writing_points_processor
+    mapping_proto_trajectory
 )
 
 google_library(io_xray_points_processor
@@ -139,6 +140,8 @@ google_library(io_xray_points_processor
     io_cairo_types
     io_points_processor
     mapping_3d_hybrid_grid
+    mapping_detect_floors
+    mapping_proto_trajectory
     transform_rigid_transform
 )
 

cartographer/io/points_processor_pipeline_builder.cc

@@ -26,27 +26,43 @@
 #include "cartographer/io/ply_writing_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"
 
 namespace cartographer {
 namespace io {
 
-PointsProcessorPipelineBuilder::PointsProcessorPipelineBuilder() {
+template <typename PointsProcessorType>
-  RegisterNonStatic<CountingPointsProcessor>();
+void RegisterPlainPointsProcessor(PointsProcessorPipelineBuilder* const builder) {
-  RegisterNonStatic<FixedRatioSamplingPointsProcessor>();
+  builder->Register(
-  RegisterNonStatic<MinMaxRangeFiteringPointsProcessor>();
+      PointsProcessorType::kConfigurationFileActionName,
-  RegisterNonStatic<OutlierRemovingPointsProcessor>();
+      [](common::LuaParameterDictionary* const dictionary,
-  RegisterNonStatic<PcdWritingPointsProcessor>();
+         PointsProcessor* const next) -> std::unique_ptr<PointsProcessor> {
-  RegisterNonStatic<PlyWritingPointsProcessor>();
+        return PointsProcessorType::FromDictionary(dictionary, next);
-  RegisterNonStatic<XRayPointsProcessor>();
+      });
-  RegisterNonStatic<XyzWriterPointsProcessor>();
 }
 
-PointsProcessorPipelineBuilder* PointsProcessorPipelineBuilder::instance() {
+void RegisterBuiltInPointsProcessors(
-  static PointsProcessorPipelineBuilder instance;
+    const mapping::proto::Trajectory& trajectory,
-  return &instance;
+    PointsProcessorPipelineBuilder* builder) {
+  RegisterPlainPointsProcessor<CountingPointsProcessor>(builder);
+  RegisterPlainPointsProcessor<FixedRatioSamplingPointsProcessor>(builder);
+  RegisterPlainPointsProcessor<MinMaxRangeFiteringPointsProcessor>(builder);
+  RegisterPlainPointsProcessor<OutlierRemovingPointsProcessor>(builder);
+  RegisterPlainPointsProcessor<PcdWritingPointsProcessor>(builder);
+  RegisterPlainPointsProcessor<PlyWritingPointsProcessor>(builder);
+  RegisterPlainPointsProcessor<XyzWriterPointsProcessor>(builder);
+
+  builder->Register(
+      XRayPointsProcessor::kConfigurationFileActionName,
+      [&trajectory](
+          common::LuaParameterDictionary* const dictionary,
+          PointsProcessor* const next) -> std::unique_ptr<PointsProcessor> {
+        return XRayPointsProcessor::FromDictionary(trajectory, dictionary,
+                                                   next);
+      });
 }
 
-void PointsProcessorPipelineBuilder::RegisterType(const std::string& name,
+void PointsProcessorPipelineBuilder::Register(const std::string& name,
                                               FactoryFunction factory) {
   CHECK(factories_.count(name) == 0) << "A points processor with named '"
                                      << name
@@ -54,6 +70,8 @@ void PointsProcessorPipelineBuilder::RegisterType(const std::string& name,
   factories_[name] = factory;
 }
 
+PointsProcessorPipelineBuilder::PointsProcessorPipelineBuilder() {}
+
 std::vector<std::unique_ptr<PointsProcessor>>
 PointsProcessorPipelineBuilder::CreatePipeline(
     common::LuaParameterDictionary* const dictionary) const {

cartographer/io/points_processor_pipeline_builder.h

@@ -23,53 +23,46 @@
 
 #include "cartographer/common/lua_parameter_dictionary.h"
 #include "cartographer/io/points_processor.h"
+#include "cartographer/mapping/proto/trajectory.pb.h"
 
 namespace cartographer {
 namespace io {
 
-// Singleton that knows how to build a points processor pipeline out of a Lua
+// Builder to create a points processor pipeline out of a Lua configuration.
-// configuration. All the PointsProcessor shipping with Cartographer are already
+// You can register all built-in PointsProcessors using
-// registered with 'instance', but can register new classes with it that must
+// 'RegisterBuiltInPointsProcessors'. Non-built-in PointsProcessors must define
-// define its name and a way to build itself out of a LuaParameterDictionary.
+// a name and a factory method for building itself from a
-// See the various 'PointsProcessor's for examples.
+// LuaParameterDictionary. See the various built-in PointsProcessors for
+// examples.
 class PointsProcessorPipelineBuilder {
  public:
+  using FactoryFunction = std::function<std::unique_ptr<PointsProcessor>(
+      common::LuaParameterDictionary*, PointsProcessor* next)>;
+
+  PointsProcessorPipelineBuilder();
+
   PointsProcessorPipelineBuilder(const PointsProcessorPipelineBuilder&) =
       delete;
   PointsProcessorPipelineBuilder& operator=(
       const PointsProcessorPipelineBuilder&) = delete;
 
-  static PointsProcessorPipelineBuilder* instance();
+  // Register a new PointsProcessor type uniquly identified by 'name' which will
-
+  // be created using 'factory'.
-  template <typename PointsProcessorType>
+  void Register(const std::string& name, FactoryFunction factory);
-  void Register() {
-    instance()->RegisterNonStatic<PointsProcessorType>();
-  }
 
   std::vector<std::unique_ptr<PointsProcessor>> CreatePipeline(
       common::LuaParameterDictionary* dictionary) const;
 
  private:
-  using FactoryFunction = std::function<std::unique_ptr<PointsProcessor>(
-      common::LuaParameterDictionary*, PointsProcessor* next)>;
-
-  template <typename PointsProcessorType>
-  void RegisterNonStatic() {
-    RegisterType(
-        PointsProcessorType::kConfigurationFileActionName,
-        [](common::LuaParameterDictionary* const dictionary,
-           PointsProcessor* const next) -> std::unique_ptr<PointsProcessor> {
-          return PointsProcessorType::FromDictionary(dictionary, next);
-        });
-  }
-
-  PointsProcessorPipelineBuilder();
-
-  void RegisterType(const std::string& name, FactoryFunction factory);
-
   std::unordered_map<std::string, FactoryFunction> factories_;
 };
 
+// Register all 'PointsProcessor' that ship with Cartographer with this
+// 'builder'.
+void RegisterBuiltInPointsProcessors(
+    const mapping::proto::Trajectory& trajectory,
+    PointsProcessorPipelineBuilder* builder);
+
 }  // namespace io
 }  // namespace cartographer
 

cartographer/io/xray_points_processor.cc

@@ -25,12 +25,15 @@
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/math.h"
 #include "cartographer/io/cairo_types.h"
+#include "cartographer/mapping/detect_floors.h"
 #include "cartographer/mapping_3d/hybrid_grid.h"
 
 namespace cartographer {
 namespace io {
 namespace {
 
+using Voxels = mapping_3d::HybridGridBase<bool>;
+
 // Takes the logarithm of each value in 'mat', clamping to 0 as smallest value.
 void TakeLogarithm(Eigen::MatrixXf* mat) {
   for (int y = 0; y < mat->rows(); ++y) {
@@ -77,48 +80,7 @@ void WritePng(const string& filename, const Eigen::MatrixXf& mat) {
            CAIRO_STATUS_SUCCESS);
 }
 
-}  // namespace
+void WriteVoxels(const string& filename, const Voxels& voxels) {
-
-XRayPointsProcessor::XRayPointsProcessor(const double voxel_size,
-                                         const transform::Rigid3f& transform,
-                                         const string& output_filename,
-                                         PointsProcessor* next)
-    : next_(next),
-      output_filename_(output_filename),
-      transform_(transform),
-      voxels_(voxel_size, Eigen::Vector3f::Zero()) {}
-
-std::unique_ptr<XRayPointsProcessor> XRayPointsProcessor::FromDictionary(
-    common::LuaParameterDictionary* dictionary, PointsProcessor* next) {
-  return common::make_unique<XRayPointsProcessor>(
-      dictionary->GetDouble("voxel_size"),
-      transform::FromDictionary(dictionary->GetDictionary("transform").get())
-          .cast<float>(),
-      dictionary->GetString("filename"), next);
-}
-
-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(std::move(batch));
-}
-
-PointsProcessor::FlushResult XRayPointsProcessor::Flush() {
-  WriteImage();
-  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() {
   Eigen::Array3i min(std::numeric_limits<int>::max(),
                      std::numeric_limits<int>::max(),
                      std::numeric_limits<int>::max());
@@ -127,7 +89,7 @@ void XRayPointsProcessor::WriteImage() {
                      std::numeric_limits<int>::min());
 
   // Find the maximum and minimum cells.
-  for (Voxels::Iterator it(voxels_); !it.Done(); it.Next()) {
+  for (Voxels::Iterator it(voxels); !it.Done(); it.Next()) {
     const Eigen::Array3i idx = it.GetCellIndex();
     min = min.min(idx);
     max = max.max(idx);
@@ -144,12 +106,98 @@ void XRayPointsProcessor::WriteImage() {
   const int xsize = max[1] - min[1] + 1;
   const int ysize = max[2] - min[2] + 1;
   Eigen::MatrixXf image = Eigen::MatrixXf::Zero(ysize, xsize);
-  for (Voxels::Iterator it(voxels_); !it.Done(); it.Next()) {
+  for (Voxels::Iterator it(voxels); !it.Done(); it.Next()) {
     const Eigen::Array2i pixel = voxel_index_to_pixel(it.GetCellIndex());
     ++image(pixel.y(), pixel.x());
   }
   TakeLogarithm(&image);
-  WritePng(output_filename_, image);
+  WritePng(filename, image);
+}
+
+bool ContainedIn(
+    const common::Time& time,
+    const std::vector<common::Interval<common::Time>>& time_intervals) {
+  for (const auto& interval : time_intervals) {
+    if (interval.start <= time && time <= interval.end) {
+      return true;
+    }
+  }
+  return false;
+}
+
+void Insert(const PointsBatch& batch, const transform::Rigid3f& transform,
+            Voxels* voxels) {
+  for (const auto& point : batch.points) {
+    const Eigen::Vector3f camera_point = transform * point;
+    *voxels->mutable_value(voxels->GetCellIndex(camera_point)) = true;
+  }
+}
+
+}  // namespace
+
+XRayPointsProcessor::XRayPointsProcessor(
+    const double voxel_size, const transform::Rigid3f& transform,
+    const std::vector<mapping::Floor>& floors, const string& output_filename,
+    PointsProcessor* next)
+    : next_(next),
+      floors_(floors),
+      output_filename_(output_filename),
+      transform_(transform) {
+  for (int i = 0; i < (floors_.empty() ? 1 : floors.size()); ++i) {
+    voxels_.emplace_back(voxel_size, Eigen::Vector3f::Zero());
+  }
+}
+
+std::unique_ptr<XRayPointsProcessor> XRayPointsProcessor::FromDictionary(
+    const mapping::proto::Trajectory& trajectory,
+    common::LuaParameterDictionary* dictionary, PointsProcessor* next) {
+  std::vector<mapping::Floor> floors;
+  if (dictionary->HasKey("separate_floors") &&
+      dictionary->GetBool("separate_floors")) {
+    floors = mapping::DetectFloors(trajectory);
+  }
+
+  return common::make_unique<XRayPointsProcessor>(
+      dictionary->GetDouble("voxel_size"),
+      transform::FromDictionary(dictionary->GetDictionary("transform").get())
+          .cast<float>(),
+      floors, dictionary->GetString("filename"), next);
+}
+
+void XRayPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
+  if (floors_.empty()) {
+    CHECK_EQ(voxels_.size(), 1);
+    Insert(*batch, transform_, &voxels_[0]);
+  } else {
+    for (int i = 0; i < floors_.size(); ++i) {
+      if (!ContainedIn(batch->time, floors_[i].timespans)) {
+        continue;
+      }
+      Insert(*batch, transform_, &voxels_[i]);
+    }
+  }
+  next_->Process(std::move(batch));
+}
+
+PointsProcessor::FlushResult XRayPointsProcessor::Flush() {
+  if (floors_.empty()) {
+    CHECK_EQ(voxels_.size(), 1);
+    WriteVoxels(output_filename_ + ".png", voxels_[0]);
+  } else {
+    for (size_t i = 0; i < floors_.size(); ++i) {
+      WriteVoxels(output_filename_ + std::to_string(i) + ".png", voxels_[i]);
+    }
+  }
+
+  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);
 }
 
 }  // namespace io

cartographer/io/xray_points_processor.h

@@ -19,24 +19,25 @@
 
 #include "cartographer/common/lua_parameter_dictionary.h"
 #include "cartographer/io/points_processor.h"
+#include "cartographer/mapping/detect_floors.h"
+#include "cartographer/mapping/proto/trajectory.pb.h"
 #include "cartographer/mapping_3d/hybrid_grid.h"
 #include "cartographer/transform/rigid_transform.h"
 
 namespace cartographer {
 namespace io {
 
-// Creates X-ray cuts through the points with pixels being 'voxel_size' big. All
+// Creates X-ray cuts through the points with pixels being 'voxel_size' big.
-// images created from a single XRayPointsProcessor have the same dimensions and
-// are centered on the center of the bounding box, so that they can easily be
-// combined into a movie.
 class XRayPointsProcessor : public PointsProcessor {
  public:
   constexpr static const char* kConfigurationFileActionName =
       "write_xray_image";
   XRayPointsProcessor(double voxel_size, const transform::Rigid3f& transform,
+                      const std::vector<mapping::Floor>& floors,
                       const string& output_filename, PointsProcessor* next);
 
   static std::unique_ptr<XRayPointsProcessor> FromDictionary(
+      const mapping::proto::Trajectory& trajectory,
       common::LuaParameterDictionary* dictionary, PointsProcessor* next);
 
   ~XRayPointsProcessor() override {}
@@ -45,14 +46,16 @@ class XRayPointsProcessor : public PointsProcessor {
   FlushResult Flush() override;
 
  private:
-  using Voxels = mapping_3d::HybridGridBase<bool>;
-
-  void WriteImage();
-
   PointsProcessor* const next_;
+
+  // If empty, we do not separate into floors.
+  std::vector<mapping::Floor> floors_;
+
   const string output_filename_;
   const transform::Rigid3f transform_;
-  Voxels voxels_;
+
+  // Only has one entry if we do not separate into floors.
+  std::vector<mapping_3d::HybridGridBase<bool>> voxels_;
 };
 
 }  // namespace io