pcd_writing_points_processor.cc
Go to the documentation of this file.
1 /*
2  * Copyright 2016 The Cartographer Authors
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
18 
19 #include <iomanip>
20 #include <sstream>
21 #include <string>
22 
26 #include "glog/logging.h"
27 
28 namespace cartographer {
29 namespace io {
30 
31 namespace {
32 
33 // Writes the PCD header claiming 'num_points' will follow it into
34 // 'output_file'.
35 void WriteBinaryPcdHeader(const bool has_color, const int64 num_points,
36  FileWriter* const file_writer) {
37  std::string color_header_field = !has_color ? "" : " rgb";
38  std::string color_header_type = !has_color ? "" : " U";
39  std::string color_header_size = !has_color ? "" : " 4";
40  std::string color_header_count = !has_color ? "" : " 1";
41 
42  std::ostringstream stream;
43  stream << "# generated by Cartographer\n"
44  << "VERSION .7\n"
45  << "FIELDS x y z" << color_header_field << "\n"
46  << "SIZE 4 4 4" << color_header_size << "\n"
47  << "TYPE F F F" << color_header_type << "\n"
48  << "COUNT 1 1 1" << color_header_count << "\n"
49  << "WIDTH " << std::setw(15) << std::setfill('0') << num_points << "\n"
50  << "HEIGHT 1\n"
51  << "VIEWPOINT 0 0 0 1 0 0 0\n"
52  << "POINTS " << std::setw(15) << std::setfill('0') << num_points
53  << "\n"
54  << "DATA binary\n";
55  const std::string out = stream.str();
56  file_writer->WriteHeader(out.data(), out.size());
57 }
58 
59 void WriteBinaryPcdPointCoordinate(const Eigen::Vector3f& point,
60  FileWriter* const file_writer) {
61  char buffer[12];
62  memcpy(buffer, &point[0], sizeof(float));
63  memcpy(buffer + 4, &point[1], sizeof(float));
64  memcpy(buffer + 8, &point[2], sizeof(float));
65  CHECK(file_writer->Write(buffer, 12));
66 }
67 
68 void WriteBinaryPcdPointColor(const Uint8Color& color,
69  FileWriter* const file_writer) {
70  char buffer[4];
71  buffer[0] = color[2];
72  buffer[1] = color[1];
73  buffer[2] = color[0];
74  buffer[3] = 0;
75  CHECK(file_writer->Write(buffer, 4));
76 }
77 
78 } // namespace
79 
80 std::unique_ptr<PcdWritingPointsProcessor>
82  FileWriterFactory file_writer_factory,
83  common::LuaParameterDictionary* const dictionary,
84  PointsProcessor* const next) {
85  return common::make_unique<PcdWritingPointsProcessor>(
86  file_writer_factory(dictionary->GetString("filename")), next);
87 }
88 
90  std::unique_ptr<FileWriter> file_writer, PointsProcessor* const next)
91  : next_(next),
92  num_points_(0),
93  has_colors_(false),
94  file_writer_(std::move(file_writer)) {}
95 
97  WriteBinaryPcdHeader(has_colors_, num_points_, file_writer_.get());
98  CHECK(file_writer_->Close());
99 
100  switch (next_->Flush()) {
102  return FlushResult::kFinished;
103 
105  LOG(FATAL) << "PCD generation must be configured to occur after any "
106  "stages that require multiple passes.";
107  }
108  LOG(FATAL);
109 }
110 
111 void PcdWritingPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
112  if (batch->points.empty()) {
113  next_->Process(std::move(batch));
114  return;
115  }
116 
117  if (num_points_ == 0) {
118  has_colors_ = !batch->colors.empty();
119  WriteBinaryPcdHeader(has_colors_, 0, file_writer_.get());
120  }
121  for (size_t i = 0; i < batch->points.size(); ++i) {
122  WriteBinaryPcdPointCoordinate(batch->points[i], file_writer_.get());
123  if (!batch->colors.empty()) {
124  WriteBinaryPcdPointColor(ToUint8Color(batch->colors[i]),
125  file_writer_.get());
126  }
127  ++num_points_;
128  }
129  next_->Process(std::move(batch));
130 }
131 
132 } // namespace io
133 } // namespace cartographer
virtual void Process(std::unique_ptr< PointsBatch > points_batch)=0
std::function< std::unique_ptr< FileWriter >(const std::string &filename)> FileWriterFactory
Definition: file_writer.h:66
int64_t int64
Definition: port.h:33
PcdWritingPointsProcessor(std::unique_ptr< FileWriter > file_writer, PointsProcessor *next)
Uint8Color ToUint8Color(const FloatColor &color)
Definition: color.h:42
static std::unique_ptr< PcdWritingPointsProcessor > FromDictionary(FileWriterFactory file_writer_factory, common::LuaParameterDictionary *dictionary, PointsProcessor *next)
std::array< uint8, 3 > Uint8Color
Definition: color.h:28
virtual FlushResult Flush()=0
void Process(std::unique_ptr< PointsBatch > batch) override


cartographer
Author(s): The Cartographer Authors
autogenerated on Mon Feb 28 2022 22:00:58