Support 16 bit heightmaps

graphics/include/ignition/common/ImageHeightmap.hh

@@ -17,6 +17,7 @@
 #ifndef IGNITION_COMMON_IMAGEHEIGHTMAPDATA_HH_
 #define IGNITION_COMMON_IMAGEHEIGHTMAPDATA_HH_
 
+#include <limits>
 #include <string>
 #include <vector>
 #include <ignition/math/Vector3.hh>
@@ -63,6 +64,81 @@ namespace ignition
 
       /// \brief Image containing the heightmap data.
       private: ignition::common::Image img;
+
+      /// \brief Get Heightmap heights given the image
+      /// \param[in] _data Image data
+      /// \param[in] _pitch Size of a row of image pixels in bytes
+      /// \param[in] _subSampling Subsampling factor
+      /// \param[in] _vertSize Number of points per row.
+      /// \param[in] _size Real dimmensions of the terrain.
+      /// \param[in] _scale Vector3 used to scale the height.
+      /// \param[in] _flipY If true, it inverts the order in which the vector
+      /// is filled.
+      /// \param[out] _heights Vector containing the terrain heights.
+      private: template <typename T>
+      void FillHeights(T *_data, int _imgHeight, int _imgWidth,
+        unsigned int _pitch, int _subSampling, unsigned int _vertSize,
+        const ignition::math::Vector3d &_size,
+        const ignition::math::Vector3d &_scale,
+        bool _flipY, std::vector<float> &_heights)
+      {
+        // bytes per pixel
+        const unsigned int bpp = _pitch / _imgWidth;
+        // number of channels in a pixel
+        const unsigned int channels = bpp / sizeof(T);
+        // number of pixels in a row of image
+        const unsigned int pitchInPixels = _pitch / bpp;
+
+        const double maxPixelValue =
+            static_cast<double>(std::numeric_limits<T>::max());
+
+        // Iterate over all the vertices
+        for (unsigned int y = 0; y < _vertSize; ++y)
+        {
+          // yf ranges between 0 and 4
+          const double yf = y / static_cast<double>(_subSampling);
+          const int y1 = static_cast<int>(std::floor(yf));
+          int y2 = static_cast<int>(std::ceil(yf));
+          if (y2 >= _imgHeight)
+            y2 = _imgHeight - 1;
+          const double dy = yf - y1;
+
+          for (unsigned int x = 0; x < _vertSize; ++x)
+          {
+            const double xf = x / static_cast<double>(_subSampling);
+            const int x1 = static_cast<int>(std::floor(xf));
+            int x2 = static_cast<int>(std::ceil(xf));
+            if (x2 >= _imgWidth)
+              x2 = _imgWidth - 1;
+            const double dx = xf - x1;
+
+            const double px1 = static_cast<int>(
+              _data[(y1 * pitchInPixels + x1) * channels]) / maxPixelValue;
+            const double px2 = static_cast<int>(
+              _data[(y1 * pitchInPixels + x2) * channels]) / maxPixelValue;
+            const float h1 = (px1 - ((px1 - px2) * dx));
+
+            const double px3 = static_cast<int>(
+              _data[(y2 * pitchInPixels + x1) * channels]) / maxPixelValue;
+            const double px4 = static_cast<int>(
+              _data[(y2 * pitchInPixels + x2) * channels]) / maxPixelValue;
+            const float h2 = (px3 - ((px3 - px4) * dx));
+            float h = (h1 - ((h1 - h2) * dy)) * _scale.Z();
+
+            // invert pixel definition so 1=ground, 0=full height,
+            //   if the terrain size has a negative z component
+            //   this is mainly for backward compatibility
+            if (_size.Z() < 0)
+              h = 1.0 - h;
+
+            // Store the height for future use
+            if (!_flipY)
+              _heights[y * _vertSize + x] = h;
+            else
+              _heights[(_vertSize - y - 1) * _vertSize + x] = h;
+          }
+        }
+      }
     };
   }
 }

graphics/src/ImageHeightmap.cc

@@ -54,57 +54,44 @@ void ImageHeightmap::FillHeightMap(int _subSampling,
   // Bytes per row
   unsigned int pitch = this->img.Pitch();
 
-  // Bytes per pixel
-  unsigned int bpp = pitch / imgWidth;
+  // Get the image format so we can arrange our heightmap
+  // Currently supported: 8-bit and 16-bit.
+  auto imgFormat = this->img.PixelFormat();
 
   unsigned char *data = nullptr;
   unsigned int count;
   this->img.Data(&data, count);
 
-  // Iterate over all the vertices
-  for (unsigned int y = 0; y < _vertSize; ++y)
+  if (imgFormat == ignition::common::Image::PixelFormatType::L_INT8 ||
+    imgFormat == ignition::common::Image::PixelFormatType::RGB_INT8 ||
+    imgFormat == ignition::common::Image::PixelFormatType::RGBA_INT8 ||
+    imgFormat == ignition::common::Image::PixelFormatType::BAYER_BGGR8 ||
+    imgFormat == ignition::common::Image::PixelFormatType::BAYER_GBRG8 ||
+    imgFormat == ignition::common::Image::PixelFormatType::BAYER_GRBG8 ||
+    imgFormat == ignition::common::Image::PixelFormatType::BAYER_GRBG8 ||
+    imgFormat == ignition::common::Image::PixelFormatType::BAYER_RGGB8 ||
+    imgFormat == ignition::common::Image::PixelFormatType::BGR_INT8 ||
+    imgFormat == ignition::common::Image::PixelFormatType::BGRA_INT8)
   {
-    // yf ranges between 0 and 4
-    double yf = y / static_cast<double>(_subSampling);
-    int y1 = static_cast<int>(std::floor(yf));
-    int y2 = static_cast<int>(std::ceil(yf));
-    if (y2 >= imgHeight)
-      y2 = imgHeight-1;
-    double dy = yf - y1;
-
-    for (unsigned int x = 0; x < _vertSize; ++x)
-    {
-      double xf = x / static_cast<double>(_subSampling);
-      int x1 = static_cast<int>(std::floor(xf));
-      int x2 = static_cast<int>(std::ceil(xf));
-      if (x2 >= imgWidth)
-        x2 = imgWidth-1;
-      double dx = xf - x1;
-
-      double px1 = static_cast<int>(data[y1 * pitch + x1 * bpp]) / 255.0;
-      double px2 = static_cast<int>(data[y1 * pitch + x2 * bpp]) / 255.0;
-      float h1 = (px1 - ((px1 - px2) * dx));
-
-      double px3 = static_cast<int>(data[y2 * pitch + x1 * bpp]) / 255.0;
-      double px4 = static_cast<int>(data[y2 * pitch + x2 * bpp]) / 255.0;
-      float h2 = (px3 - ((px3 - px4) * dx));
-
-      float h = (h1 - ((h1 - h2) * dy)) * _scale.Z();
-
-      // invert pixel definition so 1=ground, 0=full height,
-      //   if the terrain size has a negative z component
-      //   this is mainly for backward compatibility
-      if (_size.Z() < 0)
-        h = 1.0 - h;
-
-      // Store the height for future use
-      if (!_flipY)
-        _heights[y * _vertSize + x] = h;
-      else
-        _heights[(_vertSize - y - 1) * _vertSize + x] = h;
-    }
+    this->FillHeights<unsigned char>(data, imgHeight, imgWidth, pitch,
+        _subSampling, _vertSize, _size, _scale, _flipY, _heights);
+  }
+  else if (imgFormat == ignition::common::Image::PixelFormatType::BGR_INT16 ||
+    imgFormat == ignition::common::Image::PixelFormatType::L_INT16 ||
+    imgFormat == ignition::common::Image::PixelFormatType::RGB_FLOAT16 ||
+    imgFormat == ignition::common::Image::PixelFormatType::RGB_INT16 ||
+    imgFormat == ignition::common::Image::PixelFormatType::R_FLOAT16)
+  {
+    uint16_t *dataShort = reinterpret_cast<uint16_t *>(data);
+    this->FillHeights<uint16_t>(dataShort, imgHeight, imgWidth, pitch,
+        _subSampling, _vertSize, _size, _scale, _flipY, _heights);
+  }
+  else
+  {
+    ignerr << "Unsupported image format, "
+      "heightmap will not be loaded" << std::endl;
+    return;
   }
-
   delete [] data;
 }