Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Intensities from Lidar PointCloud2 topic into the generated PCD #1762

Open
wants to merge 4 commits into
base: master
Choose a base branch
from
Open

Intensities from Lidar PointCloud2 topic into the generated PCD #1762

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
7cf62b3
Intensities from Lidar topic into the generated PCD
ikhann Oct 14, 2020
e96598c
Merge branch 'master' into master
wohe Oct 15, 2020
6fab93e
Update pcd_writing_points_processor.cc
ikhann Oct 15, 2020
8bfb644
Merge branch 'master' into master
wohe Oct 16, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
33 changes: 25 additions & 8 deletions cartographer/io/pcd_writing_points_processor.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,20 +32,25 @@ namespace {

// Writes the PCD header claiming 'num_points' will follow it into
// 'output_file'.
void WriteBinaryPcdHeader(const bool has_color, const int64 num_points,
FileWriter* const file_writer) {
void WriteBinaryPcdHeader(const bool has_color, const bool has_intensities,
const int64 num_points, FileWriter* const file_writer) {
std::string color_header_field = !has_color ? "" : " rgb";
std::string color_header_type = !has_color ? "" : " U";
std::string color_header_size = !has_color ? "" : " 4";
std::string color_header_count = !has_color ? "" : " 1";

std::string intensity_header_field = !has_intensities ? "" : " intensity";
wohe marked this conversation as resolved.
Show resolved Hide resolved
std::string intensity_header_type = !has_intensities ? "" : " F";
std::string intensity_header_size = !has_intensities ? "" : " 4";
wohe marked this conversation as resolved.
Show resolved Hide resolved
std::string intensity_header_count = !has_intensities ? "" : " 1";

std::ostringstream stream;
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"
<< "FIELDS x y z" << intensity_header_field << color_header_field <<"\n"
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Any reason why the ordering between color and intensity is swapped here compared to above for arguments? Some consistency would be nice.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The order is not important.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You mean not important for correctness, but this is setting the bar too low. Please have a look at https://google.github.io/styleguide/cppguide.html#Goals. Inconsistencies should be there for a reason, not consistency. If you aim for readable code everyone who later reads the code, including yourself, has an easier time.

<< "SIZE 4 4 4" << intensity_header_size << color_header_size << "\n"
<< "TYPE F F F" << intensity_header_type << color_header_type << "\n"
<< "COUNT 1 1 1" << intensity_header_count << 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"
Expand All @@ -65,6 +70,12 @@ void WriteBinaryPcdPointCoordinate(const Eigen::Vector3f& point,
CHECK(file_writer->Write(buffer, 12));
}

void WriteBinaryPcdIntensity(const float intensity,
FileWriter* const file_writer) {
CHECK(file_writer->Write(reinterpret_cast<const char*>(&intensity),
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is undefined behavior. See https://en.cppreference.com/w/cpp/language/reinterpret_cast. Use memcpy as above?
nit: You probably want to #include <cstring> for this and use std::memcpy. Similar for the existing code.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks. These comments were implemented in the commit below.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Did you see my nit? It is good practice to include what you use (IWYU) which here means: #include <cstring>. Also, in C++ memcpy is in the std namespace.

sizeof(float)));
}

void WriteBinaryPcdPointColor(const Uint8Color& color,
FileWriter* const file_writer) {
char buffer[4];
Expand All @@ -91,10 +102,11 @@ PcdWritingPointsProcessor::PcdWritingPointsProcessor(
: next_(next),
num_points_(0),
has_colors_(false),
has_intensities_(false),
file_writer_(std::move(file_writer)) {}

PointsProcessor::FlushResult PcdWritingPointsProcessor::Flush() {
WriteBinaryPcdHeader(has_colors_, num_points_, file_writer_.get());
WriteBinaryPcdHeader(has_colors_, has_intensities_, num_points_, file_writer_.get());
CHECK(file_writer_->Close());

switch (next_->Flush()) {
Expand All @@ -116,7 +128,8 @@ void PcdWritingPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {

if (num_points_ == 0) {
has_colors_ = !batch->colors.empty();
WriteBinaryPcdHeader(has_colors_, 0, file_writer_.get());
has_intensities_ = !batch->intensities.empty();
WriteBinaryPcdHeader(has_colors_, has_intensities_, 0, file_writer_.get());
}
for (size_t i = 0; i < batch->points.size(); ++i) {
WriteBinaryPcdPointCoordinate(batch->points[i].position,
Expand All @@ -125,6 +138,10 @@ void PcdWritingPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
WriteBinaryPcdPointColor(ToUint8Color(batch->colors[i]),
file_writer_.get());
}
if (has_intensities_) {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Isn't this wrong? Which would be another argument for consistency. In case of both colors and intensities you declare that intensities come first, but here you right them in the wrong order. Or do I misread this?

WriteBinaryPcdIntensity(batch->intensities[i],
file_writer_.get());
}
++num_points_;
}
next_->Process(std::move(batch));
Expand Down
1 change: 1 addition & 0 deletions cartographer/io/pcd_writing_points_processor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,7 @@ class PcdWritingPointsProcessor : public PointsProcessor {

int64 num_points_;
bool has_colors_;
bool has_intensities_;
std::unique_ptr<FileWriter> file_writer_;
};

Expand Down