ply_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 PLY header claiming 'num_points' will follow it into
34 // 'output_file'.
35 void WriteBinaryPlyHeader(const bool has_color, const int64 num_points,
36  FileWriter* const file_writer) {
37  string color_header = !has_color ? ""
38  : "property uchar red\n"
39  "property uchar green\n"
40  "property uchar blue\n";
41  std::ostringstream stream;
42  stream << "ply\n"
43  << "format binary_little_endian 1.0\n"
44  << "comment generated by Cartographer\n"
45  << "element vertex " << std::setw(15) << std::setfill('0')
46  << num_points << "\n"
47  << "property float x\n"
48  << "property float y\n"
49  << "property float z\n"
50  << color_header << "end_header\n";
51  const string out = stream.str();
52  CHECK(file_writer->WriteHeader(out.data(), out.size()));
53 }
54 
55 void WriteBinaryPlyPointCoordinate(const Eigen::Vector3f& point,
56  FileWriter* const file_writer) {
57  char buffer[12];
58  memcpy(buffer, &point[0], sizeof(float));
59  memcpy(buffer + 4, &point[1], sizeof(float));
60  memcpy(buffer + 8, &point[2], sizeof(float));
61  CHECK(file_writer->Write(buffer, 12));
62 }
63 
64 void WriteBinaryPlyPointColor(const Color& color,
65  FileWriter* const file_writer) {
66  CHECK(file_writer->Write(reinterpret_cast<const char*>(color.data()),
67  color.size()));
68 }
69 
70 } // namespace
71 
72 std::unique_ptr<PlyWritingPointsProcessor>
74  FileWriterFactory file_writer_factory,
75  common::LuaParameterDictionary* const dictionary,
76  PointsProcessor* const next) {
77  return common::make_unique<PlyWritingPointsProcessor>(
78  file_writer_factory(dictionary->GetString("filename")), next);
79 }
80 
82  std::unique_ptr<FileWriter> file_writer, PointsProcessor* const next)
83  : next_(next),
84  num_points_(0),
85  has_colors_(false),
86  file_(std::move(file_writer)) {}
87 
89  WriteBinaryPlyHeader(has_colors_, num_points_, file_.get());
90  CHECK(file_->Close()) << "Closing PLY file_writer failed.";
91 
92  switch (next_->Flush()) {
95 
97  LOG(FATAL) << "PLY generation must be configured to occur after any "
98  "stages that require multiple passes.";
99  }
100  LOG(FATAL);
101 }
102 
103 void PlyWritingPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
104  if (batch->points.empty()) {
105  next_->Process(std::move(batch));
106  return;
107  }
108 
109  if (num_points_ == 0) {
110  has_colors_ = !batch->colors.empty();
111  WriteBinaryPlyHeader(has_colors_, 0, file_.get());
112  }
113  if (has_colors_) {
114  CHECK_EQ(batch->points.size(), batch->colors.size())
115  << "First PointsBatch had colors, but encountered one without. "
116  "frame_id: "
117  << batch->frame_id;
118  }
119 
120  for (size_t i = 0; i < batch->points.size(); ++i) {
121  WriteBinaryPlyPointCoordinate(batch->points[i], file_.get());
122  if (has_colors_) {
123  WriteBinaryPlyPointColor(batch->colors[i], file_.get());
124  }
125  ++num_points_;
126  }
127  next_->Process(std::move(batch));
128 }
129 
130 } // namespace io
131 } // namespace cartographer
virtual void Process(std::unique_ptr< PointsBatch > points_batch)=0
std::function< std::unique_ptr< FileWriter >(const string &filename)> FileWriterFactory
Definition: file_writer.h:63
std::array< uint8_t, 3 > Color
Definition: points_batch.h:31
void Process(std::unique_ptr< PointsBatch > batch) override
PlyWritingPointsProcessor(std::unique_ptr< FileWriter > file, PointsProcessor *next)
virtual FlushResult Flush()=0
int64_t int64
Definition: port.h:31
static std::unique_ptr< PlyWritingPointsProcessor > FromDictionary(FileWriterFactory file_writer_factory, common::LuaParameterDictionary *dictionary, PointsProcessor *next)


cartographer
Author(s):
autogenerated on Wed Jun 5 2019 21:57:58