Add a PCD writing points processor. (#92)

cartographer/io/CMakeLists.txt

@@ -72,3 +72,16 @@ google_library(io_xyz_writing_points_processor
   DEPENDS
     io_points_processor
 )
+
+google_library(io_pcd_points_processor
+  USES_EIGEN
+  SRCS
+    pcd_writing_points_processor.cc
+  HDRS
+    pcd_writing_points_processor.h
+  DEPENDS
+    common_math
+    io_points_processor
+    mapping_3d_hybrid_grid
+    transform_rigid_transform
+)

cartographer/io/pcd_writing_points_processor.cc

@@ -0,0 +1,119 @@
+/*
+ * 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/pcd_writing_points_processor.h"
+
+#include <iomanip>
+
+#include "glog/logging.h"
+
+namespace cartographer {
+namespace io {
+
+namespace {
+
+// Writes the PCD header claiming 'num_points' will follow it into
+// 'output_file'.
+void WriteBinaryPcdHeader(const bool has_color, const int64 num_points,
+                          std::ofstream* stream) {
+  string color_header_field = !has_color ? "" : " rgb";
+  string color_header_type = !has_color ? "" : " U";
+  string color_header_size = !has_color ? "" : " 4";
+  string color_header_count = !has_color ? "" : " 1";
+
+  (*stream) << "# generated by Cartographer\n"
+            << "VERSION .7\n"
+            << "FIELDS x y z" << color_header_field << "\n"
+            << "SIZE 4 4 4" << color_header_size << "\n"
+            << "TYPE F F F" << color_header_type << "\n"
+            << "COUNT 1 1 1" << color_header_count << "\n"
+            << "WIDTH " << std::setw(15) << std::setfill('0')
+            << num_points << "\n"
+            << "HEIGHT 1\n"
+            << "VIEWPOINT 0 0 0 1 0 0 0\n"
+            << "POINTS " << std::setw(15) << std::setfill('0')
+            << num_points << "\n"
+            << "DATA binary\n";
+}
+
+void WriteBinaryPcdPointCoordinate(const Eigen::Vector3f& point,
+                                   std::ofstream* stream) {
+  char buffer[12];
+  memcpy(buffer, &point[0], sizeof(float));
+  memcpy(buffer + 4, &point[1], sizeof(float));
+  memcpy(buffer + 8, &point[2], sizeof(float));
+  for (int i = 0; i < 12; ++i) {
+    stream->put(buffer[i]);
+  }
+}
+
+void WriteBinaryPcdPointColor(const Color& color, std::ostream* stream) {
+  // Pack the color as uint32 little-endian
+  stream->put(color[2]);
+  stream->put(color[1]);
+  stream->put(color[0]);
+  stream->put(0);
+}
+
+}  // namespace
+
+PcdWritingPointsProcessor::PcdWritingPointsProcessor(const string& filename,
+                                                     PointsProcessor* next)
+    : next_(next),
+      num_points_(0),
+      has_colors_(false),
+      file_(filename, std::ios_base::out | std::ios_base::binary) {}
+
+PointsProcessor::FlushResult PcdWritingPointsProcessor::Flush() {
+  file_.flush();
+  file_.seekp(0);
+
+  WriteBinaryPcdHeader(has_colors_, num_points_, &file_);
+  file_.flush();
+
+  switch (next_->Flush()) {
+    case FlushResult::kFinished:
+      return FlushResult::kFinished;
+
+    case FlushResult::kRestartStream:
+      LOG(FATAL) << "PCD generation must be configured to occur after any "
+                    "stages that require multiple passes.";
+  }
+  LOG(FATAL);
+}
+
+void PcdWritingPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
+  if (batch->points.empty()) {
+    next_->Process(std::move(batch));
+    return;
+  }
+
+  if (num_points_ == 0) {
+    has_colors_ = !batch->colors.empty();
+    WriteBinaryPcdHeader(has_colors_, 0, &file_);
+  }
+  for (size_t i = 0; i < batch->points.size(); ++i) {
+    WriteBinaryPcdPointCoordinate(batch->points[i], &file_);
+    if (!batch->colors.empty()) {
+      WriteBinaryPcdPointColor(batch->colors[i], &file_);
+    }
+    ++num_points_;
+  }
+  next_->Process(std::move(batch));
+}
+
+}  // namespace io
+}  // namespace cartographer

cartographer/io/pcd_writing_points_processor.h

@@ -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 <fstream>
+
+#include "cartographer/io/points_processor.h"
+
+namespace cartographer {
+namespace io {
+
+// Streams a PCD file to disk. The header is written in 'Flush'.
+class PcdWritingPointsProcessor : public PointsProcessor {
+ public:
+  PcdWritingPointsProcessor(const string& filename, PointsProcessor* next);
+  ~PcdWritingPointsProcessor() override {}
+
+  PcdWritingPointsProcessor(const PcdWritingPointsProcessor&) = delete;
+  PcdWritingPointsProcessor& operator=(const PcdWritingPointsProcessor&) =
+      delete;
+
+  void Process(std::unique_ptr<PointsBatch> batch) override;
+  FlushResult Flush() override;
+
+ private:
+  PointsProcessor* const next_;
+
+  int64 num_points_;
+  bool has_colors_;
+  std::ofstream file_;
+};
+
+}  // namespace io
+}  // namespace cartographer