Draw Trajectories onto X-Rays and ProbabilityGrids. (#421)

This behavior can be turned off with the 'draw_trajectories' setting in Lua.

Fixes #174.

cartographer/io/draw_trajectories.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/draw_trajectories.h"
+
+#include "cartographer/io/image.h"
+#include "cartographer/transform/transform.h"
+
+namespace cartographer {
+namespace io {
+
+void DrawTrajectory(const mapping::proto::Trajectory& trajectory,
+                    const Color& color, PoseToPixelFunction pose_to_pixel,
+                    cairo_surface_t* surface) {
+  if (trajectory.node_size() == 0) {
+    return;
+  }
+  constexpr double kTrajectoryWidth = 4.;
+  constexpr double kTrajectoryEndMarkers = 6.;
+  constexpr double kAlpha = 0.7;
+
+  auto cr = ::cartographer::io::MakeUniqueCairoPtr(cairo_create(surface));
+
+  cairo_set_source_rgba(cr.get(), color[0] / 255., color[1] / 255.,
+                        color[2] / 255., kAlpha);
+  cairo_set_line_width(cr.get(), kTrajectoryWidth);
+
+  for (const auto& node : trajectory.node()) {
+    const Eigen::Array2i pixel =
+        pose_to_pixel(transform::ToRigid3(node.pose()));
+    cairo_line_to(cr.get(), pixel.x(), pixel.y());
+  }
+  cairo_stroke(cr.get());
+
+  // Draw beginning and end markers.
+  {
+    const Eigen::Array2i pixel =
+        pose_to_pixel(transform::ToRigid3(trajectory.node(0).pose()));
+    cairo_set_source_rgba(cr.get(), 0., 1., 0., kAlpha);
+    cairo_arc(cr.get(), pixel.x(), pixel.y(), kTrajectoryEndMarkers, 0,
+              2 * M_PI);
+    cairo_fill(cr.get());
+  }
+  {
+    const Eigen::Array2i pixel = pose_to_pixel(transform::ToRigid3(
+        trajectory.node(trajectory.node_size() - 1).pose()));
+    cairo_set_source_rgba(cr.get(), 1., 0., 0., kAlpha);
+    cairo_arc(cr.get(), pixel.x(), pixel.y(), kTrajectoryEndMarkers, 0,
+              2 * M_PI);
+    cairo_fill(cr.get());
+  }
+  cairo_surface_flush(surface);
+}
+
+}  // namespace io
+}  // namespace cartographer

cartographer/io/draw_trajectories.h

@@ -0,0 +1,41 @@
+/*
+ * 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_DRAW_TRAJECTORIES_H_
+#define CARTOGRAPHER_IO_DRAW_TRAJECTORIES_H_
+
+#include "cairo/cairo.h"
+#include "cartographer/io/color.h"
+#include "cartographer/mapping/proto/trajectory.pb.h"
+#include "cartographer/transform/rigid_transform.h"
+
+namespace cartographer {
+namespace io {
+
+using PoseToPixelFunction =
+    std::function<Eigen::Array2i(const transform::Rigid3d& pose)>;
+
+// Draws the 'trajectory' with the given 'color' onto 'surface'. The
+// 'pose_to_pixel' function must translate a trajectory node's position into the
+// pixel on 'surface'.
+void DrawTrajectory(const mapping::proto::Trajectory& trajectory,
+                    const Color& color, PoseToPixelFunction pose_to_pixel,
+                    cairo_surface_t* surface);
+
+}  // namespace io
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_IO_DRAW_TRAJECTORIES_H_

cartographer/io/image.h

@@ -33,13 +33,13 @@ class Image {
   void SetPixel(int x, int y, const Color& color);
   void WritePng(FileWriter* const file_writer);
 
- private:
   // Returns a pointer to a cairo surface that contains the current pixel data.
   // The 'Image' object must therefore outlive the returned surface object. It
   // is undefined behavior to call any of the mutating functions while a pointer
   // to this surface is alive.
   UniqueCairoSurfacePtr GetCairoSurface();
 
+ private:
   int width_;
   int height_;
   int stride_;

cartographer/io/points_processor_pipeline_builder.cc

@@ -60,8 +60,23 @@ void RegisterFileWritingPointsProcessor(
       });
 }
 
+template <typename PointsProcessorType>
+void RegisterFileWritingPointsProcessorWithTrajectories(
+    const std::vector<mapping::proto::Trajectory>& trajectories,
+    FileWriterFactory file_writer_factory,
+    PointsProcessorPipelineBuilder* const builder) {
+  builder->Register(
+      PointsProcessorType::kConfigurationFileActionName,
+      [&trajectories, file_writer_factory](
+          common::LuaParameterDictionary* const dictionary,
+          PointsProcessor* const next) -> std::unique_ptr<PointsProcessor> {
+        return PointsProcessorType::FromDictionary(
+            trajectories, file_writer_factory, dictionary, next);
+      });
+}
+
 void RegisterBuiltInPointsProcessors(
-    const mapping::proto::Trajectory& trajectory,
+    const std::vector<mapping::proto::Trajectory>& trajectories,
     FileWriterFactory file_writer_factory,
     PointsProcessorPipelineBuilder* builder) {
   RegisterPlainPointsProcessor<CountingPointsProcessor>(builder);
@@ -78,19 +93,11 @@ void RegisterBuiltInPointsProcessors(
       file_writer_factory, builder);
   RegisterFileWritingPointsProcessor<HybridGridPointsProcessor>(
       file_writer_factory, builder);
-  RegisterFileWritingPointsProcessor<ProbabilityGridPointsProcessor>(
+  RegisterFileWritingPointsProcessorWithTrajectories<XRayPointsProcessor>(
-      file_writer_factory, builder);
+      trajectories, file_writer_factory, builder);
-
+  RegisterFileWritingPointsProcessorWithTrajectories<
-  // X-Ray is an odd ball since it requires the trajectory to figure out the
+      ProbabilityGridPointsProcessor>(trajectories, file_writer_factory,
-  // different building levels we walked on to separate the images.
+                                      builder);
-  builder->Register(
-      XRayPointsProcessor::kConfigurationFileActionName,
-      [&trajectory, file_writer_factory](
-          common::LuaParameterDictionary* const dictionary,
-          PointsProcessor* const next) -> std::unique_ptr<PointsProcessor> {
-        return XRayPointsProcessor::FromDictionary(
-            trajectory, file_writer_factory, dictionary, next);
-      });
 }
 
 void PointsProcessorPipelineBuilder::Register(const std::string& name,

cartographer/io/points_processor_pipeline_builder.h

@@ -61,7 +61,7 @@ class PointsProcessorPipelineBuilder {
 // Register all 'PointsProcessor' that ship with Cartographer with this
 // 'builder'.
 void RegisterBuiltInPointsProcessors(
-    const mapping::proto::Trajectory& trajectory,
+    const std::vector<mapping::proto::Trajectory>& trajectories,
     FileWriterFactory file_writer_factory,
     PointsProcessorPipelineBuilder* builder);
 

cartographer/io/probability_grid_points_processor.cc

@@ -4,6 +4,7 @@
 #include "cartographer/common/lua_parameter_dictionary.h"
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/math.h"
+#include "cartographer/io/draw_trajectories.h"
 #include "cartographer/io/image.h"
 #include "cartographer/io/points_batch.h"
 
@@ -11,8 +12,11 @@ namespace cartographer {
 namespace io {
 namespace {
 
-void WriteGrid(const mapping_2d::ProbabilityGrid& probability_grid,
+void WriteGrid(
-               FileWriter* const file_writer) {
+    const mapping_2d::ProbabilityGrid& probability_grid,
+    const ProbabilityGridPointsProcessor::DrawTrajectories& draw_trajectories,
+    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);
@@ -20,9 +24,6 @@ void WriteGrid(const mapping_2d::ProbabilityGrid& probability_grid,
     LOG(WARNING) << "Not writing output: empty probability grid";
     return;
   }
-  const auto grid_index_to_pixel = [cell_limits](const Eigen::Array2i& index) {
-    return Eigen::Array2i(index(0), index(1));
-  };
   const auto compute_color_value = [&probability_grid](
                                        const Eigen::Array2i& index) {
     if (probability_grid.IsKnown(index)) {
@@ -35,15 +36,26 @@ void WriteGrid(const mapping_2d::ProbabilityGrid& probability_grid,
       return kUnknownValue;
     }
   };
-  int width = cell_limits.num_x_cells;
+  Image image(cell_limits.num_x_cells, cell_limits.num_y_cells);
-  int height = cell_limits.num_y_cells;
-  Image image(width, height);
   for (auto xy_index :
        cartographer::mapping_2d::XYIndexRangeIterator(cell_limits)) {
     auto index = xy_index + offset;
     uint8 value = compute_color_value(index);
-    const Eigen::Array2i pixel = grid_index_to_pixel(xy_index);
+    image.SetPixel(xy_index.x(), xy_index.y(), {{value, value, value}});
-    image.SetPixel(pixel.x(), pixel.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());
+    }
   }
   image.WritePng(file_writer);
   CHECK(file_writer->Close());
@@ -65,22 +77,34 @@ ProbabilityGridPointsProcessor::ProbabilityGridPointsProcessor(
     const double resolution,
     const mapping_2d::proto::RangeDataInserterOptions&
         range_data_inserter_options,
-    std::unique_ptr<FileWriter> file_writer, PointsProcessor* const next)
+    const DrawTrajectories& draw_trajectories,
-    : next_(next),
+    std::unique_ptr<FileWriter> file_writer,
+    const std::vector<mapping::proto::Trajectory>& trajectories,
+    PointsProcessor* const next)
+    : draw_trajectories_(draw_trajectories),
+      trajectories_(trajectories),
       file_writer_(std::move(file_writer)),
+      next_(next),
       range_data_inserter_(range_data_inserter_options),
       probability_grid_(CreateProbabilityGrid(resolution)) {}
 
 std::unique_ptr<ProbabilityGridPointsProcessor>
 ProbabilityGridPointsProcessor::FromDictionary(
+    const std::vector<mapping::proto::Trajectory>& trajectories,
     FileWriterFactory file_writer_factory,
     common::LuaParameterDictionary* const dictionary,
     PointsProcessor* const next) {
+  const auto draw_trajectories = (!dictionary->HasKey("draw_trajectories") ||
+                                  dictionary->GetBool("draw_trajectories"))
+                                     ? DrawTrajectories::kYes
+                                     : DrawTrajectories::kNo;
   return common::make_unique<ProbabilityGridPointsProcessor>(
       dictionary->GetDouble("resolution"),
       mapping_2d::CreateRangeDataInserterOptions(
           dictionary->GetDictionary("range_data_inserter").get()),
-      file_writer_factory(dictionary->GetString("filename") + ".png"), next);
+      draw_trajectories,
+      file_writer_factory(dictionary->GetString("filename") + ".png"),
+      trajectories, next);
 }
 
 void ProbabilityGridPointsProcessor::Process(
@@ -91,7 +115,8 @@ void ProbabilityGridPointsProcessor::Process(
 }
 
 PointsProcessor::FlushResult ProbabilityGridPointsProcessor::Flush() {
-  WriteGrid(probability_grid_, file_writer_.get());
+  WriteGrid(probability_grid_, draw_trajectories_, trajectories_,
+            file_writer_.get());
   switch (next_->Flush()) {
     case FlushResult::kRestartStream:
       LOG(FATAL) << "ProbabilityGrid generation must be configured to occur "

cartographer/io/probability_grid_points_processor.h

@@ -7,6 +7,7 @@
 #include "cartographer/io/file_writer.h"
 #include "cartographer/io/points_batch.h"
 #include "cartographer/io/points_processor.h"
+#include "cartographer/mapping/proto/trajectory.pb.h"
 #include "cartographer/mapping_2d/probability_grid.h"
 #include "cartographer/mapping_2d/proto/range_data_inserter_options.pb.h"
 #include "cartographer/mapping_2d/range_data_inserter.h"
@@ -22,17 +23,22 @@ class ProbabilityGridPointsProcessor : public PointsProcessor {
  public:
   constexpr static const char* kConfigurationFileActionName =
       "write_probability_grid";
+  enum class DrawTrajectories { kNo, kYes };
   ProbabilityGridPointsProcessor(
       double resolution,
       const mapping_2d::proto::RangeDataInserterOptions&
           range_data_inserter_options,
-      std::unique_ptr<FileWriter> file_writer, PointsProcessor* next);
+      const DrawTrajectories& draw_trajectories,
+      std::unique_ptr<FileWriter> file_writer,
+      const std::vector<mapping::proto::Trajectory>& trajectorios,
+      PointsProcessor* next);
   ProbabilityGridPointsProcessor(const ProbabilityGridPointsProcessor&) =
       delete;
   ProbabilityGridPointsProcessor& operator=(
       const ProbabilityGridPointsProcessor&) = delete;
 
   static std::unique_ptr<ProbabilityGridPointsProcessor> FromDictionary(
+      const std::vector<mapping::proto::Trajectory>& trajectories,
       FileWriterFactory file_writer_factory,
       common::LuaParameterDictionary* dictionary, PointsProcessor* next);
 
@@ -42,8 +48,10 @@ class ProbabilityGridPointsProcessor : public PointsProcessor {
   FlushResult Flush() override;
 
  private:
-  PointsProcessor* const next_;
+  const DrawTrajectories draw_trajectories_;
+  const std::vector<mapping::proto::Trajectory> trajectories_;
   std::unique_ptr<FileWriter> file_writer_;
+  PointsProcessor* const next_;
   mapping_2d::RangeDataInserter range_data_inserter_;
   mapping_2d::ProbabilityGrid probability_grid_;
 };

cartographer/io/xray_points_processor.cc

@@ -23,6 +23,7 @@
 #include "cartographer/common/lua_parameter_dictionary.h"
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/math.h"
+#include "cartographer/io/draw_trajectories.h"
 #include "cartographer/io/image.h"
 #include "cartographer/mapping/detect_floors.h"
 #include "cartographer/mapping_3d/hybrid_grid.h"
@@ -46,7 +47,7 @@ double Mix(const double a, const double b, const double t) {
   return a * (1. - t) + t * b;
 }
 
-// Write 'mat' as a pleasing-to-look-at PNG into 'filename'
+// Convert 'mat' into a pleasing-to-look-at image.
 Image IntoImage(const PixelDataMatrix& mat) {
   Image image(mat.cols(), mat.rows());
   float max = std::numeric_limits<float>::min();
@@ -104,10 +105,14 @@ bool ContainedIn(const common::Time& time,
 
 XRayPointsProcessor::XRayPointsProcessor(
     const double voxel_size, const transform::Rigid3f& transform,
-    const std::vector<mapping::Floor>& floors, const string& output_filename,
+    const std::vector<mapping::Floor>& floors,
+    const DrawTrajectories& draw_trajectories, const string& output_filename,
+    const std::vector<mapping::proto::Trajectory>& trajectories,
     FileWriterFactory file_writer_factory, PointsProcessor* const next)
-    : next_(next),
+    : draw_trajectories_(draw_trajectories),
+      trajectories_(trajectories),
       file_writer_factory_(file_writer_factory),
+      next_(next),
       floors_(floors),
       output_filename_(output_filename),
       transform_(transform) {
@@ -118,21 +123,29 @@ XRayPointsProcessor::XRayPointsProcessor(
 }
 
 std::unique_ptr<XRayPointsProcessor> XRayPointsProcessor::FromDictionary(
-    const mapping::proto::Trajectory& trajectory,
+    const std::vector<mapping::proto::Trajectory>& trajectories,
     FileWriterFactory file_writer_factory,
     common::LuaParameterDictionary* const dictionary,
     PointsProcessor* const next) {
   std::vector<mapping::Floor> floors;
-  if (dictionary->HasKey("separate_floors") &&
+  const bool separate_floor = dictionary->HasKey("separate_floors") &&
-      dictionary->GetBool("separate_floors")) {
+                              dictionary->GetBool("separate_floors");
-    floors = mapping::DetectFloors(trajectory);
+  const auto draw_trajectories = (!dictionary->HasKey("draw_trajectories") ||
+                                  dictionary->GetBool("draw_trajectories"))
+                                     ? DrawTrajectories::kYes
+                                     : DrawTrajectories::kNo;
+  if (separate_floor) {
+    CHECK_EQ(trajectories.size(), 0)
+        << "Can only detect floors with a single trajectory.";
+    floors = mapping::DetectFloors(trajectories.at(0));
   }
 
   return common::make_unique<XRayPointsProcessor>(
       dictionary->GetDouble("voxel_size"),
       transform::FromDictionary(dictionary->GetDictionary("transform").get())
           .cast<float>(),
-      floors, dictionary->GetString("filename"), file_writer_factory, next);
+      floors, draw_trajectories, dictionary->GetString("filename"),
+      trajectories, file_writer_factory, next);
 }
 
 void XRayPointsProcessor::WriteVoxels(const Aggregation& aggregation,
@@ -166,9 +179,20 @@ void XRayPointsProcessor::WriteVoxels(const Aggregation& aggregation,
     pixel_data.mean_b = column_data.sum_b / column_data.count;
     ++pixel_data.num_occupied_cells_in_column;
   }
-  Image image = IntoImage(pixel_data_matrix);
 
-  // TODO(hrapp): Draw trajectories here.
+  Image image = IntoImage(pixel_data_matrix);
+  if (draw_trajectories_ == DrawTrajectories::kYes) {
+    for (size_t i = 0; i < trajectories_.size(); ++i) {
+      DrawTrajectory(
+          trajectories_[i], GetColor(i),
+          [&voxel_index_to_pixel, &aggregation,
+           this](const transform::Rigid3d& pose) -> Eigen::Array2i {
+            return voxel_index_to_pixel(aggregation.voxels.GetCellIndex(
+                (transform_ * pose.cast<float>()).translation()));
+          },
+          image.GetCairoSurface().get());
+    }
+  }
 
   image.WritePng(file_writer);
   CHECK(file_writer->Close());

cartographer/io/xray_points_processor.h

@@ -36,14 +36,16 @@ class XRayPointsProcessor : public PointsProcessor {
  public:
   constexpr static const char* kConfigurationFileActionName =
       "write_xray_image";
-  XRayPointsProcessor(double voxel_size, const transform::Rigid3f& transform,
+  enum class DrawTrajectories { kNo, kYes };
-                      const std::vector<mapping::Floor>& floors,
+  XRayPointsProcessor(
-                      const string& output_filename,
+      double voxel_size, const transform::Rigid3f& transform,
-                      FileWriterFactory file_writer_factory,
+      const std::vector<mapping::Floor>& floors,
-                      PointsProcessor* next);
+      const DrawTrajectories& draw_trajectories, const string& output_filename,
+      const std::vector<mapping::proto::Trajectory>& trajectories,
+      FileWriterFactory file_writer_factory, PointsProcessor* next);
 
   static std::unique_ptr<XRayPointsProcessor> FromDictionary(
-      const mapping::proto::Trajectory& trajectory,
+      const std::vector<mapping::proto::Trajectory>& trajectories,
       FileWriterFactory file_writer_factory,
       common::LuaParameterDictionary* dictionary, PointsProcessor* next);
 
@@ -71,8 +73,10 @@ class XRayPointsProcessor : public PointsProcessor {
                    FileWriter* const file_writer);
   void Insert(const PointsBatch& batch, Aggregation* aggregation);
 
-  PointsProcessor* const next_;
+  const DrawTrajectories draw_trajectories_;
+  const std::vector<mapping::proto::Trajectory> trajectories_;
   FileWriterFactory file_writer_factory_;
+  PointsProcessor* const next_;
 
   // If empty, we do not separate into floors.
   std::vector<mapping::Floor> floors_;