Save RGB as integer all point types

PointCloudLibrary · Nov 2, 2015 · 10a2124 · 10a2124
1 parent 14d7e10
commit 10a2124
Show file tree

Hide file tree

Showing 2 changed files with 70 additions and 14 deletions.
diff --git a/io/include/pcl/io/impl/pcd_io.hpp b/io/include/pcl/io/impl/pcd_io.hpp
@@ -91,7 +91,10 @@ pcl::PCDWriter::generateHeader (const pcl::PointCloud<PointT> &cloud, const int
     // Add the regular dimension
     field_names << " " << fields[i].name;
     field_sizes << " " << pcl::getFieldSize (fields[i].datatype);
-    field_types << " " << pcl::getFieldType (fields[i].datatype);
+    if ("rgb" == fields[i].name)
+      field_types << " " << "U";
+    else
+      field_types << " " << pcl::getFieldType (fields[i].datatype);
     int count = abs (static_cast<int> (fields[i].count));
     if (count == 0) count = 1;  // check for 0 counts (coming from older converter code)
     field_counts << " " << count;
@@ -576,12 +579,30 @@ pcl::PCDWriter::writeASCII (const std::string &file_name, const pcl::PointCloud<
           }
           case pcl::PCLPointField::FLOAT32:
           {
-            float value;
-            memcpy (&value, reinterpret_cast<const char*> (&cloud.points[i]) + fields[d].offset + c * sizeof (float), sizeof (float));
-            if (pcl_isnan (value))
-              stream << "nan";
+            /*
+             * Despite the float type, store the rgb field as uint32
+             * because several fully opaque color values are mapped to
+             * nan.
+             */
+            if ("rgb" == fields[d].name)
+            {
+              uint32_t value;
+              memcpy (&value, reinterpret_cast<const char*> (&cloud.points[i]) + fields[d].offset + c * sizeof (float), sizeof (float));
+              if (pcl_isnan (value))
+                stream << "nan";
+              else
+                stream << boost::numeric_cast<uint32_t>(value);
+              break;
+            }
             else
-              stream << boost::numeric_cast<float>(value);
+            {
+              float value;
+              memcpy (&value, reinterpret_cast<const char*> (&cloud.points[i]) + fields[d].offset + c * sizeof (float), sizeof (float));
+              if (pcl_isnan (value))
+                stream << "nan";
+              else
+                stream << boost::numeric_cast<float>(value);
+            }
             break;
           }
           case pcl::PCLPointField::FLOAT64:
@@ -877,12 +898,29 @@ pcl::PCDWriter::writeASCII (const std::string &file_name,
           }
           case pcl::PCLPointField::FLOAT32:
           {
-            float value;
-            memcpy (&value, reinterpret_cast<const char*> (&cloud.points[indices[i]]) + fields[d].offset + c * sizeof (float), sizeof (float));
-            if (pcl_isnan (value))
-              stream << "nan";
+            /*
+             * Despite the float type, store the rgb field as uint32
+             * because several fully opaque color values are mapped to
+             * nan.
+             */
+            if ("rgb" == fields[i].name)
+            {
+              uint32_t value;
+              memcpy (&value, reinterpret_cast<const char*> (&cloud.points[indices[i]]) + fields[d].offset + c * sizeof (float), sizeof (float));
+              if (pcl_isnan (value))
+                stream << "nan";
+              else
+                stream << boost::numeric_cast<uint32_t>(value);
+            }
             else
-              stream << boost::numeric_cast<float>(value);
+            {
+              float value;
+              memcpy (&value, reinterpret_cast<const char*> (&cloud.points[indices[i]]) + fields[d].offset + c * sizeof (float), sizeof (float));
+              if (pcl_isnan (value))
+                stream << "nan";
+              else
+                stream << boost::numeric_cast<float>(value);
+            }
             break;
           }
           case pcl::PCLPointField::FLOAT64:

diff --git a/io/src/pcd_io.cpp b/io/src/pcd_io.cpp
@@ -1105,11 +1105,21 @@ pcl::PCDWriter::generateHeaderASCII (const pcl::PCLPointCloud2 &cloud,
   {
     // Ignore invalid padded dimensions that are inherited from binary data
     if (cloud.fields[d].name != "_")
-      stream << pcl::getFieldType (cloud.fields[d].datatype) << " ";
+    {
+      if (cloud.fields[d].name == "rgb")
+        stream << "U ";
+      else
+        stream << pcl::getFieldType (cloud.fields[d].datatype) << " ";
+    }
   }
   // Ignore invalid padded dimensions that are inherited from binary data
   if (cloud.fields[cloud.fields.size () - 1].name != "_")
-    stream << pcl::getFieldType (cloud.fields[cloud.fields.size () - 1].datatype);
+  {
+    if (cloud.fields[cloud.fields.size () - 1].name == "rgb")
+      stream << "U";
+    else
+      stream << pcl::getFieldType (cloud.fields[cloud.fields.size () - 1].datatype);
+  }
 
   // Remove trailing spaces
   result = stream.str ();
@@ -1370,7 +1380,15 @@ pcl::PCDWriter::writeASCII (const std::string &file_name, const pcl::PCLPointClo
           }
           case pcl::PCLPointField::FLOAT32:
           {
-            copyValueString<pcl::traits::asType<pcl::PCLPointField::FLOAT32>::type>(cloud, i, point_size, d, c, stream);
+            /*
+             * Despite the float type, store the rgb field as uint32
+             * because several fully opaque color values are mapped to
+             * nan.
+             */
+            if ("rgb" == cloud.fields[d].name)
+              copyValueString<pcl::traits::asType<pcl::PCLPointField::UINT32>::type>(cloud, i, point_size, d, c, stream);
+            else
+              copyValueString<pcl::traits::asType<pcl::PCLPointField::FLOAT32>::type>(cloud, i, point_size, d, c, stream);
             break;
           }
           case pcl::PCLPointField::FLOAT64: