diff --git a/CHANGELOG.md b/CHANGELOG.md index 7370cfc..7b876ed 100644 --- 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 index c07ea1d..84b0ac9 100644 --- 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 index 26d165b..72f5c03 100644 --- 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 index b09fef8..e2317a0 100644 --- 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 m_supportedFeatures; }; #endif