Intensities from Lidar topic into the generated PCD

Signed-off-by: Ханинаев Даниил Витальевич <daniil.khaninaev@PC870-Ubuntu.nami.local>
Signed-off-by: Daniil Khaninaev <khaninaev@yahoo.com>

Author: ikhann

cartographer/io/pcd_writing_points_processor.cc

@@ -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";
+  std::string intensity_header_type = !has_intensities ? "" : " F";
+  std::string intensity_header_size = !has_intensities ? "" : " 4";
+  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"
+         << "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"
@@ -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),
+                           sizeof(float)));
+}
+
 void WriteBinaryPcdPointColor(const Uint8Color& color,
                               FileWriter* const file_writer) {
   char buffer[4];
@@ -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()) {
@@ -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,
@@ -125,6 +138,10 @@ void PcdWritingPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
       WriteBinaryPcdPointColor(ToUint8Color(batch->colors[i]),
                                file_writer_.get());
     }
+    if (has_intensities_) {
+      WriteBinaryPcdIntensity(batch->intensities[i],
+                              file_writer_.get());
+    }
     ++num_points_;
   }
   next_->Process(std::move(batch));

cartographer/io/pcd_writing_points_processor.h

@@ -48,6 +48,7 @@ class PcdWritingPointsProcessor : public PointsProcessor {
 
   int64 num_points_;
   bool has_colors_;
+  bool has_intensities_;
   std::unique_ptr<FileWriter> file_writer_;
 };