12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Hand through intensities into the generated PLY - if they exist. (#885)

master
Holger Rapp 2018-02-02 18:29:18 +01:00 committed by Wally B. Feed
parent 655bd65abc
commit c212bbb698
2 changed files with 30 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
cartographer/io/ply_writing_points_processor.cc
View File

@ -32,12 +32,15 @@ namespace {
// Writes the PLY header claiming 'num_points' will follow it into // Writes the PLY header claiming 'num_points' will follow it into
// 'output_file'. // 'output_file'.
void WriteBinaryPlyHeader(const bool has_color, const int64 num_points, void WriteBinaryPlyHeader(const bool has_color, const bool has_intensities,
const int64 num_points,
FileWriter* const file_writer) { FileWriter* const file_writer) {
std::string color_header = !has_color ? "" const std::string color_header = !has_color ? ""
: "property uchar red\n" : "property uchar red\n"
"property uchar green\n" "property uchar green\n"
"property uchar blue\n"; "property uchar blue\n";
const std::string intensity_header =
!has_intensities ? "" : "property float intensity\n";
std::ostringstream stream; std::ostringstream stream;
stream << "ply\n" stream << "ply\n"
<< "format binary_little_endian 1.0\n" << "format binary_little_endian 1.0\n"
@ -47,13 +50,14 @@ void WriteBinaryPlyHeader(const bool has_color, const int64 num_points,
<< "property float x\n" << "property float x\n"
<< "property float y\n" << "property float y\n"
<< "property float z\n" << "property float z\n"
<< color_header << "end_header\n"; << color_header << intensity_header << "end_header\n";
const std::string out = stream.str(); const std::string out = stream.str();
CHECK(file_writer->WriteHeader(out.data(), out.size())); CHECK(file_writer->WriteHeader(out.data(), out.size()));
} }
void WriteBinaryPlyPointCoordinate(const Eigen::Vector3f& point, void WriteBinaryPlyPointCoordinate(const Eigen::Vector3f& point,
FileWriter* const file_writer) { FileWriter* const file_writer) {
// TODO(sirver): This ignores endianness.
char buffer[12]; char buffer[12];
memcpy(buffer, &point[0], sizeof(float)); memcpy(buffer, &point[0], sizeof(float));
memcpy(buffer + 4, &point[1], sizeof(float)); memcpy(buffer + 4, &point[1], sizeof(float));
@ -61,6 +65,13 @@ void WriteBinaryPlyPointCoordinate(const Eigen::Vector3f& point,
CHECK(file_writer->Write(buffer, 12)); CHECK(file_writer->Write(buffer, 12));
} }
void WriteBinaryIntensity(const float intensity,
FileWriter* const file_writer) {
// TODO(sirver): This ignores endianness.
CHECK(file_writer->Write(reinterpret_cast<const char*>(&intensity),
sizeof(float)));
}
void WriteBinaryPlyPointColor(const Uint8Color& color, void WriteBinaryPlyPointColor(const Uint8Color& color,
FileWriter* const file_writer) { FileWriter* const file_writer) {
CHECK(file_writer->Write(reinterpret_cast<const char*>(color.data()), CHECK(file_writer->Write(reinterpret_cast<const char*>(color.data()),
@ -86,7 +97,7 @@ PlyWritingPointsProcessor::PlyWritingPointsProcessor(
file_(std::move(file_writer)) {} file_(std::move(file_writer)) {}
PointsProcessor::FlushResult PlyWritingPointsProcessor::Flush() { PointsProcessor::FlushResult PlyWritingPointsProcessor::Flush() {
WriteBinaryPlyHeader(has_colors_, num_points_, file_.get()); WriteBinaryPlyHeader(has_colors_, has_intensities_, num_points_, file_.get());
CHECK(file_->Close()) << "Closing PLY file_writer failed."; CHECK(file_->Close()) << "Closing PLY file_writer failed.";
switch (next_->Flush()) { switch (next_->Flush()) {
@ -108,7 +119,8 @@ void PlyWritingPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
if (num_points_ == 0) { if (num_points_ == 0) {
has_colors_ = !batch->colors.empty(); has_colors_ = !batch->colors.empty();
WriteBinaryPlyHeader(has_colors_, 0, file_.get()); has_intensities_ = !batch->intensities.empty();
WriteBinaryPlyHeader(has_colors_, has_intensities_, 0, file_.get());
} }
if (has_colors_) { if (has_colors_) {
CHECK_EQ(batch->points.size(), batch->colors.size()) CHECK_EQ(batch->points.size(), batch->colors.size())
@ -116,12 +128,21 @@ void PlyWritingPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
"frame_id: " "frame_id: "
<< batch->frame_id; << batch->frame_id;
} }
if (has_intensities_) {
CHECK_EQ(batch->points.size(), batch->intensities.size())
<< "First PointsBatch had intensities, but encountered one without. "
"frame_id: "
<< batch->frame_id;
}
for (size_t i = 0; i < batch->points.size(); ++i) { for (size_t i = 0; i < batch->points.size(); ++i) {
WriteBinaryPlyPointCoordinate(batch->points[i], file_.get()); WriteBinaryPlyPointCoordinate(batch->points[i], file_.get());
if (has_colors_) { if (has_colors_) {
WriteBinaryPlyPointColor(ToUint8Color(batch->colors[i]), file_.get()); WriteBinaryPlyPointColor(ToUint8Color(batch->colors[i]), file_.get());
} }
if (has_intensities_) {
WriteBinaryIntensity(batch->intensities[i], file_.get());
}
++num_points_; ++num_points_;
} }
next_->Process(std::move(batch)); next_->Process(std::move(batch));

1
cartographer/io/ply_writing_points_processor.h
View File

@ -46,6 +46,7 @@ class PlyWritingPointsProcessor : public PointsProcessor {
int64 num_points_; int64 num_points_;
bool has_colors_; bool has_colors_;
bool has_intensities_;
std::unique_ptr<FileWriter> file_; std::unique_ptr<FileWriter> file_;
}; };