Merge pull request #30 from ste93/depth_quantization

Depth quantization and fixes to image rotation
robotology · Jun 8, 2022 · 887d846 · 887d846
2 parents 77dd210 + 2a39c3a
commit 887d846
Show file tree

Hide file tree

Showing 3 changed files with 26 additions and 3 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -9,6 +9,8 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 ### Added
 - Added `rotateImage180` parameter to rotate the image when the camera is mounted upside down (https://github.com/robotology/yarp-device-realsense2/pull/27).
 
+- Added `QUANT_PARAM` group parameter with `depth_quant` parameter (integer) to limit decimal places in depth image  (https://github.com/robotology/yarp-device-realsense2/pull/30).
+
 ### Changed
 - If all the distortion parameters are zero, explicitly specify that the image has `YARP_DISTORTION_NONE` distortion (https://github.com/robotology/yarp-device-realsense2/pull/26).
 - Changed minimum required YARP version to 3.5 (https://github.com/robotology/yarp-device-realsense2/pull/26).

diff --git a/src/devices/realsense2/realsense2Driver.cpp b/src/devices/realsense2/realsense2Driver.cpp
@@ -772,6 +772,15 @@ bool realsense2Driver::open(Searchable& config)
     {
         params.push_back(&(p.second));
     }
+    //Manage depth quantization parameter
+    if(config.check("QUANT_PARAM")) {
+        yarp::os::Property quantCfg;
+        quantCfg.fromString(config.findGroup("QUANT_PARAM").toString());
+        m_depthQuantizationEnabled = true;
+        if (quantCfg.check("depth_quant")) {
+            m_depthDecimalNum = quantCfg.find("depth_quant").asInt32();
+        }
+    }
 
     if(config.check("rotateImage180")){
         m_rotateImage180 = config.find("rotateImage180").asBool();
@@ -1082,7 +1091,7 @@ bool realsense2Driver::getImage(FlexImage& Frame, Stamp *timeStamp, rs2::framese
         for (int i = 0; i < (color_frm.get_width() * color_frm.get_height()); i++) {
             for (size_t pixelIndex = 0; pixelIndex < bytesPerPixel(format); pixelIndex++) {
                 ((char *)Frame.getRawImage())[i * bytesPerPixel(format) + pixelIndex] = ((char *)color_frm.get_data())[
-                        ( Frame.getRawImageSize() - ((i+1) * bytesPerPixel(format) ) + pixelIndex];
+                        ( Frame.getRawImageSize()  -(( i-1) * bytesPerPixel(format))) + pixelIndex];
             }
         }
     } else {
@@ -1116,14 +1125,22 @@ bool realsense2Driver::getImage(depthImage& Frame, Stamp *timeStamp, const rs2::
 
     float* rawImage = &Frame.pixel(0,0);
     const auto * rawImageRs =(const uint16_t *) depth_frm.get_data();
+    double nearPlane;
+    double farPlane;
+    int intTemp;
+    getDepthClipPlanes(nearPlane, farPlane);
+    float powCoeff = pow(10.0f, (float) m_depthDecimalNum);
     for(int i = 0; i < w * h; i++)
     {
-
         if (m_rotateImage180) {
-            rawImage[i] = m_scale * rawImageRs[(w * h - 1) - i];
+            rawImage[i] = m_scale * rawImageRs[(w * h) - i -1];
         }else {
             rawImage[i] = m_scale * rawImageRs[i];
         }
+        if (m_depthQuantizationEnabled) {
+            intTemp = (int) (rawImage[i] * powCoeff);
+            rawImage[i] = ((float) intTemp) / powCoeff;
+        }
     }
 
     m_depth_stamp.update();

diff --git a/src/devices/realsense2/realsense2Driver.h b/src/devices/realsense2/realsense2Driver.h
@@ -133,6 +133,10 @@ class realsense2Driver :
     rs2_stream  m_alignment_stream{RS2_STREAM_COLOR};
 
 
+    // Data quantization related parameters
+    bool                             m_depthQuantizationEnabled{false};
+    int                              m_depthDecimalNum{0};
+
     yarp::os::Stamp m_rgb_stamp;
     yarp::os::Stamp m_depth_stamp;
     mutable std::string m_lastError;