From 90e2f02e9d67f420565e0e0beb79a50f37d6ba95 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Tue, 25 Oct 2016 12:11:44 +0200 Subject: [PATCH] Add a PCD writing points processor. (#92) --- cartographer/io/CMakeLists.txt | 13 ++ .../io/pcd_writing_points_processor.cc | 119 ++++++++++++++++++ .../io/pcd_writing_points_processor.h | 46 +++++++ 3 files changed, 178 insertions(+) create mode 100644 cartographer/io/pcd_writing_points_processor.cc create mode 100644 cartographer/io/pcd_writing_points_processor.h diff --git a/cartographer/io/CMakeLists.txt b/cartographer/io/CMakeLists.txt index 8269e53..1c6c099 100644 --- a/cartographer/io/CMakeLists.txt +++ b/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 +) diff --git a/cartographer/io/pcd_writing_points_processor.cc b/cartographer/io/pcd_writing_points_processor.cc new file mode 100644 index 0000000..7765f4c --- /dev/null +++ b/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 + +#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 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 diff --git a/cartographer/io/pcd_writing_points_processor.h b/cartographer/io/pcd_writing_points_processor.h new file mode 100644 index 0000000..8fb1be7 --- /dev/null +++ b/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 + +#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 batch) override; + FlushResult Flush() override; + + private: + PointsProcessor* const next_; + + int64 num_points_; + bool has_colors_; + std::ofstream file_; +}; + +} // namespace io +} // namespace cartographer