updated flip realsense (#27)

robotology · Apr 14, 2022 · 7ee8604 · 7ee8604
1 parent 3d1724a
commit 7ee8604
Show file tree

Hide file tree

Showing 4 changed files with 28 additions and 4 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -6,6 +6,9 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ## [Unreleased]
 
+### Added
+- Added `rotateImage180` parameter to rotate the image when the camera is mounted upside down (https://github.com/robotology/yarp-device-realsense2/pull/27).
+
 ### 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/README.md b/README.md
@@ -226,6 +226,7 @@ Parameters used by this device are:
 |:----------------------------:|:-----------------:|:--------------:|:------------:|:-------:|:-------------:|:---------------:|:-------------------------------------------------------------------------------------:|:---------------------------------------------------------------------:|
 |  `stereoMode`                |     -             | bool           | Read / write |         |   false       |  No(see notes)  | Flag for using the realsense as stereo camera                                         |  This option is to use it with yarp::dev::ServerGrabber as network wrapper. The stereo images provided are raw images(yarp::sig::PixelMono) and note that not all the realsense devices have the stereo streams. |
 |  `verbose`                   |     -             | bool           | Read / write |         |   false       |  No             | Flag for enabling debug prints                                                        |                                                                       |
+|  `rotateImage180`            |                   | bool           | Read / write | -       |   -           |  No             | Flag for enabling rotating the image 180 degrees                                      |  Parameter useful when a camera is mounted upside down |
 |  `SETTINGS`                  |     -             | group          | Read / write | -       |   -           |  Yes            | Initial setting of the device.                                                        |  Properties must be read/writable in order for setting to work        |
 |                              | `rgbResolution`   | int, int       | Read / write | pixels  |   -           |  Yes            | Size of rgb image in pixels                                                           |  2 values expected as height, width                                   |
 |                              | `depthResolution` | int, int       | Read / write | pixels  |   -           |  Yes            | Size of depth image in pixels                                                         |  Values are height, width                                             |

diff --git a/src/devices/realsense2/realsense2Driver.cpp b/src/devices/realsense2/realsense2Driver.cpp
@@ -773,6 +773,12 @@ bool realsense2Driver::open(Searchable& config)
         params.push_back(&(p.second));
     }
 
+    if(config.check("rotateImage180")){
+        m_rotateImage180 = config.find("rotateImage180").asBool();
+        if (m_rotateImage180) {
+            yCInfo(REALSENSE2) << "parameter rotateImage180 enabled, the image is rotated";
+        }
+    }
     m_verbose = config.check("verbose");
     if (config.check("stereoMode")) {
         m_stereoMode = config.find("stereoMode").asBool();
@@ -1072,8 +1078,16 @@ bool realsense2Driver::getImage(FlexImage& Frame, Stamp *timeStamp, rs2::framese
         yCError(REALSENSE2) << "Device and local copy data size doesn't match";
         return false;
     }
-
-    memcpy((void*)Frame.getRawImage(), (void*)color_frm.get_data(), mem_to_wrt);
+    if (m_rotateImage180) {
+        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];
+            }
+        }
+    } else {
+        memcpy((void*)Frame.getRawImage(), (void*)color_frm.get_data(), mem_to_wrt);
+    }
     m_rgb_stamp.update();
     if (timeStamp != nullptr)
     {
@@ -1102,10 +1116,14 @@ 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();
-
     for(int i = 0; i < w * h; i++)
     {
-        rawImage[i] = m_scale * rawImageRs[i];
+
+        if (m_rotateImage180) {
+            rawImage[i] = m_scale * rawImageRs[(w * h - 1) - i];
+        }else {
+            rawImage[i] = m_scale * rawImageRs[i];
+        }
     }
 
     m_depth_stamp.update();

diff --git a/src/devices/realsense2/realsense2Driver.h b/src/devices/realsense2/realsense2Driver.h
@@ -37,6 +37,7 @@ class realsense2Driver :
     typedef yarp::os::Property                        Property;
     typedef yarp::sig::FlexImage                      FlexImage;
 
+
 public:
     realsense2Driver();
     ~realsense2Driver() override = default;
@@ -141,6 +142,7 @@ class realsense2Driver :
     bool m_needAlignment;
     int m_fps;
     float m_scale;
+    bool m_rotateImage180{false};
     std::vector<cameraFeature_id_t> m_supportedFeatures;
 };
 #endif