Towards a ROS map writing PointsProcessor. (cartographer-project#598)

- Refactor ProbabilityGridPointsProcessor for code reuse. - Adds Image::width and height. - Adds GetFilename for FileWriter. Related to cartographer-project/cartographer_ros#475.
magazino · Oct 30, 2017 · 1a9b2ff · 1a9b2ff
1 parent 119e05a
commit 1a9b2ff
Show file tree

Hide file tree

Showing 5 changed files with 133 additions and 56 deletions.
diff --git a/cartographer/io/file_writer.cc b/cartographer/io/file_writer.cc
@@ -20,7 +20,7 @@ namespace cartographer {
 namespace io {
 
 StreamFileWriter::StreamFileWriter(const string& filename)
-    : out_(filename, std::ios::out | std::ios::binary) {}
+    : filename_(filename), out_(filename, std::ios::out | std::ios::binary) {}
 
 StreamFileWriter::~StreamFileWriter() {}
 
@@ -49,5 +49,7 @@ bool StreamFileWriter::WriteHeader(const char* const data, const size_t len) {
   return Write(data, len);
 }
 
+string StreamFileWriter::GetFilename() { return filename_; }
+
 }  // namespace io
 }  // namespace cartographer
diff --git a/cartographer/io/file_writer.h b/cartographer/io/file_writer.h
@@ -42,6 +42,7 @@ class FileWriter {
 
   virtual bool Write(const char* data, size_t len) = 0;
   virtual bool Close() = 0;
+  virtual string GetFilename() = 0;
 };
 
 // An Implementation of file using std::ofstream.
@@ -54,8 +55,10 @@ class StreamFileWriter : public FileWriter {
   bool Write(const char* data, size_t len) override;
   bool WriteHeader(const char* data, size_t len) override;
   bool Close() override;
+  string GetFilename() override;
 
  private:
+  const string filename_;
   std::ofstream out_;
 };
 

diff --git a/cartographer/io/image.h b/cartographer/io/image.h
@@ -1,3 +1,19 @@
+/*
+ * Copyright 2017 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_IMAGE_H_
 #define CARTOGRAPHER_IO_IMAGE_H_
 
@@ -41,6 +57,9 @@ class Image {
   // to this surface is alive.
   UniqueCairoSurfacePtr GetCairoSurface();
 
+  int width() const { return width_; }
+  int height() const { return height_; }
+
  private:
   int width_;
   int height_;

diff --git a/cartographer/io/probability_grid_points_processor.cc b/cartographer/io/probability_grid_points_processor.cc
@@ -1,3 +1,19 @@
+/*
+ * Copyright 2017 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/probability_grid_points_processor.h"
 
 #include "Eigen/Core"
@@ -12,63 +28,28 @@ namespace cartographer {
 namespace io {
 namespace {
 
-void WriteGrid(
+void DrawTrajectoriesIntoImage(
     const mapping_2d::ProbabilityGrid& probability_grid,
-    const ProbabilityGridPointsProcessor::DrawTrajectories& draw_trajectories,
+    const Eigen::Array2i& offset,
     const std::vector<mapping::proto::Trajectory>& trajectories,
-    FileWriter* const file_writer) {
-  Eigen::Array2i offset;
-  mapping_2d::CellLimits cell_limits;
-  probability_grid.ComputeCroppedLimits(&offset, &cell_limits);
-  if (cell_limits.num_x_cells == 0 || cell_limits.num_y_cells == 0) {
-    LOG(WARNING) << "Not writing output: empty probability grid";
-    return;
-  }
-  const auto compute_color_value = [&probability_grid](
-                                       const Eigen::Array2i& index) {
-    if (probability_grid.IsKnown(index)) {
-      const float probability = 1.f - probability_grid.GetProbability(index);
-      return static_cast<uint8>(
-          255 * ((probability - mapping::kMinProbability) /
-                 (mapping::kMaxProbability - mapping::kMinProbability)));
-    } else {
-      constexpr uint8 kUnknownValue = 128;
-      return kUnknownValue;
-    }
-  };
-  Image image(cell_limits.num_x_cells, cell_limits.num_y_cells);
-  for (const auto& xy_index :
-       cartographer::mapping_2d::XYIndexRangeIterator(cell_limits)) {
-    const auto index = xy_index + offset;
-    uint8 value = compute_color_value(index);
-    image.SetPixel(xy_index.x(), xy_index.y(), {{value, value, value}});
-  }
-
-  if (draw_trajectories ==
-      ProbabilityGridPointsProcessor::DrawTrajectories::kYes) {
-    for (size_t i = 0; i < trajectories.size(); ++i) {
-      DrawTrajectory(trajectories[i], GetColor(i),
-                     [&probability_grid, &offset](
-                         const transform::Rigid3d& pose) -> Eigen::Array2i {
-                       return probability_grid.limits().GetCellIndex(
-                                  pose.cast<float>().translation().head<2>()) -
-                              offset;
-                     },
-                     image.GetCairoSurface().get());
-    }
+    cairo_surface_t* cairo_surface) {
+  for (size_t i = 0; i < trajectories.size(); ++i) {
+    DrawTrajectory(trajectories[i], GetColor(i),
+                   [&probability_grid,
+                    &offset](const transform::Rigid3d& pose) -> Eigen::Array2i {
+                     return probability_grid.limits().GetCellIndex(
+                                pose.cast<float>().translation().head<2>()) -
+                            offset;
+                   },
+                   cairo_surface);
   }
-  image.WritePng(file_writer);
-  CHECK(file_writer->Close());
 }
 
-mapping_2d::ProbabilityGrid CreateProbabilityGrid(const double resolution) {
-  constexpr int kInitialProbabilityGridSize = 100;
-  Eigen::Vector2d max =
-      0.5 * kInitialProbabilityGridSize * resolution * Eigen::Vector2d::Ones();
-  return mapping_2d::ProbabilityGrid(cartographer::mapping_2d::MapLimits(
-      resolution, max,
-      mapping_2d::CellLimits(kInitialProbabilityGridSize,
-                             kInitialProbabilityGridSize)));
+uint8 ProbabilityToColor(float probability_from_grid) {
+  const float probability = 1.f - probability_from_grid;
+  return ::cartographer::common::RoundToInt(
+      255 * ((probability - mapping::kMinProbability) /
+             (mapping::kMaxProbability - mapping::kMinProbability)));
 }
 
 }  // namespace
@@ -115,8 +96,19 @@ void ProbabilityGridPointsProcessor::Process(
 }
 
 PointsProcessor::FlushResult ProbabilityGridPointsProcessor::Flush() {
-  WriteGrid(probability_grid_, draw_trajectories_, trajectories_,
-            file_writer_.get());
+  Eigen::Array2i offset;
+  std::unique_ptr<Image> image =
+      DrawProbabilityGrid(probability_grid_, &offset);
+  if (image != nullptr) {
+    if (draw_trajectories_ ==
+        ProbabilityGridPointsProcessor::DrawTrajectories::kYes) {
+      DrawTrajectoriesIntoImage(probability_grid_, offset, trajectories_,
+                                image->GetCairoSurface().get());
+    }
+    image->WritePng(file_writer_.get());
+    CHECK(file_writer_->Close());
+  }
+
   switch (next_->Flush()) {
     case FlushResult::kRestartStream:
       LOG(FATAL) << "ProbabilityGrid generation must be configured to occur "
@@ -128,5 +120,40 @@ PointsProcessor::FlushResult ProbabilityGridPointsProcessor::Flush() {
   LOG(FATAL);
 }
 
+std::unique_ptr<Image> DrawProbabilityGrid(
+    const mapping_2d::ProbabilityGrid& probability_grid,
+    Eigen::Array2i* offset) {
+  mapping_2d::CellLimits cell_limits;
+  probability_grid.ComputeCroppedLimits(offset, &cell_limits);
+  if (cell_limits.num_x_cells == 0 || cell_limits.num_y_cells == 0) {
+    LOG(WARNING) << "Not writing output: empty probability grid";
+    return nullptr;
+  }
+  auto image = common::make_unique<Image>(cell_limits.num_x_cells,
+                                          cell_limits.num_y_cells);
+  for (const auto& xy_index :
+       cartographer::mapping_2d::XYIndexRangeIterator(cell_limits)) {
+    const auto index = xy_index + *offset;
+    constexpr uint8 kUnknownValue = 128;
+    const uint8 value =
+        probability_grid.IsKnown(index)
+            ? ProbabilityToColor(probability_grid.GetProbability(index))
+            : kUnknownValue;
+    ;
+    image->SetPixel(xy_index.x(), xy_index.y(), {{value, value, value}});
+  }
+  return image;
+}
+
+mapping_2d::ProbabilityGrid CreateProbabilityGrid(const double resolution) {
+  constexpr int kInitialProbabilityGridSize = 100;
+  Eigen::Vector2d max =
+      0.5 * kInitialProbabilityGridSize * resolution * Eigen::Vector2d::Ones();
+  return mapping_2d::ProbabilityGrid(cartographer::mapping_2d::MapLimits(
+      resolution, max,
+      mapping_2d::CellLimits(kInitialProbabilityGridSize,
+                             kInitialProbabilityGridSize)));
+}
+
 }  // namespace io
 }  // namespace cartographer
diff --git a/cartographer/io/probability_grid_points_processor.h b/cartographer/io/probability_grid_points_processor.h
@@ -1,10 +1,26 @@
+/*
+ * 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_PROBABILITY_GRID_POINTS_PROCESSOR_H_
 #define CARTOGRAPHER_IO_PROBABILITY_GRID_POINTS_PROCESSOR_H_
 
 #include <memory>
-#include <string>
 
 #include "cartographer/io/file_writer.h"
+#include "cartographer/io/image.h"
 #include "cartographer/io/points_batch.h"
 #include "cartographer/io/points_processor.h"
 #include "cartographer/mapping/proto/trajectory.pb.h"
@@ -56,6 +72,16 @@ class ProbabilityGridPointsProcessor : public PointsProcessor {
   mapping_2d::ProbabilityGrid probability_grid_;
 };
 
+// Draws 'probability_grid' into an image and fills in 'offset' with the cropped
+// map limits. Returns 'nullptr' if probability_grid was empty.
+std::unique_ptr<Image> DrawProbabilityGrid(
+    const mapping_2d::ProbabilityGrid& probability_grid,
+    Eigen::Array2i* offset);
+
+// Create an initially empty probability grid with 'resolution' and a small
+// size, suitable for a PointsBatchProcessor.
+mapping_2d::ProbabilityGrid CreateProbabilityGrid(const double resolution);
+
 }  // namespace io
 }  // namespace cartographer